diff --git a/controller_stopper/CMakeLists.txt b/controller_stopper/CMakeLists.txt new file mode 100644 index 0000000..64caa2e --- /dev/null +++ b/controller_stopper/CMakeLists.txt @@ -0,0 +1,202 @@ +cmake_minimum_required(VERSION 2.8.3) +project(controller_stopper) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + controller_manager_msgs + roscpp + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# controller_manager_msgs# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES controller_stopper +CATKIN_DEPENDS controller_manager_msgs roscpp std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/controller_stopper.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +add_executable(${PROJECT_NAME}_node + src/controller_stopper.cpp + src/controller_stopper_node.cpp +) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME}_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_controller_stopper.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/controller_stopper/README.md b/controller_stopper/README.md new file mode 100644 index 0000000..21bf1c7 --- /dev/null +++ b/controller_stopper/README.md @@ -0,0 +1,32 @@ +# 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. + +## Nodes +### controller_stopper_node + + + +#### Parameters + * **"consistent_controllers (list of strings)"** + + 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. + diff --git a/controller_stopper/include/controller_stopper/controller_stopper.h b/controller_stopper/include/controller_stopper/controller_stopper.h new file mode 100644 index 0000000..8f247a2 --- /dev/null +++ b/controller_stopper/include/controller_stopper/controller_stopper.h @@ -0,0 +1,49 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-06-12 + * + */ +//---------------------------------------------------------------------- +#ifndef CONTROLLER_STOPPER_CONTROLLER_STOPPER_H_INCLUDED +#define CONTROLLER_STOPPER_CONTROLLER_STOPPER_H_INCLUDED + +#include +#include + +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_; +}; +#endif // ifndef CONTROLLER_STOPPER_CONTROLLER_STOPPER_H_INCLUDED diff --git a/controller_stopper/package.xml b/controller_stopper/package.xml new file mode 100644 index 0000000..b273cbd --- /dev/null +++ b/controller_stopper/package.xml @@ -0,0 +1,64 @@ + + + controller_stopper + 0.0.1 + A small helper node that stops and restarts ROS controllers based on a boolean status topic. + + + + + Felix Mauch + + + + + + Apache 2.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + controller_manager_msgs + roscpp + std_msgs + controller_manager_msgs + roscpp + std_msgs + controller_manager_msgs + roscpp + std_msgs + + + + diff --git a/controller_stopper/src/controller_stopper.cpp b/controller_stopper/src/controller_stopper.cpp new file mode 100644 index 0000000..d640621 --- /dev/null +++ b/controller_stopper/src/controller_stopper.cpp @@ -0,0 +1,110 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-06-12 + * + */ +//---------------------------------------------------------------------- + +#include + +#include +#include + +#include + +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; +} diff --git a/controller_stopper/src/controller_stopper_node.cpp b/controller_stopper/src/controller_stopper_node.cpp new file mode 100644 index 0000000..128fd7f --- /dev/null +++ b/controller_stopper/src/controller_stopper_node.cpp @@ -0,0 +1,30 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@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(""); + + ControllerStopper stopper(nh); + + ros::spin(); + return 0; +} diff --git a/ur_rtde_driver/launch/ur10_ros_control.launch b/ur_rtde_driver/launch/ur10_ros_control.launch index 19831dc..3283a7c 100644 --- a/ur_rtde_driver/launch/ur10_ros_control.launch +++ b/ur_rtde_driver/launch/ur10_ros_control.launch @@ -58,4 +58,13 @@ + + + + - "joint_state_controller" + - "speed_scaling_state_controller" + - "force_torque_sensor_controller" + + +