控制程序中加入键盘控制

  又是哭瞎的调试过程，不是码农，不做很深入地追究，只想实现想要的效果，随便记录下。期待有更好的想法。

设计目标：方便小车调试，能用键盘控制小车急停与移动。

  刚开始是为了调试的时候防止小车出现不可控制的意外要能通过键盘急停，想用Ctrl+c退出的时候小车可以停下，于是添加了signal函数，在节点退出的时候发布速度为0的信息给base_controller，但是发现有时候可以有时候又不可以。由于通过.launch文件启动的节点，在Ctrl+c退出的时候各节点退出时间不固定，这时候发布速度话题很不稳定，这个方法不可取。

  以前在MFC中也用过键盘控制很简单，直接检测键盘事件即可。在ROS下也有键盘控制移动机器人的源码，但是由于自己设计的控制部分是不断地发布速度话题的，如果采用控制一个节点键盘控制一个节点，发布的速度话题就很混乱，小车应该停不下来。暂时没有很深入地考虑一定采用这种方式的解决方案。由于本来的键盘控制源码里面是采用了线程的方式，所以考虑直接在控制程序中开一个专门用来处理键盘事件的线程。

  大部分直接copy源码就可以了，线程部分需要小改一下。键盘控制源码中是：

SmartCarKeyboardTeleopNode tbk;
boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop,&tbk));
ros::spin();
t.interrupt();
t.join();
tcsetattr(kfd, TCSANOW, &cooked);
return(0);

我的控制部分是：

int main(int argc, char** argv)
{
    ros::init(argc, argv, "tracking");
    ros::NodeHandle n_;
    ros::Subscriber odom_sub_ = n_.subscribe("odom", 10, odom_message);
    cmdvel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 10);
    ...
    ...
    while(ros::ok())
    {
        ...
        ...
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

将键盘控制加入主控制中：

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <boost/thread.hpp>
#include <csignal>
#include <termios.h>

boost::thread t；
int kfd=0;
struct termios cooked, raw;
void signalHandler(int signum)
{
    tcsetattr(kfd, TCSANOW, &cooked);
    ...
}
void callbackThread()
{
    ...(键盘的监听与发布控制命令)
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "tracking");
    ros::NodeHandle n_;
    ros::Subscriber odom_sub_ = n_.subscribe("odom", 10, odom_message);
    cmdvel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 10);
    ...
    t = boost::thread(callbackThread);
    signal(SIGINT, signalHandler);
    while(ros::ok())
    {
        ...
        ...
        ros::spinOnce();
        loop_rate.sleep();
    }
    （t.interrupt();）
    （t.join();）
    return 0;
}

  下面介绍一下我的想法与试验结果：

//线程中断
t.interrupt();
//用来指定当前主线程等待其他线程执行完毕后，再来继续执行t.join()后面的代码
t.join();

  看了wiki介绍的自定义消息队列 + 处理线程部分就是直接把这个放在while循环外的，也可以看看这个博客写的，很全了http://blog.chinaunix.net/uid-27875-id-5817906.html。从实质功能上，我的while循环里是主要控制算法部分，所以不可能让主线程阻塞等待子线程，所以也只能把这两句放到循环外。但其实放到while循环外也不可能会执行到，没有什么意义，所以我是直接把这两句删了。
  线程可以正常工作，能实现键盘控制，原来的回调函数也没有受到影响。但是！但是！但是！

//在程序结束时在恢复原来的配置
tcsetattr(kfd, TCSANOW, &cooked);

这句没有放到正常的位置，导致一直出现很奇怪的现象。Ctrl+C结束程序后，终端不显示键盘打的字符了，但其实盲打再按回车是执行程序的。刚开始怀疑是不是没有退出线程，还是主线程没有退出只是被挂起了。。。（走了很多弯路，因为一开始程序还不是上面的这个版本，根本不知道线程的部分应该怎么插入，搜了很多博客也没找到啊，心累。）后面通过查看进程和线程，确定是都退出了的。然后怀疑是键盘控制没有释放，再一步步调键盘里面的源码，最终确定上面这一句是关键。（以前看过代码解释的，都怪我忘了。）由于要在关闭节点之前才能恢复键盘原来的配置，不能放在while循环外也不能放在循环内，循环外不起作用，循环内则线程失效，所以添加signal函数，在退出节点之前恢复键盘控制。经过测试，一切OK。