Update the dashboard client for ROS2 (#5)

* Implement dashboard_client_node

* Implement createDashboardTriggerSrv()

* Implement the Trigger-type services

* Clang format

* Add unlock protective service. Add query service for program running.

* Add get_loaded_program service.

* Add program and installation loading services.

* Finish all querying and commanding services.

* Uncomment dashboard node.

* Remove blank line in service cb. Remove spare service declaration.

Co-authored-by: TODO <@picknik.ai>
Co-authored-by: Lovro <[email protected]>

Author: AndyZe

ur_robot_driver/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(ament_cmake REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(ur_client_library REQUIRED)
 find_package(ur_dashboard_msgs REQUIRED)
 
@@ -26,6 +27,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   hardware_interface
   pluginlib
   rclcpp
+  std_srvs
   ur_client_library
   ur_dashboard_msgs
 )
@@ -59,12 +61,12 @@ pluginlib_export_plugin_description_file(hardware_interface hardware_interface_p
 # target_link_libraries(ur_robot_driver_node ${catkin_LIBRARIES} ur_client_library::urcl)
 # ament_target_dependencies(ur_robot_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
-# add_executable(dashboard_client
-#   src/dashboard_client_ros.cpp
-#   src/dashboard_client_node.cpp
-# )
-# target_link_libraries(dashboard_client ${catkin_LIBRARIES} ur_client_library::urcl)
-# ament_target_dependencies(dashboard_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+add_executable(dashboard_client
+  src/dashboard_client_ros.cpp
+  src/dashboard_client_node.cpp
+)
+target_link_libraries(dashboard_client ${catkin_LIBRARIES} ur_client_library::urcl)
+ament_target_dependencies(dashboard_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 # add_executable(robot_state_helper
 #   src/robot_state_helper.cpp

ur_robot_driver/include/ur_robot_driver/dashboard_client_ros.h

@@ -27,10 +27,15 @@
 
 #pragma once
 
+// System
 #include <regex>
 
-#include <ur_client_library/ur/dashboard_client.h>
+// ROS
+#include <rclcpp/rclcpp.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
+// UR client library
+#include <ur_client_library/ur/dashboard_client.h>
 #include <ur_dashboard_msgs/msg/program_state.hpp>
 #include <ur_dashboard_msgs/srv/add_to_log.hpp>
 #include <ur_dashboard_msgs/srv/get_loaded_program.hpp>
@@ -51,5 +56,77 @@ namespace ur_robot_driver
  */
 class DashboardClientROS
 {
+public:
+  /*!
+   * \brief Constructor that shall be used by default
+   *
+   * \param nh Node handle pointing to the name space the dashboard-related functionalities are to
+   * be found
+   * \param robot_ip IP address of the robot
+   */
+  DashboardClientROS(const rclcpp::Node::SharedPtr& node, const std::string& robot_ip);
+  DashboardClientROS() = delete;
+  virtual ~DashboardClientROS() = default;
+
+private:
+  inline rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr createDashboardTriggerSrv(const std::string& topic,
+                                                                                      const std::string& command,
+                                                                                      const std::string& expected)
+  {
+    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service = node_->create_service<std_srvs::srv::Trigger>(
+        "topic", [&, command, expected](const std::shared_ptr<std_srvs::srv::Trigger::Request> req,
+                                        const std::shared_ptr<std_srvs::srv::Trigger::Response> resp) {
+          resp->message = this->client_.sendAndReceive(command);
+          resp->success = std::regex_match(resp->message, std::regex(expected));
+        });
+
+    return service;
+  }
+
+  bool handleRunningQuery(ur_dashboard_msgs::srv::IsProgramRunning::Request::SharedPtr req,
+                          ur_dashboard_msgs::srv::IsProgramRunning::Response::SharedPtr resp);
+
+  bool handleSavedQuery(ur_dashboard_msgs::srv::IsProgramSaved::Request::SharedPtr req,
+                        ur_dashboard_msgs::srv::IsProgramSaved::Response::SharedPtr resp);
+
+  bool handleSafetyModeQuery(ur_dashboard_msgs::srv::GetSafetyMode::Request::SharedPtr req,
+                             ur_dashboard_msgs::srv::GetSafetyMode::Response::SharedPtr resp);
+
+  bool handleRobotModeQuery(ur_dashboard_msgs::srv::GetRobotMode::Request::SharedPtr req,
+                            ur_dashboard_msgs::srv::GetRobotMode::Response::SharedPtr resp);
+
+  bool connect();
+
+  std::shared_ptr<rclcpp::Node> node_;
+  urcl::DashboardClient client_;
+
+  // Commanding services
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr brake_release_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr clear_operational_mode_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr close_popup_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr close_safety_popup_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr pause_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr play_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr power_off_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr power_on_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr restart_safety_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr shutdown_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr stop_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr unlock_protective_stop_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::Load>::SharedPtr load_installation_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::Load>::SharedPtr load_program_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr quit_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::AddToLog>::SharedPtr add_to_log_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reconnect_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::RawRequest>::SharedPtr raw_request_service_;
+
+  // Query services
+  rclcpp::Service<ur_dashboard_msgs::srv::IsProgramRunning>::SharedPtr running_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::GetLoadedProgram>::SharedPtr get_loaded_program_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::IsProgramSaved>::SharedPtr is_program_saved_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::Popup>::SharedPtr popup_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::GetProgramState>::SharedPtr program_state_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::GetSafetyMode>::SharedPtr safety_mode_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::GetRobotMode>::SharedPtr robot_mode_service_;
 };
 }  // namespace ur_robot_driver

ur_robot_driver/include/ur_robot_driver/hardware_interface.h

@@ -77,5 +77,4 @@ class URHardwareInterface : public hardware_interface::components::SystemInterfa
   status status_;
   std::vector<double> joint_angle_commands_, current_joint_angles_;
 };
-
 }  // namespace ur_robot_driver

ur_robot_driver/package.xml

@@ -24,6 +24,8 @@
   <buildtool_depend>pluginlib</buildtool_depend>
   <buildtool_depend>rclcpp</buildtool_depend>
 
+  <depend>rclcpp</depend>
+  <depend>std_srvs</depend>
   <depend>ur_client_library</depend>
   <depend>ur_dashboard_msgs</depend>
 

ur_robot_driver/src/dashboard_client_node.cpp

@@ -25,9 +25,20 @@
  */
 //----------------------------------------------------------------------
 
+#include <rclcpp/rclcpp.hpp>
 #include <ur_robot_driver/dashboard_client_ros.h>
 
 int main(int argc, char** argv)
 {
+  rclcpp::init(argc, argv);
+  rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("ur_dashboard_client");
+
+  // The IP address under which the robot is reachable.
+  const std::string robot_ip = node->declare_parameter<std::string>("robot_ip", "192.168.56.101");
+
+  ur_robot_driver::DashboardClientROS client(node, robot_ip);
+
+  rclcpp::spin(node);
+
   return 0;
 }