[add] publish_imu option in robot.launch when func:=full

Author: Ziwen Zhuang

unitree_legged_real/include/convert.h

@@ -30,7 +30,11 @@ unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state)
 {
     unitree_legged_msgs::IMU ros_msg;
 
-    for (int i(0); i < 4; i++) ros_msg.quaternion[i] = state.quaternion[i];
+    // make sure the ros_msg quaternion is in (x, y, z, w) order
+    ros_msg.quaternion[0] = state.quaternion[1];
+    ros_msg.quaternion[1] = state.quaternion[2];
+    ros_msg.quaternion[2] = state.quaternion[3];
+    ros_msg.quaternion[3] = state.quaternion[0];
 
     for (int i(0); i < 3; i++)
     {

unitree_legged_real/include/real_robot.h

@@ -29,8 +29,11 @@ class UnitreeRos: public RosUdpHandler
     int state_check_times_max = 50; // When changing mode through UDP, the service handler checks the updated state at most this times
     int state_check_freq = 10; // The frequency of checking mode update.
     int timer_freq = 50; // default timer frequency for some default functions.
+    bool publish_imu = false;
+    int imu_publish_freq = 100;
+    int imu_publish_seq = 0;
 
-    ros::Publisher pose_estimation_publisher;
+    ros::Publisher imu_publisher;
     ros::Publisher wirelessRemote_publisher;
     ros::Publisher position_limit_publisher;
 
@@ -44,13 +47,12 @@ class UnitreeRos: public RosUdpHandler
 
     ros::Subscriber low_motor_subscriber;
 
+    ros::Timer imu_publish_timer;
     ros::Timer wirelessRemote_publish_timer;
     ros::Timer protect_limit_publish_timer;
 
 protected:
     // For simplicity, some states and commands must be processed and estimate
-    void pose_estimate_and_publish();
-
     bool set_gaitType_srv_callback(
         unitree_legged_srvs::SetGaitType::Request &req,
         unitree_legged_srvs::SetGaitType::Response &res
@@ -70,6 +72,7 @@ class UnitreeRos: public RosUdpHandler
 
     void low_motor_callback(const unitree_legged_msgs::LegsCmd::ConstPtr &msg);
 
+    void imu_publish_callback(const ros::TimerEvent& event);
     void wirelessRemote_publish_callback(const ros::TimerEvent& event);
     void protect_limit_publish_callback(const ros::TimerEvent& event);
 
@@ -81,6 +84,8 @@ class UnitreeRos: public RosUdpHandler
             float position_protect_limit,
             int power_protect_level,
             bool cmd_check,
+            bool start_stand,
+            bool publish_imu,
             bool dryrun                                     // If true, does not send the udp message in udp_send() but do everything else.
         );
     void publisher_init();

unitree_legged_real/include/ros_udp_node.h

@@ -40,6 +40,7 @@ class RosUdpHandler
     float position_protect_limit;
     int low_power_protect_level;
     float low_cmd_default_tau = 0.65f;
+    bool start_stand; // if true, the motor will be initialized to mode 10, otherwise mode 0.
 
     UNITREE_LEGGED_SDK::HighCmd high_cmd_buffer = {0};
     UNITREE_LEGGED_SDK::HighState high_state_buffer = {0};
@@ -85,6 +86,7 @@ class RosUdpHandler
             float position_protect_limit,                   // Please check safety.h, 0.0 is the least limit
             int power_protect_level,                        // Refer to unitree_legged_sdk/safety.h
             bool cmd_check,
+            bool start_stand,                               // If true, defualt motor mode is 10, otherwise 0.
             bool dryrun                                     // If true, does not send the udp message in udp_send() but do everything else.
         );
 };

unitree_legged_real/launch/robot.launch

@@ -9,6 +9,8 @@
     <arg name="power_protect_level" default="1"/>
     <arg name="output" default="screen"/>
     <arg name="func" default="full"/>
+    <arg name="publish_imu" default="false"/>
+    <arg name="start_stand" default="false"/>
     <!-- camera configurations, only when func:=full and camera_mode!=none -->
     <arg name="camera_mode" default="none"/>
     <arg name="image_fps" default="30"/>
@@ -23,6 +25,8 @@
             <param name="ctrl_level" type="str" value="$(arg ctrl_level)"/>
             <param name="position_protect_limit" type="double" value="$(arg position_protect_limit)"/>
             <param name="power_protect_level" type="int" value="$(arg power_protect_level)"/>
