feat: ROS2 support

#34

.gitignore

@@ -1,6 +1,8 @@
 build
 devel
 logs
+install
+log
 .catkin_tools
 .vscode
 *.csv

README.md

@@ -2,10 +2,13 @@
 
 [中文文档](README_CN.md)
 
+**Version Select: [ROS-Noetic](https://github.com/fan-ziqi/rl_sar/tree/main) | [ROS2-Foxy](https://github.com/fan-ziqi/rl_sar/tree/ros2)**
+
 This repository provides a framework for simulation verification and physical deployment of robot reinforcement learning algorithms, suitable for quadruped robots, wheeled robots, and humanoid robots. "sar" stands for "simulation and real"
 
 feature:
 - Support legged_gym based on IaacGym and IsaacLab based on IsaacSim. Use `framework` to distinguish.
+- The code has two versions: **ROS** and **ROS2**
 - The code supports both cpp and python, you can find python version in `src/rl_sar/scripts`
 
 [Click to discuss on Discord](https://discord.gg/vmVjkhVugU)
@@ -20,10 +23,10 @@ git clone https://github.com/fan-ziqi/rl_sar.git
 
 ## Dependency
 
-This project uses `ros-noetic` (Ubuntu 20.04) and requires the installation of the following ROS dependency packages:
+This project uses `ros2-foxy` (Ubuntu 20.04) and requires the installation of the following ROS dependency packages:
 
 ```bash
-sudo apt install ros-noetic-teleop-twist-keyboard ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller
+sudo apt install ros-$ROS_DISTRO-teleop-twist-keyboard ros-$ROS_DISTRO-ros2-control ros-$ROS_DISTRO-ros2-controllers ros-$ROS_DISTRO-control-toolbox ros-$ROS_DISTRO-robot-state-publisher ros-$ROS_DISTRO-joint-state-publisher-gui ros-$ROS_DISTRO-gazebo-ros2-control ros-$ROS_DISTRO-gazebo-ros-pkgs ros-$ROS_DISTRO-xacro
 ```
 
 Download and deploy `libtorch` at any location
@@ -72,14 +75,17 @@ Compile in the root directory of the project
 
 ```bash
 cd ..
-catkin build
+colcon build --merge-install --symlink-install
 ```
 
-If catkin build report errors: `Unable to find either executable 'empy' or Python module 'em'`, run `catkin config -DPYTHON_EXECUTABLE=/usr/bin/python3` before `catkin build`
-
 ## Running
 
-In the following text, **\<ROBOT\>_\<PLATFORM\>** is used to represent different environments, which can be `a1_isaacgym`, `a1_isaacsim`, `go2_isaacgym`, `gr1t1_isaacgym`, or `gr1t2_isaacgym`.
+In the following text, **\<ROBOT\>_\<PLATFORM\>** is used to represent different environments. Currently supported list:
+
+|       | isaacgym | isaacsim |
+|-------|----------|----------|
+| a1    | ✓        | ✓        |
+| go2   | ✓        |          |
 
 Before running, copy the trained pt model file to `rl_sar/src/rl_sar/models/<ROBOT>_<PLATFORM>`, and configure the parameters in `config.yaml`.
 
@@ -88,24 +94,25 @@ Before running, copy the trained pt model file to `rl_sar/src/rl_sar/models/<ROB
 Open a terminal, launch the gazebo simulation environment
 
 ```bash
-source devel/setup.bash
+source install/setup.bash
-roslaunch rl_sar gazebo_<ROBOT>_<PLATFORM>.launch
+ros2 launch rl_sar gazebo.launch.py rname:=<ROBOT> framework:=<PLATFORM>
+(e.g. ros2 launch rl_sar gazebo.launch.py rname:=a1 framework:=isaacgym)
 ```
 
 Open a new terminal, launch the control program
 
 ```bash
-source devel/setup.bash
+source install/setup.bash
-(for cpp version)    rosrun rl_sar rl_sim
+(for cpp version)    ros2 run rl_sar rl_sim
-(for python version) rosrun rl_sar rl_sim.py
+(for python version) ros2 run rl_sar rl_sim.py
 ```
 
 Control:
 
-* Press **\<Enter\>** to toggle simulation start/stop.
+<!-- * Press **\<Enter\>** to toggle simulation start/stop. -->
 * **W** and **S** controls x-axis, **A** and **D** controls yaw, and **J** and **L** controls y-axis.
 * Press **\<Space\>** to sets all control commands to zero.
-* If robot falls down, press **R** to reset Gazebo environment.
+<!-- * If robot falls down, press **R** to reset Gazebo environment. -->
 
 ### Real Robots
 
@@ -119,8 +126,8 @@ Unitree A1 can be connected using both wireless and wired methods:
 Open a new terminal and start the control program
 
 ```bash
-source devel/setup.bash
+source install/local.bash
-rosrun rl_sar rl_real_a1
+ros2 run rl_sar rl_real_a1
 ```
 
 Press the **R2** button on the controller to switch the robot to the default standing position, press **R1** to switch to RL control mode, and press **L2** in any state to switch to the initial lying position. The left stick controls x-axis up and down, controls yaw left and right, and the right stick controls y-axis left and right.
@@ -133,8 +140,8 @@ Or press **0** on the keyboard to switch the robot to the default standing posit
 2. Use the `ifconfig` command to find the name of the network interface for the 123 network segment, such as `enxf8e43b808e06`. In the following steps, replace `<YOUR_NETWORK_INTERFACE>` with the actual network interface name.
 3. Open a new terminal and start the control program:
     ```bash
-    source devel/setup.bash
+    source install/local.bash
-    rosrun rl_sar rl_real_go2 <YOUR_NETWORK_INTERFACE>
+    ros2 run rl_sar rl_real_go2 <YOUR_NETWORK_INTERFACE>
     ```
 4. Go2 supports both joy and keyboard control, using the same method as mentioned above for A1.
 
@@ -146,11 +153,11 @@ Take A1 as an example below
 2. Run the control program, and the program will log all data after execution.
 3. Stop the control program and start training the actuator network. Note that `rl_sar/src/rl_sar/models/` is omitted before the following paths.
     ```bash
-    rosrun rl_sar actuator_net.py --mode train --data a1/motor.csv --output a1/motor.pt
+    ros2 run rl_sar actuator_net.py --mode train --data a1/motor.csv --output a1/motor.pt
     ```
 4. Verify the trained actuator network.
     ```bash
-    rosrun rl_sar actuator_net.py --mode play --data a1/motor.csv --output a1/motor.pt
+    ros2 run rl_sar actuator_net.py --mode play --data a1/motor.csv --output a1/motor.pt
     ```
 
 ## Add Your Robot
@@ -160,8 +167,8 @@ In the following text, **\<ROBOT\>_\<PLATFORM\>** is used to represent your robo
 1. Create a model package named `<ROBOT>_description` in the `rl_sar/src/robots` directory. Place the robot's URDF file in the `rl_sar/src/robots/<ROBOT>_description/urdf` directory and name it `<ROBOT>.urdf`. Additionally, create a joint configuration file with the namespace `<ROBOT>_gazebo` in the `rl_sar/src/robots/<ROBOT>_description/config` directory.
 2. Place the trained RL model files in the `rl_sar/src/rl_sar/models/<ROBOT>_<PLATFORM>` directory, and create a new `config.yaml` file in this path. Refer to the `rl_sar/src/rl_sar/models/a1_isaacgym/config.yaml` file to modify the parameters.
 3. Modify the `forward()` function in the code as needed to adapt to different models.
-4. If you need to run simulations, modify the launch files as needed by referring to those in the `rl_sar/src/rl_sar/launch` directory.
+<!-- 4. If you need to run simulations, modify the launch files as needed by referring to those in the `rl_sar/src/rl_sar/launch` directory. -->
-5. If you need to run on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed.
+4. If you need to run on the physical robot, modify the file `rl_sar/src/rl_sar/src/rl_real_a1.cpp` as needed.
 
 ## Contributing
 

README_CN.md

@@ -2,10 +2,13 @@
 
 [English document](README.md)
 
+**版本选择: [ROS-Noetic](https://github.com/fan-ziqi/rl_sar/tree/main) | [ROS2-Foxy](https://github.com/fan-ziqi/rl_sar/tree/ros2)**
+
 本仓库提供了机器人强化学习算法的仿真验证与实物部署框架，适配四足机器人、轮足机器人、人形机器人。"sar"代表"simulation and real"
 
 特性：
 - 支持基于IaacGym的legged_gym，也支持基于IsaacSim的IsaacLab，用`framework`加以区分。
+- 代码有**ROS**和**ROS2**两个版本
 - 代码有python和cpp两个版本，python版本可以在`src/rl_sar/scripts`中找到
 
 [点击在Discord上讨论](https://discord.gg/MC9KguQHtt)
@@ -20,10 +23,10 @@ git clone https://github.com/fan-ziqi/rl_sar.git
 
 ## 依赖
 
-本项目使用`ros-noetic`(Ubuntu20.04)，且需要安装以下的ros依赖包
+本项目使用`ros2-foxy`(Ubuntu20.04)，且需要安装以下的ros依赖包
 
 ```bash
-sudo apt install ros-noetic-teleop-twist-keyboard ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller
+sudo apt install ros-$ROS_DISTRO-teleop-twist-keyboard ros-$ROS_DISTRO-ros2-control ros-$ROS_DISTRO-ros2-controllers ros-$ROS_DISTRO-control-toolbox ros-$ROS_DISTRO-robot-state-publisher ros-$ROS_DISTRO-joint-state-publisher-gui ros-$ROS_DISTRO-gazebo-ros2-control ros-$ROS_DISTRO-gazebo-ros-pkgs ros-$ROS_DISTRO-xacro
 ```
 
 在任意位置下载并部署`libtorch`
@@ -72,14 +75,17 @@ sudo ldconfig
 
 ```bash
 cd ..
-catkin build
+colcon build --merge-install --symlink-install
 ```
 
-如果 catkin build 报错: `Unable to find either executable 'empy' or Python module 'em'`, 在`catkin build` 之前执行 `catkin config -DPYTHON_EXECUTABLE=/usr/bin/python3`
-
 ## 运行
 
-下文中使用 **\<ROBOT\>_\<PLATFORM\>** 代替表示不同的环境，可以是 `a1_isaacgym` 、 `a1_isaacsim` 、 `go2_isaacgym` 、 `gr1t1_isaacgym` 、 `gr1t2_isaacgym`
+下文中使用 **\<ROBOT\>** 和 **\<PLATFORM\>** 代替表示不同的机器人和框架。目前支持列表：
+
+|       | isaacgym | isaacsim |
+|-------|----------|----------|
+| a1    | ✓        | ✓        |
+| go2   | ✓        |          |
 
 运行前请将训练好的pt模型文件拷贝到`rl_sar/src/rl_sar/models/<ROBOT>_<PLATFORM>`中，并配置`config.yaml`中的参数。
 
@@ -88,24 +94,25 @@ catkin build
 打开一个终端，启动gazebo仿真环境
 
 ```bash
-source devel/setup.bash
+source install/setup.bash
-roslaunch rl_sar gazebo_<ROBOT>_<PLATFORM>.launch
+ros2 launch rl_sar gazebo.launch.py rname:=<ROBOT> framework:=<PLATFORM>
+(e.g. ros2 launch rl_sar gazebo.launch.py rname:=a1 framework:=isaacgym)
 ```
 
 打开一个新终端，启动控制程序
 
 ```bash
-source devel/setup.bash
+source install/setup.bash
-(for cpp version)    rosrun rl_sar rl_sim
+(for cpp version)    ros2 run rl_sar rl_sim
-(for python version) rosrun rl_sar rl_sim.py
+(for python version) ros2 run rl_sar rl_sim.py
 ```
 
 控制：
 
-* 按 **\<Enter\>** 切换仿真器运行/停止。
+<!-- * 按 **\<Enter\>** 切换仿真器运行/停止。 -->
-* **W** 和 **S** 控制x轴，**A** 和 **D** 控制yaw轴，**J** 和 **L** 控制y轴，按下空格重置控制指令。
+* **W** 和 **S** 控制x轴，**A** 和 **D** 控制yaw轴，**J** 和 **L** 控制y轴。
 * 按 **\<Space\>** 将所有控制指令设置为零。
-* 如果机器人摔倒，按 **R** 重置Gazebo环境。
+<!-- * 如果机器人摔倒，按 **R** 重置Gazebo环境。 -->
 
 ### 真实机器人
 
@@ -119,8 +126,8 @@ source devel/setup.bash
 新建终端，启动控制程序
 
 ```bash
-source devel/setup.bash
+source install/local.bash
-rosrun rl_sar rl_real_a1
+ros2 run rl_sar rl_real_a1
 ```
 
 按下遥控器的**R2**键让机器人切换到默认站起姿态，按下**R1**键切换到RL控制模式，任意状态按下**L2**切换到最初的趴下姿态。左摇杆上下控制x左右控制yaw，右摇杆左右控制y。
@@ -133,8 +140,8 @@ rosrun rl_sar rl_real_a1
 2. 通过`ifconfig`命令查看123网段的网卡名字，如`enxf8e43b808e06`，下文用 \<YOUR_NETWORK_INTERFACE\> 代替
 3. 新建终端，启动控制程序
     ```bash
-    source devel/setup.bash
+    source install/local.bash
-    rosrun rl_sar rl_real_go2 <YOUR_NETWORK_INTERFACE>
+    ros2 run rl_sar rl_real_go2 <YOUR_NETWORK_INTERFACE>
     ```
 4. Go2支持手柄与键盘控制，方法与上面a1相同
 
@@ -144,13 +151,13 @@ rosrun rl_sar rl_real_a1
 
 1. 取消注释`rl_real_a1.cpp`中最上面的`#define CSV_LOGGER`，你也可以在仿真程序中修改对应部分采集仿真数据用来测试训练过程。
 2. 运行控制程序，程序会在执行后记录所有数据。
-3. 停止控制程序，开始训练执行器网络。注意，下面的路径前均省略了`rl_sar/src/rl_sar/models/`。
+3. 停止控制程序，开始训练执行器网络。
     ```bash
-    rosrun rl_sar actuator_net.py --mode train --data a1/motor.csv --output a1/motor.pt
+    ros2 run rl_sar actuator_net.py --mode train --data a1_isaacgym/motor.csv --output a1_isaacgym/motor.pt
     ```
 4. 验证已经训练好的训练执行器网络。
     ```bash
-    rosrun rl_sar actuator_net.py --mode play --data a1/motor.csv --output a1/motor.pt
+    ros2 run rl_sar actuator_net.py --mode play --data a1_isaacgym/motor.csv --output a1_isaacgym/motor.pt
     ```
 
 ## 添加你的机器人
@@ -160,8 +167,8 @@ rosrun rl_sar rl_real_a1
 1. 在`rl_sar/src/robots`路径下创建名为`<ROBOT>_description`的模型包，将模型的urdf放到`rl_sar/src/robots/<ROBOT>_description/urdf`路径下并命名为`<ROBOT>.urdf`，并在`rl_sar/src/robots/<ROBOT>_description/config`路径下创建命名空间为`<ROBOT>_gazebo`的关节配置文件
 2. 将训练好的RL模型文件放到`rl_sar/src/rl_sar/models/<ROBOT>_<PLATFORM>`路径下，并在此路径中新建config.yaml文件，参考`rl_sar/src/rl_sar/models/a1_isaacgym/config.yaml`文件修改其中参数
 3. 按需修改代码中的`forward()`函数，以适配不同的模型
-4. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改
+<!-- 4. 若需要运行仿真，则参考`rl_sar/src/rl_sar/launch`路径下的launch文件自行修改 -->
-5. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改
+4. 若需要运行实物，则参考`rl_sar/src/rl_sar/src/rl_real_a1.cpp`文件自行修改
 
 ## 贡献
 

src/rl_sar/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(rl_sar)
 
 add_definitions(-DCMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
@@ -10,33 +10,27 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g")
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}")
 find_package(Torch REQUIRED)
+set(CMAKE_INSTALL_RPATH "${Torch_DIR}/lib")
+set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
+set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
 find_package(gazebo REQUIRED)
 
-find_package(catkin REQUIRED COMPONENTS
+find_package(ament_cmake REQUIRED)
-  controller_manager
+find_package(joint_state_controller REQUIRED)
-  genmsg
+find_package(robot_state_publisher REQUIRED)
-  joint_state_controller
+find_package(rclcpp REQUIRED)
-  robot_state_publisher
+find_package(gazebo_ros REQUIRED)
-  roscpp
+find_package(std_msgs REQUIRED)
-  gazebo_ros
+find_package(robot_msgs REQUIRED)
-  std_msgs
+find_package(robot_joint_controller REQUIRED)
-  tf
+find_package(rclpy REQUIRED)
-  geometry_msgs
+find_package(gazebo_msgs REQUIRED)
-  robot_msgs
+find_package(std_srvs REQUIRED)
-  robot_joint_controller
-  rospy
-)
 
 find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
 
-catkin_package(
-  CATKIN_DEPENDS
-  robot_joint_controller
-  rospy
-)
-
 link_directories(/usr/local/lib)
 include_directories(${YAML_CPP_INCLUDE_DIR})
 
@@ -44,6 +38,11 @@ include_directories(${YAML_CPP_INCLUDE_DIR})
 include_directories(library/unitree_legged_sdk_3.2/include)
 link_directories(library/unitree_legged_sdk_3.2/lib)
 set(UNITREE_A1_LIBS -pthread unitree_legged_sdk_amd64 lcm)
+file(GLOB GLOB_UNITREE_LEGGED_SDK "${PROJECT_SOURCE_DIR}/library/unitree_legged_sdk_3.2/lib/*.so")
+install(FILES
+  ${GLOB_UNITREE_LEGGED_SDK}
+  DESTINATION lib/
+)
 
 # Unitree Go2
 include_directories(library/unitree_sdk2/include)
@@ -52,10 +51,16 @@ include_directories(library/unitree_sdk2/thirdparty/include)
 include_directories(library/unitree_sdk2/thirdparty/include/ddscxx)
 link_directories(library/unitree_sdk2/thirdparty/lib/x86_64)
 set(UNITREE_GO2_LIBS -pthread unitree_sdk2 ddsc ddscxx)
+file(GLOB GLOB_UNITREE_SDK2 "${PROJECT_SOURCE_DIR}/library/unitree_sdk2/lib/x86_64/*.so")
+file(GLOB GLOB_UNITREE_SDK2_THIRDPARTY "${PROJECT_SOURCE_DIR}/library/unitree_sdk2/thirdparty/lib/x86_64/*.so")
+install(FILES
+  ${GLOB_UNITREE_SDK2_THIRDPARTY}
+  ${GLOB_UNITREE_SDK2_THIRDPARTY}
+  DESTINATION lib/
+)
 
 include_directories(
   include
-  ${catkin_INCLUDE_DIRS}
   library/matplotlibcpp
   library/observation_buffer
   library/rl_sdk
@@ -78,24 +83,53 @@ set_property(TARGET observation_buffer PROPERTY CXX_STANDARD 14)
 
 add_executable(rl_sim src/rl_sim.cpp )
 target_link_libraries(rl_sim
-  ${catkin_LIBRARIES} -pthread
+  -pthread
   rl_sdk observation_buffer yaml-cpp
 )
 
 add_executable(rl_real_a1 src/rl_real_a1.cpp )
 target_link_libraries(rl_real_a1
-  ${catkin_LIBRARIES} ${UNITREE_A1_LIBS}
+  ${UNITREE_A1_LIBS}
   rl_sdk observation_buffer yaml-cpp
 )
 
 add_executable(rl_real_go2 src/rl_real_go2.cpp )
 target_link_libraries(rl_real_go2
-  ${catkin_LIBRARIES} ${UNITREE_GO2_LIBS}
+  ${UNITREE_GO2_LIBS}
   rl_sdk observation_buffer yaml-cpp
 )
 
-catkin_install_python(PROGRAMS
+ament_target_dependencies(rl_sim
+  joint_state_controller
+  robot_state_publisher
+  rclcpp
+  gazebo_ros
+  std_msgs
+  robot_msgs
+  robot_joint_controller
+  rclpy
+  gazebo_msgs
+  std_srvs
+)
+
+install(TARGETS
+  rl_sim
+  rl_sdk
+  observation_buffer
+  rl_real_a1
+  rl_real_go2
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(PROGRAMS
   scripts/rl_sim.py
   scripts/actuator_net.py
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  DESTINATION lib/${PROJECT_NAME}
 )
+
+install(
+  DIRECTORY launch worlds models
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+ament_package()

src/rl_sar/include/rl_sim.hpp

@@ -4,19 +4,22 @@
 #include "rl_sdk.hpp"
 #include "observation_buffer.hpp"
 #include "loop.hpp"
-#include <ros/ros.h>
+#include "robot_msgs/msg/robot_command.hpp"
-#include <gazebo_msgs/ModelStates.h>
+#include "robot_msgs/msg/robot_state.hpp"
-#include <sensor_msgs/JointState.h>
+#include <rclcpp/rclcpp.hpp>
-#include "std_srvs/Empty.h"
+#include <sensor_msgs/msg/joint_state.hpp>
-#include <geometry_msgs/Twist.h>
+#include <sensor_msgs/msg/imu.hpp>
-#include "robot_msgs/MotorCommand.h"
+#include <geometry_msgs/msg/twist.hpp>
+#include <std_srvs/srv/empty.hpp>
+#include <gazebo_msgs/srv/set_model_state.hpp>
+#include <rcl_interfaces/srv/get_parameters.hpp>
+
 #include <csignal>
-#include <gazebo_msgs/SetModelState.h>
 
 #include "matplotlibcpp.h"
 namespace plt = matplotlibcpp;
 
-class RL_Sim : public RL
+class RL_Sim : public RL, public rclcpp::Node
 {
 public:
     RL_Sim();
@@ -48,29 +51,27 @@ private:
 
     // ros interface
     std::string ros_namespace;
-    geometry_msgs::Twist vel;
+    sensor_msgs::msg::Imu gazebo_imu;
-    geometry_msgs::Pose pose;
+    geometry_msgs::msg::Twist cmd_vel;
-    geometry_msgs::Twist cmd_vel;
+    robot_msgs::msg::RobotCommand robot_command_publisher_msg;
-    ros::Subscriber model_state_subscriber;
+    robot_msgs::msg::RobotState robot_state_subscriber_msg;
-    ros::Subscriber joint_state_subscriber;
+    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr gazebo_imu_subscriber;
-    ros::Subscriber cmd_vel_subscriber;
+    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_subscriber;
-    ros::ServiceClient gazebo_set_model_state_client;
+    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscriber;
-    ros::ServiceClient gazebo_pause_physics_client;
+    rclcpp::Client<gazebo_msgs::srv::SetModelState>::SharedPtr gazebo_set_model_state_client;
-    ros::ServiceClient gazebo_unpause_physics_client;
+    rclcpp::Client<std_srvs::srv::Empty>::SharedPtr gazebo_pause_physics_client;
-    std::map<std::string, ros::Publisher> joint_publishers;
+    rclcpp::Client<std_srvs::srv::Empty>::SharedPtr gazebo_unpause_physics_client;
-    std::vector<robot_msgs::MotorCommand> joint_publishers_commands;
+    rclcpp::Publisher<robot_msgs::msg::RobotCommand>::SharedPtr robot_command_publisher;
-    void ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg);
+    rclcpp::Subscription<robot_msgs::msg::RobotState>::SharedPtr robot_state_subscriber;
-    void JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
+    rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr param_client;
-    void CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg);
+    void GazeboImuCallback(const sensor_msgs::msg::Imu::SharedPtr msg);
+    void CmdvelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+    void RobotStateCallback(const robot_msgs::msg::RobotState::SharedPtr msg);
 
     // others
     std::string gazebo_model_name;
     int motiontime = 0;
     std::map<std::string, size_t> sorted_to_original_index;
-    std::vector<double> mapped_joint_positions;
-    std::vector<double> mapped_joint_velocities;
-    std::vector<double> mapped_joint_efforts;
-    void MapData(const std::vector<double> &source_data, std::vector<double> &target_data);
 };
 
 #endif // RL_SIM_HPP

src/rl_sar/launch/gazebo.launch.py

@@ -0,0 +1,94 @@
+import os
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, TextSubstitution, Command
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from ament_index_python.packages import get_package_share_directory
+
+def generate_launch_description():
+    rname = LaunchConfiguration("rname")
+    framework = LaunchConfiguration("framework")
+
+    wname = "stair"
+    robot_name = ParameterValue(Command(["echo -n ", rname, "_", framework]), value_type=str)
+    ros_namespace = ParameterValue(Command(["echo -n ", "/", rname, "_gazebo"]), value_type=str)
+    gazebo_model_name = ParameterValue(Command(["echo -n ", rname, "_gazebo"]), value_type=str)
+
+    robot_description = ParameterValue(
+        Command([
+            "xacro ",
+            Command(["echo -n ", Command(["ros2 pkg prefix ", rname, "_description"])]),
+            "/share/", rname, "_description/xacro/robot.xacro"
+        ]),
+        value_type=str
+    )
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[{"robot_description": robot_description}],
+    )
+
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory("gazebo_ros"), "launch", "gazebo.launch.py")
+        ),
+        launch_arguments={
+            "verbose": "false",
+            "world": os.path.join(get_package_share_directory("rl_sar"), "worlds", wname + ".world"),
+        }.items(),
+    )
+
+    spawn_entity = Node(
+        package="gazebo_ros",
+        executable="spawn_entity.py",
+        arguments=["-topic", "/robot_description", "-entity", "robot_model"],
+        output="screen",
+    )
+
+    joint_state_broadcaster_node = Node(
+        package="controller_manager",
+        executable="spawner.py",
+        arguments=["joint_state_broadcaster"],
+        output="screen",
+    )
+
+    robot_joint_controller_node = Node(
+        package="controller_manager",
+        executable="spawner.py",
+        arguments=["robot_joint_controller"],
+        output="screen",
+    )
+
+    param_node = Node(
+        package="demo_nodes_cpp",
+        executable="parameter_blackboard",
+        name="param_node",
+        parameters=[{
+            "robot_name": robot_name,
+            "gazebo_model_name": gazebo_model_name,
+        }],
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            "rname",
+            description="Robot name (e.g., a1, go2)",
+            default_value=TextSubstitution(text=""),
+        ),
+        DeclareLaunchArgument(
+            "framework",
+            description="Framework (isaacgym or isaacsim)",
+            default_value=TextSubstitution(text=""),
+        ),
+        robot_state_publisher_node,
+        gazebo,
+        spawn_entity,
+        joint_state_broadcaster_node,
+        robot_joint_controller_node,
+        param_node,
+    ])

