EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

the lowstate bridge is completed.

This commit is contained in:
Rooholla-Khorrambakht 2024-01-04 15:10:50 -05:00
parent 95cff05d6a
commit 9bb096405d
5 changed files with 354 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Go2Py/idl/LowState.idl
View File

@ -6,6 +6,11 @@ module msgs{
float tau_est[12]; float tau_est[12];
float tmp[12]; float tmp[12];
float contact[4]; float contact[4];
float quat[4];
float gyro[3];
float accel[3];
float rpy[3];
octet imu_tmp;
float voltage; float voltage;
float current; float current;
}; };

5
Go2Py/msgs/_LowState.py
View File

@ -28,6 +28,11 @@ class LowState(idl.IdlStruct, typename="msgs.LowState"):
tau_est: types.array[types.float32, 12] tau_est: types.array[types.float32, 12]
tmp: types.array[types.float32, 12] tmp: types.array[types.float32, 12]
contact: types.array[types.float32, 4] contact: types.array[types.float32, 4]
quat: types.array[types.float32, 4]
gyro: types.array[types.float32, 3]
accel: types.array[types.float32, 3]
rpy: types.array[types.float32, 3]
imu_tmp: types.uint8
voltage: types.float32 voltage: types.float32
current: types.float32 current: types.float32

9
cpp_bridge/include/go2py/LowState.cpp
View File

