ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
There are two normal ways to tell a node to shut down:
roslaunch
or when you do Ctrl-C
.rosnode kill
.If you want to cover all your bases, you need to override roscpp's handlers for both. In practice, most people seem to just override SIGINT, probably because it's actually documented and rosnode kill
is less-used.
What roscpp really needs is an "on shutdown" callback. That's ticketed here: https://code.ros.org/trac/ros/ticket/3417
In the meantime, it is possible (if a little messy) to do this correctly. An example is the nodelet.cpp code. The relevant parts look like:
#include <signal.h>
#include <ros/ros.h>
#include <ros/xmlrpc_manager.h>
// Signal-safe flag for whether shutdown is requested
sig_atomic_t volatile g_request_shutdown = 0;
// Replacement SIGINT handler
void mySigIntHandler(int sig)
{
g_request_shutdown = 1;
}
// Replacement "shutdown" XMLRPC callback
void shutdownCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
{
int num_params = 0;
if (params.getType() == XmlRpc::XmlRpcValue::TypeArray)
num_params = params.size();
if (num_params > 1)
{
std::string reason = params[1];
ROS_WARN("Shutdown request received. Reason: [%s]", reason.c_str());
g_request_shutdown = 1; // Set flag
}
result = ros::xmlrpc::responseInt(1, "", 0);
}
int main(int argc, char** argv)
{
// Override SIGINT handler
ros::init(argc, argv, "MyNode", ros::init_options::NoSigintHandler);
signal(SIGINT, mySigIntHandler);
// Override XMLRPC shutdown
ros::XMLRPCManager::instance()->unbind("shutdown");
ros::XMLRPCManager::instance()->bind("shutdown", shutdownCallback);
// Create publishers, subscribers, etc.
// Do our own spin loop
while (!g_request_shutdown)
{
// Do non-callback stuff
ros::spinOnce();
usleep(100000);
}
// Do pre-shutdown tasks
ros::shutdown();
}