src/rl_sar/library/rl_sdk/rl_sdk.cpp

@@ -486,6 +486,7 @@ void RL::CSVInit(std::string robot_name)
     // csv_filename += "_" + timestamp;
 
     csv_filename += ".csv";
+    std::cout << LOGGER::INFO << "Recording motor data in " << csv_filename << std::endl;
     std::ofstream file(csv_filename.c_str());
 
     for(int i = 0; i < 12; ++i) { file << "tau_cal_" << i << ","; }

src/rl_sar/library/rl_sdk/rl_sdk.hpp

@@ -11,10 +11,10 @@
 
 namespace LOGGER
 {
-    const char *const INFO    = "\033[0;37m[INFO]\033[0m ";
+    const std::string INFO    = "\033[0;37m[INFO]\033[0m ";
-    const char *const WARNING = "\033[0;33m[WARNING]\033[0m ";
+    const std::string WARNING = "\033[0;33m[WARNING]\033[0m ";
-    const char *const ERROR   = "\033[0;31m[ERROR]\033[0m ";
+    const std::string ERROR   = "\033[0;31m[ERROR]\033[0m ";
-    const char *const DEBUG   = "\033[0;32m[DEBUG]\033[0m ";
+    const std::string DEBUG   = "\033[0;32m[DEBUG]\033[0m ";
 }
 
 template <typename T>
@@ -45,7 +45,7 @@ struct RobotState
         std::vector<T> q = std::vector<T>(32, 0.0);
         std::vector<T> dq = std::vector<T>(32, 0.0);
         std::vector<T> ddq = std::vector<T>(32, 0.0);
-        std::vector<T> tauEst = std::vector<T>(32, 0.0);
+        std::vector<T> tau_est = std::vector<T>(32, 0.0);
         std::vector<T> cur = std::vector<T>(32, 0.0);
     } motor_state;
 };
@@ -156,7 +156,7 @@ public:
     // others
     std::string robot_name;
     STATE running_state = STATE_RL_RUNNING; // default running_state set to STATE_RL_RUNNING
-    bool simulation_running = false;
+    bool simulation_running = true;
 
     // protect func
     void TorqueProtect(torch::Tensor origin_output_torques);

src/rl_sar/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
     <name>rl_sar</name>
     <version>2.0.0</version>
     <description>The rl_sar package</description>
@@ -8,29 +8,19 @@
 
     <license>TODO</license>
 
-    <buildtool_depend>catkin</buildtool_depend>
+    <buildtool_depend>ament_cmake</buildtool_depend>
-    <buildtool_depend>genmsg</buildtool_depend>
+    <depend>rclcpp</depend>
-    <build_depend>controller_manager</build_depend>
+    <depend>std_msgs</depend>
-    <build_depend>joint_state_controller</build_depend>
+    <depend>gazebo_ros</depend>
-    <build_depend>robot_state_publisher</build_depend>
+    <depend>yaml-cpp</depend>
-    <build_depend>roscpp</build_depend>
-    <build_depend>std_msgs</build_depend>
-    <build_export_depend>controller_manager</build_export_depend>
-    <build_export_depend>joint_state_controller</build_export_depend>
-    <build_export_depend>robot_state_publisher</build_export_depend>
-    <build_export_depend>roscpp</build_export_depend>
-    <build_export_depend>std_msgs</build_export_depend>
-    <exec_depend>controller_manager</exec_depend>
-    <exec_depend>joint_state_controller</exec_depend>
-    <exec_depend>robot_state_publisher</exec_depend>
-    <exec_depend>roscpp</exec_depend>
-    <exec_depend>std_msgs</exec_depend>
-    <build_depend>rospy</build_depend>
-    <exec_depend>rospy</exec_depend>
     <depend>robot_msgs</depend>
     <depend>robot_joint_controller</depend>
+    <depend>controller_manager</depend>
+    <depend>joint_state_controller</depend>
+    <depend>robot_state_publisher</depend>
+    <depend>rospy</depend>
 
     <export>
-
+        <build_type>ament_cmake</build_type>
     </export>
 </package>

src/rl_sar/scripts/actuator_net.py

@@ -1,3 +1,4 @@
+#!/usr/bin/env python3
 import os
 import argparse
 from matplotlib import pyplot as plt
@@ -8,8 +9,9 @@ import torch.nn.functional as F
 from torch.utils.data import Dataset, DataLoader
 from torch.optim import Adam
 import pandas as pd
+from ament_index_python.packages import get_package_share_directory
 
-BASE_PATH = os.path.join(os.path.dirname(__file__), "../")
+BASE_PATH = os.path.join(get_package_share_directory('rl_sar'))
 
 class Config:
     def __init__(self):

src/rl_sar/scripts/rl_sdk.py

@@ -41,7 +41,7 @@ class RobotState:
             self.q = [0.0] * 32
             self.dq = [0.0] * 32
             self.ddq = [0.0] * 32
-            self.tauEst = [0.0] * 32
+            self.tau_est = [0.0] * 32
             self.cur = [0.0] * 32
 
 class STATE(Enum):
@@ -123,7 +123,7 @@ class RL:
         # others
         self.robot_name = ""
         self.running_state = STATE.STATE_RL_RUNNING  # default running_state set to STATE_RL_RUNNING
-        self.simulation_running = False
+        self.simulation_running = True
 
         ### protected in cpp ###
         # rl module
@@ -228,10 +228,10 @@ class RL:
                 self.getup_percent = min(self.getup_percent, 1.0)
                 for i in range(self.params.num_of_dofs):
                     command.motor_command.q[i] = (1 - self.getup_percent) * self.now_state.motor_state.q[i] + self.getup_percent * self.params.default_dof_pos[0][i].item()
-                    command.motor_command.dq[i] = 0
+                    command.motor_command.dq[i] = 0.0
                     command.motor_command.kp[i] = self.params.fixed_kp[0][i].item()
                     command.motor_command.kd[i] = self.params.fixed_kd[0][i].item()
-                    command.motor_command.tau[i] = 0
+                    command.motor_command.tau[i] = 0.0
                 print("\r" + LOGGER.INFO + f"Getting up {self.getup_percent * 100.0:.1f}", end='', flush=True)
 
             if self.control.control_state == STATE.STATE_RL_INIT:
@@ -261,10 +261,10 @@ class RL:
             print("\r" + LOGGER.INFO + f"RL Controller x: {self.control.x:.1f} y: {self.control.y:.1f} yaw: {self.control.yaw:.1f}", end='', flush=True)
             for i in range(self.params.num_of_dofs):
                 command.motor_command.q[i] = self.output_dof_pos[0][i].item()
-                command.motor_command.dq[i] = 0
+                command.motor_command.dq[i] = 0.0
                 command.motor_command.kp[i] = self.params.rl_kp[0][i].item()
                 command.motor_command.kd[i] = self.params.rl_kd[0][i].item()
-                command.motor_command.tau[i] = 0
+                command.motor_command.tau[i] = 0.0
 
             if self.control.control_state == STATE.STATE_POS_GETDOWN:
                 self.control.control_state = STATE.STATE_WAITING
@@ -289,10 +289,10 @@ class RL:
                 self.getdown_percent = min(1.0, self.getdown_percent)
                 for i in range(self.params.num_of_dofs):
                     command.motor_command.q[i] = (1 - self.getdown_percent) * self.now_state.motor_state.q[i] + self.getdown_percent * self.start_state.motor_state.q[i]
-                    command.motor_command.dq[i] = 0
+                    command.motor_command.dq[i] = 0.0
                     command.motor_command.kp[i] = self.params.fixed_kp[0][i].item()
                     command.motor_command.kd[i] = self.params.fixed_kd[0][i].item()
-                    command.motor_command.tau[i] = 0
+                    command.motor_command.tau[i] = 0.0
                 print("\r" + LOGGER.INFO + f"Getting down {self.getdown_percent * 100.0:.1f}", end='', flush=True)
 
             if self.getdown_percent == 1:
@@ -424,6 +424,7 @@ class RL:
         # self.csv_filename += f"_{timestamp}"
 
         self.csv_filename += ".csv"
+        print(LOGGER.INFO + f"Recording motor data in '{os.path.abspath(self.csv_filename)}'")
 
         with open(self.csv_filename, 'w', newline='') as file:
             writer = csv.writer(file)

src/rl_sar/scripts/rl_sim.py

@@ -1,90 +1,94 @@
-import sys
+#!/usr/bin/env python3
 import os
 import torch
 import threading
 import time
-import rospy
+import rclpy
-import numpy as np
+from rclpy.node import Node
-from gazebo_msgs.msg import ModelStates
+from rclpy.qos import qos_profile_system_default
-from sensor_msgs.msg import JointState
+from sensor_msgs.msg import Imu
-from geometry_msgs.msg import Twist, Pose
+from geometry_msgs.msg import Twist
-from robot_msgs.msg import MotorCommand
+from robot_msgs.msg import RobotCommand as RobotCommandMsg
-from gazebo_msgs.srv import SetModelState, SetModelStateRequest
+from robot_msgs.msg import RobotState as RobotStateMsg
+from robot_msgs.msg import MotorCommand as MotorCommandMsg
+from robot_msgs.msg import MotorState as MotorStateMsg
+from gazebo_msgs.srv import SetModelState
 from std_srvs.srv import Empty
-
+from rcl_interfaces.srv import GetParameters
-path = os.path.abspath(".")
+from ament_index_python.packages import get_package_share_directory
-sys.path.insert(0, path + "/src/rl_sar/scripts")
 from rl_sdk import *
 from observation_buffer import *
 
-CSV_LOGGER = False
+CSV_LOGGER = True
 
-class RL_Sim(RL):
+class RL_Sim(RL, Node):
     def __init__(self):
-        super().__init__()
+        Node.__init__(self, "rl_sim_node")
+        RL.__init__(self)
 
         # member variables for RL_Sim
-        self.vel = Twist()
-        self.pose = Pose()
         self.cmd_vel = Twist()
+        self.gazebo_imu = Imu()
+        self.robot_state_subscriber_msg = RobotStateMsg()
+        self.robot_command_publisher_msg = RobotCommandMsg()
 
-        # start ros node
+        self.ros_namespace = self.get_namespace()
-        rospy.init_node("rl_sim")
+
+        # get params from param_node
+        self.param_client = self.create_client(GetParameters, '/param_node/get_parameters')
+        while not self.param_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().warn("Waiting for param_node service to be available...")
+        request = GetParameters.Request()
+        request.names = ['robot_name', 'gazebo_model_name']
+        future = self.param_client.call_async(request)
+        rclpy.spin_until_future_complete(self, future)
+        if len(future.result().values) < 2:
+            self.get_logger().warn("Failed to get all parameters from param_node")
+        else:
+            self.robot_name = future.result().values[0].string_value
+            self.gazebo_model_name = future.result().values[1].string_value
+            self.get_logger().info(f"Get param robot_name: {self.robot_name}")
+            self.get_logger().info(f"Get param gazebo_model_name: {self.gazebo_model_name}")
 
         # read params from yaml
-        self.robot_name = rospy.get_param("robot_name", "")
         self.ReadYaml(self.robot_name)
         for i in range(len(self.params.observations)):
             if self.params.observations[i] == "ang_vel":
                 self.params.observations[i] = "ang_vel_world"
 
-        # history
+        # init rl
+        torch.set_grad_enabled(False)
         if len(self.params.observations_history) != 0:
             self.history_obs_buf = ObservationBuffer(1, self.params.num_observations, len(self.params.observations_history))
-
+        self.robot_command_publisher_msg.motor_command = [MotorCommandMsg() for _ in range(self.params.num_of_dofs)]
-        # Due to the fact that the robot_state_publisher sorts the joint names alphabetically,
+        self.robot_state_subscriber_msg.motor_state = [MotorStateMsg() for _ in range(self.params.num_of_dofs)]
-        # the mapping table is established according to the order defined in the YAML file
-        sorted_joint_controller_names = sorted(self.params.joint_controller_names)
-        self.sorted_to_original_index = {}
-        for i in range(len(self.params.joint_controller_names)):
-            self.sorted_to_original_index[sorted_joint_controller_names[i]] = i
-        self.mapped_joint_positions = [0.0] * self.params.num_of_dofs
-        self.mapped_joint_velocities = [0.0] * self.params.num_of_dofs
-        self.mapped_joint_efforts = [0.0] * self.params.num_of_dofs
-
-        # init
-        torch.set_grad_enabled(False)
-        self.joint_publishers_commands = [MotorCommand() for _ in range(self.params.num_of_dofs)]
         self.InitObservations()
         self.InitOutputs()
         self.InitControl()
+        self.running_state = STATE.STATE_RL_RUNNING
 
         # model
-        model_path = os.path.join(os.path.dirname(__file__), f"../models/{self.robot_name}/{self.params.model_name}")
+        model_path = os.path.join(get_package_share_directory('rl_sar'), 'models', self.robot_name, self.params.model_name)
         self.model = torch.jit.load(model_path)
 
         # publisher
-        self.ros_namespace = rospy.get_param("ros_namespace", "")
+        self.robot_command_publisher = self.create_publisher(RobotCommandMsg, self.ros_namespace + "robot_joint_controller/command", qos_profile_system_default)
-        self.joint_publishers = {}
-        for i in range(self.params.num_of_dofs):
-            topic_name = f"{self.ros_namespace}{self.params.joint_controller_names[i]}/command"
-            self.joint_publishers[self.params.joint_controller_names[i]] = rospy.Publisher(topic_name, MotorCommand, queue_size=10)
 
         # subscriber
-        self.cmd_vel_subscriber = rospy.Subscriber("/cmd_vel", Twist, self.CmdvelCallback, queue_size=10)
+        self.cmd_vel_subscriber = self.create_subscription(Twist, "/cmd_vel", self.CmdvelCallback, qos_profile_system_default)
-        self.model_state_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.ModelStatesCallback, queue_size=10)
+        self.gazebo_imu_subscriber = self.create_subscription(Imu, "/imu", self.GazeboImuCallback, qos_profile_system_default)
-        joint_states_topic = f"{self.ros_namespace}joint_states"
+        self.robot_state_subscriber = self.create_subscription(RobotStateMsg, self.ros_namespace + "robot_joint_controller/state", self.RobotStateCallback, qos_profile_system_default)
-        self.joint_state_subscriber = rospy.Subscriber(joint_states_topic, JointState, self.JointStatesCallback, queue_size=10)
 
         # service
