ROS C++程序终止/结束进程&& 多线程终止运行程序

Y

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

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

W

首先利用signal()函数将stopCmdSigintHandler注册为SIGINT信号的处理程序
signal(SIGINT,stopCmdSigintHandler);/* Interactive attention signal. */
注意如果这个函数是类内的成员函数，那么需要将其定义为静态成员函数
static void stopCmdSigintHandler(int signum);
这是因为signal()函数需要一个C风格的函数指针作为参数，而非成员函数指针
一般定义如下

void stopCmdSigintHandler(int signum)
{
    std::cout << "stop robot" << std::endl;
    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();
}

stop_track_thread用于终止多线程程序，在线程循环中周期性地检查stop_track_thread标志。当stop_track_thread标志被设置为true时，线程会退出循环并终止。需要设置为原子变量多线程函数示例

    void thread_func()
    {
        // register the signal handler
        signal(SIGINT, stopCmdSigintHandler);

        while(!stop_track_thread)
        {
            // do some work here
        }
    }

这个函数还可以停止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 object
        spinner->stop();

        // publish a zero velocity command
        geometry_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 node
        ros::shutdown();
    }
}

int main(int argc, char** argv)
{
    // initialize the ROS node
    ros::init(argc, argv, "my_node");

    // create a ROS node handle
    ros::NodeHandle nh;

    // create a ROS Spinner object with 4 threads
    spinner = new ros::AsyncSpinner(4);

    // register the signal handler
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);

    // create a publisher for the cmd_vel topic
    cmd_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 object
    spinner->start();

    // start the ROS node
    ros::waitForShutdown();

    return 0;
}

rostopic echo /cmd_vel

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

可以看到终止后发布停止命令
虽然测试能行，但我的小车导航并未就此停止，
为什么？我干了什么！曾经一度以为是我程序的问题
后来休息一会儿，发现原因出在
<remap from='/cmd_vel' to='nav_vel'/>
没错我把twist_mux多路切换器启动包含在原有的launch文件中了，导致停止的时候一起停止了，分开启动就没问题了