From dc10d01fed14bdbb887abf2892e23a0db5ed2bc6 Mon Sep 17 00:00:00 2001
From: fan-ziqi <fanziqi614@gmail.com>
Date: Sat, 25 May 2024 10:54:32 +0800
Subject: [PATCH] feat: add gazebo service

---
 .gitignore                           |  2 +-
 src/rl_sar/include/rl_sim.hpp        |  2 ++
 src/rl_sar/library/rl_sdk/rl_sdk.cpp |  1 +
 src/rl_sar/library/rl_sdk/rl_sdk.hpp |  1 +
 src/rl_sar/src/rl_sim.cpp            | 10 ++++++++++
 5 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/.gitignore b/.gitignore
index efc853a..9aecb2f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -7,5 +7,5 @@ logs
 *lite3*
 *fldlar*
 .cache
-.json
+*.json
 # *gr1t1*
\ No newline at end of file
diff --git a/src/rl_sar/include/rl_sim.hpp b/src/rl_sar/include/rl_sim.hpp
index 44841d4..5606d55 100644
--- a/src/rl_sar/include/rl_sim.hpp
+++ b/src/rl_sar/include/rl_sim.hpp
@@ -7,6 +7,7 @@
 #include <ros/ros.h>
 #include <gazebo_msgs/ModelStates.h>
 #include <sensor_msgs/JointState.h>
+#include "std_srvs/Empty.h"
 #include <geometry_msgs/Twist.h>
 #include "robot_msgs/MotorCommand.h"
 #include <csignal>
@@ -55,6 +56,7 @@ private:
     ros::Subscriber joint_state_subscriber;
     ros::Subscriber cmd_vel_subscriber;
     std::map<std::string, ros::Publisher> torque_publishers;
+    ros::ServiceClient gazebo_reset_client;
     void ModelStatesCallback(const gazebo_msgs::ModelStates::ConstPtr &msg);
     void JointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
     void CmdvelCallback(const geometry_msgs::Twist::ConstPtr &msg);
diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.cpp b/src/rl_sar/library/rl_sdk/rl_sdk.cpp
index 3e65870..2caaa5a 100644
--- a/src/rl_sar/library/rl_sdk/rl_sdk.cpp
+++ b/src/rl_sar/library/rl_sdk/rl_sdk.cpp
@@ -242,6 +242,7 @@ void RL::RunKeyboard()
             case 'j': keyboard.y += 0.1; break;
             case 'l': keyboard.y -= 0.1; break;
             case ' ': keyboard.x = 0; keyboard.y = 0; keyboard.yaw = 0; break;
+            case 'r': keyboard.keyboard_state = STATE_RESET_SIMULATION; break;
             default: break;
         }
     }
diff --git a/src/rl_sar/library/rl_sdk/rl_sdk.hpp b/src/rl_sar/library/rl_sdk/rl_sdk.hpp
index b2542ec..69cd972 100644
--- a/src/rl_sar/library/rl_sdk/rl_sdk.hpp
+++ b/src/rl_sar/library/rl_sdk/rl_sdk.hpp
@@ -48,6 +48,7 @@ enum STATE {
     STATE_RL_INIT,
     STATE_RL_RUNNING,
     STATE_POS_GETDOWN,
+    STATE_RESET_SIMULATION,
 };
 
 struct KeyBoard
diff --git a/src/rl_sar/src/rl_sim.cpp b/src/rl_sar/src/rl_sim.cpp
index f0592d3..5250b3c 100644
--- a/src/rl_sar/src/rl_sim.cpp
+++ b/src/rl_sar/src/rl_sim.cpp
@@ -57,6 +57,9 @@ RL_Sim::RL_Sim()
     model_state_subscriber = nh.subscribe<gazebo_msgs::ModelStates>("/gazebo/model_states", 10, &RL_Sim::ModelStatesCallback, this);
     joint_state_subscriber = nh.subscribe<sensor_msgs::JointState>(ros_namespace + "joint_states", 10, &RL_Sim::JointStatesCallback, this);
 
+    // service
+    gazebo_reset_client = nh.serviceClient<std_srvs::Empty>("/gazebo/reset_simulation");
+    
     // loop
     loop_keyboard = std::make_shared<LoopFunc>("loop_keyboard", 0.05 ,    boost::bind(&RL_Sim::RunKeyboard,  this));
     loop_control  = std::make_shared<LoopFunc>("loop_control" , 0.002,    boost::bind(&RL_Sim::RobotControl, this));
@@ -135,6 +138,13 @@ void RL_Sim::RobotControl()
 
     motiontime++;
 
+    if(keyboard.keyboard_state == STATE_RESET_SIMULATION)
+    {
+        keyboard.keyboard_state = STATE_WAITING;
+        std_srvs::Empty srv;
+        gazebo_reset_client.call(srv);
+    }
+
     GetState(&robot_state);
     StateController(&robot_state, &robot_command);
     SetCommand(&robot_command);