-        self.gazebo_model_name = rospy.get_param("gazebo_model_name", "")
+        self.gazebo_set_model_state_client = self.create_client(SetModelState, "/gazebo/set_model_state")
-        self.gazebo_set_model_state_client = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
+        self.gazebo_pause_physics_client = self.create_client(Empty, "/gazebo/pause_physics")
-        self.gazebo_pause_physics_client = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
+        self.gazebo_unpause_physics_client = self.create_client(Empty, "/gazebo/unpause_physics")
-        self.gazebo_unpause_physics_client = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)
 
         # loops
         self.thread_control = threading.Thread(target=self.ThreadControl)
         self.thread_rl = threading.Thread(target=self.ThreadRL)
+        self.thread_control.daemon = True
+        self.thread_rl.daemon = True
         self.thread_control.start()
         self.thread_rl.start()
 
@@ -96,87 +100,82 @@ class RL_Sim(RL):
         if CSV_LOGGER:
             self.CSVInit(self.robot_name)
 
-        print(LOGGER.INFO + "RL_Sim start")
+        self.get_logger().info("RL_Sim start")
 
     def __del__(self):
-        print(LOGGER.INFO + "RL_Sim exit")
+        self.get_logger().info("RL_Sim exit")
 
     def GetState(self, state):
         if self.params.framework == "isaacgym":
-            state.imu.quaternion[3] = self.pose.orientation.w
+            state.imu.quaternion[3] = self.gazebo_imu.orientation.w
-            state.imu.quaternion[0] = self.pose.orientation.x
+            state.imu.quaternion[0] = self.gazebo_imu.orientation.x
-            state.imu.quaternion[1] = self.pose.orientation.y
+            state.imu.quaternion[1] = self.gazebo_imu.orientation.y
-            state.imu.quaternion[2] = self.pose.orientation.z
+            state.imu.quaternion[2] = self.gazebo_imu.orientation.z
         elif self.params.framework == "isaacsim":
-            state.imu.quaternion[0] = self.pose.orientation.w
+            state.imu.quaternion[0] = self.gazebo_imu.orientation.w
-            state.imu.quaternion[1] = self.pose.orientation.x
+            state.imu.quaternion[1] = self.gazebo_imu.orientation.x
-            state.imu.quaternion[2] = self.pose.orientation.y
+            state.imu.quaternion[2] = self.gazebo_imu.orientation.y
-            state.imu.quaternion[3] = self.pose.orientation.z
+            state.imu.quaternion[3] = self.gazebo_imu.orientation.z
 
-        state.imu.gyroscope[0] = self.vel.angular.x
+        state.imu.gyroscope[0] = self.gazebo_imu.angular_velocity.x
-        state.imu.gyroscope[1] = self.vel.angular.y
+        state.imu.gyroscope[1] = self.gazebo_imu.angular_velocity.y
-        state.imu.gyroscope[2] = self.vel.angular.z
+        state.imu.gyroscope[2] = self.gazebo_imu.angular_velocity.z
 
-        # state.imu.accelerometer
+        state.imu.accelerometer[0] = self.gazebo_imu.linear_acceleration.x
+        state.imu.accelerometer[1] = self.gazebo_imu.linear_acceleration.y
+        state.imu.accelerometer[2] = self.gazebo_imu.linear_acceleration.z
 
         for i in range(self.params.num_of_dofs):
-            state.motor_state.q[i] = self.mapped_joint_positions[i]
+            state.motor_state.q[i] = self.robot_state_subscriber_msg.motor_state[i].q
-            state.motor_state.dq[i] = self.mapped_joint_velocities[i]
+            state.motor_state.dq[i] = self.robot_state_subscriber_msg.motor_state[i].dq
-            state.motor_state.tauEst[i] = self.mapped_joint_efforts[i]
+            state.motor_state.tau_est[i] = self.robot_state_subscriber_msg.motor_state[i].tau_est
 
     def SetCommand(self, command):
         for i in range(self.params.num_of_dofs):
-            self.joint_publishers_commands[i].q = command.motor_command.q[i]
+            self.robot_command_publisher_msg.motor_command[i].q = float(command.motor_command.q[i])
-            self.joint_publishers_commands[i].dq = command.motor_command.dq[i]
+            self.robot_command_publisher_msg.motor_command[i].dq = float(command.motor_command.dq[i])
-            self.joint_publishers_commands[i].kp = command.motor_command.kp[i]
+            self.robot_command_publisher_msg.motor_command[i].kp = float(command.motor_command.kp[i])
-            self.joint_publishers_commands[i].kd = command.motor_command.kd[i]
+            self.robot_command_publisher_msg.motor_command[i].kd = float(command.motor_command.kd[i])
-            self.joint_publishers_commands[i].tau = command.motor_command.tau[i]
+            self.robot_command_publisher_msg.motor_command[i].tau = float(command.motor_command.tau[i])
 
-        for i in range(self.params.num_of_dofs):
+        self.robot_command_publisher.publish(self.robot_command_publisher_msg)
-            self.joint_publishers[self.params.joint_controller_names[i]].publish(self.joint_publishers_commands[i])
 
     def RobotControl(self):
-        if self.control.control_state == STATE.STATE_RESET_SIMULATION:
+        # NOT AVAILABLE NOW
-            set_model_state = SetModelStateRequest().model_state
+        # if self.control.control_state == STATE.STATE_RESET_SIMULATION:
-            set_model_state.model_name = self.gazebo_model_name
+        #     set_model_state = SetModelState.Request()
-            set_model_state.pose.position.z = 1.0
+        #     set_model_state.model_state.model_name = self.gazebo_model_name
-            set_model_state.reference_frame = "world"
+        #     set_model_state.model_state.pose.position.z = 1.0
-            self.gazebo_set_model_state_client(set_model_state)
+        #     set_model_state.model_state.reference_frame = "world"
-            self.control.control_state = STATE.STATE_WAITING
+        #     self.gazebo_set_model_state_client.call_async(set_model_state)
-        if self.control.control_state == STATE.STATE_TOGGLE_SIMULATION:
+        #     self.control.control_state = STATE.STATE_WAITING
-            if self.simulation_running:
+        # if self.control.control_state == STATE.STATE_TOGGLE_SIMULATION:
-                self.gazebo_pause_physics_client()
+        #     if self.simulation_running:
-                print("\r\n" + LOGGER.INFO + "Simulation Stop")
+        #         self.gazebo_pause_physics_client.call_async(Empty.Request())
-            else:
+        #         self.get_logger().info("Simulation Stop")
-                self.gazebo_unpause_physics_client()
+        #     else:
-                print("\r\n" + LOGGER.INFO + "Simulation Start")
+        #         self.gazebo_unpause_physics_client.call_async(Empty.Request())
-            self.simulation_running = not self.simulation_running
+        #         self.get_logger().info("Simulation Start")
-            self.control.control_state = STATE.STATE_WAITING
+        #     self.simulation_running = not self.simulation_running
+        #     self.control.control_state = STATE.STATE_WAITING
 
         if self.simulation_running:
             self.GetState(self.robot_state)
             self.StateController(self.robot_state, self.robot_command)
             self.SetCommand(self.robot_command)
 
-    def ModelStatesCallback(self, msg):
+    def GazeboImuCallback(self, msg):
-        self.vel = msg.twist[2]
+        self.gazebo_imu = msg
-        self.pose = msg.pose[2]
 
     def CmdvelCallback(self, msg):
         self.cmd_vel = msg
 
-    def MapData(self, source_data, target_data):
+    def RobotStateCallback(self, msg):
-        for i in range(len(source_data)):
+        self.robot_state_subscriber_msg = msg
-            target_data[i] = source_data[self.sorted_to_original_index[self.params.joint_controller_names[i]]]
-
-    def JointStatesCallback(self, msg):
-        self.MapData(msg.position, self.mapped_joint_positions)
-        self.MapData(msg.velocity, self.mapped_joint_velocities)
-        self.MapData(msg.effort, self.mapped_joint_efforts)
 
     def RunModel(self):
         if self.running_state == STATE.STATE_RL_RUNNING and self.simulation_running:
-            self.obs.lin_vel = torch.tensor([[self.vel.linear.x, self.vel.linear.y, self.vel.linear.z]])
+            # self.obs.lin_vel NOT USE
             self.obs.ang_vel = torch.tensor(self.robot_state.imu.gyroscope).unsqueeze(0)
             # self.obs.commands = torch.tensor([[self.cmd_vel.linear.x, self.cmd_vel.linear.y, self.cmd_vel.angular.z]])
             self.obs.commands = torch.tensor([[self.control.x, self.control.y, self.control.yaw]])
@@ -199,7 +198,9 @@ class RL_Sim(RL):
             self.output_dof_pos = self.ComputePosition(self.obs.actions)
 
             if CSV_LOGGER:
-                tau_est = torch.tensor(self.mapped_joint_efforts).unsqueeze(0)
+                tau_est = torch.empty(1, self.params.num_of_dofs, dtype=torch.float32)
+                for i in range(self.params.num_of_dofs):
+                    tau_est[0, i] = self.robot_state_subscriber_msg.motor_state[i].tau_est
                 self.CSVLogger(self.output_torques, tau_est, self.obs.dof_pos, self.output_dof_pos, self.obs.dof_vel)
 
     def Forward(self):
@@ -219,21 +220,27 @@ class RL_Sim(RL):
     def ThreadControl(self):
         thread_period = self.params.dt
         thread_name = "thread_control"
-        print(f"[Thread Start] named: {thread_name}, period: {thread_period * 1000:.0f}(ms), cpu unspecified")
+        self.get_logger().info(f"[Thread Start] named: {thread_name}, period: {thread_period * 1000:.0f}(ms), cpu unspecified")
-        while not rospy.is_shutdown():
+        while rclpy.ok():
             self.RobotControl()
             time.sleep(thread_period)
-        print("[Thread End] named: " + thread_name)
+        self.get_logger().info("[Thread End] named: " + thread_name)
 
     def ThreadRL(self):
         thread_period = self.params.dt * self.params.decimation
         thread_name = "thread_rl"
-        print(f"[Thread Start] named: {thread_name}, period: {thread_period * 1000:.0f}(ms), cpu unspecified")
+        self.get_logger().info(f"[Thread Start] named: {thread_name}, period: {thread_period * 1000:.0f}(ms), cpu unspecified")
-        while not rospy.is_shutdown():
+        while rclpy.ok():
             self.RunModel()
             time.sleep(thread_period)
-        print("[Thread End] named: " + thread_name)
+        self.get_logger().info("[Thread End] named: " + thread_name)
 
 if __name__ == "__main__":
+    rclpy.init()
     rl_sim = RL_Sim()
-    rospy.spin()
+    try:
+        rclpy.spin(rl_sim)
+    except KeyboardInterrupt:
+        rl_sim.get_logger().info("Shutdown signal received, shutting down...")
+        rclpy.shutdown()
+

src/rl_sar/src/rl_real_a1.cpp

@@ -116,7 +116,7 @@ void RL_Real::GetState(RobotState<double> *state)
     {
         state->motor_state.q[i] = this->unitree_low_state.motorState[state_mapping[i]].q;
         state->motor_state.dq[i] = this->unitree_low_state.motorState[state_mapping[i]].dq;
-        state->motor_state.tauEst[i] = this->unitree_low_state.motorState[state_mapping[i]].tauEst;
+        state->motor_state.tau_est[i] = this->unitree_low_state.motorState[state_mapping[i]].tauEst;
     }
 }
 
@@ -178,7 +178,7 @@ void RL_Real::RunModel()
         this->output_dof_pos = this->ComputePosition(this->obs.actions);
 
 #ifdef CSV_LOGGER
-        torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tauEst).unsqueeze(0);
+        torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tau_est).unsqueeze(0);
         this->CSVLogger(this->output_torques, tau_est, this->obs.dof_pos, this->output_dof_pos, this->obs.dof_vel);
 #endif
     }

src/rl_sar/src/rl_real_go2.cpp

@@ -122,7 +122,7 @@ void RL_Real::GetState(RobotState<double> *state)
     {
         state->motor_state.q[i] = this->unitree_low_state.motor_state()[state_mapping[i]].q();
         state->motor_state.dq[i] = this->unitree_low_state.motor_state()[state_mapping[i]].dq();
-        state->motor_state.tauEst[i] = this->unitree_low_state.motor_state()[state_mapping[i]].tau_est();
+        state->motor_state.tau_est[i] = this->unitree_low_state.motor_state()[state_mapping[i]].tau_est();
     }
 }
 
@@ -183,7 +183,7 @@ void RL_Real::RunModel()
         this->output_dof_pos = this->ComputePosition(this->obs.actions);
 
 #ifdef CSV_LOGGER
-        torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tauEst).unsqueeze(0);
+        torch::Tensor tau_est = torch::tensor(this->robot_state.motor_state.tau_est).unsqueeze(0);
         this->CSVLogger(this->output_torques, tau_est, this->obs.dof_pos, this->output_dof_pos, this->obs.dof_vel);
 #endif
     }

src/rl_sar/src/rl_sim.cpp

@@ -1,14 +1,43 @@
 #include "rl_sim.hpp"
 
 // #define PLOT
-// #define CSV_LOGGER
+#define CSV_LOGGER
 
 RL_Sim::RL_Sim()
+    : rclcpp::Node("rl_sim_node")
 {
-    ros::NodeHandle nh;
+    this->ros_namespace = this->get_namespace();
+
+    // get params from param_node
+    param_client = this->create_client<rcl_interfaces::srv::GetParameters>("/param_node/get_parameters");
+    while (!param_client->wait_for_service(std::chrono::seconds(1)))
+    {
+        std::cout << LOGGER::WARNING << "Waiting for param_node service to be available..." << std::endl;
+    }
+    auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
+    request->names.push_back("robot_name");
+    request->names.push_back("gazebo_model_name");
+    auto future = param_client->async_send_request(request);
+    rclcpp::spin_until_future_complete(this->get_node_base_interface(), future);
+    if (future.get()->values.size() < 2)
+    {
+        std::cout << LOGGER::WARNING << "Failed to get all parameters from param_node" << std::endl;
+    }
+    else
+    {
+        this->robot_name = future.get()->values[0].string_value;
+        this->gazebo_model_name = future.get()->values[1].string_value;
+        std::cout << LOGGER::INFO << "Get param robot_name: " << this->robot_name << std::endl;
+        std::cout << LOGGER::INFO << "Get param gazebo_model_name: " << this->gazebo_model_name << std::endl;
+    }
+
+    // // get params from this node
+    // this->declare_parameter<std::string>("robot_name", "");
+    // this->get_parameter("robot_name", this->robot_name);
+    // this->declare_parameter<std::string>("gazebo_model_name", "");
+    // this->get_parameter("gazebo_model_name", this->gazebo_model_name);
 
     // read params from yaml
-    nh.param<std::string>("robot_name", this->robot_name, "");
     this->ReadYaml(this->robot_name);
     for (std::string &observation : this->params.observations)
     {
@@ -18,17 +47,6 @@ RL_Sim::RL_Sim()
             observation = "ang_vel_world";
         }
     }
-    // Due to the fact that the robot_state_publisher sorts the joint names alphabetically,
-    // the mapping table is established according to the order defined in the YAML file
-    std::vector<std::string> sorted_joint_controller_names = this->params.joint_controller_names;
-    std::sort(sorted_joint_controller_names.begin(), sorted_joint_controller_names.end());
-    for (size_t i = 0; i < this->params.joint_controller_names.size(); ++i)
-    {
-        this->sorted_to_original_index[sorted_joint_controller_names[i]] = i;
-    }
-    this->mapped_joint_positions = std::vector<double>(this->params.num_of_dofs, 0.0);
-    this->mapped_joint_velocities = std::vector<double>(this->params.num_of_dofs, 0.0);
-    this->mapped_joint_efforts = std::vector<double>(this->params.num_of_dofs, 0.0);
 
     // init rl
     torch::autograd::GradMode::set_enabled(false);
@@ -36,7 +54,8 @@ RL_Sim::RL_Sim()
     {
         this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size());
     }
-    this->joint_publishers_commands.resize(this->params.num_of_dofs);
+    this->robot_command_publisher_msg.motor_command.resize(this->params.num_of_dofs);
+    this->robot_state_subscriber_msg.motor_state.resize(this->params.num_of_dofs);
     this->InitObservations();
     this->InitOutputs();
     this->InitControl();
@@ -47,24 +66,26 @@ RL_Sim::RL_Sim()
     this->model = torch::jit::load(model_path);
 
     // publisher
-    nh.param<std::string>("ros_namespace", this->ros_namespace, "");
+    this->robot_command_publisher = this->create_publisher<robot_msgs::msg::RobotCommand>(
-    for (int i = 0; i < this->params.num_of_dofs; ++i)
+        this->ros_namespace + "robot_joint_controller/command", rclcpp::SystemDefaultsQoS());
-    {
-        // joint need to rename as xxx_joint
-        this->joint_publishers[this->params.joint_controller_names[i]] =
-            nh.advertise<robot_msgs::MotorCommand>(this->ros_namespace + this->params.joint_controller_names[i] + "/command", 10);
-    }
 
     // subscriber
-    this->cmd_vel_subscriber = nh.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, &RL_Sim::CmdvelCallback, this);
+    this->cmd_vel_subscriber = this->create_subscription<geometry_msgs::msg::Twist>(
-    this->model_state_subscriber = nh.subscribe<gazebo_msgs::ModelStates>("/gazebo/model_states", 10, &RL_Sim::ModelStatesCallback, this);
+        "/cmd_vel", rclcpp::SystemDefaultsQoS(),
-    this->joint_state_subscriber = nh.subscribe<sensor_msgs::JointState>(this->ros_namespace + "joint_states", 10, &RL_Sim::JointStatesCallback, this);
+        [this] (const geometry_msgs::msg::Twist::SharedPtr msg) {this->CmdvelCallback(msg);}
+    );
+    this->gazebo_imu_subscriber = this->create_subscription<sensor_msgs::msg::Imu>(
+        "/imu", rclcpp::SystemDefaultsQoS(), [this] (const sensor_msgs::msg::Imu::SharedPtr msg) {this->GazeboImuCallback(msg);}
+    );
+    this->robot_state_subscriber = this->create_subscription<robot_msgs::msg::RobotState>(
+        this->ros_namespace + "robot_joint_controller/state", rclcpp::SystemDefaultsQoS(),
+        [this] (const robot_msgs::msg::RobotState::SharedPtr msg) {this->RobotStateCallback(msg);}
+    );
 
     // service
-    nh.param<std::string>("gazebo_model_name", this->gazebo_model_name, "");
+    this->gazebo_set_model_state_client = this->create_client<gazebo_msgs::srv::SetModelState>("/gazebo/set_model_state");
-    this->gazebo_set_model_state_client = nh.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");
+    this->gazebo_pause_physics_client = this->create_client<std_srvs::srv::Empty>("/gazebo/pause_physics");
-    this->gazebo_pause_physics_client = nh.serviceClient<std_srvs::Empty>("/gazebo/pause_physics");
+    this->gazebo_unpause_physics_client = this->create_client<std_srvs::srv::Empty>("/gazebo/unpause_physics");
-    this->gazebo_unpause_physics_client = nh.serviceClient<std_srvs::Empty>("/gazebo/unpause_physics");
 
     // loop
     this->loop_control = std::make_shared<LoopFunc>("loop_control", this->params.dt, std::bind(&RL_Sim::RobotControl, this));
