From 9bb096405d2c25e8d4ea49b0e07050a4c7cd3dc1 Mon Sep 17 00:00:00 2001 From: Rooholla-Khorrambakht Date: Thu, 4 Jan 2024 15:10:50 -0500 Subject: [PATCH] the lowstate bridge is completed. --- Go2Py/idl/LowState.idl | 5 + Go2Py/msgs/_LowState.py | 5 + cpp_bridge/include/go2py/LowState.cpp | 9 +- cpp_bridge/include/go2py/LowState.hpp | 333 ++++++++++++++++++++++++-- cpp_bridge/src/lowlevel/bridge.cpp | 58 ++--- 5 files changed, 354 insertions(+), 56 deletions(-) diff --git a/Go2Py/idl/LowState.idl b/Go2Py/idl/LowState.idl index 1fd2377..5aa1fd5 100644 --- a/Go2Py/idl/LowState.idl +++ b/Go2Py/idl/LowState.idl @@ -6,6 +6,11 @@ module msgs{ float tau_est[12]; float tmp[12]; float contact[4]; + float quat[4]; + float gyro[3]; + float accel[3]; + float rpy[3]; + octet imu_tmp; float voltage; float current; }; diff --git a/Go2Py/msgs/_LowState.py b/Go2Py/msgs/_LowState.py index 8856e56..16b9c65 100644 --- a/Go2Py/msgs/_LowState.py +++ b/Go2Py/msgs/_LowState.py @@ -28,6 +28,11 @@ class LowState(idl.IdlStruct, typename="msgs.LowState"): tau_est: types.array[types.float32, 12] tmp: types.array[types.float32, 12] 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 current: types.float32 diff --git a/cpp_bridge/include/go2py/LowState.cpp b/cpp_bridge/include/go2py/LowState.cpp index dba5607..310a16b 100644 --- a/cpp_bridge/include/go2py/LowState.cpp +++ b/cpp_bridge/include/go2py/LowState.cpp @@ -43,8 +43,13 @@ propvec &get_type_props<::msgs::LowState>() { props.push_back(entity_properties_t(1, 3, false, get_bit_bound(), extensibility::ext_final, false)); //::tau_est props.push_back(entity_properties_t(1, 4, false, get_bit_bound(), extensibility::ext_final, false)); //::tmp props.push_back(entity_properties_t(1, 5, false, get_bit_bound(), extensibility::ext_final, false)); //::contact - props.push_back(entity_properties_t(1, 6, false, get_bit_bound(), extensibility::ext_final, false)); //::voltage - props.push_back(entity_properties_t(1, 7, false, get_bit_bound(), extensibility::ext_final, false)); //::current + props.push_back(entity_properties_t(1, 6, false, get_bit_bound(), extensibility::ext_final, false)); //::quat + props.push_back(entity_properties_t(1, 7, false, get_bit_bound(), extensibility::ext_final, false)); //::gyro + props.push_back(entity_properties_t(1, 8, false, get_bit_bound(), extensibility::ext_final, false)); //::accel + props.push_back(entity_properties_t(1, 9, false, get_bit_bound(), extensibility::ext_final, false)); //::rpy + props.push_back(entity_properties_t(1, 10, false, get_bit_bound(), extensibility::ext_final, false)); //::imu_tmp + props.push_back(entity_properties_t(1, 11, false, get_bit_bound(), extensibility::ext_final, false)); //::voltage + props.push_back(entity_properties_t(1, 12, false, get_bit_bound(), extensibility::ext_final, false)); //::current entity_properties_t::finish(props, keylist); props_end = props.data() + props.size(); diff --git a/cpp_bridge/include/go2py/LowState.hpp b/cpp_bridge/include/go2py/LowState.hpp index 4356439..0fc6c0a 100644 --- a/cpp_bridge/include/go2py/LowState.hpp +++ b/cpp_bridge/include/go2py/LowState.hpp @@ -9,6 +9,7 @@ #ifndef DDSCXX_LOWSTATE_HPP #define DDSCXX_LOWSTATE_HPP +#include #include namespace msgs @@ -22,6 +23,11 @@ private: std::array tau_est_ = { }; std::array tmp_ = { }; std::array contact_ = { }; + std::array quat_ = { }; + std::array gyro_ = { }; + std::array accel_ = { }; + std::array rpy_ = { }; + uint8_t imu_tmp_ = 0; float voltage_ = 0.0f; float current_ = 0.0f; @@ -35,6 +41,11 @@ public: const std::array& tau_est, const std::array& tmp, const std::array& contact, + const std::array& quat, + const std::array& gyro, + const std::array& accel, + const std::array& rpy, + uint8_t imu_tmp, float voltage, float current) : q_(q), @@ -43,6 +54,11 @@ public: tau_est_(tau_est), tmp_(tmp), contact_(contact), + quat_(quat), + gyro_(gyro), + accel_(accel), + rpy_(rpy), + imu_tmp_(imu_tmp), voltage_(voltage), current_(current) { } @@ -70,6 +86,25 @@ public: std::array& contact() { return this->contact_; } void contact(const std::array& _val_) { this->contact_ = _val_; } void contact(std::array&& _val_) { this->contact_ = _val_; } + const std::array& quat() const { return this->quat_; } + std::array& quat() { return this->quat_; } + void quat(const std::array& _val_) { this->quat_ = _val_; } + void quat(std::array&& _val_) { this->quat_ = _val_; } + const std::array& gyro() const { return this->gyro_; } + std::array& gyro() { return this->gyro_; } + void gyro(const std::array& _val_) { this->gyro_ = _val_; } + void gyro(std::array&& _val_) { this->gyro_ = _val_; } + const std::array& accel() const { return this->accel_; } + std::array& accel() { return this->accel_; } + void accel(const std::array& _val_) { this->accel_ = _val_; } + void accel(std::array&& _val_) { this->accel_ = _val_; } + const std::array& rpy() const { return this->rpy_; } + std::array& rpy() { return this->rpy_; } + void rpy(const std::array& _val_) { this->rpy_ = _val_; } + void rpy(std::array&& _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() { return this->voltage_; } void voltage(float _val_) { this->voltage_ = _val_; } @@ -86,6 +121,11 @@ public: tau_est_ == _other.tau_est_ && tmp_ == _other.tmp_ && contact_ == _other.contact_ && + quat_ == _other.quat_ && + gyro_ == _other.gyro_ && + accel_ == _other.accel_ && + rpy_ == _other.rpy_ && + imu_tmp_ == _other.imu_tmp_ && voltage_ == _other.voltage_ && current_ == _other.current_; } @@ -118,13 +158,13 @@ template <> constexpr bool TopicTraits<::msgs::LowState>::isKeyless() } #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<> inline const uint8_t * TopicTraits<::msgs::LowState>::type_map_blob() { static const uint8_t blob[] = { - 0xf3, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0xf1, 0xd4, 0x69, 0x80, 0x35, 0xf5, 0x8b, 0x51, - 0xea, 0x7f, 0x47, 0x3a, 0x7b, 0x09, 0x0f, 0x00, 0xdb, 0x00, 0x00, 0x00, 0xf1, 0x51, 0x01, 0x00, - 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xcb, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, + 0x73, 0x01, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0xf1, 0xec, 0xa8, 0x61, 0xd2, 0xd7, 0x0d, 0xa1, + 0x5b, 0xdc, 0x10, 0x55, 0xb8, 0xdc, 0x20, 0x00, 0x5b, 0x01, 0x00, 0x00, 0xf1, 0x51, 0x01, 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, 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, @@ -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, 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, - 0x04, 0x09, 0x2f, 0x8a, 0x6b, 0xf3, 0x00, 0x00, 0x0b, 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, 0x09, 0x43, 0xb5, 0xc9, 0x17, 0x00, 0x52, 0x01, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, - 0xf2, 0x84, 0x20, 0x63, 0xda, 0x5c, 0x28, 0xbb, 0x27, 0x92, 0xe4, 0x1c, 0xce, 0x56, 0xbc, 0x00, - 0x3a, 0x01, 0x00, 0x00, 0xf2, 0x51, 0x01, 0x00, 0x17, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x04, 0x09, 0x2f, 0x8a, 0x6b, 0xf3, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, + 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x09, 0x21, 0xd7, + 0xdc, 0x6a, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, + 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x03, 0x09, 0x41, 0xe1, 0xdb, 0x58, 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, - 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, 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, @@ -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, 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, - 0x61, 0x63, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0x00, 0x00, 0x00, 0x06, 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, 0x07, 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, 0x84, 0x20, 0x63, 0xda, 0x5c, 0x28, 0xbb, - 0x27, 0x92, 0xe4, 0x1c, 0xce, 0x56, 0xbc, 0xf1, 0xd4, 0x69, 0x80, 0x35, 0xf5, 0x8b, 0x51, 0xea, - 0x7f, 0x47, 0x3a, 0x7b, 0x09, 0x0f, }; + 0x61, 0x63, 0x74, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, + 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x09, 0x00, 0x00, + 0x05, 0x00, 0x00, 0x00, 0x71, 0x75, 0x61, 0x74, 0x00, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x00, 0x00, + 0x07, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x03, 0x09, 0x00, 0x00, 0x05, 0x00, 0x00, 0x00, 0x67, 0x79, 0x72, 0x6f, 0x00, 0x00, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, 0x90, 0xf3, 0x01, 0x00, 0x00, 0x00, + 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; } template<> inline const uint8_t * TopicTraits<::msgs::LowState>::type_info_blob() { static const uint8_t blob[] = { 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, - 0x7b, 0x09, 0x0f, 0x00, 0xdf, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, + 0x14, 0x00, 0x00, 0x00, 0xf1, 0xec, 0xa8, 0x61, 0xd2, 0xd7, 0x0d, 0xa1, 0x5b, 0xdc, 0x10, 0x55, + 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, - 0x14, 0x00, 0x00, 0x00, 0xf2, 0x84, 0x20, 0x63, 0xda, 0x5c, 0x28, 0xbb, 0x27, 0x92, 0xe4, 0x1c, - 0xce, 0x56, 0xbc, 0x00, 0x3e, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, + 0x14, 0x00, 0x00, 0x00, 0xf2, 0xf7, 0x0b, 0xf4, 0x7b, 0xfb, 0x17, 0xea, 0xce, 0xcb, 0x7f, 0x4c, + 0xbc, 0x81, 0x4c, 0x00, 0xea, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, }; return blob; } @@ -291,12 +350,68 @@ bool write(T& streamer, const ::msgs::LowState& instance, entity_properties_t *p case 6: if (!streamer.start_member(*prop)) 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; if (!streamer.finish_member(*prop)) return false; break; 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)) return false; if (!write(streamer, instance.current())) @@ -400,12 +515,68 @@ bool read(T& streamer, ::msgs::LowState& instance, entity_properties_t *props) { case 6: if (!streamer.start_member(*prop)) 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; if (!streamer.finish_member(*prop)) return false; break; 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)) return false; if (!read(streamer, instance.current())) @@ -509,12 +680,68 @@ bool move(T& streamer, const ::msgs::LowState& instance, entity_properties_t *pr case 6: if (!streamer.start_member(*prop)) 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; if (!streamer.finish_member(*prop)) return false; break; 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)) return false; if (!move(streamer, instance.current())) @@ -618,12 +845,68 @@ bool max(T& streamer, const ::msgs::LowState& instance, entity_properties_t *pro case 6: if (!streamer.start_member(*prop)) 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; if (!streamer.finish_member(*prop)) return false; break; 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)) return false; if (!max(streamer, instance.current())) diff --git a/cpp_bridge/src/lowlevel/bridge.cpp b/cpp_bridge/src/lowlevel/bridge.cpp index 7bf087b..bcef812 100644 --- a/cpp_bridge/src/lowlevel/bridge.cpp +++ b/cpp_bridge/src/lowlevel/bridge.cpp @@ -78,12 +78,8 @@ private: void LowCmdWrite(); std::thread go2py_thread; bool running = true; - void go2py_callback(const msgs::LowCmd& msg); - DDSPublisher lowstate_publisher; - DDSSubscriber lowcmd_subscriber; - - -private: + + private: float dt = 0.002; // 0.001~0.01 unitree_go::msg::dds_::LowCmd_ low_cmd{}; // default init unitree_go::msg::dds_::LowState_ low_state{}; // default init @@ -93,21 +89,26 @@ private: ChannelSubscriberPtr lowstate_subscriber; /*LowCmd write thread*/ ThreadPtr lowCmdWriteThreadPtr; + // Callback function for the Go2Py command subscriber + void go2py_callback(const msgs::LowCmd& msg); + // State publisher to Go2Py + DDSPublisher lowstate_publisher; + // Command subscriber from Go2Py + DDSSubscriber lowcmd_subscriber; }; void Go2LowLevelBridge::Init() { InitLowCmd(); - /*create publisher*/ + /*create robot lowstate publisher*/ lowcmd_publisher.reset(new ChannelPublisher(TOPIC_LOWCMD)); lowcmd_publisher->InitChannel(); - /*create subscriber*/ + /*create robot lowcmd subscriber*/ lowstate_subscriber.reset(new ChannelSubscriber(TOPIC_LOWSTATE)); 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) { 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) { 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.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.current() = (float)low_state.power_a(); - - for(int i=0; i<4; i++) - lowstate.contact()[i] = (float)low_state.foot_force()[i]; - + lowstate.imu_tmp() = (float)low_state.imu_state().temperature(); 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) { if (argc < 2) { - std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl; + std::cout << "Usage: " << argv[0] << "domain_id" << std::endl; exit(-1); } + int domain_id = std::stoi(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(); while (1) {