From e2cfdd9e879f933f7d863a4b5cffff7ec0c8f237 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 31 Jan 2023 21:29:18 +0000 Subject: [PATCH 1/2] Added controller stopper node --- controller_manager/CMakeLists.txt | 13 +- .../controller_manager/controller_stopper.h | 74 ++++++++++ controller_manager/package.xml | 3 + controller_manager/src/controller_stopper.cpp | 137 ++++++++++++++++++ .../src/controller_stopper_node.cpp | 53 +++++++ 5 files changed, 279 insertions(+), 1 deletion(-) create mode 100644 controller_manager/include/controller_manager/controller_stopper.h create mode 100644 controller_manager/src/controller_stopper.cpp create mode 100644 controller_manager/src/controller_stopper_node.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index d7709cac9..cf3ec6fb2 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS hardware_interface pluginlib roscpp + std_msgs ) catkin_python_setup() @@ -24,6 +25,7 @@ catkin_package( hardware_interface pluginlib roscpp + std_msgs ) @@ -43,6 +45,15 @@ add_library(${PROJECT_NAME} add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +add_executable(controller_stopper_node + src/controller_stopper.cpp + src/controller_stopper_node.cpp +) +add_dependencies(controller_stopper_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(controller_stopper_node + ${catkin_LIBRARIES} +) + ############# ## Testing ## @@ -85,7 +96,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) # Install targets -install(TARGETS ${PROJECT_NAME} +install(TARGETS ${PROJECT_NAME} controller_stopper_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} diff --git a/controller_manager/include/controller_manager/controller_stopper.h b/controller_manager/include/controller_manager/controller_stopper.h new file mode 100644 index 000000000..80aeeb0e4 --- /dev/null +++ b/controller_manager/include/controller_manager/controller_stopper.h @@ -0,0 +1,74 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright 2019 FZI Forschungszentrum Informatik +// Created on behalf of Universal Robots A/S +// +// 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 Willow Garage Inc, hiDOF Inc, 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 OWNER 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. +////////////////////////////////////////////////////////////////////////////// + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-06-12 + * + */ +//---------------------------------------------------------------------- +#ifndef CONTROLLER_STOPPER_CONTROLLER_STOPPER_H_INCLUDED +#define CONTROLLER_STOPPER_CONTROLLER_STOPPER_H_INCLUDED + +#include +#include + +namespace controller_manager { + +class ControllerStopper { + public: + ControllerStopper() = delete; + ControllerStopper(const ros::NodeHandle& nh); + virtual ~ControllerStopper() = default; + + private: + void robotRunningCallback(const std_msgs::BoolConstPtr& msg); + + /*! + * \brief Queries running stoppable controllers. + * + * Queries the controller manager for running controllers and compares the result with the + * consistent_controllers_. The remaining running controllers are stored in stopped_controllers_ + */ + void findStoppableControllers(); + + ros::NodeHandle nh_; + ros::NodeHandle priv_nh_; + ros::Subscriber robot_running_sub_; + ros::ServiceClient controller_manager_srv_; + ros::ServiceClient controller_list_srv_; + + std::vector consistent_controllers_; + std::vector stopped_controllers_; + + bool robot_running_; +}; +} // namespace controller_manager +#endif // ifndef CONTROLLER_STOPPER_CONTROLLER_STOPPER_H_INCLUDED diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 371c9ef43..17b8c7538 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -30,15 +30,18 @@ controller_manager_msgs hardware_interface pluginlib + std_msgs controller_interface controller_manager_msgs hardware_interface pluginlib + std_msgs std_msgs rosparam rospy + std_msgs rostest diff --git a/controller_manager/src/controller_stopper.cpp b/controller_manager/src/controller_stopper.cpp new file mode 100644 index 000000000..05a7cdc64 --- /dev/null +++ b/controller_manager/src/controller_stopper.cpp @@ -0,0 +1,137 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright 2019 FZI Forschungszentrum Informatik +// Created on behalf of Universal Robots A/S +// +// 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 Willow Garage Inc, hiDOF Inc, 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 OWNER 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. +////////////////////////////////////////////////////////////////////////////// +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-06-12 + * + */ +//---------------------------------------------------------------------- + +#include + +#include +#include + +#include + +namespace controller_manager{ + +ControllerStopper::ControllerStopper(const ros::NodeHandle& nh) : nh_(nh), priv_nh_("~"), robot_running_(true) +{ + // Subscribes 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. + robot_running_sub_ = nh_.subscribe("robot_running", 1, &ControllerStopper::robotRunningCallback, this); + + // Controller manager service to switch controllers + controller_manager_srv_ = nh_.serviceClient("controller_manager/" + "switch_controller"); + // Controller manager service to list controllers + controller_list_srv_ = nh_.serviceClient("controller_manager/" + "list_controllers"); + ROS_INFO_STREAM("Waiting for controller manager service to come up on " << nh_.resolveName("controller_manager/" + "switch_controller")); + controller_manager_srv_.waitForExistence(); + ROS_INFO_STREAM("Service available."); + ROS_INFO_STREAM("Waiting for controller list service to come up on " << nh_.resolveName("controller_manager/" + "list_controllers")); + controller_list_srv_.waitForExistence(); + ROS_INFO_STREAM("Service available."); + + // Consistent controllers will not be stopped when the robot stops. Defaults to + // ["joint_state_controller"] + if (!priv_nh_.getParam("consistent_controllers", consistent_controllers_)) + { + consistent_controllers_.push_back("joint_state_controller"); + } + + ROS_DEBUG("Waiting for running controllers"); + // Before we can work properly, we need to know which controllers there are + while (stopped_controllers_.empty()) + { + findStoppableControllers(); + ros::Duration(1).sleep(); + } + ROS_DEBUG("Initialization finished"); +} + +void ControllerStopper::findStoppableControllers() +{ + controller_manager_msgs::ListControllers list_srv; + controller_list_srv_.call(list_srv); + stopped_controllers_.clear(); + for (auto& controller : list_srv.response.controller) + { + // Check if in consistent_controllers + // Else: + // Add to stopped_controllers + if (controller.state == "running") + { + auto it = std::find(consistent_controllers_.begin(), consistent_controllers_.end(), controller.name); + if (it == consistent_controllers_.end()) + { + stopped_controllers_.push_back(controller.name); + } + } + } +} + +void ControllerStopper::robotRunningCallback(const std_msgs::BoolConstPtr& msg) +{ + ROS_DEBUG_STREAM("robotRunningCallback with data " << std::boolalpha << msg->data); + if (msg->data && !robot_running_) + { + ROS_DEBUG_STREAM("Starting controllers"); + controller_manager_msgs::SwitchController srv; + srv.request.strictness = srv.request.STRICT; + srv.request.start_controllers = stopped_controllers_; + if (!controller_manager_srv_.call(srv)) + { + ROS_ERROR_STREAM("Could not activate requested controllers"); + } + } + else if (!msg->data && robot_running_) + { + ROS_DEBUG_STREAM("Stopping controllers"); + // stop all controllers except the once in consistent_controllers_ + findStoppableControllers(); + controller_manager_msgs::SwitchController srv; + srv.request.strictness = srv.request.STRICT; + srv.request.stop_controllers = stopped_controllers_; + if (!controller_manager_srv_.call(srv)) + { + ROS_ERROR_STREAM("Could not stop requested controllers"); + } + } + robot_running_ = msg->data; +} +} // namespace controller_manager diff --git a/controller_manager/src/controller_stopper_node.cpp b/controller_manager/src/controller_stopper_node.cpp new file mode 100644 index 000000000..721df8e15 --- /dev/null +++ b/controller_manager/src/controller_stopper_node.cpp @@ -0,0 +1,53 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright 2019 FZI Forschungszentrum Informatik +// Created on behalf of Universal Robots A/S +// +// 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 Willow Garage Inc, hiDOF Inc, 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 OWNER 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. +////////////////////////////////////////////////////////////////////////////// + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2019-06-12 + * + */ +//---------------------------------------------------------------------- + +#include + +#include + +int main(int argc, char** argv) +{ + // Set up ROS. + ros::init(argc, argv, "controller_stopper_node"); + ros::NodeHandle nh; + ros::NodeHandle priv_nh(""); + + controller_manager::ControllerStopper stopper(nh); + + ros::spin(); + return 0; +} From 095e96294daad1449cd3bb215f3493ce74c6b94b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 31 Jan 2023 21:32:46 +0000 Subject: [PATCH 2/2] Add small documentation to controller_stopper to README --- controller_manager/README.md | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/controller_manager/README.md b/controller_manager/README.md index 95a2473cd..4886323b5 100644 --- a/controller_manager/README.md +++ b/controller_manager/README.md @@ -5,3 +5,34 @@ infrastructure to load, unload, start and stop controllers. Detailed user documentation can be found in the package's [ROS wiki page](http://wiki.ros.org/controller_manager). + +### 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* get stopped. If status returns to `true` the stopped controllers are restarted. + +For this, the package provides the `controller_stopper_node` executable. + +#### Parameters + +##### consistent_controllers (default: ["joint_state_controller"]) + +Consistent controllers will not be stopped when the robot stops. Defaults to ["joint_state_controller"] + +#### Service Clients + +##### controller_manager/list_controllers ([controller_manager_msgs/ListControllers](http://docs.ros.org/api/controller_manager_msgs/html/srv/ListControllers.html)) + +Controller manager service to list controllers + +##### controller_manager/switch_controller ([controller_manager_msgs/SwitchController](http://docs.ros.org/api/controller_manager_msgs/html/srv/SwitchController.html)) + +Controller manager service to switch controllers + +#### Subscribed topics + +##### robot_running ([std_msgs/Bool](http://docs.ros.org/api/std_msgs/html/msg/Bool.html)) + +Subscribes 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. +