@@ -107,30 +128,32 @@ void RL_Sim::GetState(RobotState<double> *state)
 {
     if (this->params.framework == "isaacgym")
     {
-        state->imu.quaternion[3] = this->pose.orientation.w;
+        state->imu.quaternion[3] = this->gazebo_imu.orientation.w;
-        state->imu.quaternion[0] = this->pose.orientation.x;
+        state->imu.quaternion[0] = this->gazebo_imu.orientation.x;
-        state->imu.quaternion[1] = this->pose.orientation.y;
+        state->imu.quaternion[1] = this->gazebo_imu.orientation.y;
-        state->imu.quaternion[2] = this->pose.orientation.z;
+        state->imu.quaternion[2] = this->gazebo_imu.orientation.z;
     }
     else if (this->params.framework == "isaacsim")
     {
-        state->imu.quaternion[0] = this->pose.orientation.w;
+        state->imu.quaternion[0] = this->gazebo_imu.orientation.w;
-        state->imu.quaternion[1] = this->pose.orientation.x;
+        state->imu.quaternion[1] = this->gazebo_imu.orientation.x;
-        state->imu.quaternion[2] = this->pose.orientation.y;
+        state->imu.quaternion[2] = this->gazebo_imu.orientation.y;
-        state->imu.quaternion[3] = this->pose.orientation.z;
+        state->imu.quaternion[3] = this->gazebo_imu.orientation.z;
     }
 
-    state->imu.gyroscope[0] = this->vel.angular.x;
+    state->imu.gyroscope[0] = this->gazebo_imu.angular_velocity.x;
-    state->imu.gyroscope[1] = this->vel.angular.y;
+    state->imu.gyroscope[1] = this->gazebo_imu.angular_velocity.y;
-    state->imu.gyroscope[2] = this->vel.angular.z;
+    state->imu.gyroscope[2] = this->gazebo_imu.angular_velocity.z;
 
-    // state->imu.accelerometer
+    state->imu.accelerometer[0] = this->gazebo_imu.linear_acceleration.x;
+    state->imu.accelerometer[1] = this->gazebo_imu.linear_acceleration.y;
+    state->imu.accelerometer[2] = this->gazebo_imu.linear_acceleration.z;
 
     for (int i = 0; i < this->params.num_of_dofs; ++i)
     {
-        state->motor_state.q[i] = this->mapped_joint_positions[i];
+        state->motor_state.q[i] = this->robot_state_subscriber_msg.motor_state[i].q;
-        state->motor_state.dq[i] = this->mapped_joint_velocities[i];
+        state->motor_state.dq[i] = this->robot_state_subscriber_msg.motor_state[i].dq;
-        state->motor_state.tauEst[i] = this->mapped_joint_efforts[i];
+        state->motor_state.tau_est[i] = this->robot_state_subscriber_msg.motor_state[i].tau_est;
     }
 }
 
@@ -138,49 +161,79 @@ void RL_Sim::SetCommand(const RobotCommand<double> *command)
 {
     for (int i = 0; i < this->params.num_of_dofs; ++i)
     {
-        this->joint_publishers_commands[i].q = command->motor_command.q[i];
+        this->robot_command_publisher_msg.motor_command[i].q = command->motor_command.q[i];
-        this->joint_publishers_commands[i].dq = command->motor_command.dq[i];
+        this->robot_command_publisher_msg.motor_command[i].dq = command->motor_command.dq[i];
-        this->joint_publishers_commands[i].kp = command->motor_command.kp[i];
+        this->robot_command_publisher_msg.motor_command[i].kp = command->motor_command.kp[i];
-        this->joint_publishers_commands[i].kd = command->motor_command.kd[i];
+        this->robot_command_publisher_msg.motor_command[i].kd = command->motor_command.kd[i];
-        this->joint_publishers_commands[i].tau = command->motor_command.tau[i];
+        this->robot_command_publisher_msg.motor_command[i].tau = command->motor_command.tau[i];
     }
 
-    for (int i = 0; i < this->params.num_of_dofs; ++i)
+    this->robot_command_publisher->publish(this->robot_command_publisher_msg);
-    {
-        this->joint_publishers[this->params.joint_controller_names[i]].publish(this->joint_publishers_commands[i]);
-    }
 }
 
 void RL_Sim::RobotControl()
 {
     std::lock_guard<std::mutex> lock(robot_state_mutex);
 
-    if (this->control.control_state == STATE_RESET_SIMULATION)
+    // if (this->control.control_state == STATE_RESET_SIMULATION)
-    {
+    // {
-        gazebo_msgs::SetModelState set_model_state;
+    //     auto set_model_state_request = std::make_shared<gazebo_msgs::srv::SetModelState::Request>();
-        set_model_state.request.model_state.model_name = this->gazebo_model_name;
+    //     set_model_state_request->model_state.model_name = this->gazebo_model_name;
-        set_model_state.request.model_state.pose.position.z = 1.0;
+    //     set_model_state_request->model_state.pose.position.z = 1.0;
-        set_model_state.request.model_state.reference_frame = "world";
+    //     set_model_state_request->model_state.reference_frame = "world";
-        this->gazebo_set_model_state_client.call(set_model_state);
+    //     this->gazebo_set_model_state_client->async_send_request(set_model_state_request,
-
+    //         [this](rclcpp::Client<gazebo_msgs::srv::SetModelState>::SharedFuture future)
-        this->control.control_state = STATE_WAITING;
+    //         {
-    }
+    //             if (future.get())
-    if (this->control.control_state == STATE_TOGGLE_SIMULATION)
+    //             {
-    {
+    //                 std::cout << LOGGER::INFO << "Reset Simulation" << std::endl;
-        std_srvs::Empty empty;
+    //                 this->control.control_state = STATE_WAITING;
-        if (simulation_running)
+    //             }
-        {
+    //             else
-            this->gazebo_pause_physics_client.call(empty);
+    //             {
-            std::cout << std::endl << LOGGER::INFO << "Simulation Stop" << std::endl;
+    //                 std::cout << LOGGER::WARNING << "Failed to reset simulation" << std::endl;
-        }
+    //             }
-        else
+    //         }
-        {
+    //     );
-            this->gazebo_unpause_physics_client.call(empty);
+    // }
-            std::cout << std::endl << LOGGER::INFO << "Simulation Start" << std::endl;
+    // if (this->control.control_state == STATE_TOGGLE_SIMULATION)
-        }
+    // {
-        simulation_running = !simulation_running;
+    //     auto empty_request = std::make_shared<std_srvs::srv::Empty::Request>();
-        this->control.control_state = STATE_WAITING;
+    //     if (simulation_running)
-    }
+    //     {
+    //         this->gazebo_pause_physics_client->async_send_request(empty_request,
+    //             [this](rclcpp::Client<std_srvs::srv::Empty>::SharedFuture future)
+    //             {
+    //                 if (future.get())
+    //                 {
+    //                     std::cout << LOGGER::INFO << "Simulation Stop" << std::endl;
+    //                 }
+    //                 else
+    //                 {
+    //                     std::cout << LOGGER::WARNING << "Failed to toggle simulation" << std::endl;
+    //                 }
+    //             }
+    //         );
+    //     }
+    //     else
+    //     {
+    //         this->gazebo_unpause_physics_client->async_send_request(empty_request,
+    //             [this](rclcpp::Client<std_srvs::srv::Empty>::SharedFuture future)
+    //             {
+    //                 if (future.get())
+    //                 {
+    //                     std::cout << LOGGER::INFO << "Simulation Start" << std::endl;
+    //                 }
+    //                 else
+    //                 {
+    //                     std::cout << LOGGER::WARNING << "Failed to toggle simulation" << std::endl;
+    //                 }
+    //             }
+    //         );
+    //     }
+    //     simulation_running = !simulation_running;
+    //     this->control.control_state = STATE_WAITING;
+    // }
     if (simulation_running)
     {
         this->motiontime++;
@@ -190,30 +243,19 @@ void RL_Sim::RobotControl()
     }
 }
 
-void RL_Sim::ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg)
+void RL_Sim::GazeboImuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
 {
-    this->vel = msg->twist[2];
+    this->gazebo_imu = *msg;
-    this->pose = msg->pose[2];
 }
 
-void RL_Sim::CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg)
+void RL_Sim::CmdvelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
 {
     this->cmd_vel = *msg;
 }
 
-void RL_Sim::MapData(const std::vector<double> &source_data, std::vector<double> &target_data)
+void RL_Sim::RobotStateCallback(const robot_msgs::msg::RobotState::SharedPtr msg)
 {
-    for (size_t i = 0; i < source_data.size(); ++i)
+    this->robot_state_subscriber_msg = *msg;
-    {
-        target_data[i] = source_data[this->sorted_to_original_index[this->params.joint_controller_names[i]]];
-    }
-}
-
-void RL_Sim::JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
-{
-    MapData(msg->position, this->mapped_joint_positions);
-    MapData(msg->velocity, this->mapped_joint_velocities);
-    MapData(msg->effort, this->mapped_joint_efforts);
 }
 
 void RL_Sim::RunModel()
@@ -222,7 +264,7 @@ void RL_Sim::RunModel()
 
     if (this->running_state == STATE_RL_RUNNING && simulation_running)
     {
-        this->obs.lin_vel = torch::tensor({{this->vel.linear.x, this->vel.linear.y, this->vel.linear.z}});
+        // this->obs.lin_vel NOT USE
         this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0);
         // this->obs.commands = torch::tensor({{this->cmd_vel.linear.x, this->cmd_vel.linear.y, this->cmd_vel.angular.z}});
         this->obs.commands = torch::tensor({{this->control.x, this->control.y, this->control.yaw}});
@@ -247,7 +289,11 @@ void RL_Sim::RunModel()
         this->output_dof_pos = this->ComputePosition(this->obs.actions);
 
 #ifdef CSV_LOGGER
-        torch::Tensor tau_est = torch::tensor(this->mapped_joint_efforts).unsqueeze(0);
+        torch::Tensor tau_est = torch::empty({1, this->params.num_of_dofs}, torch::kFloat);
+        for (int i = 0; i < this->params.num_of_dofs; ++i)
+        {
+            tau_est[0][i] = this->robot_state_subscriber_msg.motor_state[i].tau_est;
+        }
         this->CSVLogger(this->output_torques, tau_est, this->obs.dof_pos, this->output_dof_pos, this->obs.dof_vel);
 #endif
     }
@@ -291,8 +337,8 @@ void RL_Sim::Plot()
     {
         this->plot_real_joint_pos[i].erase(this->plot_real_joint_pos[i].begin());
         this->plot_target_joint_pos[i].erase(this->plot_target_joint_pos[i].begin());
-        this->plot_real_joint_pos[i].push_back(this->mapped_joint_positions[i]);
+        this->plot_real_joint_pos[i].push_back(this->robot_state_subscriber_msg.motor_state[i].q);
-        this->plot_target_joint_pos[i].push_back(this->joint_publishers_commands[i].q);
+        this->plot_target_joint_pos[i].push_back(this->robot_command_publisher_msg.motor_command[i].q);
         plt::subplot(4, 3, i + 1);
         plt::named_plot("_real_joint_pos", this->plot_t, this->plot_real_joint_pos[i], "r");
         plt::named_plot("_target_joint_pos", this->plot_t, this->plot_target_joint_pos[i], "b");
@@ -302,17 +348,10 @@ void RL_Sim::Plot()
     plt::pause(0.01);
 }
 
-void signalHandler(int signum)
-{
-    ros::shutdown();
-    exit(0);
-}
-
 int main(int argc, char **argv)
 {
-    signal(SIGINT, signalHandler);
+    rclcpp::init(argc, argv);
-    ros::init(argc, argv, "rl_sar");
+    rclcpp::spin(std::make_shared<RL_Sim>());
-    RL_Sim rl_sar;
+    rclcpp::shutdown();
-    ros::spin();
     return 0;
 }

src/robot_joint_controller/CMakeLists.txt

@@ -1,31 +1,66 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(robot_joint_controller)
 
-find_package(catkin REQUIRED COMPONENTS
+if(DEFINED ENV{ROS_DISTRO})
-    controller_interface
+    set(ROS_DISTRO_ENV $ENV{ROS_DISTRO})
-    hardware_interface
+    if(ROS_DISTRO_ENV STREQUAL "foxy")
-    pluginlib
+        add_definitions(-DROS_DISTRO_FOXY)
-    roscpp
+    elseif(ROS_DISTRO_ENV STREQUAL "humble")
-    realtime_tools
+        add_definitions(-DROS_DISTRO_HUMBLE)
-    robot_msgs
+    else()
-)
+        add_definitions(-DROS_DISTRO_UNKNOWN)
+    endif()
+endif()
 
-catkin_package(
+find_package(ament_cmake REQUIRED)
-    CATKIN_DEPENDS
+find_package(rclcpp REQUIRED)
-    robot_msgs
+find_package(urdf REQUIRED)
-    controller_interface
+find_package(control_toolbox REQUIRED)
-    hardware_interface
+find_package(realtime_tools REQUIRED)
-    pluginlib
+find_package(hardware_interface REQUIRED)
-    roscpp
+find_package(controller_interface REQUIRED)
-    INCLUDE_DIRS include
+find_package(pluginlib REQUIRED)
-    LIBRARIES ${PROJECT_NAME}
+find_package(robot_msgs REQUIRED)
-)
+find_package(geometry_msgs REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 
-include_directories(include ${catkin_INCLUDE_DIRS})
+include_directories(include)
 
-link_directories(${catkin_LIB_DIRS} lib)
+add_library(${PROJECT_NAME} SHARED
-
-add_library(robot_joint_controller 
   src/robot_joint_controller.cpp
+  src/robot_joint_controller_group.cpp
 )
-target_link_libraries(robot_joint_controller ${catkin_LIBRARIES})
+
+target_include_directories(${PROJECT_NAME} PRIVATE include)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  urdf
+  control_toolbox
+  realtime_tools
+  hardware_interface
+  controller_interface
+  pluginlib
+  robot_msgs
+  geometry_msgs
+  rcl_interfaces
+)
+
+pluginlib_export_plugin_description_file(controller_interface robot_joint_controller_plugins.xml)
+
+install(DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+
+ament_export_libraries(${PROJECT_NAME})
+ament_export_dependencies(rclcpp urdf control_toolbox realtime_tools hardware_interface controller_interface pluginlib robot_msgs geometry_msgs rcl_interfaces)
+install(
+        TARGETS ${PROJECT_NAME}
+        EXPORT ${PROJECT_NAME}
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+        INCLUDES DESTINATION include
+)
+
+ament_package()

src/robot_joint_controller/include/robot_joint_controller.h

@@ -1,72 +0,0 @@
-#ifndef ROBOT_JOINT_CONTROLLER_H
-#define ROBOT_JOINT_CONTROLLER_H
-
-#include <ros/node_handle.h>
-#include <urdf/model.h>
-#include <control_toolbox/pid.h>
-#include <realtime_tools/realtime_publisher.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <controller_interface/controller.h>
-#include <std_msgs/Float64.h>
-#include <realtime_tools/realtime_buffer.h>
-#include <controller_interface/controller.h>
-#include <hardware_interface/joint_command_interface.h>
-#include "robot_msgs/MotorCommand.h"
-#include "robot_msgs/MotorState.h"
-#include <geometry_msgs/WrenchStamped.h>
-
-#include <stdio.h>
-#include <stdint.h>
-#include <algorithm>
-#include <math.h>
-
-#define PosStopF (2.146E+9f)
-#define VelStopF (16000.0f)
-
-typedef struct
-{
-    uint8_t mode;
-    double pos;
-    double posStiffness;
-    double vel;
-    double velStiffness;
-    double torque;
-} ServoCommand;
-
-namespace robot_joint_controller
-{
-    class RobotJointController : public controller_interface::Controller<hardware_interface::EffortJointInterface>
-    {
-    private:
-        hardware_interface::JointHandle joint;
-        ros::Subscriber sub_command, sub_ft;
-        control_toolbox::Pid pid_controller_;
-        std::unique_ptr<realtime_tools::RealtimePublisher<robot_msgs::MotorState>> controller_state_publisher_;
-
-    public:
-        std::string name_space;
-        std::string joint_name;
-        urdf::JointConstSharedPtr joint_urdf;
-        realtime_tools::RealtimeBuffer<robot_msgs::MotorCommand> command;
-        robot_msgs::MotorCommand lastCommand;
-        robot_msgs::MotorState lastState;
-        ServoCommand servoCommand;
-
-        RobotJointController();
-        ~RobotJointController();
-        virtual bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
-        virtual void starting(const ros::Time &time);
-        virtual void update(const ros::Time &time, const ros::Duration &period);
-        virtual void stopping();
-        void setCommandCB(const robot_msgs::MotorCommandConstPtr &msg);
-        void positionLimits(double &position);
-        void velocityLimits(double &velocity);
-        void effortLimits(double &effort);
-
-        void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false);
-        void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);
-        void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
-    };
-}
-
-#endif

src/robot_joint_controller/include/robot_joint_controller.hpp

@@ -0,0 +1,92 @@
+#ifndef ROBOT_JOINT_CONTROLLER_HPP
+#define ROBOT_JOINT_CONTROLLER_HPP
+
+#include "rclcpp/logging.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include <urdf/model.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <realtime_tools/realtime_buffer.h>
+#include "controller_interface/controller_interface.hpp"
+#include <std_msgs/msg/float64.hpp>
+#include <rcl_interfaces/srv/get_parameters.hpp>
+#include "robot_msgs/msg/motor_command.hpp"
+#include "robot_msgs/msg/motor_state.hpp"
+#include "visibility_control.h"
+
+#include <stdio.h>
+#include <stdint.h>
+#include <algorithm>
+#include <math.h>
+#include <cstdlib>
+#include <memory>
+
+#define PosStopF (2.146E+9f)
+#define VelStopF (16000.0f)
+
+typedef struct
+{
+    uint8_t mode;
+    double pos;
+    double pos_stiffness;
+    double vel;
+    double vel_stiffness;
+    double torque;
+} ServoCommand;
+
+namespace robot_joint_controller
+{
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+class RobotJointController : public controller_interface::ControllerInterface
+{
+public:
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    RobotJointController();
+
+#if defined(ROS_DISTRO_FOXY)
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    controller_interface::return_type update() override;
+#elif defined(ROS_DISTRO_HUMBLE)
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
+#endif
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+    // ROBOT_JOINT_CONTROLLER_PUBLIC
+    // CallbackReturn on_init() override;
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override;
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+
+    void UpdateFunc(const double &period_seconds);
+    void SetCommandCallback(const robot_msgs::msg::MotorCommand::SharedPtr msg);
+    void PositionLimit(double &position);
+    void VelocityLimit(double &velocity);
+    void EffortLimit(double &effort);
+
+protected:
+    std::string name_space_;
+    std::string joint_name_;
+    realtime_tools::RealtimeBuffer<robot_msgs::msg::MotorCommand> rt_command_ptr_;
+    robot_msgs::msg::MotorCommand last_command_;
+    robot_msgs::msg::MotorState last_state_;
+    ServoCommand servo_command_;
+    urdf::JointConstSharedPtr joints_urdf_;
+    rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr robot_description_client_;
+    rclcpp::Subscription<robot_msgs::msg::MotorCommand>::SharedPtr joints_command_subscriber_;
+    std::shared_ptr<realtime_tools::RealtimePublisher<robot_msgs::msg::MotorState>> controller_state_publisher_;
+#if defined(ROS_DISTRO_FOXY)
+    rclcpp::Time previous_update_timestamp_{0};
+#endif
+};
+
+} // namespace robot_joint_controller
+
+#endif // ROBOT_JOINT_CONTROLLER_HPP

src/robot_joint_controller/include/robot_joint_controller_group.hpp

