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

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();