Foxy controller stopper (#324)

* Added controller stopper node (Backport #309)
* Add state interface to ur_description
* Async tasks to seperate thread and initialized state interface (Backport parts of #206) (#372)
* Add system_interface initialized

Co-authored-by: Mads Holm Peters <[email protected]>

Author: fmauch

ur_bringup/launch/ur_control.launch.py

@@ -243,6 +243,24 @@ def generate_launch_description():
         parameters=[{"robot_ip": robot_ip}],
     )
 
+    controller_stopper_node = Node(
+        package="ur_robot_driver",
+        executable="controller_stopper_node",
+        name="controller_stopper",
+        output="screen",
+        emulate_tty=True,
+        parameters=[
+            {
+                "consistent_controllers": [
+                    "io_and_status_controller",
+                    "force_torque_sensor_broadcaster",
+                    "joint_state_broadcaster",
+                    "speed_scaling_state_broadcaster",
+                ]
+            },
+        ],
+    )
+
     robot_state_publisher_node = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
@@ -300,6 +318,7 @@ def generate_launch_description():
     nodes_to_start = [
         control_node,
         dashboard_client_node,
+        controller_stopper_node,
         robot_state_publisher_node,
         rviz_node,
         joint_state_broadcaster_spawner,

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -36,6 +36,7 @@
 #include "ur_dashboard_msgs/msg/safety_mode.hpp"
 #include "ur_msgs/srv/set_io.hpp"
 #include "ur_msgs/srv/set_speed_slider_fraction.hpp"
+#include "std_msgs/msg/bool.hpp"
 
 namespace ur_controllers
 {
@@ -64,7 +65,9 @@ enum StateInterfaces
   ROBOT_MODE = 52,
   ROBOT_STATUS_BITS = 53,
   SAFETY_MODE = 57,
-  SAFETY_STATUS_BITS = 58
+  SAFETY_STATUS_BITS = 58,
+  INITIALIZED_FLAG = 69,
+  PROGRAM_RUNNING = 70,
 };
 
 class GPIOController : public controller_interface::ControllerInterface
@@ -98,6 +101,8 @@ class GPIOController : public controller_interface::ControllerInterface
 
   void publishSafetyMode();
 
+  void publishProgramRunning();
+
 protected:
   void initMsgs();
 
@@ -116,14 +121,16 @@ class GPIOController : public controller_interface::ControllerInterface
   std::shared_ptr<rclcpp::Publisher<ur_msgs::msg::ToolDataMsg>> tool_data_pub_;
   std::shared_ptr<rclcpp::Publisher<ur_dashboard_msgs::msg::RobotMode>> robot_mode_pub_;
   std::shared_ptr<rclcpp::Publisher<ur_dashboard_msgs::msg::SafetyMode>> safety_mode_pub_;
+  std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Bool>> program_state_pub_;
 
   ur_msgs::msg::IOStates io_msg_;
   ur_msgs::msg::ToolDataMsg tool_data_msg_;
   ur_dashboard_msgs::msg::RobotMode robot_mode_msg_;
   ur_dashboard_msgs::msg::SafetyMode safety_mode_msg_;
+  std_msgs::msg::Bool program_running_msg_;
 
   static constexpr double ASYNC_WAITING = 2.0;
-  // TODO(anyone) publishers to add: program_state_pub_, tcp_pose_pub_
+  // TODO(anyone) publishers to add: tcp_pose_pub_
   // TODO(anyone) subscribers to add: script_command_sub_
   // TODO(anyone) service servers to add: resend_robot_program_srv_, deactivate_srv_, set_payload_srv_, tare_sensor_srv_
 };

ur_controllers/src/gpio_controller.cpp

@@ -100,6 +100,10 @@ controller_interface::InterfaceConfiguration ur_controllers::GPIOController::sta
   for (size_t i = 0; i < 11; ++i) {
     config.names.emplace_back("gpio/safety_status_bit_" + std::to_string(i));
   }
+  config.names.emplace_back("system_interface/initialized");
+
+  // program running
+  config.names.emplace_back("gpio/program_running");
 
   return config;
 }
@@ -117,6 +121,7 @@ controller_interface::return_type ur_controllers::GPIOController::update()
   publishToolData();
   publishRobotMode();
   publishSafetyMode();
+  publishProgramRunning();
   return controller_interface::return_type::OK;
 }
 
@@ -194,9 +199,24 @@ void GPIOController::publishSafetyMode()
   }
 }
 
