Add UR Pstop Manager (#192)

src/moveit_studio_ur_pstop_manager/CMakeLists.txt

@@ -0,0 +1,70 @@
+cmake_minimum_required(VERSION 3.22)
+project(moveit_studio_ur_pstop_manager CXX)
+
+find_package(moveit_studio_common REQUIRED)
+moveit_studio_package()
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(controller_manager_msgs REQUIRED)
+find_package(moveit_studio_agent_msgs)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(ur_dashboard_msgs REQUIRED)
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  controller_manager_msgs
+  moveit_studio_agent_msgs
+  pluginlib
+  rclcpp
+  rclcpp_components
+  std_srvs
+  ur_dashboard_msgs
+)
+
+add_executable(protective_stop_manager_node src/protective_stop_manager_node.cpp)
+target_include_directories(protective_stop_manager_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(protective_stop_manager_node ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+# Mock UR dashboard client
+add_executable(mock_ur_dashboard_client_node
+  src/mock_ur_dashboard_client.cpp
+  src/mock_ur_dashboard_client_node.cpp)
+target_include_directories(mock_ur_dashboard_client_node
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  PUBLIC $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(mock_ur_dashboard_client_node ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(TARGETS
+    mock_ur_dashboard_client_node
+    protective_stop_manager_node
+  ARCHIVE DESTINATION lib/${PROJECT_NAME}
+  LIBRARY DESTINATION lib/${PROJECT_NAME}
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
+
+#############
+## Testing ##
+#############
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  add_subdirectory(test)
+endif()
+
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

src/moveit_studio_ur_pstop_manager/README.md

@@ -0,0 +1,9 @@
+# Protective Stop Recovery
+
+The `ProtectiveStopManager` class provides a service that allows the user to recover when the robot enters a protective stop state and to resume normal execution.
+At the moment, this feature is implemented as a service that calls other services, namely [`unlock_protective_stop`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/a6e209d393b35b3c67015e022ce4a4eff238a111/ur_robot_driver/src/dashboard_client_ros.cpp#L95-L96) and [`resend_robot_program`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/a6e209d393b35b3c67015e022ce4a4eff238a111/ur_controllers/src/gpio_controller.cpp#L271-L273).
+In the future, it would be preferable to implement this feature differently, so that one ROS service does not have to call other services.
+
+The [`resend_robot_program` callback](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/a6e209d393b35b3c67015e022ce4a4eff238a111/ur_controllers/src/gpio_controller.cpp#L363) must be implemented by a controller.
+So, one option is to re-implement the [ROS Dashboard Client](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/src/dashboard_client_ros.cpp) as a controller rather than a node, and to use this new controller to advertise the `resend_robot_program` service (instead of the [`GPIOController`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_controllers/src/gpio_controller.cpp)).
+This controller could also create the `recover_from_protective_stop` service (currently created by the ProtectiveStopManager).  `recover_from_protective_stop` could then directly call the functions that implement `unlock_protective_stop` and `resend_robot_program`, rather than calling those services.

src/moveit_studio_ur_pstop_manager/include/moveit_studio_ur_pstop_manager/mock_ur_dashboard_client.hpp

@@ -0,0 +1,90 @@
+// Copyright 2023 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice, this
+//   list of conditions and the following disclaimer.
+//
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+//
+// * Neither the name of the copyright holder nor the names of its
+//   contributors may be used to endorse or promote products derived from
+//   this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_srvs/srv/trigger.hpp>
+
+#include <ur_dashboard_msgs/srv/get_safety_mode.hpp>
+#include <ur_dashboard_msgs/srv/is_program_running.hpp>
+
+namespace moveit_studio::ur_pstop_manager
+{
+/**
+ * \brief This class is intended for use in simulation, and mocks some of the services that the DashboardClientROS class
+ * provides for real hardware.
+ */
+class MockURDashboardClient : public rclcpp::Node
+{
+public:
+  explicit MockURDashboardClient(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+
+private:
+  /**
+   * @brief Returns the current safety mode for the robot.  Always returns NORMAL, since our simulated robot does not
+   * enter any faulted states.
+   *
+   * @param request The request object.
+   * @param response The response, containing the robot's safety mode (always NORMAL).
+   */
+  void handleSafetyModeQuery(const ur_dashboard_msgs::srv::GetSafetyMode::Request::SharedPtr request,
+                             ur_dashboard_msgs::srv::GetSafetyMode::Response::SharedPtr response);
+
+  /**
+   * @brief Indicates whether the UR control script is currently running.  Always returns true.
+   *
+   * @param request The request object.
+   * @param response The response, indicating whether the control program is currently running (always returns true).
+   */
+  void handleRunningQuery(const ur_dashboard_msgs::srv::IsProgramRunning::Request::SharedPtr request,
+                          ur_dashboard_msgs::srv::IsProgramRunning::Response::SharedPtr response);
+
+  /**
+   * @brief Resets the robot from protective stop.  Always returns true.
+   *
+   * @param request The request object.
+   * @param response The response, indicating whether the protective stop reset was successful (always returns true).
+   */
+  void handleUnlockPStopQuery(const std_srvs::srv::Trigger::Request::SharedPtr /*request*/,
+                              std_srvs::srv::Trigger::Response::SharedPtr response) const;
+
+  /**
+   * @brief Stops the UR control script.  Always returns true.
+   *
+   * @param request The request object.
+   * @param response The response, indicating whether the UR control script was shutdown successfully (always returns true).
+   */
+  void handleStopProgramQuery(const std_srvs::srv::Trigger::Request::SharedPtr /*request*/,
+                              std_srvs::srv::Trigger::Response::SharedPtr response) const;
+
+  rclcpp::Service<ur_dashboard_msgs::srv::GetSafetyMode>::SharedPtr safety_mode_service_;
+  rclcpp::Service<ur_dashboard_msgs::srv::IsProgramRunning>::SharedPtr program_running_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr unlock_pstop_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr resend_program_service_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr stop_program_service_;
+};
+}  // namespace moveit_studio::ur_pstop_manager

src/moveit_studio_ur_pstop_manager/include/moveit_studio_ur_pstop_manager/protective_stop_manager_node.hpp

@@ -0,0 +1,102 @@
+// Copyright 2023 PickNik Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice, this
+//   list of conditions and the following disclaimer.
+//
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+//
+// * Neither the name of the copyright holder nor the names of its
+//   contributors may be used to endorse or promote products derived from
+//   this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// Implementing this feature as a node that provides a service that calls other ROS services is not ideal.  Please see
+// the README for a discussion of alternatives.
+
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_srvs/srv/trigger.hpp>
+
+#include <controller_manager_msgs/srv/switch_controller.hpp>
+#include <moveit_studio_agent_msgs/msg/fault_status.hpp>
+#include <ur_dashboard_msgs/srv/get_safety_mode.hpp>
+#include <ur_dashboard_msgs/srv/is_program_running.hpp>
+
+namespace moveit_studio::ur_pstop_manager
+{
+class ProtectiveStopManager : public rclcpp::Node
+{
+public:
+  ProtectiveStopManager(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+
+private:
+  using SwitchController = controller_manager_msgs::srv::SwitchController;
+
+  /**
+   * @brief This is the callback for the "recover_from_protective_stop" service.  It unlocks the protective stop, stops
+   * the program currently running on the arm, and re-sends the control program.
+   *
+   * @param request An empty request.
+   * @param response Indicates whether the protective stop was successfully released.
+   */
+  void recoverFromProtectiveStop(const std_srvs::srv::Trigger::Request::SharedPtr request,
+                                 std_srvs::srv::Trigger::Response::SharedPtr response);
+
+  /**
+   * @brief Helper function to check whether a service is unavailable.  If the service is unavailable, this function
+   * also sets displays the appropriate error message and sets the Response.
+   *
+   * @param client The client for the service we are checking.
+   * @param response The response object, which will indicate success or failure.
+   * @return true The service is unavailable.
+   * @return false The service is available.
+   */
+  bool indicateUnavailableService(rclcpp::ClientBase::SharedPtr client,
+                                  std_srvs::srv::Trigger::Response::SharedPtr response = nullptr);
+
+  /**
+   * @brief Callback function that publishes the current fault status of the robot.
+   *
+   */
+  void publishFaultStatus();
+
+  /**
+   * @brief Determines whether the UR control program is currently running.
+   *
+   * @return true The program is running.
+   * @return false The program is not running.
+   * @return std::nullopt An error occurred when attempting to call the service.
+   */
+  std::optional<bool> isProgramRunning(std_srvs::srv::Trigger::Response::SharedPtr response = nullptr);
+
+  std::vector<std::string> all_controller_names;
+  std::vector<std::string> active_controller_names;
+
+  rclcpp::CallbackGroup::SharedPtr reentrant_callback_group_;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr recovery_service_;
+  rclcpp::Client<ur_dashboard_msgs::srv::IsProgramRunning>::SharedPtr is_program_running_client_;
+  rclcpp::Client<SwitchController>::SharedPtr switch_controller_client_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr resend_program_client_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr stop_program_client_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr unlock_pstop_client_;
+  rclcpp::Client<ur_dashboard_msgs::srv::GetSafetyMode>::SharedPtr get_safety_mode_client_;
+  rclcpp::Publisher<moveit_studio_agent_msgs::msg::FaultStatus>::SharedPtr fault_status_publisher_;
+  rclcpp::TimerBase::SharedPtr fault_status_timer_;
+};
+}  // namespace moveit_studio::ur_pstop_manager

src/moveit_studio_ur_pstop_manager/package.xml

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>moveit_studio_ur_pstop_manager</name>
+  <version>4.0.0</version>
+  <description>Provides a node to monitor the protective stop state of the UR5, and reset protective
+    stops when necessary.</description>
+  <maintainer email="support@picknik.ai">MoveIt Pro Maintainer</maintainer>
+  <license>BSD 3-Clause</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+  <build_depend>moveit_studio_common</build_depend>
+
+  <depend>controller_manager_msgs</depend>
+  <depend>moveit_studio_agent_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_srvs</depend>
+  <depend>ur_dashboard_msgs</depend>
+  <depend>ur_robot_driver</depend>
+
+  <test_depend>ament_clang_format</test_depend>
+  <test_depend>ament_clang_tidy</test_depend>
+  <test_depend>ament_cmake_copyright</test_depend>
+  <test_depend>ament_cmake_lint_cmake</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>picknik_ament_copyright</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>