diff --git a/hardwares/hardware_unitree_mujoco/CMakeLists.txt b/hardwares/hardware_unitree_mujoco/CMakeLists.txt index 51cc797..7ff3324 100644 --- a/hardwares/hardware_unitree_mujoco/CMakeLists.txt +++ b/hardwares/hardware_unitree_mujoco/CMakeLists.txt @@ -36,7 +36,9 @@ add_library( target_include_directories(${PROJECT_NAME} PUBLIC "$" - "$") + "$" + PRIVATE src +) target_link_libraries(${PROJECT_NAME} unitree_sdk2) ament_target_dependencies( ${PROJECT_NAME} diff --git a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h b/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h index 11fb473..08418d3 100644 --- a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h +++ b/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h @@ -45,17 +45,17 @@ protected: }; - void init_low_cmd(); + void initLowCmd(); - void low_state_message_handler(const void *messages); + void lowStateMessageHandle(const void *messages); - unitree_go::msg::dds_::LowCmd_ _lowCmd{}; // default init - unitree_go::msg::dds_::LowState_ _lowState{}; // default init + unitree_go::msg::dds_::LowCmd_ low_cmd_{}; // default init + unitree_go::msg::dds_::LowState_ low_state_{}; // default init /*publisher*/ - unitree::robot::ChannelPublisherPtr lowcmd_publisher; + unitree::robot::ChannelPublisherPtr low_cmd_publisher_; /*subscriber*/ - unitree::robot::ChannelSubscriberPtr lowstate_subscriber; + unitree::robot::ChannelSubscriberPtr lows_tate_subscriber_; }; diff --git a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/crc32.h b/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/crc32.h deleted file mode 100644 index 01b2d65..0000000 --- a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/crc32.h +++ /dev/null @@ -1,34 +0,0 @@ -// -// Created by biao on 24-7-1. -// - -#ifndef STAND_GO2_CRC32_H -#define STAND_GO2_CRC32_H -#include - -static uint32_t crc32_core(const uint32_t *ptr, uint32_t len) { - unsigned int xbit = 0; - unsigned int data = 0; - unsigned int CRC32 = 0xFFFFFFFF; - const unsigned int dwPolynomial = 0x04c11db7; - - for (unsigned int i = 0; i < len; i++) { - xbit = 1 << 31; - data = ptr[i]; - for (unsigned int bits = 0; bits < 32; bits++) { - if (CRC32 & 0x80000000) { - CRC32 <<= 1; - CRC32 ^= dwPolynomial; - } else { - CRC32 <<= 1; - } - - if (data & xbit) CRC32 ^= dwPolynomial; - xbit >>= 1; - } - } - - return CRC32; -} - -#endif // STAND_GO2_CRC32_H diff --git a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp index 6d94b26..d000e75 100644 --- a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp +++ b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp @@ -2,9 +2,9 @@ // Created by biao on 24-9-9. // -#include -#include +#include "hardware_unitree_mujoco/HardwareUnitree.h" #include +#include "crc32.h" #define TOPIC_LOWCMD "rt/lowcmd" #define TOPIC_LOWSTATE "rt/lowstate" @@ -37,21 +37,21 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa } ChannelFactory::Instance()->Init(1, "lo"); - lowcmd_publisher = + low_cmd_publisher_ = std::make_shared >( TOPIC_LOWCMD); - lowcmd_publisher->InitChannel(); + low_cmd_publisher_->InitChannel(); /*create subscriber*/ - lowstate_subscriber = + lows_tate_subscriber_ = std::make_shared >( TOPIC_LOWSTATE); - lowstate_subscriber->InitChannel( + lows_tate_subscriber_->InitChannel( [this](auto &&PH1) { - low_state_message_handler(std::forward(PH1)); + lowStateMessageHandle(std::forward(PH1)); }, 1); - init_low_cmd(); + initLowCmd(); return SystemInterface::on_init(info); @@ -109,25 +109,24 @@ std::vector HardwareUnitree::export_comman } return_type HardwareUnitree::read(const rclcpp::Time &time, const rclcpp::Duration &period) { - // joint states for (int i(0); i < 12; ++i) { - joint_position_[i] = _lowState.motor_state()[i].q(); - joint_velocities_[i] = _lowState.motor_state()[i].dq(); - joint_effort_[i] = _lowState.motor_state()[i].tau_est(); + joint_position_[i] = low_state_.motor_state()[i].q(); + joint_velocities_[i] = low_state_.motor_state()[i].dq(); + joint_effort_[i] = low_state_.motor_state()[i].tau_est(); } // imu states - imu_states_[0] = _lowState.imu_state().quaternion()[0]; // w - imu_states_[1] = _lowState.imu_state().quaternion()[1]; // x - imu_states_[2] = _lowState.imu_state().quaternion()[2]; // y - imu_states_[3] = _lowState.imu_state().quaternion()[3]; // z - imu_states_[4] = _lowState.imu_state().gyroscope()[0]; - imu_states_[5] = _lowState.imu_state().gyroscope()[1]; - imu_states_[6] = _lowState.imu_state().gyroscope()[2]; - imu_states_[7] = _lowState.imu_state().accelerometer()[0]; - imu_states_[8] = _lowState.imu_state().accelerometer()[1]; - imu_states_[9] = _lowState.imu_state().accelerometer()[2]; + imu_states_[0] = low_state_.imu_state().quaternion()[0]; // w + imu_states_[1] = low_state_.imu_state().quaternion()[1]; // x + imu_states_[2] = low_state_.imu_state().quaternion()[2]; // y + imu_states_[3] = low_state_.imu_state().quaternion()[3]; // z + imu_states_[4] = low_state_.imu_state().gyroscope()[0]; + imu_states_[5] = low_state_.imu_state().gyroscope()[1]; + imu_states_[6] = low_state_.imu_state().gyroscope()[2]; + imu_states_[7] = low_state_.imu_state().accelerometer()[0]; + imu_states_[8] = low_state_.imu_state().accelerometer()[1]; + imu_states_[9] = low_state_.imu_state().accelerometer()[2]; return return_type::OK; } @@ -135,39 +134,39 @@ return_type HardwareUnitree::read(const rclcpp::Time &time, const rclcpp::Durati return_type HardwareUnitree::write(const rclcpp::Time &, const rclcpp::Duration &) { // send command for (int i(0); i < 12; ++i) { - _lowCmd.motor_cmd()[i].mode() = 0x01; - _lowCmd.motor_cmd()[i].q() = static_cast(joint_position_command_[i]); - _lowCmd.motor_cmd()[i].dq() = static_cast(joint_velocities_command_[i]); - _lowCmd.motor_cmd()[i].kp() = static_cast(joint_kp_command_[i]); - _lowCmd.motor_cmd()[i].kd() = static_cast(joint_kd_command_[i]); - _lowCmd.motor_cmd()[i].tau() = static_cast(joint_effort_command_[i]); + low_cmd_.motor_cmd()[i].mode() = 0x01; + low_cmd_.motor_cmd()[i].q() = static_cast(joint_position_command_[i]); + low_cmd_.motor_cmd()[i].dq() = static_cast(joint_velocities_command_[i]); + low_cmd_.motor_cmd()[i].kp() = static_cast(joint_kp_command_[i]); + low_cmd_.motor_cmd()[i].kd() = static_cast(joint_kd_command_[i]); + low_cmd_.motor_cmd()[i].tau() = static_cast(joint_effort_command_[i]); } - _lowCmd.crc() = crc32_core(reinterpret_cast(&_lowCmd), - (sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1); - lowcmd_publisher->Write(_lowCmd); + low_cmd_.crc() = crc32_core(reinterpret_cast(&low_cmd_), + (sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1); + low_cmd_publisher_->Write(low_cmd_); return return_type::OK; } -void HardwareUnitree::init_low_cmd() { - _lowCmd.head()[0] = 0xFE; - _lowCmd.head()[1] = 0xEF; - _lowCmd.level_flag() = 0xFF; - _lowCmd.gpio() = 0; +void HardwareUnitree::initLowCmd() { + low_cmd_.head()[0] = 0xFE; + low_cmd_.head()[1] = 0xEF; + low_cmd_.level_flag() = 0xFF; + low_cmd_.gpio() = 0; for (int i = 0; i < 20; i++) { - _lowCmd.motor_cmd()[i].mode() = + low_cmd_.motor_cmd()[i].mode() = 0x01; // motor switch to servo (PMSM) mode - _lowCmd.motor_cmd()[i].q() = 0; - _lowCmd.motor_cmd()[i].kp() = 0; - _lowCmd.motor_cmd()[i].dq() = 0; - _lowCmd.motor_cmd()[i].kd() = 0; - _lowCmd.motor_cmd()[i].tau() = 0; + low_cmd_.motor_cmd()[i].q() = 0; + low_cmd_.motor_cmd()[i].kp() = 0; + low_cmd_.motor_cmd()[i].dq() = 0; + low_cmd_.motor_cmd()[i].kd() = 0; + low_cmd_.motor_cmd()[i].tau() = 0; } } -void HardwareUnitree::low_state_message_handler(const void *messages) { - _lowState = *(unitree_go::msg::dds_::LowState_ *) messages; +void HardwareUnitree::lowStateMessageHandle(const void *messages) { + low_state_ = *(unitree_go::msg::dds_::LowState_ *) messages; } #include "pluginlib/class_list_macros.hpp" diff --git a/hardwares/hardware_unitree_mujoco/src/crc32.h b/hardwares/hardware_unitree_mujoco/src/crc32.h new file mode 100644 index 0000000..edb8d89 --- /dev/null +++ b/hardwares/hardware_unitree_mujoco/src/crc32.h @@ -0,0 +1,34 @@ +// +// Created by biao on 24-7-1. +// + +#ifndef STAND_GO2_CRC32_H +#define STAND_GO2_CRC32_H +#include + +static uint32_t crc32_core(const uint32_t *ptr, uint32_t len) { + unsigned int xbit = 0; + unsigned int data = 0; + unsigned int CRC32 = 0xFFFFFFFF; + const unsigned int dwPolynomial = 0x04c11db7; + + for (unsigned int i = 0; i < len; i++) { + xbit = 1 << 31; + data = ptr[i]; + for (unsigned int bits = 0; bits < 32; bits++) { + if (CRC32 & 0x80000000) { + CRC32 <<= 1; + CRC32 ^= dwPolynomial; + } else { + CRC32 <<= 1; + } + + if (data & xbit) CRC32 ^= dwPolynomial; + xbit >>= 1; + } + } + + return CRC32; +} + +#endif // STAND_GO2_CRC32_H