Y

起因是在运行机器人的时候，我直接ctrl+C结束程序，但是机器人还在跑，我不得不跟着跑
好在有急停。

于是想写一个结束进程发布停止命令的程序

W

首先利用signal()函数将stopCmdSigintHandler注册为SIGINT信号的处理程序
signal(SIGINT,DynamicReplanFSM::stopCmdSigintHandler);/* Interactive attention signal. */
注意这个函数是类内的静态成员函数
static void stopCmdSigintHandler(int signum);

void DynamicReplanFSM::stopCmdSigintHandler(int signum)
{std::cout << "stop robot" << std::endl;stop_track_thread=true;//用于终止多线程程序，在线程循环中周期性地检查stop_track_thread标志。当stop_track_thread标志被设置为true时，线程会退出循环并终止。需要设置为原子变量spinner->stop();  //geometry_msgs::Twist cmd_msg;cmd_msg.linear.x = 0.0;cmd_msg.linear.y = 0.0;cmd_msg.linear.z = 0.0;cmd_msg.angular.z = 0.0;cmd_msg.angular.y = 0.0;cmd_msg.angular.x = 0.0;cmd_vel.publish(cmd_msg);cmd_vel.publish(cmd_msg);sleep(1);cmd_vel.publish(cmd_msg);ROS_INFO("Command | X: % .2f | Y: % .2f | Z: % .2f | Roll: % .2f | ""Pitch: % .2f | Yaw: % .2f ",cmd_msg.linear.x, cmd_msg.linear.y, cmd_msg.linear.z,cmd_msg.angular.x, cmd_msg.angular.y, cmd_msg.angular.z);std::cout << "pub cmd to robot" << std::endl;// ros::shutdown();// ros::waitForShutdown();
}

这个函数还可以停止Ros Spinner对象，如果不用，可以注释掉
示例程序如下

#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>ros::Publisher cmd_vel_pub;
ros::AsyncSpinner* spinner;void signal_handler(int signal)
{if(signal == SIGINT || signal == SIGTERM){// stop the ROS Spinner objectspinner->stop();// publish a zero velocity commandgeometry_msgs::Twist cmd_vel;cmd_vel.linear.x = 0.0;cmd_vel.angular.z = 0.0;cmd_vel_pub.publish(cmd_vel);// shutdown the ROS noderos::shutdown();}
}int main(int argc, char** argv)
{// initialize the ROS noderos::init(argc, argv, "my_node");// create a ROS node handleros::NodeHandle nh;// create a ROS Spinner object with 4 threadsspinner = new ros::AsyncSpinner(4);// register the signal handlersignal(SIGINT, signal_handler);signal(SIGTERM, signal_handler);// create a publisher for the cmd_vel topiccmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);geometry_msgs::Twist cmd_vel;cmd_vel.linear.x = 0.0;cmd_vel.angular.z = 0.6;int i=0;ros::Rate rate(10);while(i++<100){std::cout<<"i = "<<i<<std::endl;cmd_vel_pub.publish(cmd_vel);rate.sleep();}// ...// start the ROS Spinner objectspinner->start();// start the ROS noderos::waitForShutdown();return 0;
}

rostopic echo /cmd_vel

linear: x: 0.0y: 0.0z: 0.0
angular: x: 0.0y: 0.0z: 0.6
---
linear: x: 0.0y: 0.0z: 0.0
angular: x: 0.0y: 0.0z: 0.6
---
linear: x: 0.0y: 0.0z: 0.0
angular: x: 0.0y: 0.0z: 0.6
---
linear: x: 0.0y: 0.0z: 0.0
angular: x: 0.0y: 0.0z: 0.0

可以看到终止后发布停止命令