From 691734dc19fec8549d55ab2a174af5044e7bf2cc Mon Sep 17 00:00:00 2001 From: Zhenbiao Huang Date: Sat, 14 Sep 2024 11:41:38 +0800 Subject: [PATCH] add imu to hardware interface --- commands/keyboard_input/src/KeyboardInput.cpp | 4 +++- .../go2_description/config/robot_control.yaml | 10 +++++++- .../go2_description/launch/hardware.launch.py | 12 +++++----- .../go2_description/xacro/ros2_control.xacro | 13 ++++++++++ .../hardware_unitree_mujoco/HardwareUnitree.h | 2 ++ .../src/HardwareUnitree.cpp | 24 +++++++++++++++++++ 6 files changed, 57 insertions(+), 8 deletions(-) diff --git a/commands/keyboard_input/src/KeyboardInput.cpp b/commands/keyboard_input/src/KeyboardInput.cpp index 74d4da7..b18c4be 100644 --- a/commands/keyboard_input/src/KeyboardInput.cpp +++ b/commands/keyboard_input/src/KeyboardInput.cpp @@ -27,8 +27,10 @@ void KeyboardInput::timer_callback() { inputs_.rx = 0; inputs_.ry = 0; } - publisher_->publish(inputs_); + } else { + inputs_.command = 0; } + publisher_->publish(inputs_); } void KeyboardInput::check_command(const char key) { diff --git a/descriptions/go2_description/config/robot_control.yaml b/descriptions/go2_description/config/robot_control.yaml index c352b35..de546fb 100644 --- a/descriptions/go2_description/config/robot_control.yaml +++ b/descriptions/go2_description/config/robot_control.yaml @@ -6,4 +6,12 @@ controller_manager: # Define the available controllers joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster \ No newline at end of file + type: joint_state_broadcaster/JointStateBroadcaster + + imu_sensor_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + +imu_sensor_broadcaster: + ros__parameters: + sensor_name: "imu_sensor" + frame_id: "imu_link" \ No newline at end of file diff --git a/descriptions/go2_description/launch/hardware.launch.py b/descriptions/go2_description/launch/hardware.launch.py index 01cfad5..911ed46 100644 --- a/descriptions/go2_description/launch/hardware.launch.py +++ b/descriptions/go2_description/launch/hardware.launch.py @@ -58,12 +58,12 @@ def launch_setup(context, *args, **kwargs): arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ), - # Node( - # package="controller_manager", - # executable="spawner", - # arguments=["imu_sensor_broadcaster", - # "--controller-manager", "/controller_manager"], - # ), + Node( + package="controller_manager", + executable="spawner", + arguments=["imu_sensor_broadcaster", + "--controller-manager", "/controller_manager"], + ), ] diff --git a/descriptions/go2_description/xacro/ros2_control.xacro b/descriptions/go2_description/xacro/ros2_control.xacro index 80dba85..1976767 100644 --- a/descriptions/go2_description/xacro/ros2_control.xacro +++ b/descriptions/go2_description/xacro/ros2_control.xacro @@ -151,6 +151,19 @@ + + + + + + + + + + + + + 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 869c581..11fb473 100644 --- a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h +++ b/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h @@ -36,6 +36,8 @@ protected: std::vector joint_velocities_; std::vector joint_effort_; + std::vector imu_states_; + std::unordered_map > joint_interfaces = { {"position", {}}, {"velocity", {}}, diff --git a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp index cb0f58f..6caa434 100644 --- a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp +++ b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp @@ -28,6 +28,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa joint_velocities_.assign(12, 0); joint_effort_.assign(12, 0); + imu_states_.assign(10, 0); + for (const auto &joint: info_.joints) { for (const auto &interface: joint.state_interfaces) { joint_interfaces[interface.name].push_back(joint.name); @@ -73,6 +75,13 @@ std::vector HardwareUnitree::export_state_in state_interfaces.emplace_back(joint_name, "effort", &joint_velocities_[ind++]); } + // export imu sensor state interface + for (uint i = 0; i < info_.sensors[0].state_interfaces.size(); i++) { + state_interfaces.emplace_back( + info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &imu_states_[i]); + } + + return state_interfaces; } @@ -100,11 +109,26 @@ 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(); } + + // imu states + imu_states_[0] = _lowState.imu_state().quaternion()[0]; + imu_states_[1] = _lowState.imu_state().quaternion()[1]; + imu_states_[2] = _lowState.imu_state().quaternion()[2]; + imu_states_[3] = _lowState.imu_state().quaternion()[3]; + 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]; + return return_type::OK; }