Add set_safety_mode Service in dsr_controller (#164)

to access recovery mode among safety modes
doosan-robotics · Feb 11, 2025 · 1b025f9 · 1b025f9
1 parent 8ac1f0e
commit 1b025f9
Show file tree

Hide file tree

Showing 5 changed files with 37 additions and 3 deletions.
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -1,5 +1,5 @@
 {
-    "editor.tabSize": 8,
+    "editor.tabSize": 2,
     "editor.rulers": [
         100
     ],

diff --git a/dsr_controller2/include/dsr_controller2/dsr_controller2.hpp b/dsr_controller2/include/dsr_controller2/dsr_controller2.hpp
@@ -62,6 +62,8 @@
 #include "dsr_msgs2/srv/servo_off.hpp"
 #include "dsr_msgs2/srv/set_robot_control.hpp"
 #include "dsr_msgs2/srv/change_collision_sensitivity.hpp"
+#include "dsr_msgs2/srv/set_safety_mode.hpp"
+
 
 // motion
 #include "dsr_msgs2/srv/move_joint.hpp"
@@ -555,6 +557,8 @@ class RobotController : public controller_interface::ControllerInterface
   rclcpp::Service<dsr_msgs2::srv::ServoOff>::SharedPtr                m_nh_srv_servo_off;
   rclcpp::Service<dsr_msgs2::srv::SetRobotControl>::SharedPtr         m_nh_srv_set_robot_control;
   rclcpp::Service<dsr_msgs2::srv::ChangeCollisionSensitivity>::SharedPtr m_nh_srv_change_collision_sensitivity;
+  rclcpp::Service<dsr_msgs2::srv::SetSafetyMode>::SharedPtr           m_nh_srv_set_safety_mode;
+
   //----- MOTION
   rclcpp::Service<dsr_msgs2::srv::MoveJoint>::SharedPtr               m_nh_srv_move_joint;
   rclcpp::Service<dsr_msgs2::srv::MoveLine>::SharedPtr                m_nh_srv_move_line;

diff --git a/dsr_controller2/src/dsr_controller2.cpp b/dsr_controller2/src/dsr_controller2.cpp
@@ -187,18 +187,25 @@ auto servo_off_cb = [this](const std::shared_ptr<dsr_msgs2::srv::ServoOff::Reque
 
 auto set_robot_control_cb = [this](const std::shared_ptr<dsr_msgs2::srv::SetRobotControl::Request> req, std::shared_ptr<dsr_msgs2::srv::SetRobotControl::Response> res) -> void
 {
-    RCLCPP_INFO(rclcpp::get_logger("dsr_controller2"),"set_robot_control_cb() called and calling Drfl->servo_off()");
+    RCLCPP_INFO(rclcpp::get_logger("dsr_controller2"),"set_robot_control_cb() called and calling Drfl->set_robot_control_cb()");
     Drfl->set_robot_control((ROBOT_CONTROL)req->robot_control);
     res->success = true;
 };
 
 auto change_collision_sensitivity_cb = [this](const std::shared_ptr<dsr_msgs2::srv::ChangeCollisionSensitivity::Request> req, std::shared_ptr<dsr_msgs2::srv::ChangeCollisionSensitivity::Response> res)-> void
 {
-    RCLCPP_INFO(rclcpp::get_logger("dsr_controller2"),"change_collision_sensitivity_cb() called and calling Drfl->servo_off()");
+    RCLCPP_INFO(rclcpp::get_logger("dsr_controller2"),"change_collision_sensitivity_cb() called and calling Drfl->change_collision_sensitivity_cb()");
     Drfl->change_collision_sensitivity((float)req->sensitivity);
     res->success = true;
 };
 
+auto set_safety_mode_cb = [this](const std::shared_ptr<dsr_msgs2::srv::SetSafetyMode::Request> req, std::shared_ptr<dsr_msgs2::srv::SetSafetyMode::Response> res) -> void
+{
+    RCLCPP_INFO(rclcpp::get_logger("dsr_controller2"),"set_safety_mode() called and calling Drfl->set_safety_mode()");
+    Drfl->set_safety_mode((SAFETY_MODE)req->safety_mode, (SAFETY_MODE_EVENT)req->safety_event);
+    res->success = true;
+};
+
 //----- MOTION Service Call-back functions ------------------------------------------------------------
 auto movej_cb = [this](const std::shared_ptr<dsr_msgs2::srv::MoveJoint::Request> req, std::shared_ptr<dsr_msgs2::srv::MoveJoint::Response> res)-> void
 {   
@@ -1937,6 +1944,8 @@ auto torque_rt_cb = [this](const std::shared_ptr<dsr_msgs2::msg::TorqueRtStream>
   m_nh_srv_servo_off                  = get_node()->create_service<dsr_msgs2::srv::ServoOff>("system/servo_off", servo_off_cb);   
   m_nh_srv_set_robot_control          = get_node()->create_service<dsr_msgs2::srv::SetRobotControl>("system/set_robot_control", set_robot_control_cb);
   m_nh_srv_change_collision_sensitivity = get_node()->create_service<dsr_msgs2::srv::ChangeCollisionSensitivity>("system/change_collision_sensitivity", change_collision_sensitivity_cb);   
+  m_nh_srv_set_safety_mode          = get_node()->create_service<dsr_msgs2::srv::SetSafetyMode>("system/set_safety_mode", set_safety_mode_cb);
+
 
   //  motion Operations
   m_nh_srv_move_joint                 = get_node()->create_service<dsr_msgs2::srv::MoveJoint>("motion/move_joint", movej_cb);                                

diff --git a/dsr_msgs2/CMakeLists.txt b/dsr_msgs2/CMakeLists.txt
@@ -49,6 +49,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/system/ServoOff.srv"
   "srv/system/SetRobotControl.srv"
   "srv/system/ChangeCollisionSensitivity.srv"
+  "srv/system/SetSafetyMode.srv"
 
   "srv/motion/MoveJoint.srv"
   "srv/motion/MoveLine.srv"

diff --git a/dsr_msgs2/srv/system/SetSafetyMode.srv b/dsr_msgs2/srv/system/SetSafetyMode.srv
@@ -0,0 +1,20 @@
+#____________________________________________________________________________________________
+# set_safety_mode
+# safety_mode:
+# 0: SAFETY_MODE_MANUAL
+# 1: SAFETY_MODE_AUTONOMOUS
+# 2: SAFETY_MODE_RECOVERY
+# 3: SAFETY_MODE_BACKDRIVE
+# 4: SAFETY_MODE_MEASURE
+# 5: SAFETY_MODE_INITIALIZE
+# safety_event:
+# 0: SAFETY_MODE_EVENT_ENTER
+# 1: SAFETY_MODE_EVENT_MOVE
+# 2: SAFETY_MODE_EVENT_STOP
+# 3: SAFETY_MODE_EVENT_LAST
+#____________________________________________________________________________________________
+
+int8 safety_mode
+int8 safety_event
+---
+bool success