+void GPIOController::publishProgramRunning()
+{
+  auto program_running_value = static_cast<uint8_t>(state_interfaces_[StateInterfaces::PROGRAM_RUNNING].get_value());
+  bool program_running = program_running_value == 1.0 ? true : false;
+  if (program_running_msg_.data != program_running) {
+    program_running_msg_.data = program_running;
+    program_state_pub_->publish(program_running_msg_);
+  }
+}
+
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
 ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
 {
+  while (state_interfaces_[StateInterfaces::INITIALIZED_FLAG].get_value() == 0.0) {
+    RCLCPP_INFO(get_node()->get_logger(), "Waiting for system interface to initialize...");
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  }
+
   try {
     // register publisher
     io_pub_ = get_node()->create_publisher<ur_msgs::msg::IOStates>("~/io_states", rclcpp::SystemDefaultsQoS());
@@ -210,6 +230,11 @@ ur_controllers::GPIOController::on_activate(const rclcpp_lifecycle::State& /*pre
     safety_mode_pub_ =
         get_node()->create_publisher<ur_dashboard_msgs::msg::SafetyMode>("~/safety_mode", rclcpp::SystemDefaultsQoS());
 
+    auto program_state_pub_qos = rclcpp::SystemDefaultsQoS();
+    program_state_pub_qos.transient_local();
+    program_state_pub_ =
+        get_node()->create_publisher<std_msgs::msg::Bool>("~/robot_program_running", program_state_pub_qos);
+
     set_io_srv_ = get_node()->create_service<ur_msgs::srv::SetIO>(
         "~/set_io", std::bind(&GPIOController::setIO, this, std::placeholders::_1, std::placeholders::_2));
 
@@ -231,6 +256,7 @@ ur_controllers::GPIOController::on_deactivate(const rclcpp_lifecycle::State& /*p
     tool_data_pub_.reset();
     robot_mode_pub_.reset();
     safety_mode_pub_.reset();
+    program_state_pub_.reset();
     set_io_srv_.reset();
     set_speed_slider_srv_.reset();
   } catch (...) {

ur_description/urdf/ur.ros2_control.xacro

@@ -244,8 +244,13 @@
         <state_interface name="safety_status_bit_8"/>
         <state_interface name="safety_status_bit_9"/>
         <state_interface name="safety_status_bit_10"/>
+        <state_interface name="program_running"/>
       </joint>
 
+      <gpio name="system_interface">
+        <state_interface name="initialized"/>
+      </gpio>
+
     </ros2_control>
   </xacro:macro>
 

ur_robot_driver/CMakeLists.txt

@@ -18,6 +18,9 @@ find_package(rclcpp REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(ur_client_library REQUIRED)
 find_package(ur_dashboard_msgs REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(controller_manager_msgs REQUIRED)
 
 include_directories(
   include
@@ -30,6 +33,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   std_srvs
   ur_client_library
   ur_dashboard_msgs
+  std_msgs
+  controller_manager_msgs
 )
 
 add_library(ur_robot_driver_plugin
@@ -62,8 +67,14 @@ add_executable(dashboard_client
 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(controller_stopper_node
+  src/controller_stopper.cpp
+  src/controller_stopper_node.cpp
+)
+ament_target_dependencies(controller_stopper_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 install(
-  TARGETS dashboard_client
+  TARGETS dashboard_client controller_stopper_node
   DESTINATION lib/${PROJECT_NAME}
 )
 

ur_robot_driver/README.md

@@ -92,3 +92,7 @@ execution without the teach pendant. The **headless** mode might be removed in f
 
 **Note for the e-Series:** In order to leverage the **headless** mode on the e-Series the robot must
 be in **remote_control_mode** as explained above.
+
+## controller_stopper
+A small helper node that stops and restarts ROS controllers based on a boolean status topic. When the status goes to `false`, all running controllers except a set of predefined *consistent_controllers* gets stopped. If status returns to `true` the stopped controllers are restarted.
+This is done by Subscribing to a robot's running state topic. Ideally this topic is latched and only publishes on changes. However, this node only reacts on state changes, so a state published each cycle would also be fine.

ur_robot_driver/include/ur_robot_driver/controller_stopper.hpp

@@ -0,0 +1,75 @@
+// Copyright 2019 FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Exner [email protected]
+ * \date    2019-06-12
+ *
+ * \author  Mads Holm Peters
+ * \date    2022-02-25
+ *
+ */
+//----------------------------------------------------------------------
+#ifndef UR_ROBOT_DRIVER__CONTROLLER_STOPPER_HPP_
+#define UR_ROBOT_DRIVER__CONTROLLER_STOPPER_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "controller_manager_msgs/srv/list_controllers.hpp"
+#include "controller_manager_msgs/srv/switch_controller.hpp"
+#include "std_msgs/msg/bool.hpp"
+
+class ControllerStopper
+{
+public:
+  ControllerStopper() = delete;
+  ControllerStopper(const rclcpp::Node::SharedPtr& node, bool stop_controllers_on_startup);
+  virtual ~ControllerStopper() = default;
+
+private:
+  void robotRunningCallback(const std_msgs::msg::Bool::ConstSharedPtr msg);
+
+  /*!
+   * \brief Queries running stoppable controllers and the controllers are stopped.
+   *
+   * Queries the controller manager for running controllers and compares the result with the
+   * consistent_controllers_. The remaining running controllers are stored in stopped_controllers_
+   * and stopped afterwards.
+   */
+  void findAndStopControllers();
+
+  /*!
+   * \brief Starts the controllers stored in stopped_controllers_.
+   *
+   */
+  void startControllers();
+
+  std::shared_ptr<rclcpp::Node> node_;
+  rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr controller_manager_srv_;
+  rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedPtr controller_list_srv_;
+
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr robot_running_sub_;
+
+  std::vector<std::string> consistent_controllers_;
+  std::vector<std::string> stopped_controllers_;
+
+  bool stop_controllers_on_startup_;
+  bool robot_running_;
+};
+#endif  // UR_ROBOT_DRIVER__CONTROLLER_STOPPER_HPP_

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -100,6 +100,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
 
   static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::quiet_NaN();
 
+  void asyncThread();
+
 protected:
   template <typename T>
   void readData(const std::unique_ptr<urcl::rtde_interface::DataPackage>& data_pkg, const std::string& var_name,
@@ -155,6 +157,9 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   double target_speed_fraction_cmd_;
   double scaling_async_success_;
   bool first_pass_;
+  bool initialized_;
+  double system_interface_initialized_;
+  bool async_thread_shutdown_;
 
   // copy of non double values
   std::array<double, 18> actual_dig_out_bits_copy_;
@@ -170,11 +175,13 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
 
   bool robot_program_running_;
   bool non_blocking_read_;
+  double robot_program_running_copy_;
 
   PausingState pausing_state_;
   double pausing_ramp_up_increment_;
 
   std::unique_ptr<urcl::UrDriver> ur_driver_;
+  std::shared_ptr<std::thread> async_thread_;
 };
 }  // namespace ur_robot_driver