@@ -0,0 +1,92 @@
+#ifndef ROBOT_JOINT_CONTROLLER_GROUP_HPP
+#define ROBOT_JOINT_CONTROLLER_GROUP_HPP
+
+#include "rclcpp/logging.hpp"
+#include "rclcpp/qos.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include <urdf/model.h>
+#include <realtime_tools/realtime_publisher.h>
+#include <realtime_tools/realtime_buffer.h>
+#include "controller_interface/controller_interface.hpp"
+#include <std_msgs/msg/float64.hpp>
+#include <rcl_interfaces/srv/get_parameters.hpp>
+#include "robot_msgs/msg/robot_command.hpp"
+#include "robot_msgs/msg/robot_state.hpp"
+#include "visibility_control.h"
+
+#include <stdio.h>
+#include <stdint.h>
+#include <algorithm>
+#include <math.h>
+#include <cstdlib>
+#include <memory>
+
+#define PosStopF (2.146E+9f)
+#define VelStopF (16000.0f)
+
+typedef struct
+{
+    uint8_t mode;
+    double pos;
+    double pos_stiffness;
+    double vel;
+    double vel_stiffness;
+    double torque;
+} ServoCommand;
+
+namespace robot_joint_controller
+{
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+class RobotJointControllerGroup : public controller_interface::ControllerInterface
+{
+public:
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    RobotJointControllerGroup();
+
+#if defined(ROS_DISTRO_FOXY)
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    controller_interface::return_type update() override;
+#elif defined(ROS_DISTRO_HUMBLE)
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
+#endif
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+    // ROBOT_JOINT_CONTROLLER_PUBLIC
+    // CallbackReturn on_init() override;
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override;
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;
+    ROBOT_JOINT_CONTROLLER_PUBLIC
+    CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+
+    void UpdateFunc(const double &period_seconds);
+    void SetCommandCallback(const robot_msgs::msg::RobotCommand::SharedPtr msg);
+    void PositionLimit(double &position, int &index);
+    void VelocityLimit(double &velocity, int &index);
+    void EffortLimit(double &effort, int &index);
+
+protected:
+    std::string name_space_;
+    std::vector<std::string> joint_names_;
+    realtime_tools::RealtimeBuffer<robot_msgs::msg::RobotCommand> rt_command_ptr_;
+    robot_msgs::msg::RobotCommand last_command_;
+    robot_msgs::msg::RobotState last_state_;
+    ServoCommand servo_command_;
+    std::vector<urdf::JointConstSharedPtr> joints_urdf_;
+    rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedPtr robot_description_client_;
+    rclcpp::Subscription<robot_msgs::msg::RobotCommand>::SharedPtr joints_command_subscriber_;
+    std::shared_ptr<realtime_tools::RealtimePublisher<robot_msgs::msg::RobotState>> controller_state_publisher_;
+#if defined(ROS_DISTRO_FOXY)
+    rclcpp::Time previous_update_timestamp_{0};
+#endif
+};
+
+} // namespace robot_joint_controller
+
+#endif // ROBOT_JOINT_CONTROLLER_GROUP_HPP

src/robot_joint_controller/include/visibility_control.h

@@ -0,0 +1,35 @@
+#ifndef ROBOT_JOINT_CONTROLLER__VISIBILITY_CONTROL_H_
+#define ROBOT_JOINT_CONTROLLER__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define ROBOT_JOINT_CONTROLLER_EXPORT __attribute__((dllexport))
+#define ROBOT_JOINT_CONTROLLER_IMPORT __attribute__((dllimport))
+#else
+#define ROBOT_JOINT_CONTROLLER_EXPORT __declspec(dllexport)
+#define ROBOT_JOINT_CONTROLLER_IMPORT __declspec(dllimport)
+#endif
+#ifdef ROBOT_JOINT_CONTROLLER_BUILDING_DLL
+#define ROBOT_JOINT_CONTROLLER_PUBLIC ROBOT_JOINT_CONTROLLER_EXPORT
+#else
+#define ROBOT_JOINT_CONTROLLER_PUBLIC ROBOT_JOINT_CONTROLLER_IMPORT
+#endif
+#define ROBOT_JOINT_CONTROLLER_PUBLIC_TYPE ROBOT_JOINT_CONTROLLER_PUBLIC
+#define ROBOT_JOINT_CONTROLLER_LOCAL
+#else
+#define ROBOT_JOINT_CONTROLLER_EXPORT __attribute__((visibility("default")))
+#define ROBOT_JOINT_CONTROLLER_IMPORT
+#if __GNUC__ >= 4
+#define ROBOT_JOINT_CONTROLLER_PUBLIC __attribute__((visibility("default")))
+#define ROBOT_JOINT_CONTROLLER_LOCAL __attribute__((visibility("hidden")))
+#else
+#define ROBOT_JOINT_CONTROLLER_PUBLIC
+#define ROBOT_JOINT_CONTROLLER_LOCAL
+#endif
+#define ROBOT_JOINT_CONTROLLER_PUBLIC_TYPE
+#endif
+
+#endif  // ROBOT_JOINT_CONTROLLER__VISIBILITY_CONTROL_H_

src/robot_joint_controller/package.xml

@@ -1,27 +1,27 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>robot_joint_controller</name>
   <version>0.0.0</version>
   <description>The robot_joint_controller package</description>
+
   <maintainer email="fanziqi614@gmail.com">Ziqi Fan</maintainer>
-    <license>Apache</license>
+  <license>Apache-2.0</license>
-    <buildtool_depend>catkin</buildtool_depend>
+
-    <build_depend>controller_interface</build_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
-    <build_depend>hardware_interface</build_depend>
+
-    <build_depend>pluginlib</build_depend>
+  <depend>rclcpp</depend>
-    <build_depend>roscpp</build_depend>
+  <depend>urdf</depend>
-    <build_export_depend>controller_interface</build_export_depend>
+  <depend>control_toolbox</depend>
-    <build_export_depend>hardware_interface</build_export_depend>
+  <depend>realtime_tools</depend>
-    <build_export_depend>pluginlib</build_export_depend>
+  <depend>hardware_interface</depend>
-    <build_export_depend>roscpp</build_export_depend>
+  <depend>controller_interface</depend>
-    <exec_depend>controller_interface</exec_depend>
+  <depend>pluginlib</depend>
-    <exec_depend>hardware_interface</exec_depend>
-    <exec_depend>pluginlib</exec_depend>
-    <exec_depend>roscpp</exec_depend>
   <depend>robot_msgs</depend>
-    <!-- The export tag contains other, unspecified, tags -->
+  <depend>geometry_msgs</depend>
+  <depend>rcl_interfaces</depend>
+
   <export>
-        <!-- Other tools can request additional information be placed here -->
+    <build_type>ament_cmake</build_type>
-        <controller_interface plugin="${prefix}/robot_joint_controller_plugins.xml"/>
+    <pluginlib plugin="${prefix}/share/${PROJECT_NAME}/robot_joint_controller_plugins.xml"/>
   </export>
 </package>

src/robot_joint_controller/robot_joint_controller_plugins.xml

@@ -1,8 +1,12 @@
-<library path="lib/librobot_joint_controller">
+<library path="robot_joint_controller">
   <class name="robot_joint_controller/RobotJointController"
          type="robot_joint_controller::RobotJointController"
-           base_class_type="controller_interface::ControllerBase"/>
+         base_class_type="controller_interface::ControllerInterface">
-        <description>
+    <description>The robot joint single controller</description>
-            The robot joint controller.
+  </class>
-        </description>
+  <class name="robot_joint_controller/RobotJointControllerGroup"
+         type="robot_joint_controller::RobotJointControllerGroup"
+         base_class_type="controller_interface::ControllerInterface">
+    <description>The robot joint group controller</description>
+  </class>
 </library>

src/robot_joint_controller/src/robot_joint_controller.cpp

@@ -1,198 +1,231 @@
-#include "robot_joint_controller.h"
+#include "robot_joint_controller.hpp"
-#include <pluginlib/class_list_macros.h>
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
-
+#include <pluginlib/class_list_macros.hpp>
-// #define rqtTune // use rqt or not
-
-double clamp(double &value, double min, double max)
-{
-    if (value < min)
-    {
-        value = min;
-    }
-    else if (value > max)
-    {
-        value = max;
-    }
-    return value;
-}
 
 namespace robot_joint_controller
 {
+using hardware_interface::HW_IF_POSITION;
+using hardware_interface::HW_IF_VELOCITY;
+using hardware_interface::HW_IF_EFFORT;
 
-    RobotJointController::RobotJointController()
+RobotJointController::RobotJointController()
+    : controller_interface::ControllerInterface(),
+        joints_command_subscriber_(nullptr),
+        controller_state_publisher_(nullptr)
+{
+    memset(&last_command_, 0, sizeof(robot_msgs::msg::MotorCommand));
+    memset(&last_state_, 0, sizeof(robot_msgs::msg::MotorState));
+    memset(&servo_command_, 0, sizeof(ServoCommand));
+}
+
+// CallbackReturn RobotJointController::on_init()
+// {
+//     return CallbackReturn::SUCCESS;
+// }
+
+CallbackReturn RobotJointController::on_configure(const rclcpp_lifecycle::State &previous_state)
+{
+    name_space_ = get_node()->get_namespace();
+
+    if (!get_node()->get_parameter("joints", joint_name_))
     {
-        memset(&lastCommand, 0, sizeof(robot_msgs::MotorCommand));
+        RCLCPP_ERROR(get_node()->get_logger(), "No joint given in namespace: '%s'", name_space_.c_str());
-        memset(&lastState, 0, sizeof(robot_msgs::MotorState));
+        return CallbackReturn::ERROR;
-        memset(&servoCommand, 0, sizeof(ServoCommand));
+    }
+    else
+    {
+        RCLCPP_WARN(get_node()->get_logger(), "joint_name_: %s", joint_name_.c_str());
     }
 
-    RobotJointController::~RobotJointController()
+    robot_description_client_ = get_node()->create_client<rcl_interfaces::srv::GetParameters>("/robot_state_publisher/get_parameters");
+
+    auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
+    request->names.push_back("robot_description_");
+
+    while (!robot_description_client_->wait_for_service(std::chrono::seconds(1)))
     {
-        sub_ft.shutdown();
+        if (!rclcpp::ok())
-        sub_command.shutdown();
+        {
+            RCLCPP_ERROR(get_node()->get_logger(), "Interrupted while waiting for the service. Exiting.");
+        }
+        RCLCPP_WARN(get_node()->get_logger(), "Service not available, waiting again...");
     }
 
-    void RobotJointController::setCommandCB(const robot_msgs::MotorCommandConstPtr &msg)
+	auto response_received_callback = [this](rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedFuture future)
     {
-        lastCommand.q = msg->q;
+        std::string robot_description = future.get()->values[0].string_value;
-        lastCommand.kp = msg->kp;
+        urdf::Model urdf;
-        lastCommand.dq = msg->dq;
+        if (!urdf.initString(robot_description))
-        lastCommand.kd = msg->kd;
+        {
-        lastCommand.tau = msg->tau;
+            RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse urdf file");
-        // the writeFromNonRT can be used in RT, if you have the guarantee that
-        //  * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)
-        //  * there is only one single rt thread
-        command.writeFromNonRT(lastCommand);
         }
 
-    // Controller initialization in non-realtime
+        joints_urdf_ = urdf.getJoint(joint_name_);
-    bool RobotJointController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
+        if (!joints_urdf_)
         {
-        name_space = n.getNamespace();
+            RCLCPP_ERROR(get_node()->get_logger(),"Could not find joint '%s' in urdf", joint_name_.c_str());
-        if (!n.getParam("joint", joint_name))
-        {
-            ROS_ERROR("No joint given in namespace: '%s')", n.getNamespace().c_str());
-            return false;
         }
-
+	};
-        // load pid param from ymal only if rqt need
+	auto future_result = robot_description_client_->async_send_request(request, response_received_callback);
-#ifdef rqtTune
-        // Load PID Controller using gains set on parameter server
-        if (!pid_controller_.init(ros::NodeHandle(n, "pid")))
-            return false;
-#endif
-
-        urdf::Model urdf; // Get URDF info about joint
-        if (!urdf.initParamWithNodeHandle("robot_description", n))
-        {
-            ROS_ERROR("Failed to parse urdf file");
-            return false;
-        }
-        joint_urdf = urdf.getJoint(joint_name);
-        if (!joint_urdf)
-        {
-            ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
-            return false;
-        }
-        joint = robot->getHandle(joint_name);
 
     // Start command subscriber
-        sub_command = n.subscribe("command", 20, &RobotJointController::setCommandCB, this);
+    joints_command_subscriber_ = get_node()->create_subscription<robot_msgs::msg::MotorCommand>(
+        "~/command", rclcpp::SystemDefaultsQoS(), std::bind(&RobotJointController::SetCommandCallback, this, std::placeholders::_1));
 
     // Start realtime state publisher
-        controller_state_publisher_.reset(
+    controller_state_publisher_ = std::make_shared<realtime_tools::RealtimePublisher<robot_msgs::msg::MotorState>>(
-            new realtime_tools::RealtimePublisher<robot_msgs::MotorState>(n, name_space + "/state", 1));
+        get_node()->create_publisher<robot_msgs::msg::MotorState>(/*name_space_ + */"~/state", rclcpp::SystemDefaultsQoS()));
 
-        return true;
+#if defined(ROS_DISTRO_FOXY)
-    }
+    previous_update_timestamp_ = get_node()->get_clock()->now();
-
-    void RobotJointController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup)
-    {
-        pid_controller_.setGains(p, i, d, i_max, i_min, antiwindup);
-    }
-
-    void RobotJointController::getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup)
-    {
-        pid_controller_.getGains(p, i, d, i_max, i_min, antiwindup);
-    }
-
-    void RobotJointController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
-    {
-        bool dummy;
-        pid_controller_.getGains(p, i, d, i_max, i_min, dummy);
-    }
-
-    // Controller startup in realtime
-    void RobotJointController::starting(const ros::Time &time)
-    {
-        double init_pos = joint.getPosition();
-        lastCommand.q = init_pos;
-        lastState.q = init_pos;
-        lastCommand.dq = 0;
-        lastState.dq = 0;
-        lastCommand.tau = 0;
-        lastState.tauEst = 0;
-        command.initRT(lastCommand);
-
-        pid_controller_.reset();
-    }
-
-    // Controller update loop in realtime
-    void RobotJointController::update(const ros::Time &time, const ros::Duration &period)
-    {
-        double currentPos, currentVel, calcTorque;
-        lastCommand = *(command.readFromRT());
-
-        // set command data
-        servoCommand.pos = lastCommand.q;
-        positionLimits(servoCommand.pos);
-        servoCommand.posStiffness = lastCommand.kp;
-        if (fabs(lastCommand.q - PosStopF) < 0.00001)
-        {
-            servoCommand.posStiffness = 0;
-        }
-        servoCommand.vel = lastCommand.dq;
-        velocityLimits(servoCommand.vel);
-        servoCommand.velStiffness = lastCommand.kd;
-        if (fabs(lastCommand.dq - VelStopF) < 0.00001)
-        {
-            servoCommand.velStiffness = 0;
-        }
-        servoCommand.torque = lastCommand.tau;
-        effortLimits(servoCommand.torque);
-
-        // rqt set P D gains
-#ifdef rqtTune
-        double i, i_max, i_min;
-        getGains(servoCommand.posStiffness, i, servoCommand.velStiffness, i_max, i_min);
 #endif
 
-        currentPos = joint.getPosition();
+    RCLCPP_INFO(get_node()->get_logger(), "configure successful");
-        // currentVel = computeVel(currentPos, (double)lastState.q, (double)lastState.dq, period.toSec());
+    return CallbackReturn::SUCCESS;
-        // calcTorque = computeTorque(currentPos, currentVel, servoCommand);
+}
-        currentVel = (currentPos - (double)lastState.q) / period.toSec();
-        calcTorque = servoCommand.posStiffness * (servoCommand.pos - currentPos) + servoCommand.velStiffness * (servoCommand.vel - currentVel) + servoCommand.torque;
-        effortLimits(calcTorque);
 
-        joint.setCommand(calcTorque);
+controller_interface::InterfaceConfiguration RobotJointController::command_interface_configuration() const
+{
+    controller_interface::InterfaceConfiguration command_interfaces_config;
+    command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
 
-        lastState.q = currentPos;
+    command_interfaces_config.names.push_back(joint_name_ + "/" + HW_IF_EFFORT);
-        lastState.dq = currentVel;
+
-        // lastState.tauEst = calcTorque;
+    return command_interfaces_config;
-        lastState.tauEst = joint.getEffort();
+
+}
+
+controller_interface::InterfaceConfiguration RobotJointController::state_interface_configuration() const
+{
+    controller_interface::InterfaceConfiguration state_interfaces_config;
+    state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+    state_interfaces_config.names.push_back(joint_name_ + "/" + HW_IF_POSITION);
+    state_interfaces_config.names.push_back(joint_name_ + "/" + HW_IF_EFFORT);
+
+    return state_interfaces_config;
+}
+
+CallbackReturn RobotJointController::on_activate(const rclcpp_lifecycle::State &previous_state)
+{
+    double init_pos = state_interfaces_[0].get_value();
+    last_command_.q = init_pos;
+    last_state_.q = init_pos;
+    last_command_.dq = 0;
+    last_state_.dq = 0;
+    last_command_.tau = 0;
+    last_state_.tau_est = 0;
+
+    // reset command buffer if a command came through callback when controller was inactive
+    rt_command_ptr_ = realtime_tools::RealtimeBuffer<robot_msgs::msg::MotorCommand>();
+
+    return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn RobotJointController::on_deactivate(const rclcpp_lifecycle::State &previous_state)
+{
+    // reset command buffer
+    rt_command_ptr_ = realtime_tools::RealtimeBuffer<robot_msgs::msg::MotorCommand>();
+    return CallbackReturn::SUCCESS;
+}
+
+#if defined(ROS_DISTRO_FOXY)
+controller_interface::return_type RobotJointController::update()
+{
+    const auto current_time = get_node()->get_clock()->now();
+    const auto period_seconds = (current_time - previous_update_timestamp_).seconds();
+    previous_update_timestamp_ = current_time;
+    auto joint_command = rt_command_ptr_.readFromRT();
+    // no command received yet
+    if (!joint_command)
+    {
+        return controller_interface::return_type::OK;
+    }
+    last_command_ = *(joint_command);
+    UpdateFunc(period_seconds);
+    return controller_interface::return_type::OK;
+}
+#elif defined(ROS_DISTRO_HUMBLE)
+controller_interface::return_type RobotJointController::update(const rclcpp::Time &time, const rclcpp::Duration &period)
+{
+    const auto period_seconds = period.seconds();
+    auto joint_command = rt_command_ptr_.readFromRT();
+    // no command received yet
+    if (!joint_command)
+    {
+        return controller_interface::return_type::OK;
+    }
+    last_command_ = *(joint_command);
+    UpdateFunc(period_seconds);
+    return controller_interface::return_type::OK;
+}
+#endif
+
+void RobotJointController::UpdateFunc(const double &period_seconds)
+{
+    double currentPos = 0.0;
+    double currentVel = 0.0;
+    double calcTorque = 0.0;
+
+    // set command data
+    servo_command_.pos = last_command_.q;
+    PositionLimit(servo_command_.pos);
+    servo_command_.pos_stiffness = last_command_.kp;
+    if (fabs(last_command_.q - PosStopF) < 0.00001)
+    {
+        servo_command_.pos_stiffness = 0;
+    }
+    servo_command_.vel = last_command_.dq;
+    VelocityLimit(servo_command_.vel);
+    servo_command_.vel_stiffness = last_command_.kd;
+    if (fabs(last_command_.dq - VelStopF) < 0.00001)
+    {
+        servo_command_.vel_stiffness = 0;
+    }
+    servo_command_.torque = last_command_.tau;
+    EffortLimit(servo_command_.torque);
+
+    currentPos = state_interfaces_[0].get_value();
+    currentVel = (currentPos - (double)(last_state_.q)) / period_seconds;
+    calcTorque = servo_command_.pos_stiffness * (servo_command_.pos - currentPos) + servo_command_.vel_stiffness * (servo_command_.vel - currentVel) + servo_command_.torque;
+    EffortLimit(calcTorque);
+
+    command_interfaces_[0].set_value(calcTorque);
+
+    last_state_.q = currentPos;
+    last_state_.dq = currentVel;
+    last_state_.tau_est = state_interfaces_[1].get_value();
 
     // publish state
     if (controller_state_publisher_ && controller_state_publisher_->trylock())
     {
-            controller_state_publisher_->msg_.q = lastState.q;
+        controller_state_publisher_->msg_ = last_state_;
-            controller_state_publisher_->msg_.dq = lastState.dq;
-            controller_state_publisher_->msg_.tauEst = lastState.tauEst;
         controller_state_publisher_->unlockAndPublish();
     }
-    }
+}
 
-    // Controller stopping in realtime
+void RobotJointController::SetCommandCallback(const robot_msgs::msg::MotorCommand::SharedPtr msg)
-    void RobotJointController::stopping() {}
+{
+    last_command_ = *msg;
+    rt_command_ptr_.writeFromNonRT(last_command_);
+}
 
-    void RobotJointController::positionLimits(double &position)
+void RobotJointController::PositionLimit(double &position)
-    {
+{
-        if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC)
+    std::clamp(position, joints_urdf_->limits->lower, joints_urdf_->limits->upper);
-            clamp(position, joint_urdf->limits->lower, joint_urdf->limits->upper);
+}
-    }
 
