feat: add robot_msgs

src/rl_sar/CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
     std_msgs
     tf
     geometry_msgs
+    robot_msgs
     unitree_legged_msgs
 )
 

src/rl_sar/include/rl_sim.hpp

@@ -11,7 +11,9 @@
 #include "unitree_legged_sdk/loop.h"
 #include <csignal>
 
-using namespace UNITREE_LEGGED_SDK;
+// #include "robot_msgs/MotorCommand.h"
+// #include "robot_msgs/RobotState.h"
+// #include "robot_msgs/RobotCommand.h"
 
 class RL_Sim : public RL
 {
@@ -33,9 +35,9 @@ public:
 
     int motiontime = 0;
 
-    std::shared_ptr<LoopFunc> loop_control;
-    std::shared_ptr<LoopFunc> loop_rl;
-    std::shared_ptr<LoopFunc> loop_plot;
+    std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_control;
+    std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_rl;
+    std::shared_ptr<UNITREE_LEGGED_SDK::LoopFunc> loop_plot;
 
     const int plot_size = 100;
     std::vector<int> plot_t;
@@ -53,6 +55,10 @@ private:
     std::map<std::string, ros::Publisher> torque_publishers;
     std::vector<unitree_legged_msgs::MotorCmd> motor_commands;
 
+    // std::vector<robot_msgs::MotorCommand> motor_commands;
+    // robot_msgs::RobotState robot_state;
+    // robot_msgs::RobotCommand robot_command;
+
     geometry_msgs::Twist vel;
     geometry_msgs::Pose pose;
     geometry_msgs::Twist cmd_vel;

src/rl_sar/package.xml

@@ -25,6 +25,7 @@
     <exec_depend>robot_state_publisher</exec_depend>
     <exec_depend>roscpp</exec_depend>
     <exec_depend>std_msgs</exec_depend>
+    <depend>robot_msgs</depend>
     <depend>unitree_legged_msgs</depend>  
 
     <export>

src/rl_sar/src/rl_sim.cpp

@@ -64,13 +64,13 @@ RL_Sim::RL_Sim()
     joint_state_subscriber_ = nh.subscribe<sensor_msgs::JointState>(
         ros_namespace + "joint_states", 10, &RL_Sim::JointStatesCallback, this);
 
-    loop_control = std::make_shared<LoopFunc>("loop_control", 0.002,    boost::bind(&RL_Sim::RobotControl, this));
-    loop_rl      = std::make_shared<LoopFunc>("loop_rl"     , 0.02 ,    boost::bind(&RL_Sim::RunModel,     this));
+    loop_control = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_control", 0.002,    boost::bind(&RL_Sim::RobotControl, this));
+    loop_rl      = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_rl"     , 0.02 ,    boost::bind(&RL_Sim::RunModel,     this));
 
     loop_control->start();
     loop_rl->start();
 #ifdef PLOT
-    loop_plot    = std::make_shared<LoopFunc>("loop_plot"   , 0.002,    boost::bind(&RL_Sim::Plot,         this));
+    loop_plot    = std::make_shared<UNITREE_LEGGED_SDK::LoopFunc>("loop_plot"   , 0.002,    boost::bind(&RL_Sim::Plot,         this));
     loop_plot->start();
 #endif
 
@@ -110,7 +110,6 @@ void RL_Sim::RobotControl()
 
 void RL_Sim::ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg)
 {
-
     vel = msg->twist[2];
     pose = msg->pose[2];
 }

src/robot_msgs/CMakeLists.txt

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  std_msgs
+  geometry_msgs
+  sensor_msgs
+)
+
+add_message_files(
+  FILES
+  MotorCommand.msg
+  MotorState.msg
+  RobotCommand.msg
+  RobotState.msg
+  IMU.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+  sensor_msgs
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+  message_runtime
+  std_msgs
+  geometry_msgs
+  sensor_msgs 
+)
+
+#############
+## Install ##
+#############
+
+# Mark topic names header files for installation
+install(
+  DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.h"
+)

src/robot_msgs/msg/IMU.msg

@@ -0,0 +1,3 @@
+float32[4] quaternion
+float32[3] gyroscope
+float32[3] accelerometer

src/robot_msgs/msg/MotorCommand.msg

@@ -0,0 +1,5 @@
+float32 q            # motor target position
+float32 dq           # motor target velocity
+float32 tau          # motor target torque
+float32 kp           # motor spring stiffness coefficient
+float32 kd           # motor damper coefficient

src/robot_msgs/msg/MotorState.msg

@@ -0,0 +1,5 @@
+float32 q            # motor current position (rad)
+float32 dq           # motor current speed (rad/s)
+float32 ddq          # motor current speed (rad/s)
+float32 tauEst       # current estimated output torque (N*m)
+float32 cur          # current estimated output cur (N*m)

src/robot_msgs/msg/RobotCommand.msg

@@ -0,0 +1 @@
+MotorCommand[32] motor_command

src/robot_msgs/msg/RobotState.msg

@@ -0,0 +1,2 @@
+IMU imu
+MotorState[32] motor_state

src/robot_msgs/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+
+<package format="2">
+  <name>robot_msgs</name>
+  <version>0.0.0</version>
+  <description>
+    The robot messgaes package.
+  </description>
+  <maintainer email="fanziqi614@gmail.com">Ziqi Fan</maintainer>
+  <license>Apache</license>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>message_runtime</depend>
+  <depend>message_generation</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  
+</package>