feat: add gazebo service

.gitignore

@@ -7,5 +7,5 @@ logs
 *lite3*
 *fldlar*
 .cache
-.json
+*.json
 # *gr1t1*

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);

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;
         }
     }

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

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);