-    void RobotJointController::velocityLimits(double &velocity)
+void RobotJointController::VelocityLimit(double &velocity)
-    {
+{
-        if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC)
+    std::clamp(velocity, -joints_urdf_->limits->velocity, joints_urdf_->limits->velocity);
-            clamp(velocity, -joint_urdf->limits->velocity, joint_urdf->limits->velocity);
+}
-    }
 
-    void RobotJointController::effortLimits(double &effort)
+void RobotJointController::EffortLimit(double &effort)
-    {
+{
-        if (joint_urdf->type == urdf::Joint::REVOLUTE || joint_urdf->type == urdf::Joint::PRISMATIC)
+    std::clamp(effort, -joints_urdf_->limits->effort, joints_urdf_->limits->effort);
-            clamp(effort, -joint_urdf->limits->effort, joint_urdf->limits->effort);
+}
-    }
 
-} // namespace
+} // namespace robot_joint_controller
 
 // Register controller to pluginlib
-PLUGINLIB_EXPORT_CLASS(robot_joint_controller::RobotJointController, controller_interface::ControllerBase);
+PLUGINLIB_EXPORT_CLASS(robot_joint_controller::RobotJointController, controller_interface::ControllerInterface)

src/robot_joint_controller/src/robot_joint_controller_group.cpp

@@ -0,0 +1,278 @@
+#include "robot_joint_controller_group.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include <pluginlib/class_list_macros.hpp>
+
+namespace robot_joint_controller
+{
+using hardware_interface::HW_IF_POSITION;
+using hardware_interface::HW_IF_VELOCITY;
+using hardware_interface::HW_IF_EFFORT;
+
+RobotJointControllerGroup::RobotJointControllerGroup()
+    : controller_interface::ControllerInterface(),
+        joints_command_subscriber_(nullptr),
+        controller_state_publisher_(nullptr)
+{
+    memset(&servo_command_, 0, sizeof(ServoCommand));
+}
+
+// CallbackReturn RobotJointControllerGroup::on_init()
+// {
+//     return CallbackReturn::SUCCESS;
+// }
+
+CallbackReturn RobotJointControllerGroup::on_configure(const rclcpp_lifecycle::State &previous_state)
+{
+    name_space_ = get_node()->get_namespace();
+
+    joint_names_ = get_node()->get_parameter("joints").as_string_array();
+
+    if (joint_names_.empty())
+    {
+        RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty");
+        return CallbackReturn::ERROR;
+    }
+    else
+    {
+        for (const auto& joint_name : joint_names_)
+        {
+            RCLCPP_WARN(get_node()->get_logger(), "joint_name: %s", joint_name.c_str());
+        }
+    }
+
+    robot_description_client_ = get_node()->create_client<rcl_interfaces::srv::GetParameters>("/robot_state_publisher/get_parameters");
+
+    auto request = std::make_shared<rcl_interfaces::srv::GetParameters::Request>();
+    request->names.push_back("robot_description");
+
+    while (!robot_description_client_->wait_for_service(std::chrono::seconds(1)))
+    {
+        if (!rclcpp::ok())
+        {
+            RCLCPP_ERROR(get_node()->get_logger(), "Interrupted while waiting for the service. Exiting.");
+        }
+        RCLCPP_WARN(get_node()->get_logger(), "Service not available, waiting again...");
+    }
+
+	auto response_received_callback = [this](rclcpp::Client<rcl_interfaces::srv::GetParameters>::SharedFuture future)
+    {
+        std::string robot_description = future.get()->values[0].string_value;
+        urdf::Model urdf;
+        if (!urdf.initString(robot_description))
+        {
+            RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse urdf file");
+        }
+
+        for (const auto& joint_name : joint_names_)
+        {
+            auto joint_urdf = urdf.getJoint(joint_name);
+            if (!joint_urdf)
+            {
+                RCLCPP_ERROR(get_node()->get_logger(),"Could not find joint '%s' in urdf", joint_name.c_str());
+            }
+            if (joint_urdf)
+            {
+                joints_urdf_.push_back(joint_urdf);
+            }
+        }
+	};
+	auto future_result = robot_description_client_->async_send_request(request, response_received_callback);
+
+    // Start command subscriber
+    joints_command_subscriber_ = get_node()->create_subscription<robot_msgs::msg::RobotCommand>(
+        "~/command", rclcpp::SystemDefaultsQoS(), std::bind(&RobotJointControllerGroup::SetCommandCallback, this, std::placeholders::_1));
+
+    // Start realtime state publisher
+    controller_state_publisher_ = std::make_shared<realtime_tools::RealtimePublisher<robot_msgs::msg::RobotState>>(
+        get_node()->create_publisher<robot_msgs::msg::RobotState>(/*name_space_ + */"~/state", rclcpp::SystemDefaultsQoS()));
+
+#if defined(ROS_DISTRO_FOXY)
+    previous_update_timestamp_ = get_node()->get_clock()->now();
+#endif
+
+    RCLCPP_INFO(get_node()->get_logger(), "configure successful");
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration RobotJointControllerGroup::command_interface_configuration() const
+{
+    controller_interface::InterfaceConfiguration command_interfaces_config;
+    command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+    for (const auto & joint_name : joint_names_)
+    {
+        command_interfaces_config.names.push_back(joint_name + "/" + HW_IF_EFFORT);
+    }
+
+    return command_interfaces_config;
+}
+
+controller_interface::InterfaceConfiguration RobotJointControllerGroup::state_interface_configuration() const
+{
+    controller_interface::InterfaceConfiguration state_interfaces_config;
+    state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+    for (const auto & joint_name : joint_names_)
+    {
+        state_interfaces_config.names.push_back(joint_name + "/" + HW_IF_POSITION);
+        state_interfaces_config.names.push_back(joint_name + "/" + HW_IF_EFFORT);
+    }
+
+    return state_interfaces_config;
+}
+
+CallbackReturn RobotJointControllerGroup::on_activate(const rclcpp_lifecycle::State &previous_state)
+{
+    last_command_ = robot_msgs::msg::RobotCommand();
+    last_command_.motor_command.resize(joint_names_.size());
+    last_state_ = robot_msgs::msg::RobotState();
+    last_state_.motor_state.resize(joint_names_.size());
+
+    for (int index = 0; index < joint_names_.size(); ++index)
+    {
+        double init_pos = state_interfaces_[index * 2].get_value();
+        last_command_.motor_command[index].q = init_pos;
+        last_state_.motor_state[index].q = init_pos;
+        last_command_.motor_command[index].dq = 0;
+        last_state_.motor_state[index].dq = 0;
+        last_command_.motor_command[index].tau = 0;
+        last_state_.motor_state[index].tau_est = 0;
+    }
+
+    // reset command buffer if a command came through callback when controller was inactive
+    rt_command_ptr_ = realtime_tools::RealtimeBuffer<robot_msgs::msg::RobotCommand>();
+    rt_command_ptr_.writeFromNonRT(last_command_);
+
+    return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn RobotJointControllerGroup::on_deactivate(const rclcpp_lifecycle::State &previous_state)
+{
+    last_command_ = robot_msgs::msg::RobotCommand();
+    last_command_.motor_command.resize(joint_names_.size());
+    last_state_ = robot_msgs::msg::RobotState();
+    last_state_.motor_state.resize(joint_names_.size());
+
+    // reset command buffer
+    rt_command_ptr_ = realtime_tools::RealtimeBuffer<robot_msgs::msg::RobotCommand>();
+    rt_command_ptr_.writeFromNonRT(last_command_);
+    return CallbackReturn::SUCCESS;
+}
+
+#if defined(ROS_DISTRO_FOXY)
+controller_interface::return_type RobotJointControllerGroup::update()
+{
+    const auto current_time = get_node()->get_clock()->now();
+    const auto period_seconds = (current_time - previous_update_timestamp_).seconds();
+    previous_update_timestamp_ = current_time;
+    auto joint_commands = rt_command_ptr_.readFromRT();
+    // no command received yet
+    if (!joint_commands)
+    {
+        return controller_interface::return_type::OK;
+    }
+    if (joint_commands->motor_command.size() != joint_names_.size())
+    {
+        RCLCPP_ERROR_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
+            "command size (%zu) does not match number of interfaces (%zu)",
+            joint_commands->motor_command.size(), joint_names_.size());
+        return controller_interface::return_type::ERROR;
+    }
+    last_command_ = *(joint_commands);
+    UpdateFunc(period_seconds);
+    return controller_interface::return_type::OK;
+}
+#elif defined(ROS_DISTRO_HUMBLE)
+controller_interface::return_type RobotJointControllerGroup::update(const rclcpp::Time &time, const rclcpp::Duration &period)
+{
+    const auto period_seconds = period.seconds();
+    auto joint_commands = rt_command_ptr_.readFromRT();
+    // no command received yet
+    if (!joint_commands)
+    {
+        return controller_interface::return_type::OK;
+    }
+    if (joint_commands->motor_command.size() != joint_names_.size())
+    {
+        RCLCPP_ERROR_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000,
+            "command size (%zu) does not match number of interfaces (%zu)",
+            joint_commands->motor_command.size(), joint_names_.size());
+        return controller_interface::return_type::ERROR;
+    }
+    last_command_ = *(joint_commands);
+    UpdateFunc(period_seconds);
+    return controller_interface::return_type::OK;
+}
+#endif
+
+void RobotJointControllerGroup::UpdateFunc(const double &period_seconds)
+{
+    std::vector<double> currentPos(joint_names_.size(), 0.0);
+    std::vector<double> currentVel(joint_names_.size(), 0.0);
+    std::vector<double> calcTorque(joint_names_.size(), 0.0);
+
+    for (int index = 0; index < joint_names_.size(); ++index)
+    {
+        // set command data
+        servo_command_.pos = last_command_.motor_command[index].q;
+        PositionLimit(servo_command_.pos, index);
+        servo_command_.pos_stiffness = last_command_.motor_command[index].kp;
+        if (fabs(last_command_.motor_command[index].q - PosStopF) < 0.00001)
+        {
+            servo_command_.pos_stiffness = 0;
+        }
+        servo_command_.vel = last_command_.motor_command[index].dq;
+        VelocityLimit(servo_command_.vel, index);
+        servo_command_.vel_stiffness = last_command_.motor_command[index].kd;
+        if (fabs(last_command_.motor_command[index].dq - VelStopF) < 0.00001)
+        {
+            servo_command_.vel_stiffness = 0;
+        }
+        servo_command_.torque = last_command_.motor_command[index].tau;
+        EffortLimit(servo_command_.torque, index);
+
+        currentPos[index] = state_interfaces_[index * 2].get_value();
+        currentVel[index] = (currentPos[index] - (double)(last_state_.motor_state[index].q)) / period_seconds;
+        calcTorque[index] = servo_command_.pos_stiffness * (servo_command_.pos - currentPos[index]) + servo_command_.vel_stiffness * (servo_command_.vel - currentVel[index]) + servo_command_.torque;
+        EffortLimit(calcTorque[index], index);
+
+        command_interfaces_[index].set_value(calcTorque[index]);
+
+        last_state_.motor_state[index].q = currentPos[index];
+        last_state_.motor_state[index].dq = currentVel[index];
+        last_state_.motor_state[index].tau_est = state_interfaces_[index + 1].get_value();
+    }
+
+    // publish state
+    if (controller_state_publisher_ && controller_state_publisher_->trylock())
+    {
+        controller_state_publisher_->msg_.motor_state = last_state_.motor_state;
+        controller_state_publisher_->unlockAndPublish();
+    }
+}
+
+void RobotJointControllerGroup::SetCommandCallback(const robot_msgs::msg::RobotCommand::SharedPtr msg)
+{
+    last_command_ = *msg;
+    rt_command_ptr_.writeFromNonRT(last_command_);
+}
+
+void RobotJointControllerGroup::PositionLimit(double &position, int &index)
+{
+    std::clamp(position, joints_urdf_[index]->limits->lower, joints_urdf_[index]->limits->upper);
+}
+
+void RobotJointControllerGroup::VelocityLimit(double &velocity, int &index)
+{
+    std::clamp(velocity, -joints_urdf_[index]->limits->velocity, joints_urdf_[index]->limits->velocity);
+}
+
+void RobotJointControllerGroup::EffortLimit(double &effort, int &index)
+{
+    std::clamp(effort, -joints_urdf_[index]->limits->effort, joints_urdf_[index]->limits->effort);
+}
+
+} // namespace robot_joint_controller
+
+// Register controller to pluginlib
+PLUGINLIB_EXPORT_CLASS(robot_joint_controller::RobotJointControllerGroup, controller_interface::ControllerInterface)

src/robot_msgs/CMakeLists.txt

@@ -1,44 +1,17 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(robot_msgs)
 
-find_package(catkin REQUIRED COMPONENTS
+# find dependencies
-  message_generation
+find_package(ament_cmake REQUIRED)
-  std_msgs
+
-  geometry_msgs
+find_package(rosidl_default_generators REQUIRED)
-  sensor_msgs
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/MotorCommand.msg"
+  "msg/MotorState.msg"
+  "msg/RobotCommand.msg"
+  "msg/RobotState.msg"
+  "msg/IMU.msg"
 )
 
-add_message_files(
+ament_package()
-  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/MotorState.msg

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

src/robot_msgs/msg/RobotCommand.msg

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

src/robot_msgs/msg/RobotState.msg

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

src/robot_msgs/package.xml

@@ -1,19 +1,19 @@
 <?xml version="1.0"?>
-
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="2">
+<package format="3">
   <name>robot_msgs</name>
   <version>0.0.0</version>
-  <description>
+  <description>The robot messages package.</description>
-    The robot messgaes package.
-  </description>
   <maintainer email="fanziqi614@gmail.com">Ziqi Fan</maintainer>
-  <license>Apache</license>
+  <license>Apache 2.0</license>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
-  <depend>message_runtime</depend>
-  <depend>message_generation</depend>
-  <depend>std_msgs</depend>
-  <depend>geometry_msgs</depend>
-  <depend>sensor_msgs</depend>
 
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
 </package>

src/robots/a1_description/CMakeLists.txt

@@ -1,18 +1,22 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
 project(a1_description)
 
-find_package(catkin REQUIRED COMPONENTS
+find_package(ament_cmake REQUIRED)
-    genmsg
+find_package(ament_lint_auto REQUIRED)
-    roscpp
+find_package(urdf REQUIRED)
-    std_msgs
+find_package(xacro REQUIRED)
-    tf
+find_package(robot_state_publisher REQUIRED)
+
+install(
+  DIRECTORY
+    config
+    launch
+    meshes
+    urdf
+    xacro
+
+  DESTINATION
+    share/${PROJECT_NAME}/
 )
 
-catkin_package(
+ament_package()
-    CATKIN_DEPENDS
-)
-
-include_directories(
-    # include
-    ${catkin_INCLUDE_DIRS}
-)

src/robots/a1_description/config/robot_control.yaml

@@ -1,70 +1,113 @@
-a1_gazebo:
+controller_manager:
-    # Publish all joint states -----------------------------------
+    ros__parameters:
-    joint_state_controller:
+        update_rate: 1000  # Hz
-        type: joint_state_controller/JointStateController
+        # use_sim_time: true  # If running in simulation
-        publish_rate: 1000  
+
+        joint_state_broadcaster:
+            type: joint_state_broadcaster/JointStateBroadcaster
+
+        imu_sensor_broadcaster:
+            type: imu_sensor_broadcaster/ImuSensorBroadcaster
 
         # FL Controllers ---------------------------------------
         FL_hip_controller:
             type: robot_joint_controller/RobotJointController
-        joint: FL_hip_joint
-        pid: {p: 100.0, i: 0.0, d: 5.0}
-
         FL_thigh_controller:
             type: robot_joint_controller/RobotJointController
-        joint: FL_thigh_joint
-        pid: {p: 300.0, i: 0.0, d: 8.0}
-
         FL_calf_controller:
             type: robot_joint_controller/RobotJointController
-        joint: FL_calf_joint
-        pid: {p: 300.0, i: 0.0, d: 8.0}
 
         # FR Controllers ---------------------------------------
         FR_hip_controller:
             type: robot_joint_controller/RobotJointController
-        joint: FR_hip_joint
-        pid: {p: 100.0, i: 0.0, d: 5.0}
-
         FR_thigh_controller:
             type: robot_joint_controller/RobotJointController
-        joint: FR_thigh_joint
-        pid: {p: 300.0, i: 0.0, d: 8.0}
-
         FR_calf_controller:
             type: robot_joint_controller/RobotJointController
-        joint: FR_calf_joint
-        pid: {p: 300.0, i: 0.0, d: 8.0}
 
         # RL Controllers ---------------------------------------
         RL_hip_controller:
             type: robot_joint_controller/RobotJointController
-        joint: RL_hip_joint
-        pid: {p: 100.0, i: 0.0, d: 5.0}
-
         RL_thigh_controller:
             type: robot_joint_controller/RobotJointController
-        joint: RL_thigh_joint
-        pid: {p: 300.0, i: 0.0, d: 8.0}
-
         RL_calf_controller:
             type: robot_joint_controller/RobotJointController
-        joint: RL_calf_joint
-        pid: {p: 300.0, i: 0.0, d: 8.0}
 
         # RR Controllers ---------------------------------------
         RR_hip_controller:
             type: robot_joint_controller/RobotJointController
+        RR_thigh_controller:
+            type: robot_joint_controller/RobotJointController
+        RR_calf_controller:
+            type: robot_joint_controller/RobotJointController
+
+
+# FL Controllers ---------------------------------------
+FL_hip_controller:
+    ros__parameters:
+        joint: FL_hip_joint
+        pid: {p: 100.0, i: 0.0, d: 5.0}
+
+FL_thigh_controller:
+    ros__parameters:
+        joint: FL_thigh_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+FL_calf_controller:
+    ros__parameters:
+        joint: FL_calf_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+# FR Controllers ---------------------------------------
+FR_hip_controller:
+    ros__parameters:
+        joint: FR_hip_joint
+        pid: {p: 100.0, i: 0.0, d: 5.0}
+
+FR_thigh_controller:
+    ros__parameters:
+        joint: FR_thigh_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+FR_calf_controller:
+    ros__parameters:
+        joint: FR_calf_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+# RL Controllers ---------------------------------------
+RL_hip_controller:
+    ros__parameters:
+        joint: RL_hip_joint
+        pid: {p: 100.0, i: 0.0, d: 5.0}
+
+RL_thigh_controller:
+    ros__parameters:
+        joint: RL_thigh_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+RL_calf_controller:
+    ros__parameters:
+        joint: RL_calf_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+# RR Controllers ---------------------------------------
+RR_hip_controller:
+    ros__parameters:
         joint: RR_hip_joint
         pid: {p: 100.0, i: 0.0, d: 5.0}
 
-    RR_thigh_controller:
+RR_thigh_controller:
-        type: robot_joint_controller/RobotJointController
+    ros__parameters:
         joint: RR_thigh_joint
         pid: {p: 300.0, i: 0.0, d: 8.0}
 
-    RR_calf_controller:
+RR_calf_controller:
-        type: robot_joint_controller/RobotJointController
+    ros__parameters:
         joint: RR_calf_joint
         pid: {p: 300.0, i: 0.0, d: 8.0}
 
+imu_sensor_broadcaster:
+    ros__parameters:
+        sensor_name: "imu_sensor"
+        frame_id: imu_link
+

src/robots/a1_description/config/robot_control_group.yaml

@@ -0,0 +1,34 @@
+controller_manager:
+    ros__parameters:
+        update_rate: 1000  # Hz
+        # use_sim_time: true  # If running in simulation
+
+        joint_state_broadcaster:
+            type: joint_state_broadcaster/JointStateBroadcaster
+
+        imu_sensor_broadcaster:
+            type: imu_sensor_broadcaster/ImuSensorBroadcaster
+
+        robot_joint_controller:
+            type: robot_joint_controller/RobotJointControllerGroup
+
+robot_joint_controller:
+    ros__parameters:
+        joints:
+            - FL_hip_joint
+            - FL_thigh_joint
+            - FL_calf_joint
+            - FR_hip_joint
+            - FR_thigh_joint
+            - FR_calf_joint
+            - RL_hip_joint
+            - RL_thigh_joint
+            - RL_calf_joint
+            - RR_hip_joint
+            - RR_thigh_joint
+            - RR_calf_joint
+
+imu_sensor_broadcaster:
+    ros__parameters:
+        sensor_name: "imu_sensor"
+        frame_id: imu_link

src/robots/a1_description/launch/a1_rviz.launch

@@ -1,23 +0,0 @@
-<launch>
-
-    <arg name="user_debug" default="false"/>
-    
-    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find a1_description)/xacro/robot.xacro'
-            DEBUG:=$(arg user_debug)"/>
-
-    <!-- for higher robot_state_publisher average rate-->
-    <!-- <param name="rate" value="1000"/> -->
-
-    <!-- send fake joint values -->
-    <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
-        <param name="use_gui" value="TRUE"/>
-    </node>
-
-    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
-        <param name="publish_frequency" type="double" value="1000.0"/>
-    </node>
-
-    <node pkg="rviz" type="rviz" name="rviz" respawn="false" output="screen"
-        args="-d $(find a1_description)/launch/check_joint.rviz"/>
-
-</launch>

src/robots/a1_description/launch/a1_rviz.launch.py

@@ -0,0 +1,83 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+from launch_ros.actions import Node
+from launch.conditions import IfCondition
+
+import xacro
+
+
+def generate_launch_description():
+
+    ####### DATA INPUT ##########
+    # urdf_file = 'moonbotX.urdf'
+    package_description = "a1_description"
+
+    use_sim_time = LaunchConfiguration('use_sim_time')
+
+    use_joint_state_publisher = LaunchConfiguration('use_joint_state_publisher')
+
+    # Process the URDF file
+    pkg_path = os.path.join(get_package_share_directory(package_description))
+    xacro_file = os.path.join(pkg_path,'xacro','robot.xacro')
+    robot_description_config = xacro.process_file(xacro_file)
+
+    jsp_arg = DeclareLaunchArgument(
+        'use_joint_state_publisher',
+        default_value='True'
+    )
+
+    # Create a robot_state_publisher node
+    params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
+
+    robot_state_publisher_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher_node',
+        emulate_tty=True,
+        output='screen',
+        parameters=[params]
+    )
+
+    # Joint State Publisher
+    joint_state_publisher_node = Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        output='screen',
+        condition=IfCondition(use_joint_state_publisher)
+    )
+
+    # RVIZ Configuration
+    rviz_config_dir = os.path.join(
+        get_package_share_directory(package_description),
+        'launch',
+        'check_joint.rviz'
+    )
+
+    rviz_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        output='screen',
+        name='rviz_node',
+        parameters=[{'use_sim_time': True}],
+        arguments=['-d', rviz_config_dir]
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use sim time if true'),
+
+        jsp_arg,
+
+        # map_publisher_node,
+        robot_state_publisher_node,
+        joint_state_publisher_node,
+        rviz_node
+
+    ])

