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文件中了,导致停止的时候一起停止了,分开启动就没问题了