feat: add joy control in gazebo simulation

README.md

@@ -26,7 +26,7 @@ git clone https://github.com/fan-ziqi/rl_sar.git
 This project uses `ros-noetic` (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-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 ros-noetic-joy
 ```
 
 Download and deploy `libtorch` at any location
@@ -103,12 +103,19 @@ source devel/setup.bash
 (for python version) rosrun rl_sar rl_sim.py
 ```
 
-Control:
+# Keyboard Controls
 
-* Press **\<Enter\>** to toggle simulation start/stop.
+* Press **\<Enter\>** to toggle the simulator between running and stopped.
-* **W** and **S** controls x-axis, **A** and **D** controls yaw, and **J** and **L** controls y-axis.
+* Use **W/S** to control forward/backward movement, **A/D** to control turning, and **J/L** to control lateral movement. Press **\<Space\>** to reset all control commands to zero.
-* Press **\<Space\>** to sets all control commands to zero.
+* If the robot falls, press **R** to reset the Gazebo environment.
-* If robot falls down, press **R** to reset Gazebo environment.
+* Press **0** to move the robot from its simulation start posture to `init_pos`, and press **1** to move the robot from `init_pos` back to its simulation start posture.
+
+# Gamepad Controls
+
+* Press **LB** to toggle the simulator between running and stopped.
+* Use **LY** to control forward/backward movement, **LX** to control turning, and **J** and **L** to control lateral movement.
+* If the robot falls, press **RB+X** to reset the Gazebo environment.
+* Press **RB+Y** to move the robot from its simulation start posture to `init_pos`, and press **RB+A** to move the robot from `init_pos` back to its simulation start posture.
 
 ### Real Robots
 

README_CN.md

@@ -26,7 +26,7 @@ git clone https://github.com/fan-ziqi/rl_sar.git
 本项目使用`ros-noetic`(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-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 ros-noetic-joy
 ```
 
 在任意位置下载并部署`libtorch`
@@ -103,12 +103,19 @@ source devel/setup.bash
 (for python version) rosrun rl_sar rl_sim.py
 ```
 
-控制：
+键盘控制：
 
 * 按 **\<Enter\>** 切换仿真器运行/停止。
-* **W** 和 **S** 控制x轴，**A** 和 **D** 控制yaw轴，**J** 和 **L** 控制y轴，按下空格重置控制指令。
+* **W/S** 控制水平移动，**A/D** 控制转向，**J/L** 控制横向移动，按 **\<Space\>** 将所有控制指令设置为零。
-* 按 **\<Space\>** 将所有控制指令设置为零。
 * 如果机器人摔倒，按 **R** 重置Gazebo环境。
+* 按 **0** 让机器人从仿真开始的姿态运动到`init_pos`，按 **1** 让机器人从`init_pos`运动到仿真开始的姿态。
+
+手柄控制：
+
+* 按 **LB** 切换仿真器运行/停止。
+* **LY** 控制水平移动，**LX** 控制转向，**J** 和 **L** 控制横向移动。
+* 如果机器人摔倒，按 **RB+X** 重置Gazebo环境。
+* 按 **RB+Y** 让机器人从仿真开始的姿态运动到`init_pos`，按 **RB+A** 让机器人从`init_pos`运动到仿真开始的姿态。
 
 ### 真实机器人
 

src/rl_sar/include/rl_sim.hpp

@@ -5,6 +5,7 @@
 #include "observation_buffer.hpp"
 #include "loop.hpp"
 #include <ros/ros.h>
+#include <sensor_msgs/Joy.h>
 #include <gazebo_msgs/ModelStates.h>
 #include <sensor_msgs/JointState.h>
 #include "std_srvs/Empty.h"
@@ -51,9 +52,11 @@ private:
     geometry_msgs::Twist vel;
     geometry_msgs::Pose pose;
     geometry_msgs::Twist cmd_vel;
+    sensor_msgs::Joy joy_msg;
     ros::Subscriber model_state_subscriber;
     ros::Subscriber joint_state_subscriber;
     ros::Subscriber cmd_vel_subscriber;
+    ros::Subscriber joy_subscriber;
     ros::ServiceClient gazebo_set_model_state_client;
     ros::ServiceClient gazebo_pause_physics_client;
     ros::ServiceClient gazebo_unpause_physics_client;
@@ -62,6 +65,7 @@ private:
     void ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg);
     void JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
     void CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg);