src/robots/a1_description/launch/check_joint.rviz

@@ -1,38 +1,33 @@
 Panels:
-  - Class: rviz/Displays
+  - Class: rviz_common/Displays
     Help Height: 78
     Name: Displays
     Property Tree Widget:
       Expanded:
         - /Global Options1
+        - /Status1
         - /RobotModel1
       Splitter Ratio: 0.5
-    Tree Height: 420
+    Tree Height: 617
-  - Class: rviz/Selection
+  - Class: rviz_common/Selection
     Name: Selection
-  - Class: rviz/Tool Properties
+  - Class: rviz_common/Tool Properties
     Expanded:
-      - /2D Pose Estimate1
+      - /2D Goal Pose1
-      - /2D Nav Goal1
       - /Publish Point1
     Name: Tool Properties
     Splitter Ratio: 0.5886790156364441
-  - Class: rviz/Views
+  - Class: rviz_common/Views
     Expanded:
       - /Current View1
     Name: Views
     Splitter Ratio: 0.5
-  - Class: rviz/Time
-    Experimental: false
-    Name: Time
-    SyncMode: 0
-    SyncSource: ""
 Visualization Manager:
   Class: ""
   Displays:
     - Alpha: 0.5
       Cell Size: 1
-      Class: rviz/Grid
+      Class: rviz_default_plugins/Grid
       Color: 160; 160; 164
       Enabled: true
       Line Style:
@@ -49,8 +44,16 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Alpha: 1
-      Class: rviz/RobotModel
+      Class: rviz_default_plugins/RobotModel
       Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
       Enabled: true
       Links:
         All Links Enabled: true
@@ -174,92 +177,86 @@ Visualization Manager:
           Show Trail: false
           Value: true
       Name: RobotModel
-      Robot Description: robot_description
       TF Prefix: ""
       Update Interval: 0
       Value: true
       Visual Enabled: true
-    - Class: rviz/Axes
-      Enabled: false
-      Length: 1
-      Name: Axes
-      Radius: 0.10000000149011612
-      Reference Frame: <Fixed Frame>
-      Value: false
-    - Class: rviz/TF
-      Enabled: false
-      Frame Timeout: 15
-      Frames:
-        All Enabled: true
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: true
-      Tree:
-        {}
-      Update Interval: 0
-      Value: false
   Enabled: true
   Global Options:
-    Background Color: 238; 238; 238
+    Background Color: 48; 48; 48
-    Default Light: true
     Fixed Frame: base
     Frame Rate: 30
   Name: root
   Tools:
-    - Class: rviz/Interact
+    - Class: rviz_default_plugins/Interact
       Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
+    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz/Select
+    - Class: rviz_default_plugins/Select
-    - Class: rviz/FocusCamera
+    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz/Measure
+    - Class: rviz_default_plugins/Measure
-    - Class: rviz/SetInitialPose
+      Line color: 128; 128; 0
-      Topic: /initialpose
+    - Class: rviz_default_plugins/SetInitialPose
-    - Class: rviz/SetGoal
+      Topic:
-      Topic: /move_base_simple/goal
+        Depth: 5
-    - Class: rviz/PublishPoint
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
       Single click: true
-      Topic: /clicked_point
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
   Value: true
   Views:
     Current:
-      Class: rviz/Orbit
+      Class: rviz_default_plugins/Orbit
-      Distance: 1.0307118892669678
+      Distance: 1.9769524335861206
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.0760490670800209
+        X: 0
-        Y: 0.11421932280063629
+        Y: 0
-        Z: -0.1576911211013794
+        Z: 0
-      Focal Shape Fixed Size: false
+      Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.0047969818115234
+      Pitch: 0.21039816737174988
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 4.558584690093994
+      Yaw: 0.9653980731964111
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 627
+  Height: 846
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000022ffc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000270000022f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065000000000000000640000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e40000022f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
-  Time:
-    collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1108
+  Width: 1200
-  X: 812
+  X: 617
-  Y: 241
+  Y: 102

src/robots/a1_description/launch/group_gazebo.launch.py

@@ -0,0 +1,59 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, RegisterEventHandler
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch.event_handlers import OnProcessExit
+import xacro
+
+def generate_launch_description():
+    package_name = 'a1_description'
+    package_path = os.path.join(get_package_share_directory(package_name))
+
+    xacro_file = os.path.join(package_path, 'xacro', 'robot.xacro')
+    robot_description = xacro.process_file(xacro_file).toxml()
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name='robot_state_publisher',
+        output="screen",
+        parameters=[{"robot_description": robot_description}],
+    )
+
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
+        ),
+        launch_arguments={'verbose': 'true'}.items()
+    )
+
+    spawn_entity = Node(
+        package='gazebo_ros',
+        executable='spawn_entity.py',
+        arguments=['-topic', '/robot_description', '-entity', 'a1_gazebo'],
+        output='screen',
+    )
+
+    joint_state_broadcaster_node = Node(
+        package="controller_manager",
+        executable="spawner.py",
+        arguments=["joint_state_broadcaster"],
+        output='screen',
+    )
+
+    robot_joint_controller_node = Node(
+        package="controller_manager",
+        executable="spawner.py",
+        arguments=["robot_joint_controller"],
+        output='screen',
+    )
+
+    return LaunchDescription([
+        robot_state_publisher_node,
+        gazebo,
+        spawn_entity,
+        joint_state_broadcaster_node,
+        robot_joint_controller_node,
+    ])

src/robots/a1_description/launch/single_gazebo.launch.py

@@ -0,0 +1,59 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, RegisterEventHandler
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch.event_handlers import OnProcessExit
+import xacro
+
+def generate_launch_description():
+    package_name = 'a1_description'
+    package_path = os.path.join(get_package_share_directory(package_name))
+
+    xacro_file = os.path.join(package_path, 'xacro', 'robot.xacro')
+    robot_description = xacro.process_file(xacro_file).toxml()
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name='robot_state_publisher',
+        output="screen",
+        parameters=[{"robot_description": robot_description}],
+    )
+
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
+        ),
+        launch_arguments={'verbose': 'true'}.items()
+    )
+
+    spawn_entity = Node(
+        package='gazebo_ros',
+        executable='spawn_entity.py',
+        arguments=['-topic', '/robot_description', '-entity', 'a1_gazebo'],
+        output='screen',
+    )
+
+    controller_names = [
+        "joint_state_broadcaster",
+        "FL_hip_controller", "FL_thigh_controller", "FL_calf_controller",
+        "FR_hip_controller", "FR_thigh_controller", "FR_calf_controller",
+        "RL_hip_controller", "RL_thigh_controller", "RL_calf_controller",
+        "RR_hip_controller", "RR_thigh_controller", "RR_calf_controller"
+    ]
+
+    controller_nodes = [Node(
+        package="controller_manager",
+        executable="spawner.py",
+        arguments=[name],
+        output='screen',
+    ) for name in controller_names]
+
+    return LaunchDescription([
+        robot_state_publisher_node,
+        gazebo,
+        spawn_entity,
+        *controller_nodes,
+    ])

src/robots/a1_description/package.xml

@@ -1,14 +1,22 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>a1_description</name>
-    <version>0.0.0</version>
+  <version>2.0.0</version>
-    <description>The a1_description package</description>
+  <description>TODO: Package description</description>
+  <maintainer email="fanziqi614@gmail.com">Ziqi Fan</maintainer>
+  <license>Apache-2.0</license>
 
-    <maintainer email="laikago@unitree.cc">unitree</maintainer>
+  <buildtool_depend>ament_cmake</buildtool_depend>
-    <license>TODO</license>
 
-    <buildtool_depend>catkin</buildtool_depend>
+  <depend>urdf</depend>
-    <depend>roscpp</depend>
+  <depend>xacro</depend>
-    <depend>std_msgs</depend>
+  <depend>robot_state_publisher</depend>
 
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <gazebo_ros gazebo_model_path="${prefix}/.."/>
+  </export>
 </package>

src/robots/a1_description/urdf/a1.urdf

@@ -44,11 +44,19 @@
     <color rgba="1.0 1.0 1.0 1.0"/>
   </material>
   <!-- ros_control plugin -->
-  <gazebo>
+  <!-- <gazebo>
     <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
       <robotNamespace>/a1_gazebo</robotNamespace>
       <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
     </plugin>
+  </gazebo> -->
+  <gazebo>
+        <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+            <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
+            <robot_param>robot_description</robot_param>
+            <robot_param_node>robot_state_publisher_node</robot_param_node>
+            <parameters>$(find a1_description)/config/robot_control.yaml</parameters>
+        </plugin>
     </gazebo>
   <!-- Show the trajectory of trunk center. -->
   <gazebo>

src/robots/a1_description/xacro/gazebo.xacro

@@ -1,15 +1,7 @@
 <?xml version="1.0"?>
 <robot>
-    <!-- ros_control plugin -->
-    <gazebo>
-        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
-            <robotNamespace>/a1_gazebo</robotNamespace>
-            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
-        </plugin>
-    </gazebo>
-
     <!-- Show the trajectory of trunk center. -->
-    <gazebo>
+    <!-- <gazebo>
         <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
             <frequency>10</frequency>
             <plot>
@@ -18,7 +10,7 @@
                 <material>Gazebo/Yellow</material>
             </plot>
         </plugin>
-    </gazebo>
+    </gazebo> -->
 
     <!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
     <!-- <gazebo>
@@ -32,30 +24,79 @@
         </plugin>
     </gazebo> -->
 
-    <gazebo>
+    <!-- <gazebo>
         <plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
             <bodyName>trunk</bodyName>
             <topicName>/apply_force/trunk</topicName>
         </plugin>
-    </gazebo>
+    </gazebo> -->
 
     <gazebo reference="imu_link">
-        <gravity>true</gravity>
       <sensor name="imu_sensor" type="imu">
-            <always_on>true</always_on>
-            <update_rate>1000</update_rate>
-            <visualize>true</visualize>
-            <topic>__default_topic__</topic>
       <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
-                <topicName>trunk_imu</topicName>
+          <ros>
-                <bodyName>imu_link</bodyName>
+            <namespace>/</namespace>
-                <updateRateHZ>1000.0</updateRateHZ>
+            <remapping>~/out:=imu</remapping>
-                <gaussianNoise>0.0</gaussianNoise>
+          </ros>
-                <xyzOffset>0 0 0</xyzOffset>
+          <initial_orientation_as_reference>false</initial_orientation_as_reference>
-                <rpyOffset>0 0 0</rpyOffset>
-                <frameName>imu_link</frameName>
         </plugin>
-            <pose>0 0 0 0 0 0</pose>
+        <always_on>true</always_on>
+        <update_rate>100</update_rate>
+        <visualize>true</visualize>
+        <imu>
+          <angular_velocity>
+            <x>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+                <bias_mean>0.0000075</bias_mean>
+                <bias_stddev>0.0000008</bias_stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+                <bias_mean>0.0000075</bias_mean>
+                <bias_stddev>0.0000008</bias_stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+                <bias_mean>0.0000075</bias_mean>
+                <bias_stddev>0.0000008</bias_stddev>
+              </noise>
+            </z>
+          </angular_velocity>
+          <linear_acceleration>
+            <x>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+                <bias_mean>0.1</bias_mean>
+                <bias_stddev>0.001</bias_stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+                <bias_mean>0.1</bias_mean>
+                <bias_stddev>0.001</bias_stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+                <bias_mean>0.1</bias_mean>
+                <bias_stddev>0.001</bias_stddev>
+              </noise>
+            </z>
+          </linear_acceleration>
+        </imu>
       </sensor>
     </gazebo>
 

src/robots/a1_description/xacro/robot.xacro

@@ -9,6 +9,7 @@
     <xacro:include filename="$(find a1_description)/xacro/leg.xacro"/>
     <xacro:include filename="$(find a1_description)/xacro/stairs.xacro"/>
     <xacro:include filename="$(find a1_description)/xacro/gazebo.xacro"/>
+    <xacro:include filename="$(find a1_description)/xacro/ros2_control.xacro"/>
     <!-- <xacro:include filename="$(find a1_gazebo)/launch/stairs.urdf.xacro"/> -->
 
     <!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->

src/robots/a1_description/xacro/ros2_control.xacro

