Port robot_state_helper to ROS2

ur_robot_driver/CMakeLists.txt

@@ -55,6 +55,7 @@ add_library(ur_robot_driver_plugin
   src/dashboard_client_ros.cpp
   src/hardware_interface.cpp
   src/urcl_log_handler.cpp
+  src/robot_state_helper.cpp
 )
 target_link_libraries(
   ur_robot_driver_plugin
@@ -103,13 +104,23 @@ add_executable(controller_stopper_node
 )
 ament_target_dependencies(controller_stopper_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+#
+# robot_state_helper
+#
+add_executable(robot_state_helper
+  src/robot_state_helper.cpp
+  src/robot_state_helper_node.cpp
+)
+target_link_libraries(robot_state_helper ${catkin_LIBRARIES} ur_client_library::urcl)
+ament_target_dependencies(robot_state_helper ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 add_executable(urscript_interface
   src/urscript_interface.cpp
 )
 ament_target_dependencies(urscript_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 install(
-  TARGETS dashboard_client ur_ros2_control_node controller_stopper_node urscript_interface
+  TARGETS dashboard_client ur_ros2_control_node controller_stopper_node urscript_interface robot_state_helper
   DESTINATION lib/${PROJECT_NAME}
 )
 

ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp

@@ -0,0 +1,72 @@
+#ifndef UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
+#define UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
+
+#include <chrono>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/create_server.hpp"
+#include "std_srvs/srv/trigger.hpp"
+
+#include "ur_dashboard_msgs/action/set_mode.hpp"
+#include "ur_dashboard_msgs/msg/safety_mode.hpp"
+#include "ur_dashboard_msgs/msg/robot_mode.hpp"
+#include "ur_client_library/ur/datatypes.h"
+
+namespace ur_robot_driver
+{
+class RobotStateHelper
+{
+public:
+  using SetModeGoalHandle = rclcpp_action::ServerGoalHandle<ur_dashboard_msgs::action::SetMode>;
+
+  RobotStateHelper(const rclcpp::Node::SharedPtr& node);
+  RobotStateHelper() = delete;
+  virtual ~RobotStateHelper() = default;
+
+private:
+  rclcpp::Node::SharedPtr node_;
+
+  void robotModeCallback(ur_dashboard_msgs::msg::RobotMode::SharedPtr msg);
+  void safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::SharedPtr msg);
+
+  void updateRobotState(bool called_from_thread = false);
+
+  void doTransition(bool called_from_thread = false);
+
+  bool safeDashboardTrigger(rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv, bool called_from_thread = false);
+
+  void setModeAcceptCallback(const std::shared_ptr<SetModeGoalHandle> goal_handle);
+  rclcpp_action::GoalResponse setModeGoalCallback(const rclcpp_action::GoalUUID& uuid,
+                                                  std::shared_ptr<const ur_dashboard_msgs::action::SetMode::Goal> goal);
+  rclcpp_action::CancelResponse
+  setModeCancelCallback(const std::shared_ptr<SetModeGoalHandle> goal_handle);
+
+  void setModeExecute(const std::shared_ptr<SetModeGoalHandle> goal_handle);
+
+  void startActionServer();
+  bool is_started_;
+
+  std::shared_ptr<ur_dashboard_msgs::action::SetMode::Result> result_;
+  std::shared_ptr<ur_dashboard_msgs::action::SetMode::Feedback> feedback_;
+  std::shared_ptr<const ur_dashboard_msgs::action::SetMode::Goal> goal_;
+  std::shared_ptr<SetModeGoalHandle> current_goal_handle_;
+
+  urcl::RobotMode robot_mode_;
+  urcl::SafetyMode safety_mode_;
+
+  rclcpp_action::Server<ur_dashboard_msgs::action::SetMode>::SharedPtr set_mode_as_;
+
+  rclcpp::Subscription<ur_dashboard_msgs::msg::RobotMode>::SharedPtr robot_mode_sub_;
+  rclcpp::Subscription<ur_dashboard_msgs::msg::SafetyMode>::SharedPtr safety_mode_sub_;
+
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr unlock_protective_stop_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr restart_safety_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_on_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_off_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr brake_release_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr stop_program_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr play_program_srv_;
+};
+}  // namespace ur_robot_driver
+
+#endif  // UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_

ur_robot_driver/launch/ur_control.launch.py

@@ -236,6 +236,14 @@ def launch_setup(context, *args, **kwargs):
         parameters=[{"robot_ip": robot_ip}],
     )
 
+    robot_state_helper_node = Node(
+        package="ur_robot_driver",
+        executable="robot_state_helper",
+        name="robot_state_helper",
+        output="screen",
+    )
+
+
     tool_communication_node = Node(
         package="ur_robot_driver",
         condition=IfCondition(use_tool_communication),
@@ -354,6 +362,7 @@ def controller_spawner(name, active=True):
         control_node,
         ur_control_node,
         dashboard_client_node,
+        robot_state_helper_node,
         tool_communication_node,
         controller_stopper_node,
         urscript_interface,