add imu to hardware interface

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) {

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
+      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"

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"],
+        ),
     ]
 
 

descriptions/go2_description/xacro/ros2_control.xacro

@@ -151,6 +151,19 @@
             <state_interface name="effort"/>
         </joint>
 
+        <sensor name="imu_sensor">
+            <state_interface name="orientation.x"/>
+            <state_interface name="orientation.y"/>
+            <state_interface name="orientation.z"/>
+            <state_interface name="orientation.w"/>
+            <state_interface name="angular_velocity.x"/>
+            <state_interface name="angular_velocity.y"/>
+            <state_interface name="angular_velocity.z"/>
+            <state_interface name="linear_acceleration.x"/>
+            <state_interface name="linear_acceleration.y"/>
+            <state_interface name="linear_acceleration.z"/>
+        </sensor>
+
     </ros2_control>
 
 </robot>

hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h

@@ -36,6 +36,8 @@ protected:
     std::vector<double> joint_velocities_;
     std::vector<double> joint_effort_;
 
+    std::vector<double> imu_states_;
+
     std::unordered_map<std::string, std::vector<std::string> > joint_interfaces = {
         {"position", {}},
         {"velocity", {}},

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<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> 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;
 }