+    void JoyCallback(const sensor_msgs::Joy::ConstPtr &msg);
 
     // others
     std::string gazebo_model_name;

src/rl_sar/launch/gazebo_a1_isaacgym.launch

@@ -51,4 +51,7 @@
         <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
     </node>
 
+    <!-- Load joy node -->
+    <node pkg="joy" type="joy_node" name="joy_node" output="screen"/>
+
 </launch>

src/rl_sar/launch/gazebo_a1_isaacsim.launch

@@ -51,4 +51,7 @@
         <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
     </node>
 
+    <!-- Load joy node -->
+    <node pkg="joy" type="joy_node" name="joy_node" output="screen"/>
+
 </launch>

src/rl_sar/launch/gazebo_go2_isaacgym.launch

@@ -51,4 +51,7 @@
         <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
     </node>
 
+    <!-- Load joy node -->
+    <node pkg="joy" type="joy_node" name="joy_node" output="screen"/>
+
 </launch>

src/rl_sar/launch/gazebo_gr1t1_isaacgym.launch

@@ -45,4 +45,7 @@
         <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
     </node>
 
+    <!-- Load joy node -->
+    <node pkg="joy" type="joy_node" name="joy_node" output="screen"/>
+
 </launch>

src/rl_sar/launch/gazebo_gr1t2_isaacgym.launch

@@ -45,4 +45,7 @@
         <remap from="/joint_states" to="/$(arg rname)_gazebo/joint_states"/>
     </node>
 
+    <!-- Load joy node -->
+    <node pkg="joy" type="joy_node" name="joy_node" output="screen"/>
+
 </launch>

src/rl_sar/src/rl_sim.cpp

@@ -57,6 +57,7 @@ RL_Sim::RL_Sim()
 
     // subscriber
     this->cmd_vel_subscriber = nh.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, &RL_Sim::CmdvelCallback, this);
+    this->joy_subscriber = nh.subscribe<sensor_msgs::Joy>("/joy", 10, &RL_Sim::JoyCallback, this);
     this->model_state_subscriber = nh.subscribe<gazebo_msgs::ModelStates>("/gazebo/model_states", 10, &RL_Sim::ModelStatesCallback, this);
     this->joint_state_subscriber = nh.subscribe<sensor_msgs::JointState>(this->ros_namespace + "joint_states", 10, &RL_Sim::JointStatesCallback, this);
 
@@ -201,6 +202,43 @@ void RL_Sim::CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg)
     this->cmd_vel = *msg;
 }
 
+void RL_Sim::JoyCallback(const sensor_msgs::Joy::ConstPtr &msg)
+{
+    this->joy_msg = *msg;
+
+    // joystick control
+    // Description of buttons and axes(F710):
+    // |__ buttons[]: A=0, B=1, X=2, Y=3, LB=4, RB=5, back=6, start=7, power=8, stickL=9, stickR=10
+    // |__ axes[]: Lx=0, Ly=1, Rx=3, Ry=4, LT=2, RT=5
+    if (this->joy_msg.buttons[5])
+    {
+        if (this->joy_msg.buttons[3]) // RB+Y
+        {
+            this->control.control_state = STATE_POS_GETUP;
+        }
+        else if (this->joy_msg.buttons[0]) // RB+A
+        {
+            this->control.control_state = STATE_POS_GETDOWN;
+        }
+        else if (this->joy_msg.buttons[1]) // RB+B
+        {
+            this->control.control_state = STATE_RL_INIT;
+        }
+        else if (this->joy_msg.buttons[2]) // RB+X
+        {
+            this->control.control_state = STATE_RESET_SIMULATION;
+        }
+    }
+    if (this->joy_msg.buttons[4]) // LB
+    {
+        this->control.control_state = STATE_TOGGLE_SIMULATION;
+    }
+
+    this->control.x = this->joy_msg.axes[1] * 1.5; // Ly
+    this->control.y = this->joy_msg.axes[3] * 1.5; // Rx
+    this->control.yaw = this->joy_msg.axes[0] * 1.5; // Lx
+}
+
 void RL_Sim::MapData(const std::vector<double> &source_data, std::vector<double> &target_data)
 {
     for (size_t i = 0; i < source_data.size(); ++i)