@ -43,8 +43,13 @@ propvec &get_type_props<::msgs::LowState>() {
props.push_back(entity_properties_t(1, 3, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::tau_est props.push_back(entity_properties_t(1, 3, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::tau_est
props.push_back(entity_properties_t(1, 4, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::tmp props.push_back(entity_properties_t(1, 4, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::tmp
props.push_back(entity_properties_t(1, 5, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::contact props.push_back(entity_properties_t(1, 5, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::contact
props.push_back(entity_properties_t(1, 6, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::voltage props.push_back(entity_properties_t(1, 6, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::quat
props.push_back(entity_properties_t(1, 7, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::current props.push_back(entity_properties_t(1, 7, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::gyro
props.push_back(entity_properties_t(1, 8, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::accel
props.push_back(entity_properties_t(1, 9, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::rpy
props.push_back(entity_properties_t(1, 10, false, get_bit_bound<uint8_t>(), extensibility::ext_final, false)); //::imu_tmp
props.push_back(entity_properties_t(1, 11, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::voltage
props.push_back(entity_properties_t(1, 12, false, get_bit_bound<float>(), extensibility::ext_final, false)); //::current
entity_properties_t::finish(props, keylist); entity_properties_t::finish(props, keylist);
props_end = props.data() + props.size(); props_end = props.data() + props.size();

333
cpp_bridge/include/go2py/LowState.hpp
View File

@ -9,6 +9,7 @@
#ifndef DDSCXX_LOWSTATE_HPP #ifndef DDSCXX_LOWSTATE_HPP
#define DDSCXX_LOWSTATE_HPP #define DDSCXX_LOWSTATE_HPP
#include <cstdint>
#include <array> #include <array>
namespace msgs namespace msgs
@ -22,6 +23,11 @@ private:
std::array<float, 12> tau_est_ = { }; std::array<float, 12> tau_est_ = { };
std::array<float, 12> tmp_ = { }; std::array<float, 12> tmp_ = { };
std::array<float, 4> contact_ = { }; std::array<float, 4> contact_ = { };
std::array<float, 4> quat_ = { };
std::array<float, 3> gyro_ = { };
std::array<float, 3> accel_ = { };
std::array<float, 3> rpy_ = { };
uint8_t imu_tmp_ = 0;
float voltage_ = 0.0f; float voltage_ = 0.0f;
float current_ = 0.0f; float current_ = 0.0f;
@ -35,6 +41,11 @@ public:
const std::array<float, 12>& tau_est, const std::array<float, 12>& tau_est,
const std::array<float, 12>& tmp, const std::array<float, 12>& tmp,
const std::array<float, 4>& contact, const std::array<float, 4>& contact,
const std::array<float, 4>& quat,
const std::array<float, 3>& gyro,
const std::array<float, 3>& accel,
const std::array<float, 3>& rpy,
uint8_t imu_tmp,
float voltage, float voltage,
float current) : float current) :
q_(q), q_(q),
@ -43,6 +54,11 @@ public:
tau_est_(tau_est), tau_est_(tau_est),
tmp_(tmp), tmp_(tmp),
contact_(contact), contact_(contact),
quat_(quat),
gyro_(gyro),
accel_(accel),
rpy_(rpy),
imu_tmp_(imu_tmp),
voltage_(voltage), voltage_(voltage),
current_(current) { } current_(current) { }
@ -70,6 +86,25 @@ public:
std::array<float, 4>& contact() { return this->contact_; } std::array<float, 4>& contact() { return this->contact_; }
void contact(const std::array<float, 4>& _val_) { this->contact_ = _val_; } void contact(const std::array<float, 4>& _val_) { this->contact_ = _val_; }
void contact(std::array<float, 4>&& _val_) { this->contact_ = _val_; } void contact(std::array<float, 4>&& _val_) { this->contact_ = _val_; }
const std::array<float, 4>& quat() const { return this->quat_; }
std::array<float, 4>& quat() { return this->quat_; }
void quat(const std::array<float, 4>& _val_) { this->quat_ = _val_; }
void quat(std::array<float, 4>&& _val_) { this->quat_ = _val_; }
const std::array<float, 3>& gyro() const { return this->gyro_; }
std::array<float, 3>& gyro() { return this->gyro_; }
void gyro(const std::array<float, 3>& _val_) { this->gyro_ = _val_; }
void gyro(std::array<float, 3>&& _val_) { this->gyro_ = _val_; }
const std::array<float, 3>& accel() const { return this->accel_; }
std::array<float, 3>& accel() { return this->accel_; }
void accel(const std::array<float, 3>& _val_) { this->accel_ = _val_; }
void accel(std::array<float, 3>&& _val_) { this->accel_ = _val_; }
const std::array<float, 3>& rpy() const { return this->rpy_; }
std::array<float, 3>& rpy() { return this->rpy_; }
void rpy(const std::array<float, 3>& _val_) { this->rpy_ = _val_; }
void rpy(std::array<float, 3>&& _val_) { this->rpy_ = _val_; }
uint8_t imu_tmp() const { return this->imu_tmp_; }
uint8_t& imu_tmp() { return this->imu_tmp_; }
void imu_tmp(uint8_t _val_) { this->imu_tmp_ = _val_; }
float voltage() const { return this->voltage_; } float voltage() const { return this->voltage_; }
float& voltage() { return this->voltage_; } float& voltage() { return this->voltage_; }
void voltage(float _val_) { this->voltage_ = _val_; } void voltage(float _val_) { this->voltage_ = _val_; }
@ -86,6 +121,11 @@ public:
tau_est_ == _other.tau_est_ && tau_est_ == _other.tau_est_ &&
tmp_ == _other.tmp_ && tmp_ == _other.tmp_ &&
contact_ == _other.contact_ && contact_ == _other.contact_ &&
quat_ == _other.quat_ &&
gyro_ == _other.gyro_ &&
accel_ == _other.accel_ &&
rpy_ == _other.rpy_ &&
imu_tmp_ == _other.imu_tmp_ &&
voltage_ == _other.voltage_ && voltage_ == _other.voltage_ &&
current_ == _other.current_; current_ == _other.current_;
} }
@ -118,13 +158,13 @@ template <> constexpr bool TopicTraits<::msgs::LowState>::isKeyless()
} }
#ifdef DDSCXX_HAS_TYPE_DISCOVERY #ifdef DDSCXX_HAS_TYPE_DISCOVERY
template<> constexpr unsigned int TopicTraits<::msgs::LowState>::type_map_blob_sz() { return 630; } template<> constexpr unsigned int TopicTraits<::msgs::LowState>::type_map_blob_sz() { return 930; }
template<> constexpr unsigned int TopicTraits<::msgs::LowState>::type_info_blob_sz() { return 100; } template<> constexpr unsigned int TopicTraits<::msgs::LowState>::type_info_blob_sz() { return 100; }
template<> inline const uint8_t * TopicTraits<::msgs::LowState>::type_map_blob() { template<> inline const uint8_t * TopicTraits<::msgs::LowState>::type_map_blob() {
static const uint8_t blob[] = { static const uint8_t blob[] = {
0xf3, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0xf1, 0xd4, 0x69, 0x80, 0x35, 0xf5, 0x8b, 0x51, 0x73, 0x01, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0xf1, 0xec, 0xa8, 0x61, 0xd2, 0xd7, 0x0d, 0xa1,
0xea, 0x7f, 0x47, 0x3a, 0x7b, 0x09, 0x0f, 0x00, 0xdb, 0x00, 0x00, 0x00, 0xf1, 0x51, 0x01, 0x00, 0x5b, 0xdc, 0x10, 0x55, 0xb8, 0xdc, 0x20, 0x00, 0x5b, 0x01, 0x00, 0x00, 0xf1, 0x51, 0x01, 0x00,
0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xcb, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x4b, 0x01, 0x00, 0x00, 0x0d, 0x00, 0x00, 0x00,
0x16, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x76, 0x94, 0xf4, 0xa6, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x76, 0x94, 0xf4, 0xa6, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00,
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
@ -135,13 +175,21 @@ template<> inline const uint8_t * TopicTraits<::msgs::LowState>::type_map_blob()
0x16, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0xfa, 0x81, 0x6e, 0xdb, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x09, 0xfa, 0x81, 0x6e, 0xdb, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00,
0x05, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
0x04, 0x09, 0x2f, 0x8a, 0x6b, 0xf3, 0x00, 0x00, 0x0b, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x04, 0x09, 0x2f, 0x8a, 0x6b, 0xf3, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
0x01, 0x00, 0x09, 0xe4, 0x37, 0xba, 0x43, 0x00, 0x0b, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x09, 0x21, 0xd7,
0x01, 0x00, 0x09, 0x43, 0xb5, 0xc9, 0x17, 0x00, 0x52, 0x01, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0xdc, 0x6a, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3,
0xf2, 0x84, 0x20, 0x63, 0xda, 0x5c, 0x28, 0xbb, 0x27, 0x92, 0xe4, 0x1c, 0xce, 0x56, 0xbc, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x41, 0xe1, 0xdb, 0x58, 0x00, 0x00,
0x3a, 0x01, 0x00, 0x00, 0xf2, 0x51, 0x01, 0x00, 0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x98, 0x30, 0x99, 0x65, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00,
0x09, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
0x03, 0x09, 0xb0, 0x7d, 0x92, 0xed, 0x00, 0x00, 0x0b, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00,
0x01, 0x00, 0x02, 0x04, 0x25, 0x70, 0x6e, 0x00, 0x0b, 0x00, 0x00, 0x00, 0x0b, 0x00, 0x00, 0x00,
0x01, 0x00, 0x09, 0xe4, 0x37, 0xba, 0x43, 0x00, 0x0b, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00,
0x01, 0x00, 0x09, 0x43, 0xb5, 0xc9, 0x17, 0x00, 0xfe, 0x01, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
0xf2, 0xf7, 0x0b, 0xf4, 0x7b, 0xfb, 0x17, 0xea, 0xce, 0xcb, 0x7f, 0x4c, 0xbc, 0x81, 0x4c, 0x00,
0xe6, 0x01, 0x00, 0x00, 0xf2, 0x51, 0x01, 0x00, 0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x0f, 0x00, 0x00, 0x00, 0x6d, 0x73, 0x67, 0x73, 0x3a, 0x3a, 0x4c, 0x6f, 0x77, 0x53, 0x74, 0x61, 0x0f, 0x00, 0x00, 0x00, 0x6d, 0x73, 0x67, 0x73, 0x3a, 0x3a, 0x4c, 0x6f, 0x77, 0x53, 0x74, 0x61,
0x74, 0x65, 0x00, 0x00, 0x16, 0x01, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x1c, 0x00, 0x00, 0x00, 0x74, 0x65, 0x00, 0x00, 0xc2, 0x01, 0x00, 0x00, 0x0d, 0x00, 0x00, 0x00, 0x1c, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
0x0c, 0x09, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x71, 0x00, 0x00, 0x00, 0x1d, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x71, 0x00, 0x00, 0x00, 0x1d, 0x00, 0x00, 0x00,
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
@ -155,23 +203,34 @@ template<> inline const uint8_t * TopicTraits<::msgs::LowState>::type_map_blob()
0x0c, 0x09, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x74, 0x6d, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x09, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x74, 0x6d, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00,
0x22, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x22, 0x00, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
0x01, 0x00, 0x00, 0x00, 0x04, 0x09, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x63, 0x6f, 0x6e, 0x74, 0x01, 0x00, 0x00, 0x00, 0x04, 0x09, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x63, 0x6f, 0x6e, 0x74,
0x61, 0x63, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x61, 0x63, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00,
0x01, 0x00, 0x09, 0x00, 0x08, 0x00, 0x00, 0x00, 0x76, 0x6f, 0x6c, 0x74, 0x61, 0x67, 0x65, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x09, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00, 0x01, 0x00, 0x09, 0x00, 0x05, 0x00, 0x00, 0x00, 0x71, 0x75, 0x61, 0x74, 0x00, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x00, 0x00,
0x08, 0x00, 0x00, 0x00, 0x63, 0x75, 0x72, 0x72, 0x65, 0x6e, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
0x22, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0xf2, 0x84, 0x20, 0x63, 0xda, 0x5c, 0x28, 0xbb, 0x03, 0x09, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x67, 0x79, 0x72, 0x6f, 0x00, 0x00, 0x00, 0x00,
0x27, 0x92, 0xe4, 0x1c, 0xce, 0x56, 0xbc, 0xf1, 0xd4, 0x69, 0x80, 0x35, 0xf5, 0x8b, 0x51, 0xea, 0x20, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00,
0x7f, 0x47, 0x3a, 0x7b, 0x09, 0x0f, }; 0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x61, 0x63, 0x63, 0x65,
0x6c, 0x00, 0x00, 0x00, 0x1e, 0x00, 0x00, 0x00, 0x09, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3,
0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00,
0x72, 0x70, 0x79, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00,
0x01, 0x00, 0x02, 0x00, 0x08, 0x00, 0x00, 0x00, 0x69, 0x6d, 0x75, 0x5f, 0x74, 0x6d, 0x70, 0x00,
0x00, 0x00, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x0b, 0x00, 0x00, 0x00, 0x01, 0x00, 0x09, 0x00,
0x08, 0x00, 0x00, 0x00, 0x76, 0x6f, 0x6c, 0x74, 0x61, 0x67, 0x65, 0x00, 0x00, 0x00, 0x00, 0x00,
0x16, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x01, 0x00, 0x09, 0x00, 0x08, 0x00, 0x00, 0x00,
0x63, 0x75, 0x72, 0x72, 0x65, 0x6e, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x22, 0x00, 0x00, 0x00,
0x01, 0x00, 0x00, 0x00, 0xf2, 0xf7, 0x0b, 0xf4, 0x7b, 0xfb, 0x17, 0xea, 0xce, 0xcb, 0x7f, 0x4c,
0xbc, 0x81, 0x4c, 0xf1, 0xec, 0xa8, 0x61, 0xd2, 0xd7, 0x0d, 0xa1, 0x5b, 0xdc, 0x10, 0x55, 0xb8,
0xdc, 0x20, };
return blob; return blob;
} }
template<> inline const uint8_t * TopicTraits<::msgs::LowState>::type_info_blob() { template<> inline const uint8_t * TopicTraits<::msgs::LowState>::type_info_blob() {
static const uint8_t blob[] = { static const uint8_t blob[] = {
0x60, 0x00, 0x00, 0x00, 0x01, 0x10, 0x00, 0x40, 0x28, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x01, 0x10, 0x00, 0x40, 0x28, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00,
0x14, 0x00, 0x00, 0x00, 0xf1, 0xd4, 0x69, 0x80, 0x35, 0xf5, 0x8b, 0x51, 0xea, 0x7f, 0x47, 0x3a, 0x14, 0x00, 0x00, 0x00, 0xf1, 0xec, 0xa8, 0x61, 0xd2, 0xd7, 0x0d, 0xa1, 0x5b, 0xdc, 0x10, 0x55,
0x7b, 0x09, 0x0f, 0x00, 0xdf, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0xb8, 0xdc, 0x20, 0x00, 0x5f, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x02, 0x10, 0x00, 0x40, 0x28, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x10, 0x00, 0x40, 0x28, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00,
0x14, 0x00, 0x00, 0x00, 0xf2, 0x84, 0x20, 0x63, 0xda, 0x5c, 0x28, 0xbb, 0x27, 0x92, 0xe4, 0x1c, 0x14, 0x00, 0x00, 0x00, 0xf2, 0xf7, 0x0b, 0xf4, 0x7b, 0xfb, 0x17, 0xea, 0xce, 0xcb, 0x7f, 0x4c,
0xce, 0x56, 0xbc, 0x00, 0x3e, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0xbc, 0x81, 0x4c, 0x00, 0xea, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, }; 0x00, 0x00, 0x00, 0x00, };
return blob; return blob;
} }
@ -291,12 +350,68 @@ bool write(T& streamer, const ::msgs::LowState& instance, entity_properties_t *p
case 6: case 6:
if (!streamer.start_member(*prop)) if (!streamer.start_member(*prop))
return false; return false;
if (!write(streamer, instance.voltage())) if (!streamer.start_consecutive(true, true))
return false;
if (!write(streamer, instance.quat()[0], instance.quat().size()))
return false;
if (!streamer.finish_consecutive())
return false; return false;
if (!streamer.finish_member(*prop)) if (!streamer.finish_member(*prop))
return false; return false;
break; break;
case 7: case 7:
if (!streamer.start_member(*prop))
return false;
if (!streamer.start_consecutive(true, true))
return false;
if (!write(streamer, instance.gyro()[0], instance.gyro().size()))
return false;
if (!streamer.finish_consecutive())
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 8:
if (!streamer.start_member(*prop))
return false;
if (!streamer.start_consecutive(true, true))
return false;
if (!write(streamer, instance.accel()[0], instance.accel().size()))
return false;
if (!streamer.finish_consecutive())
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 9:
if (!streamer.start_member(*prop))
return false;
if (!streamer.start_consecutive(true, true))
return false;
if (!write(streamer, instance.rpy()[0], instance.rpy().size()))
return false;
if (!streamer.finish_consecutive())
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 10:
if (!streamer.start_member(*prop))
return false;
if (!write(streamer, instance.imu_tmp()))
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 11:
if (!streamer.start_member(*prop))
return false;
if (!write(streamer, instance.voltage()))
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 12:
if (!streamer.start_member(*prop)) if (!streamer.start_member(*prop))
return false; return false;
if (!write(streamer, instance.current())) if (!write(streamer, instance.current()))
@ -400,12 +515,68 @@ bool read(T& streamer, ::msgs::LowState& instance, entity_properties_t *props) {
case 6: case 6:
if (!streamer.start_member(*prop)) if (!streamer.start_member(*prop))
return false; return false;
if (!read(streamer, instance.voltage())) if (!streamer.start_consecutive(true, true))
return false;
if (!read(streamer, instance.quat()[0], instance.quat().size()))
return false;
if (!streamer.finish_consecutive())
return false; return false;
if (!streamer.finish_member(*prop)) if (!streamer.finish_member(*prop))
return false; return false;
break; break;
case 7: case 7:
if (!streamer.start_member(*prop))
return false;
if (!streamer.start_consecutive(true, true))
return false;
if (!read(streamer, instance.gyro()[0], instance.gyro().size()))
return false;
if (!streamer.finish_consecutive())
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 8:
if (!streamer.start_member(*prop))
return false;
if (!streamer.start_consecutive(true, true))
return false;
if (!read(streamer, instance.accel()[0], instance.accel().size()))
return false;
if (!streamer.finish_consecutive())
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 9:
if (!streamer.start_member(*prop))
return false;
if (!streamer.start_consecutive(true, true))
return false;
if (!read(streamer, instance.rpy()[0], instance.rpy().size()))
return false;
if (!streamer.finish_consecutive())
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 10:
if (!streamer.start_member(*prop))
return false;
if (!read(streamer, instance.imu_tmp()))
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 11:
if (!streamer.start_member(*prop))
return false;
if (!read(streamer, instance.voltage()))
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 12:
if (!streamer.start_member(*prop)) if (!streamer.start_member(*prop))
return false; return false;
if (!read(streamer, instance.current())) if (!read(streamer, instance.current()))
@ -509,12 +680,68 @@ bool move(T& streamer, const ::msgs::LowState& instance, entity_properties_t *pr
case 6: case 6:
if (!streamer.start_member(*prop)) if (!streamer.start_member(*prop))
return false; return false;
if (!move(streamer, instance.voltage())) if (!streamer.start_consecutive(true, true))
return false;
if (!move(streamer, instance.quat()[0], instance.quat().size()))
return false;
if (!streamer.finish_consecutive())
return false; return false;
if (!streamer.finish_member(*prop)) if (!streamer.finish_member(*prop))
return false; return false;
break; break;
case 7: case 7:
if (!streamer.start_member(*prop))
return false;
if (!streamer.start_consecutive(true, true))
return false;
if (!move(streamer, instance.gyro()[0], instance.gyro().size()))
return false;
if (!streamer.finish_consecutive())
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 8:
if (!streamer.start_member(*prop))
return false;
if (!streamer.start_consecutive(true, true))
return false;
if (!move(streamer, instance.accel()[0], instance.accel().size()))
return false;
if (!streamer.finish_consecutive())
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 9:
if (!streamer.start_member(*prop))
return false;
if (!streamer.start_consecutive(true, true))
return false;
if (!move(streamer, instance.rpy()[0], instance.rpy().size()))
return false;
if (!streamer.finish_consecutive())
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 10:
if (!streamer.start_member(*prop))
return false;
if (!move(streamer, instance.imu_tmp()))
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 11:
if (!streamer.start_member(*prop))
return false;
if (!move(streamer, instance.voltage()))
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 12:
if (!streamer.start_member(*prop)) if (!streamer.start_member(*prop))
return false; return false;
if (!move(streamer, instance.current())) if (!move(streamer, instance.current()))
@ -618,12 +845,68 @@ bool max(T& streamer, const ::msgs::LowState& instance, entity_properties_t *pro
case 6: case 6:
if (!streamer.start_member(*prop)) if (!streamer.start_member(*prop))
return false; return false;
if (!max(streamer, instance.voltage())) if (!streamer.start_consecutive(true, true))
return false;
if (!max(streamer, instance.quat()[0], instance.quat().size()))
return false;
if (!streamer.finish_consecutive())
return false; return false;
if (!streamer.finish_member(*prop)) if (!streamer.finish_member(*prop))
return false; return false;
break; break;
case 7: case 7:
if (!streamer.start_member(*prop))
return false;
if (!streamer.start_consecutive(true, true))
return false;
if (!max(streamer, instance.gyro()[0], instance.gyro().size()))
return false;
if (!streamer.finish_consecutive())
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 8:
if (!streamer.start_member(*prop))
return false;
if (!streamer.start_consecutive(true, true))
return false;
if (!max(streamer, instance.accel()[0], instance.accel().size()))
return false;
if (!streamer.finish_consecutive())
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 9:
if (!streamer.start_member(*prop))
return false;
if (!streamer.start_consecutive(true, true))
return false;
if (!max(streamer, instance.rpy()[0], instance.rpy().size()))
return false;
if (!streamer.finish_consecutive())
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 10:
if (!streamer.start_member(*prop))
return false;
if (!max(streamer, instance.imu_tmp()))
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 11:
if (!streamer.start_member(*prop))
return false;
if (!max(streamer, instance.voltage()))
return false;
if (!streamer.finish_member(*prop))
return false;
break;
case 12:
if (!streamer.start_member(*prop)) if (!streamer.start_member(*prop))
return false; return false;
if (!max(streamer, instance.current())) if (!max(streamer, instance.current()))

58
cpp_bridge/src/lowlevel/bridge.cpp
View File

@ -78,12 +78,8 @@ private:
void LowCmdWrite(); void LowCmdWrite();
std::thread go2py_thread; std::thread go2py_thread;
bool running = true; bool running = true;
void go2py_callback(const msgs::LowCmd& msg);
DDSPublisher<msgs::LowState> lowstate_publisher; private:
DDSSubscriber<msgs::LowCmd> lowcmd_subscriber;
private:
float dt = 0.002; // 0.001~0.01 float dt = 0.002; // 0.001~0.01
unitree_go::msg::dds_::LowCmd_ low_cmd{}; // default init unitree_go::msg::dds_::LowCmd_ low_cmd{}; // default init
unitree_go::msg::dds_::LowState_ low_state{}; // default init unitree_go::msg::dds_::LowState_ low_state{}; // default init
@ -93,21 +89,26 @@ private:
ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber; ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber;
/*LowCmd write thread*/ /*LowCmd write thread*/
ThreadPtr lowCmdWriteThreadPtr; ThreadPtr lowCmdWriteThreadPtr;
// Callback function for the Go2Py command subscriber
void go2py_callback(const msgs::LowCmd& msg);
// State publisher to Go2Py
DDSPublisher<msgs::LowState> lowstate_publisher;
// Command subscriber from Go2Py
DDSSubscriber<msgs::LowCmd> lowcmd_subscriber;
}; };
void Go2LowLevelBridge::Init() void Go2LowLevelBridge::Init()
{ {
InitLowCmd(); InitLowCmd();
/*create publisher*/ /*create robot lowstate publisher*/
lowcmd_publisher.reset(new ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(TOPIC_LOWCMD)); lowcmd_publisher.reset(new ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(TOPIC_LOWCMD));
lowcmd_publisher->InitChannel(); lowcmd_publisher->InitChannel();
/*create subscriber*/ /*create robot lowcmd subscriber*/
lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE)); lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE));
lowstate_subscriber->InitChannel(std::bind(&Go2LowLevelBridge::LowStateMessageHandler, this, std::placeholders::_1), 1); lowstate_subscriber->InitChannel(std::bind(&Go2LowLevelBridge::LowStateMessageHandler, this, std::placeholders::_1), 1);
/*loop publishing thread*/ }
// lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &Go2LowLevelBridge::LowCmdWrite, this);
}
// Upon the reception of the commands form the Go2Py, forward them to the robot
void Go2LowLevelBridge::go2py_callback(const msgs::LowCmd& msg) void Go2LowLevelBridge::go2py_callback(const msgs::LowCmd& msg)
{ {
for(int i=0; i<12; i++) for(int i=0; i<12; i++)
@ -140,6 +141,7 @@ void Go2LowLevelBridge::InitLowCmd()
} }
} }
// Upon the reception of each state from the robot, forward it to the Go2Py
void Go2LowLevelBridge::LowStateMessageHandler(const void* message) void Go2LowLevelBridge::LowStateMessageHandler(const void* message)
{ {
low_state = *(unitree_go::msg::dds_::LowState_*)message; low_state = *(unitree_go::msg::dds_::LowState_*)message;
@ -153,37 +155,35 @@ void Go2LowLevelBridge::LowStateMessageHandler(const void* message)
lowstate.tau_est()[i] = low_state.motor_state()[i].tau_est(); lowstate.tau_est()[i] = low_state.motor_state()[i].tau_est();
lowstate.tmp()[i] = (float)low_state.motor_state()[i].temperature(); lowstate.tmp()[i] = (float)low_state.motor_state()[i].temperature();
} }
for(int i=0; i<4; i++)
{
lowstate.contact()[i] = (float)low_state.foot_force()[i];
lowstate.quat()[i] = low_state.imu_state().quaternion()[i];
}
for(int i=0; i<3; i++)
{
lowstate.accel()[i] = low_state.imu_state().accelerometer()[i];
lowstate.gyro()[i] = low_state.imu_state().gyroscope()[i];
lowstate.rpy()[i] = low_state.imu_state().rpy()[i];
}
lowstate.voltage() = (float)low_state.power_v(); lowstate.voltage() = (float)low_state.power_v();
lowstate.current() = (float)low_state.power_a(); lowstate.current() = (float)low_state.power_a();
lowstate.imu_tmp() = (float)low_state.imu_state().temperature();
for(int i=0; i<4; i++)
lowstate.contact()[i] = (float)low_state.foot_force()[i];
lowstate_publisher.publish(lowstate); lowstate_publisher.publish(lowstate);
} }
void Go2LowLevelBridge::LowCmdWrite()
{
low_cmd.motor_cmd()[2].q() = 0;
low_cmd.motor_cmd()[2].dq() = 0;
low_cmd.motor_cmd()[2].kp() = 0;
low_cmd.motor_cmd()[2].kd() = 0;
low_cmd.motor_cmd()[2].tau() = 0;
low_cmd.crc() = crc32_core((uint32_t *)&low_cmd, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1);
lowcmd_publisher->Write(low_cmd);
}
int main(int argc, const char** argv) int main(int argc, const char** argv)
{ {
if (argc < 2) if (argc < 2)
{ {
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl; std::cout << "Usage: " << argv[0] << "domain_id" << std::endl;
exit(-1); exit(-1);
} }
int domain_id = std::stoi(argv[1]);
// ChannelFactory::Instance()->Init(0, argv[1]); // ChannelFactory::Instance()->Init(0, argv[1]);
Go2LowLevelBridge bridge("go2py/lowcmd", "go2py/lowstate"); Go2LowLevelBridge bridge("go2_robot"+std::to_string(domain_id)+"/lowcmd",
"go2_robot"+std::to_string(domain_id)+"/lowstate");
bridge.Init(); bridge.Init();
while (1) while (1)
{ {