diff --git a/unitree_legged_real/include/convert.h b/unitree_legged_real/include/convert.h
index d3ef166..eba68c5 100644
--- a/unitree_legged_real/include/convert.h
+++ b/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++)
{
diff --git a/unitree_legged_real/include/real_robot.h b/unitree_legged_real/include/real_robot.h
index 26485c3..0653bf0 100644
--- a/unitree_legged_real/include/real_robot.h
+++ b/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();
diff --git a/unitree_legged_real/include/ros_udp_node.h b/unitree_legged_real/include/ros_udp_node.h
index f5c5984..8060699 100644
--- a/unitree_legged_real/include/ros_udp_node.h
+++ b/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.
);
};
diff --git a/unitree_legged_real/launch/robot.launch b/unitree_legged_real/launch/robot.launch
index db9444a..984425b 100644
--- a/unitree_legged_real/launch/robot.launch
+++ b/unitree_legged_real/launch/robot.launch
@@ -9,6 +9,8 @@
+
+
@@ -23,6 +25,8 @@
+
+
@@ -70,6 +74,7 @@
+
\ No newline at end of file
diff --git a/unitree_legged_real/src/real_robot.cpp b/unitree_legged_real/src/real_robot.cpp
index 26f9d6b..0089c26 100644
--- a/unitree_legged_real/src/real_robot.cpp
+++ b/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(
- this->robot_namespace_ + "/imu_estimated", 1
- );
this->position_limit_publisher = this->ros_handle.advertise(
this->robot_namespace_ + "/position_limit", 1
);
}
+ if (this->publish_imu)
+ this->imu_publisher = this->ros_handle.advertise(
+ this->robot_namespace_ + "/imu", 1
+ );
this->wirelessRemote_publisher = this->ros_handle.advertise(
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("position_protect_limit", position_protect_limit, 0.087);
int power_protect_level; nh.param("power_protect_level", power_protect_level, 1);
+ bool publish_imu; nh.param("publish_imu", publish_imu, false);
+ bool start_stand; nh.param("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();
diff --git a/unitree_legged_real/src/ros_udp_node.cpp b/unitree_legged_real/src/ros_udp_node.cpp
index 9460945..1c6b09f 100644
--- a/unitree_legged_real/src/ros_udp_node.cpp
+++ b/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("position_protect_limit", position_protect_limit, 0.087);
int power_protect_level; nh.param("power_protect_level", power_protect_level, 1);
+ bool start_stand; nh.param("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();