From c508305273fa2483f939926a72dd1a04ee033306 Mon Sep 17 00:00:00 2001 From: fan-ziqi Date: Sat, 21 Dec 2024 16:46:27 +0800 Subject: [PATCH] feat: add joy control in gazebo simulation --- README.md | 19 +++++++--- README_CN.md | 15 ++++++-- src/rl_sar/include/rl_sim.hpp | 4 ++ src/rl_sar/launch/gazebo_a1_isaacgym.launch | 3 ++ src/rl_sar/launch/gazebo_a1_isaacsim.launch | 3 ++ src/rl_sar/launch/gazebo_go2_isaacgym.launch | 3 ++ .../launch/gazebo_gr1t1_isaacgym.launch | 3 ++ .../launch/gazebo_gr1t2_isaacgym.launch | 3 ++ src/rl_sar/src/rl_sim.cpp | 38 +++++++++++++++++++ 9 files changed, 81 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index 2e77b43..3792731 100644 --- a/README.md +++ b/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 **\** to toggle simulation start/stop. -* **W** and **S** controls x-axis, **A** and **D** controls yaw, and **J** and **L** controls y-axis. -* Press **\** to sets all control commands to zero. -* If robot falls down, press **R** to reset Gazebo environment. +* Press **\** to toggle the simulator between running and stopped. +* Use **W/S** to control forward/backward movement, **A/D** to control turning, and **J/L** to control lateral movement. Press **\** to reset all control commands to zero. +* If the robot falls, press **R** to reset the 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 diff --git a/README_CN.md b/README_CN.md index 2119795..91632d3 100644 --- a/README_CN.md +++ b/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 ``` -控制： +键盘控制： * 按 **\** 切换仿真器运行/停止。 -* **W** 和 **S** 控制x轴，**A** 和 **D** 控制yaw轴，**J** 和 **L** 控制y轴，按下空格重置控制指令。 -* 按 **\** 将所有控制指令设置为零。 +* **W/S** 控制水平移动，**A/D** 控制转向，**J/L** 控制横向移动，按 **\** 将所有控制指令设置为零。 * 如果机器人摔倒，按 **R** 重置Gazebo环境。 +* 按 **0** 让机器人从仿真开始的姿态运动到`init_pos`，按 **1** 让机器人从`init_pos`运动到仿真开始的姿态。 + +手柄控制： + +* 按 **LB** 切换仿真器运行/停止。 +* **LY** 控制水平移动，**LX** 控制转向，**J** 和 **L** 控制横向移动。 +* 如果机器人摔倒，按 **RB+X** 重置Gazebo环境。 +* 按 **RB+Y** 让机器人从仿真开始的姿态运动到`init_pos`，按 **RB+A** 让机器人从`init_pos`运动到仿真开始的姿态。 ### 真实机器人 diff --git a/src/rl_sar/include/rl_sim.hpp b/src/rl_sar/include/rl_sim.hpp index 9601024..3951978 100644 --- a/src/rl_sar/include/rl_sim.hpp +++ b/src/rl_sar/include/rl_sim.hpp @@ -5,6 +5,7 @@ #include "observation_buffer.hpp" #include "loop.hpp" #include +#include #include #include #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; diff --git a/src/rl_sar/launch/gazebo_a1_isaacgym.launch b/src/rl_sar/launch/gazebo_a1_isaacgym.launch index 94c7457..1e1e218 100644 --- a/src/rl_sar/launch/gazebo_a1_isaacgym.launch +++ b/src/rl_sar/launch/gazebo_a1_isaacgym.launch @@ -51,4 +51,7 @@ + + + diff --git a/src/rl_sar/launch/gazebo_a1_isaacsim.launch b/src/rl_sar/launch/gazebo_a1_isaacsim.launch index 208004d..62a5cde 100644 --- a/src/rl_sar/launch/gazebo_a1_isaacsim.launch +++ b/src/rl_sar/launch/gazebo_a1_isaacsim.launch @@ -51,4 +51,7 @@ + + + diff --git a/src/rl_sar/launch/gazebo_go2_isaacgym.launch b/src/rl_sar/launch/gazebo_go2_isaacgym.launch index 838c95b..826c091 100644 --- a/src/rl_sar/launch/gazebo_go2_isaacgym.launch +++ b/src/rl_sar/launch/gazebo_go2_isaacgym.launch @@ -51,4 +51,7 @@ + + + diff --git a/src/rl_sar/launch/gazebo_gr1t1_isaacgym.launch b/src/rl_sar/launch/gazebo_gr1t1_isaacgym.launch index d653416..d36bc97 100644 --- a/src/rl_sar/launch/gazebo_gr1t1_isaacgym.launch +++ b/src/rl_sar/launch/gazebo_gr1t1_isaacgym.launch @@ -45,4 +45,7 @@ + + + diff --git a/src/rl_sar/launch/gazebo_gr1t2_isaacgym.launch b/src/rl_sar/launch/gazebo_gr1t2_isaacgym.launch index 59119d0..8233e61 100644 --- a/src/rl_sar/launch/gazebo_gr1t2_isaacgym.launch +++ b/src/rl_sar/launch/gazebo_gr1t2_isaacgym.launch @@ -45,4 +45,7 @@ + + + diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp index d36337e..a917d6e 100644 --- a/src/rl_sar/src/rl_sim.cpp +++ b/src/rl_sar/src/rl_sim.cpp @@ -57,6 +57,7 @@ RL_Sim::RL_Sim() // subscriber this->cmd_vel_subscriber = nh.subscribe("/cmd_vel", 10, &RL_Sim::CmdvelCallback, this); + this->joy_subscriber = nh.subscribe("/joy", 10, &RL_Sim::JoyCallback, this); this->model_state_subscriber = nh.subscribe("/gazebo/model_states", 10, &RL_Sim::ModelStatesCallback, this); this->joint_state_subscriber = nh.subscribe(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 &source_data, std::vector &target_data) { for (size_t i = 0; i < source_data.size(); ++i)