From e45a395e59d4beecd2a981bdbac151cbfcc9d105 Mon Sep 17 00:00:00 2001 From: fan-ziqi Date: Thu, 23 May 2024 17:06:35 +0800 Subject: [PATCH] feat: add robot_msgs --- src/rl_sar/CMakeLists.txt | 1 + src/rl_sar/include/rl_sim.hpp | 14 ++++++--- src/rl_sar/package.xml | 1 + src/rl_sar/src/rl_sim.cpp | 7 ++--- src/robot_msgs/CMakeLists.txt | 44 +++++++++++++++++++++++++++++ src/robot_msgs/msg/IMU.msg | 3 ++ src/robot_msgs/msg/MotorCommand.msg | 5 ++++ src/robot_msgs/msg/MotorState.msg | 5 ++++ src/robot_msgs/msg/RobotCommand.msg | 1 + src/robot_msgs/msg/RobotState.msg | 2 ++ src/robot_msgs/package.xml | 19 +++++++++++++ 11 files changed, 94 insertions(+), 8 deletions(-) create mode 100644 src/robot_msgs/CMakeLists.txt create mode 100644 src/robot_msgs/msg/IMU.msg create mode 100644 src/robot_msgs/msg/MotorCommand.msg create mode 100644 src/robot_msgs/msg/MotorState.msg create mode 100644 src/robot_msgs/msg/RobotCommand.msg create mode 100644 src/robot_msgs/msg/RobotState.msg create mode 100644 src/robot_msgs/package.xml diff --git a/src/rl_sar/CMakeLists.txt b/src/rl_sar/CMakeLists.txt index c3c229e..696c415 100644 --- a/src/rl_sar/CMakeLists.txt +++ b/src/rl_sar/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs tf geometry_msgs + robot_msgs unitree_legged_msgs ) diff --git a/src/rl_sar/include/rl_sim.hpp b/src/rl_sar/include/rl_sim.hpp index fe0faf8..029075a 100644 --- a/src/rl_sar/include/rl_sim.hpp +++ b/src/rl_sar/include/rl_sim.hpp @@ -11,7 +11,9 @@ #include "unitree_legged_sdk/loop.h" #include -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 loop_control; - std::shared_ptr loop_rl; - std::shared_ptr loop_plot; + std::shared_ptr loop_control; + std::shared_ptr loop_rl; + std::shared_ptr loop_plot; const int plot_size = 100; std::vector plot_t; @@ -53,6 +55,10 @@ private: std::map torque_publishers; std::vector motor_commands; + // std::vector 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; diff --git a/src/rl_sar/package.xml b/src/rl_sar/package.xml index 3a738ae..6cf8dbb 100644 --- a/src/rl_sar/package.xml +++ b/src/rl_sar/package.xml @@ -25,6 +25,7 @@ robot_state_publisher roscpp std_msgs + robot_msgs unitree_legged_msgs diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index 0225bbb..9fcdbdc 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -64,13 +64,13 @@ RL_Sim::RL_Sim() joint_state_subscriber_ = nh.subscribe( ros_namespace + "joint_states", 10, &RL_Sim::JointStatesCallback, this); - loop_control = std::make_shared("loop_control", 0.002, boost::bind(&RL_Sim::RobotControl, this)); - loop_rl = std::make_shared("loop_rl" , 0.02 , boost::bind(&RL_Sim::RunModel, this)); + loop_control = std::make_shared("loop_control", 0.002, boost::bind(&RL_Sim::RobotControl, this)); + loop_rl = std::make_shared("loop_rl" , 0.02 , boost::bind(&RL_Sim::RunModel, this)); loop_control->start(); loop_rl->start(); #ifdef PLOT - loop_plot = std::make_shared("loop_plot" , 0.002, boost::bind(&RL_Sim::Plot, this)); + loop_plot = std::make_shared("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]; } diff --git a/src/robot_msgs/CMakeLists.txt b/src/robot_msgs/CMakeLists.txt new file mode 100644 index 0000000..108236f --- /dev/null +++ b/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" +) \ No newline at end of file diff --git a/src/robot_msgs/msg/IMU.msg b/src/robot_msgs/msg/IMU.msg new file mode 100644 index 0000000..6e130c3 --- /dev/null +++ b/src/robot_msgs/msg/IMU.msg @@ -0,0 +1,3 @@ +float32[4] quaternion +float32[3] gyroscope +float32[3] accelerometer \ No newline at end of file diff --git a/src/robot_msgs/msg/MotorCommand.msg b/src/robot_msgs/msg/MotorCommand.msg new file mode 100644 index 0000000..94f9a97 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/robot_msgs/msg/MotorState.msg b/src/robot_msgs/msg/MotorState.msg new file mode 100644 index 0000000..fbc5477 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/src/robot_msgs/msg/RobotCommand.msg b/src/robot_msgs/msg/RobotCommand.msg new file mode 100644 index 0000000..f8f6bec --- /dev/null +++ b/src/robot_msgs/msg/RobotCommand.msg @@ -0,0 +1 @@ +MotorCommand[32] motor_command \ No newline at end of file diff --git a/src/robot_msgs/msg/RobotState.msg b/src/robot_msgs/msg/RobotState.msg new file mode 100644 index 0000000..d11a054 --- /dev/null +++ b/src/robot_msgs/msg/RobotState.msg @@ -0,0 +1,2 @@ +IMU imu +MotorState[32] motor_state \ No newline at end of file diff --git a/src/robot_msgs/package.xml b/src/robot_msgs/package.xml new file mode 100644 index 0000000..ba89639 --- /dev/null +++ b/src/robot_msgs/package.xml @@ -0,0 +1,19 @@ + + + + robot_msgs + 0.0.0 + + The robot messgaes package. + + Ziqi Fan + Apache + + catkin + message_runtime + message_generation + std_msgs + geometry_msgs + sensor_msgs + + \ No newline at end of file