+            <param name="publish_imu" type="bool" value="$(arg publish_imu)"/>
+            <param name="start_stand" type="bool" value="$(arg start_stand)"/>
         </node>
 
         <group if="$(eval arg('camera_mode') == 'raw')">
@@ -70,6 +74,7 @@
             <param name="ctrl_level" type="str" value="$(arg ctrl_level)"/>
             <param name="position_protect_limit" type="double" value="$(arg position_protect_limit)"/>
             <param name="power_protect_level" type="int" value="$(arg power_protect_level)"/>
+            <param name="start_stand" type="bool" value="$(arg start_stand)"/>
         </node>
     </group>
 </launch>

unitree_legged_real/src/real_robot.cpp

@@ -123,6 +123,35 @@ void UnitreeRos::low_motor_callback(const unitree_legged_msgs::LegsCmd::ConstPtr
     }
 }
 
+void UnitreeRos::imu_publish_callback(const ros::TimerEvent& event){
+    sensor_msgs::Imu ros_msg;
+    UNITREE_LEGGED_SDK::IMU *imu_ptr;
+    if (this->ctrl_level == UNITREE_LEGGED_SDK::HIGHLEVEL)
+        imu_ptr = &this->high_state_buffer.imu;
+    else
+        imu_ptr = &this->low_state_buffer.imu;
+    
+    // fill ros message
+    ros_msg.header.seq = this->imu_publish_seq++;
+    ros_msg.header.stamp = ros::Time::now();
+    ros_msg.header.frame_id = this->robot_namespace_ + "/imu_link";
+    ros_msg.orientation.x = imu_ptr->quaternion[0];
+    ros_msg.orientation.y = imu_ptr->quaternion[1];
+    ros_msg.orientation.z = imu_ptr->quaternion[2];
+    ros_msg.orientation.w = imu_ptr->quaternion[3];
+    // ros_msg.orientation_covariance unknown
+    ros_msg.angular_velocity.x = imu_ptr->gyroscope[0];
+    ros_msg.angular_velocity.y = imu_ptr->gyroscope[1];
+    ros_msg.angular_velocity.z = imu_ptr->gyroscope[2];
+    // ros_msg.angular_velocity_covariance unknown
+    ros_msg.linear_acceleration.x = imu_ptr->accelerometer[0];
+    ros_msg.linear_acceleration.y = imu_ptr->accelerometer[1];
+    ros_msg.linear_acceleration.z = imu_ptr->accelerometer[2];
+    // ros_mag.linear_acceleration_covariance unknown
+
+    this->imu_publisher.publish(ros_msg);
+}
+
 void UnitreeRos::wirelessRemote_publish_callback(const ros::TimerEvent& event)
 {
     unitree_legged_msgs::WirelessRemote ros_msg;
@@ -197,9 +226,21 @@ UnitreeRos::UnitreeRos(
     float position_protect_limit,
     int power_protect_level,
     bool cmd_check,
+    bool start_stand,
+    bool publish_imu,
     bool dryrun
 ):
-    RosUdpHandler(robot_namespace, udp_duration, level, position_protect_limit, power_protect_level, cmd_check, dryrun)
+    publish_imu(publish_imu),
+    RosUdpHandler(
+        robot_namespace,
+        udp_duration,
+        level,
+        position_protect_limit,
+        power_protect_level,
+        cmd_check,
+        start_stand,
+        dryrun
+    )
 {
     this->publisher_init();
     this->server_init();
@@ -214,13 +255,14 @@ void UnitreeRos::publisher_init()
 {
     if (this->ctrl_level == UNITREE_LEGGED_SDK::LOWLEVEL)
     {
-        this->pose_estimation_publisher = this->ros_handle.advertise<sensor_msgs::Imu>(
-            this->robot_namespace_ + "/imu_estimated", 1
-        );
         this->position_limit_publisher = this->ros_handle.advertise<std_msgs::Float32MultiArray>(
             this->robot_namespace_ + "/position_limit", 1
         );
     }
+    if (this->publish_imu)
+        this->imu_publisher = this->ros_handle.advertise<sensor_msgs::Imu>(
+            this->robot_namespace_ + "/imu", 1
+        );
     this->wirelessRemote_publisher = this->ros_handle.advertise<unitree_legged_msgs::WirelessRemote>(
         this->robot_namespace_ + "/wireless_remote", 1
     );
@@ -281,6 +323,12 @@ void UnitreeRos::timer_init()
             this
         );
     }
+    if (this->publish_imu)
+        this->imu_publish_timer = this->ros_handle.createTimer(
+            ros::Duration(1. / this->imu_publish_freq),
+            &UnitreeRos::imu_publish_callback,
+            this
+        );
     this->wirelessRemote_publish_timer = this->ros_handle.createTimer(
         ros::Duration(1. / this->timer_freq),
         &UnitreeRos::wirelessRemote_publish_callback,
@@ -304,6 +352,11 @@ int main(int argc, char **argv)
     if (use_low_level) level = UNITREE_LEGGED_SDK::LOWLEVEL;
     float position_protect_limit; nh.param<float>("position_protect_limit", position_protect_limit, 0.087);
     int power_protect_level; nh.param<int>("power_protect_level", power_protect_level, 1);
+    bool publish_imu; nh.param<bool>("publish_imu", publish_imu, false);
+    bool start_stand; nh.param<bool>("start_stand", start_stand, true);
+
+    if (start_stand) ROS_INFO("Motor will be initialized to mode 10, please put the leg in stand positions.");
+    else ROS_INFO("Motor will be initialized to mode 0, please put the robot on the ground or hang up.");
 
     // construct and initialize this ros node
     UnitreeRos unitree_ros_node(
@@ -313,6 +366,8 @@ int main(int argc, char **argv)
         position_protect_limit,
         power_protect_level,
         cmd_check,
+        start_stand,
+        publish_imu,
         dryrun
     );
     ros::spin();

unitree_legged_real/src/ros_udp_node.cpp

@@ -110,7 +110,10 @@ void RosUdpHandler::set_default_high_cmd()
 void RosUdpHandler::set_default_low_cmd()
 {
     // set mode
-    for (int i(0); i < 12; i++) this->low_cmd_buffer.motorCmd[i].mode = 10;
+    if (this->start_stand)
+        for (int i(0); i < 12; i++) this->low_cmd_buffer.motorCmd[i].mode = 10;
+    else
+        for (int i(0); i < 12; i++) this->low_cmd_buffer.motorCmd[i].mode = 0;
     // set q (position)
     this->low_cmd_buffer.motorCmd[UNITREE_LEGGED_SDK::FR_0].q = -0.3;
     this->low_cmd_buffer.motorCmd[UNITREE_LEGGED_SDK::FR_1].q = 0.9;
@@ -245,6 +248,7 @@ RosUdpHandler::RosUdpHandler(
     float position_protect_limit,
     int power_protect_level,
     bool cmd_check,
+    bool start_stand,
     bool dryrun
 ):
     safe(UNITREE_LEGGED_SDK::LeggedType::A1),
@@ -256,6 +260,7 @@ RosUdpHandler::RosUdpHandler(
     position_protect_limit(position_protect_limit),
     low_power_protect_level(power_protect_level),
     cmd_check(cmd_check),
+    start_stand(start_stand),
     dryrun_(dryrun),
     loop_udp_send("udp_send", udp_duration, 3, boost::bind(&RosUdpHandler::udp_send, this)),
     loop_udp_recv("udp_recv", udp_duration, 3, boost::bind(&RosUdpHandler::udp_recv, this))
@@ -302,6 +307,10 @@ int main(int argc, char **argv)
     if (use_low_level) level = UNITREE_LEGGED_SDK::LOWLEVEL;
     float position_protect_limit; nh.param<float>("position_protect_limit", position_protect_limit, 0.087);
     int power_protect_level; nh.param<int>("power_protect_level", power_protect_level, 1);
+    bool start_stand; nh.param<bool>("start_stand", start_stand, true);
+
+    if (start_stand) ROS_INFO("Motor will be initialized to mode 10, please put the leg in stand positions.");
+    else ROS_INFO("Motor will be initialized to mode 0, please put the robot on the ground or hang up.");
 
     // construct and initialize this ros node
     RosUdpHandler ros_udp_handler(
@@ -311,6 +320,7 @@ int main(int argc, char **argv)
         position_protect_limit,
         power_protect_level,
         cmd_check,
+        start_stand,
         dryrun
     );
     ros::spin();