@@ -0,0 +1,137 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <ros2_control name="GazeboSystem" type="system">
+        <hardware>
+            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+        </hardware>
+
+        <joint name="FR_hip_joint">
+            <command_interface name="effort">
+                <param name="min">-${hip_torque_max}</param>
+                <param name="max">${hip_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FL_hip_joint">
+            <command_interface name="effort">
+                <param name="min">-${hip_torque_max}</param>
+                <param name="max">${hip_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RR_hip_joint">
+            <command_interface name="effort">
+                <param name="min">-${hip_torque_max}</param>
+                <param name="max">${hip_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RL_hip_joint">
+            <command_interface name="effort">
+                <param name="min">-${hip_torque_max}</param>
+                <param name="max">${hip_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FR_thigh_joint">
+            <command_interface name="effort">
+                <param name="min">-${thigh_torque_max}</param>
+                <param name="max">${thigh_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FL_thigh_joint">
+            <command_interface name="effort">
+                <param name="min">-${thigh_torque_max}</param>
+                <param name="max">${thigh_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RR_thigh_joint">
+            <command_interface name="effort">
+                <param name="min">-${thigh_torque_max}</param>
+                <param name="max">${thigh_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RL_thigh_joint">
+            <command_interface name="effort">
+                <param name="min">-${thigh_torque_max}</param>
+                <param name="max">${thigh_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FR_calf_joint">
+            <command_interface name="effort">
+                <param name="min">-${calf_torque_max}</param>
+                <param name="max">${calf_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FL_calf_joint">
+            <command_interface name="effort">
+                <param name="min">-${calf_torque_max}</param>
+                <param name="max">${calf_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RR_calf_joint">
+            <command_interface name="effort">
+                <param name="min">-${calf_torque_max}</param>
+                <param name="max">${calf_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RL_calf_joint">
+            <command_interface name="effort">
+                <param name="min">-${calf_torque_max}</param>
+                <param name="max">${calf_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+    </ros2_control>
+
+    <gazebo>
+        <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+            <parameters>$(find a1_description)/config/robot_control_group.yaml</parameters>
+        </plugin>
+    </gazebo>
+
+</robot>

src/robots/go2_description/CMakeLists.txt

@@ -1,14 +1,22 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.8)
-
 project(go2_description)
 
-find_package(catkin REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(ament_lint_auto REQUIRED)
+find_package(urdf REQUIRED)
+find_package(xacro REQUIRED)
+find_package(robot_state_publisher REQUIRED)
 
-catkin_package()
+install(
+  DIRECTORY
+    config
+    launch
+    meshes
+    urdf
+    xacro
 
-find_package(roslaunch)
+  DESTINATION
+    share/${PROJECT_NAME}/
+)
 
-foreach(dir config launch meshes urdf)
+ament_package()
-	install(DIRECTORY ${dir}/
-		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
-endforeach(dir)

src/robots/go2_description/config/robot_control.yaml

@@ -1,70 +1,113 @@
-go2_gazebo:
+controller_manager:
-    # Publish all joint states -----------------------------------
+    ros__parameters:
-    joint_state_controller:
+        update_rate: 1000  # Hz
-        type: joint_state_controller/JointStateController
+        # use_sim_time: true  # If running in simulation
-        publish_rate: 1000  
+
+        joint_state_broadcaster:
+            type: joint_state_broadcaster/JointStateBroadcaster
+
+        imu_sensor_broadcaster:
+            type: imu_sensor_broadcaster/ImuSensorBroadcaster
 
         # FL Controllers ---------------------------------------
         FL_hip_controller:
             type: robot_joint_controller/RobotJointController
-        joint: FL_hip_joint
-        pid: {p: 100.0, i: 0.0, d: 5.0}
-
         FL_thigh_controller:
             type: robot_joint_controller/RobotJointController
-        joint: FL_thigh_joint
-        pid: {p: 300.0, i: 0.0, d: 8.0}
-
         FL_calf_controller:
             type: robot_joint_controller/RobotJointController
-        joint: FL_calf_joint
-        pid: {p: 300.0, i: 0.0, d: 8.0}
 
         # FR Controllers ---------------------------------------
         FR_hip_controller:
             type: robot_joint_controller/RobotJointController
-        joint: FR_hip_joint
-        pid: {p: 100.0, i: 0.0, d: 5.0}
-
         FR_thigh_controller:
             type: robot_joint_controller/RobotJointController
-        joint: FR_thigh_joint
-        pid: {p: 300.0, i: 0.0, d: 8.0}
-
         FR_calf_controller:
             type: robot_joint_controller/RobotJointController
-        joint: FR_calf_joint
-        pid: {p: 300.0, i: 0.0, d: 8.0}
 
         # RL Controllers ---------------------------------------
         RL_hip_controller:
             type: robot_joint_controller/RobotJointController
-        joint: RL_hip_joint
-        pid: {p: 100.0, i: 0.0, d: 5.0}
-
         RL_thigh_controller:
             type: robot_joint_controller/RobotJointController
-        joint: RL_thigh_joint
-        pid: {p: 300.0, i: 0.0, d: 8.0}
-
         RL_calf_controller:
             type: robot_joint_controller/RobotJointController
-        joint: RL_calf_joint
-        pid: {p: 300.0, i: 0.0, d: 8.0}
 
         # RR Controllers ---------------------------------------
         RR_hip_controller:
             type: robot_joint_controller/RobotJointController
+        RR_thigh_controller:
+            type: robot_joint_controller/RobotJointController
+        RR_calf_controller:
+            type: robot_joint_controller/RobotJointController
+
+
+# FL Controllers ---------------------------------------
+FL_hip_controller:
+    ros__parameters:
+        joint: FL_hip_joint
+        pid: {p: 100.0, i: 0.0, d: 5.0}
+
+FL_thigh_controller:
+    ros__parameters:
+        joint: FL_thigh_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+FL_calf_controller:
+    ros__parameters:
+        joint: FL_calf_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+# FR Controllers ---------------------------------------
+FR_hip_controller:
+    ros__parameters:
+        joint: FR_hip_joint
+        pid: {p: 100.0, i: 0.0, d: 5.0}
+
+FR_thigh_controller:
+    ros__parameters:
+        joint: FR_thigh_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+FR_calf_controller:
+    ros__parameters:
+        joint: FR_calf_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+# RL Controllers ---------------------------------------
+RL_hip_controller:
+    ros__parameters:
+        joint: RL_hip_joint
+        pid: {p: 100.0, i: 0.0, d: 5.0}
+
+RL_thigh_controller:
+    ros__parameters:
+        joint: RL_thigh_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+RL_calf_controller:
+    ros__parameters:
+        joint: RL_calf_joint
+        pid: {p: 300.0, i: 0.0, d: 8.0}
+
+# RR Controllers ---------------------------------------
+RR_hip_controller:
+    ros__parameters:
         joint: RR_hip_joint
         pid: {p: 100.0, i: 0.0, d: 5.0}
 
-    RR_thigh_controller:
+RR_thigh_controller:
-        type: robot_joint_controller/RobotJointController
+    ros__parameters:
         joint: RR_thigh_joint
         pid: {p: 300.0, i: 0.0, d: 8.0}
 
-    RR_calf_controller:
+RR_calf_controller:
-        type: robot_joint_controller/RobotJointController
+    ros__parameters:
         joint: RR_calf_joint
         pid: {p: 300.0, i: 0.0, d: 8.0}
 
+imu_sensor_broadcaster:
+    ros__parameters:
+        sensor_name: "imu_sensor"
+        frame_id: imu_link
+

src/robots/go2_description/package.xml

@@ -1,21 +1,22 @@
-<package format="2">
+<?xml version="1.0"?>
+<package format="3">
   <name>go2_description</name>
-  <version>1.0.0</version>
+  <version>2.0.0</version>
-  <description>
+  <description>TODO: Package description</description>
-    <p>URDF Description package for go2_description</p>
+  <maintainer email="fanziqi614@gmail.com">Ziqi Fan</maintainer>
-    <p>This package contains configuration data, 3D models and launch files
+  <license>Apache-2.0</license>
-for go2_description robot</p>
+
-  </description>
+  <buildtool_depend>ament_cmake</buildtool_depend>
-  <author>TODO</author>
+
-  <maintainer email="TODO@email.com" />
+  <depend>urdf</depend>
-  <license>BSD</license>
+  <depend>xacro</depend>
-  <buildtool_depend>catkin</buildtool_depend>
-  <depend>roslaunch</depend>
   <depend>robot_state_publisher</depend>
-  <depend>rviz</depend>
+
-  <depend>joint_state_publisher_gui</depend>
+  <test_depend>ament_lint_auto</test_depend>
-  <depend>gazebo</depend>
+  <test_depend>ament_lint_common</test_depend>
+
   <export>
-    <architecture_independent />
+    <build_type>ament_cmake</build_type>
+    <gazebo_ros gazebo_model_path="${prefix}/.."/>
   </export>
 </package>

src/robots/go2_description/xacro/gazebo.xacro

@@ -1,13 +1,5 @@
 <?xml version="1.0"?>
 <robot>
-    <!-- ros_control plugin -->
-    <gazebo>
-        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
-            <robotNamespace>/go2_gazebo</robotNamespace>
-            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
-        </plugin>
-    </gazebo>
-
     <!-- Show the trajectory of trunk center. -->
     <!--gazebo>
         <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
@@ -21,7 +13,7 @@
     </gazebo-->
 
     <!-- Show the trajectory of foot. You can add another trajectory about another foot. -->
-     <gazebo>
+     <!-- <gazebo>
         <plugin name="3dplot" filename="libLinkPlot3DPlugin.so">
             <frequency>100</frequency>
             <plot>
@@ -29,43 +21,86 @@
                 <pose>0 0 0 0 0 0</pose>
             </plot>
         </plugin>
-    </gazebo>
+    </gazebo> -->
 
-    <gazebo>
+    <!-- <gazebo>
         <plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
             <bodyName>trunk</bodyName>
             <topicName>/apply_force/trunk</topicName>
         </plugin>
-    </gazebo>
+    </gazebo> -->
 
     <gazebo reference="imu_link">
-        <gravity>true</gravity>
       <sensor name="imu_sensor" type="imu">
-            <always_on>true</always_on>
-            <update_rate>1000</update_rate>
-            <visualize>true</visualize>
-            <topic>__default_topic__</topic>
       <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
-                <topicName>trunk_imu</topicName>
+          <ros>
-                <bodyName>imu_link</bodyName>
+            <namespace>/</namespace>
-                <updateRateHZ>1000.0</updateRateHZ>
+            <remapping>~/out:=imu</remapping>
-                <gaussianNoise>0.0</gaussianNoise>
+          </ros>
-                <xyzOffset>0 0 0</xyzOffset>
+          <initial_orientation_as_reference>false</initial_orientation_as_reference>
-                <rpyOffset>0 0 0</rpyOffset>
-                <frameName>imu_link</frameName>
         </plugin>
-            <pose>0 0 0 0 0 0</pose>
+        <always_on>true</always_on>
+        <update_rate>100</update_rate>
+        <visualize>true</visualize>
+        <imu>
+          <angular_velocity>
+            <x>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+                <bias_mean>0.0000075</bias_mean>
+                <bias_stddev>0.0000008</bias_stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+                <bias_mean>0.0000075</bias_mean>
+                <bias_stddev>0.0000008</bias_stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>2e-4</stddev>
+                <bias_mean>0.0000075</bias_mean>
+                <bias_stddev>0.0000008</bias_stddev>
+              </noise>
+            </z>
+          </angular_velocity>
+          <linear_acceleration>
+            <x>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+                <bias_mean>0.1</bias_mean>
+                <bias_stddev>0.001</bias_stddev>
+              </noise>
+            </x>
+            <y>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+                <bias_mean>0.1</bias_mean>
+                <bias_stddev>0.001</bias_stddev>
+              </noise>
+            </y>
+            <z>
+              <noise type="gaussian">
+                <mean>0.0</mean>
+                <stddev>1.7e-2</stddev>
+                <bias_mean>0.1</bias_mean>
+                <bias_stddev>0.001</bias_stddev>
+              </noise>
+            </z>
+          </linear_acceleration>
+        </imu>
       </sensor>
     </gazebo>
 
-
-
-
-
-
-
     <!-- Foot contacts. -->
-    <gazebo reference="FR_calf">
+    <!-- <gazebo reference="FR_calf">
         <sensor name="FR_foot_contact" type="contact">
         <update_rate>100</update_rate>
         <plugin name="contactPlugin" filename="libunitreeFootContactPlugin.so"/>
@@ -100,10 +135,10 @@
             <collision>RL_calf_fixed_joint_lump__RL_foot_collision_1</collision>
         </contact>
         </sensor>
-    </gazebo>
+    </gazebo> -->
 
     <!-- Visualization of Foot contacts. -->
-    <gazebo reference="FR_foot">
+    <!-- <gazebo reference="FR_foot">
         <visual>
             <plugin name="drawForcePlugin" filename="libunitreeDrawForcePlugin.so">
                 <topicName>FR_foot_contact</topicName>
@@ -130,7 +165,7 @@
                 <topicName>RL_foot_contact</topicName>
             </plugin>
         </visual>
-    </gazebo>
+    </gazebo> -->
 
     <gazebo reference="base">
         <turnGravityOff>false</turnGravityOff>

src/robots/go2_description/xacro/robot.xacro

@@ -9,6 +9,7 @@
     <xacro:include filename="$(find go2_description)/xacro/leg.xacro"/>
     <!-- <xacro:include filename="$(find go2_description)/xacro/stairs.xacro"/> -->
     <xacro:include filename="$(find go2_description)/xacro/gazebo.xacro"/>
+    <xacro:include filename="$(find a1_description)/xacro/ros2_control.xacro"/>
     <!-- <xacro:include filename="$(find go2_gazebo)/launch/stairs.urdf.xacro"/> -->
 
     <!-- <xacro:stairs stairs="15" xpos="0" ypos="0" zpos="0" /> -->

src/robots/go2_description/xacro/ros2_control.xacro

@@ -0,0 +1,137 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <ros2_control name="GazeboSystem" type="system">
+        <hardware>
+            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+        </hardware>
+
+        <joint name="FR_hip_joint">
+            <command_interface name="effort">
+                <param name="min">-${hip_torque_max}</param>
+                <param name="max">${hip_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FL_hip_joint">
+            <command_interface name="effort">
+                <param name="min">-${hip_torque_max}</param>
+                <param name="max">${hip_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RR_hip_joint">
+            <command_interface name="effort">
+                <param name="min">-${hip_torque_max}</param>
+                <param name="max">${hip_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RL_hip_joint">
+            <command_interface name="effort">
+                <param name="min">-${hip_torque_max}</param>
+                <param name="max">${hip_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FR_thigh_joint">
+            <command_interface name="effort">
+                <param name="min">-${thigh_torque_max}</param>
+                <param name="max">${thigh_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FL_thigh_joint">
+            <command_interface name="effort">
+                <param name="min">-${thigh_torque_max}</param>
+                <param name="max">${thigh_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RR_thigh_joint">
+            <command_interface name="effort">
+                <param name="min">-${thigh_torque_max}</param>
+                <param name="max">${thigh_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RL_thigh_joint">
+            <command_interface name="effort">
+                <param name="min">-${thigh_torque_max}</param>
+                <param name="max">${thigh_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FR_calf_joint">
+            <command_interface name="effort">
+                <param name="min">-${calf_torque_max}</param>
+                <param name="max">${calf_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="FL_calf_joint">
+            <command_interface name="effort">
+                <param name="min">-${calf_torque_max}</param>
+                <param name="max">${calf_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RR_calf_joint">
+            <command_interface name="effort">
+                <param name="min">-${calf_torque_max}</param>
+                <param name="max">${calf_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+        <joint name="RL_calf_joint">
+            <command_interface name="effort">
+                <param name="min">-${calf_torque_max}</param>
+                <param name="max">${calf_torque_max}</param>
+            </command_interface>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+
+    </ros2_control>
+
+    <gazebo>
+        <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+            <parameters>$(find go2_description)/config/robot_control_group.yaml</parameters>
+        </plugin>
+    </gazebo>
+
+</robot>

src/robots/gr1t1_description/CMakeLists.txt

@@ -1,14 +0,0 @@
-cmake_minimum_required(VERSION 3.0)
-
-project(gr1t1_description)
-
-find_package(catkin REQUIRED)
-
-catkin_package()
-
-find_package(roslaunch)
-
-foreach(dir config launch meshes urdf)
-	install(DIRECTORY ${dir}/
-		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
-endforeach(dir)

src/robots/gr1t1_description/config/robot_control.yaml

@@ -1,57 +0,0 @@
-gr1t1_gazebo:
-    # Publish all joint states -----------------------------------
-    joint_state_controller:
-        type: joint_state_controller/JointStateController
-        publish_rate: 1000  
-
-    # left leg Controllers ---------------------------------------
-    l_hip_roll_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: l_hip_roll
-        pid: {p: 57.0, i: 0.0, d: 5.7}
-    
-    l_hip_yaw_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: l_hip_yaw
-        pid: {p: 43.0, i: 0.0, d: 4.3}
-
-    l_hip_pitch_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: l_hip_pitch
-        pid: {p: 114.0, i: 0.0, d: 11.4}
-
-    l_knee_pitch_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: l_knee_pitch
-        pid: {p: 114.0, i: 0.0, d: 11.4}
-
-    l_ankle_pitch_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: l_ankle_pitch
-        pid: {p: 15.3, i: 0.0, d: 1.5}
-
-    # right leg Controllers ---------------------------------------
-    r_hip_roll_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: r_hip_roll
-        pid: {p: 57.0, i: 0.0, d: 5.7}
-    
-    r_hip_yaw_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: r_hip_yaw
-        pid: {p: 43.0, i: 0.0, d: 4.3}
-
-    r_hip_pitch_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: r_hip_pitch
-        pid: {p: 114.0, i: 0.0, d: 11.4}
-
-    r_knee_pitch_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: r_knee_pitch
-        pid: {p: 114.0, i: 0.0, d: 11.4}
-
-    r_ankle_pitch_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: r_ankle_pitch
-        pid: {p: 15.3, i: 0.0, d: 1.5}

src/robots/gr1t1_description/launch/display.launch

@@ -1,20 +0,0 @@
-<launch>
-  <arg
-    name="model" />
-  <param
-    name="robot_description"
-    textfile="$(find gr1t1_description)/urdf/GR1T1_lower_limb.urdf" />
-  <node
-    name="joint_state_publisher_gui"
-    pkg="joint_state_publisher_gui"
-    type="joint_state_publisher_gui" />
-  <node
-    name="robot_state_publisher"
-    pkg="robot_state_publisher"
-    type="robot_state_publisher" />
-  <node
-    name="rviz"
-    pkg="rviz"
-    type="rviz"
-    args="-d $(find gr1t1_description)/urdf.rviz" />
-</launch>

src/robots/gr1t1_description/launch/gazebo.launch

@@ -1,20 +0,0 @@
-<launch>
-  <include
-    file="$(find gazebo_ros)/launch/empty_world.launch" />
-  <node
-    name="tf_footprint_base"
-    pkg="tf"
-    type="static_transform_publisher"
-    args="0 0 0 0 0 0 base_link base_footprint 40" />
-  <node
-    name="spawn_model"
-    pkg="gazebo_ros"
-    type="spawn_model"
-    args="-file $(find gr1t1_description)/urdf/GR1T1_lower_limb.urdf -urdf -model gr1t1_description"
-    output="screen" />
-  <node
-    name="fake_joint_calibration"
-    pkg="rostopic"
-    type="rostopic"
-    args="pub /calibrated std_msgs/Bool true" />
-</launch>

src/robots/gr1t1_description/package.xml

@@ -1,20 +0,0 @@
-<package format="2">
-  <name>gr1t1_description</name>
-  <version>1.0.0</version>
-  <description>
-    <p>URDF Description package for gr1t1_description</p>
-    <p>This package contains configuration data, 3D models and launch files for fldlar_description robot</p>
-  </description>
-  <author>Ziqi Fan</author>
-  <maintainer email="fanziqi614@gmail.com" />
-  <license>Apache</license>
-  <buildtool_depend>catkin</buildtool_depend>
-  <depend>roslaunch</depend>
-  <depend>robot_state_publisher</depend>
-  <depend>rviz</depend>
-  <depend>joint_state_publisher_gui</depend>
-  <depend>gazebo</depend>
-  <export>
-    <architecture_independent />
-  </export>
-</package>

src/robots/gr1t1_description/urdf.rviz

@@ -1,225 +0,0 @@
-Panels:
-  - Class: rviz/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-      Splitter Ratio: 0.5
-    Tree Height: 549
-  - Class: rviz/Selection
-    Name: Selection
-  - Class: rviz/Tool Properties
-    Expanded:
-      - /2D Pose Estimate1
-      - /2D Nav Goal1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: rviz/Time
-    Name: Time
-    SyncMode: 0
-    SyncSource: ""
-Preferences:
-  PromptSaveOnExit: true
-Toolbars:
-  toolButtonStyle: 2
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 1
-      Class: rviz/RobotModel
-      Collision Enabled: false
-      Enabled: true
-      Links:
-        10:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        11:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        12:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        13:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        20:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        21:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        22:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        23:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        30:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        31:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        32:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        33:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        40:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        41:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        42:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        43:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-        base:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Name: RobotModel
-      Robot Description: robot_description
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Default Light: true
-    Fixed Frame: base
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz/Interact
-      Hide Inactive Objects: true
-    - Class: rviz/MoveCamera
-    - Class: rviz/Select
-    - Class: rviz/FocusCamera
-    - Class: rviz/Measure
-    - Class: rviz/SetInitialPose
-      Theta std deviation: 0.2617993950843811
-      Topic: /initialpose
-      X std deviation: 0.5
-      Y std deviation: 0.5
-    - Class: rviz/SetGoal
-      Topic: /move_base_simple/goal
-    - Class: rviz/PublishPoint
-      Single click: true
-      Topic: /clicked_point
-  Value: true
-  Views:
-    Current:
-      Class: rviz/Orbit
-      Distance: 2.634476661682129
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Field of View: 0.7853981852531433
-      Focal Point:
-        X: 0.0988716408610344
-        Y: 0.08353396505117416
-        Z: -0.19447802007198334
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5753985047340393
-      Target Frame: <Fixed Frame>
-      Yaw: 0.47539806365966797
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 846
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Time:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1200
-  X: 278
-  Y: 96

src/robots/gr1t2_description/CMakeLists.txt

@@ -1,14 +0,0 @@
-cmake_minimum_required(VERSION 3.0)
-
-project(gr1t2_description)
-
-find_package(catkin REQUIRED)
-
-catkin_package()
-
-find_package(roslaunch)
-
-foreach(dir config launch meshes urdf)
-	install(DIRECTORY ${dir}/
-		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
-endforeach(dir)

src/robots/gr1t2_description/config/robot_control.yaml

@@ -1,57 +0,0 @@
-gr1t2_gazebo:
-    # Publish all joint states -----------------------------------
-    joint_state_controller:
-        type: joint_state_controller/JointStateController
-        publish_rate: 1000  
-
-    # left leg Controllers ---------------------------------------
-    l_hip_roll_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: l_hip_roll
-        pid: {p: 57.0, i: 0.0, d: 5.7}
-    
-    l_hip_yaw_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: l_hip_yaw
-        pid: {p: 43.0, i: 0.0, d: 4.3}
-
-    l_hip_pitch_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: l_hip_pitch
-        pid: {p: 114.0, i: 0.0, d: 11.4}
-
-    l_knee_pitch_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: l_knee_pitch
-        pid: {p: 114.0, i: 0.0, d: 11.4}
-
-    l_ankle_pitch_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: l_ankle_pitch
-        pid: {p: 15.3, i: 0.0, d: 1.5}
-
-    # right leg Controllers ---------------------------------------
-    r_hip_roll_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: r_hip_roll
-        pid: {p: 57.0, i: 0.0, d: 5.7}
-    
-    r_hip_yaw_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: r_hip_yaw
-        pid: {p: 43.0, i: 0.0, d: 4.3}
-
-    r_hip_pitch_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: r_hip_pitch
-        pid: {p: 114.0, i: 0.0, d: 11.4}
-
-    r_knee_pitch_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: r_knee_pitch
-        pid: {p: 114.0, i: 0.0, d: 11.4}
-
-    r_ankle_pitch_controller:
-        type: robot_joint_controller/RobotJointController
-        joint: r_ankle_pitch
-        pid: {p: 15.3, i: 0.0, d: 1.5}

src/robots/gr1t2_description/launch/display.launch

@@ -1,20 +0,0 @@
-<launch>
-  <arg
-    name="model" />
-  <param
-    name="robot_description"
-    textfile="$(find gr1t2_description)/urdf/GR1T2_simple.urdf" />
-  <node
-    name="joint_state_publisher_gui"
-    pkg="joint_state_publisher_gui"
-    type="joint_state_publisher_gui" />
-  <node
-    name="robot_state_publisher"
-    pkg="robot_state_publisher"
-    type="robot_state_publisher" />
-  <node
-    name="rviz"
-    pkg="rviz"
-    type="rviz"
-    args="-d $(find gr1t2_description)/urdf.rviz" />
-</launch>

src/robots/gr1t2_description/launch/gazebo.launch

@@ -1,20 +0,0 @@
-<launch>
-  <include
-    file="$(find gazebo_ros)/launch/empty_world.launch" />
-  <node
-    name="tf_footprint_base"
-    pkg="tf"
-    type="static_transform_publisher"
-    args="0 0 0 0 0 0 base_link base_footprint 40" />
-  <node
-    name="spawn_model"
-    pkg="gazebo_ros"
-    type="spawn_model"
-    args="-file $(find gr1t2_description)/urdf/GR1T2_simple.urdf -urdf -model gr1t2_description"
-    output="screen" />
-  <node
-    name="fake_joint_calibration"
-    pkg="rostopic"
-    type="rostopic"
-    args="pub /calibrated std_msgs/Bool true" />
-</launch>