控制kobuki底盘(一)

为算法控制kobuki底盘所做的准备

(一)aicrobo的成果

有了catkin_ws里aicrobo_xi系列的package后,分别用不同ternimal启动:依次为虚拟底盘,rviz,键盘控制

roslaunch aicroboxi_bringup fake_aicroboxi.launch
roslaunch aicroboxi_rviz view_mobile.launch
roslaunch kobuki_keyop keyop.launch

使用键盘时会提示,是否enable the motor ,按e使能电机后可使用方向键控制.
aicrobo写的键盘控制:

roslaunch aicroboxi_teleop keyboard_teleop.launch

按 uio/jkl/m,./ 控制底盘.

(二)三次启动太麻烦,合成一次

cd ~/catkin_ws/src/aicrobo_xi/
catkin_create_pkg aicrobo_newteleop roscpp rospy std_msgs geometry_msgs/Twist

这样创建的newteleop包下依赖roscpp,rospy,std_msgs等,创建过后产生CmakeLists.txt和package.xml文件.新建一个launch文件夹,在里面建立一个newkeyop.launch的文件:把上面的启动的三个文件的launch组合在一个文件里.

(三)写一个自己的按键控制代替ros::ok()
1)堵塞式按键扫描将影响其他进程

//堵塞式按键检测
#include <stdio.h>  //printf() setbuf()
#include <linux/input.h> //input_event  EV_KEY
#include <sys/types.h>//open() 
#include <sys/stat.h> //open()
#include <fcntl.h>    //open()
#include <unistd.h>   //read(),close()  
#define DEV_PATH "/dev/input/event3"   //difference is possible  
int main()  
{  
   int qqq=subfunc();
    int keys_fd;  
    char ret[2];  
    struct input_event t;  
    keys_fd=open(DEV_PATH, O_RDWR);  
    if(keys_fd <= 0)  
    {  
        printf("open /dev/input/event3 device error!\n");  
        return -1;  
    }  
  while(1)  
  {   
        static int temp=0;
        //printf("1r:--keys_fd:%d\n",keys_fd);//证明了这是堵塞式的按键read
        temp=read(keys_fd, &t, sizeof(t));//堵塞式的ssize_t read(int fd, void *buf, size_t count);
        //printf("2r:--temp:%d\n",temp); 

        if(temp==sizeof(t))
        {   //printf("3r:--type:%d\n",t.type);
            if(t.type==EV_KEY) //EV_KEY=1,为键盘事件.type=4为其他,type=0可能为延续
        {   
                //printf("4r:--type==EV_KEY\n"); 
                if(t.value==0 || t.value==1)  
                {  
                    printf("code:%d %s\n", t.code, (t.value) ? "Pressed" : "Released");  
                    if(t.code == KEY_ESC)break;  
                }
            }  
        }   
  }
  close(keys_fd);
/*  这段无作用
    {//清空键盘缓冲区//stdio//
    FILE *fp;
    fp=fopen("/dev/input/event3","w");
    setbuf(fp, NULL);
    } 
*/ 
    return 0;  
} 

可以检测并显示 按键的按下和松开.
2)非堵塞式按键扫描不影响其他工作

#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <termios.h>

static struct termios ori_attr, cur_attr;

static __inline
int tty_reset(void)
{
        if (tcsetattr(STDIN_FILENO, TCSANOW, &ori_attr) != 0)
                return -1;

        return 0;
}


static __inline
int tty_set(void)
{

        if ( tcgetattr(STDIN_FILENO, &ori_attr) )
                return -1;

        memcpy(&cur_attr, &ori_attr, sizeof(cur_attr) );
        cur_attr.c_lflag &= ~ICANON;
//        cur_attr.c_lflag |= ECHO;
        cur_attr.c_lflag &= ~ECHO;
        cur_attr.c_cc[VMIN] = 1;
        cur_attr.c_cc[VTIME] = 0;

        if (tcsetattr(STDIN_FILENO, TCSANOW, &cur_attr) != 0)
                return -1;

        return 0;
}

static __inline
int kbhit(void)
{

        fd_set rfds;
        struct timeval tv;
        int retval;

        /* Watch stdin (fd 0) to see when it has input. */
        FD_ZERO(&rfds);
        FD_SET(0, &rfds);
        /* Wait up to five seconds. */
        tv.tv_sec  = 0;
        tv.tv_usec = 0;

        retval = select(1, &rfds, NULL, NULL, &tv);
        /* Don't rely on the value of tv now! */

        if (retval == -1) {
                perror("select()");
                return 0;
        } else if (retval)
                return 1;
        /* FD_ISSET(0, &rfds) will be true. */
        else
                return 0;
        return 0;
}

int main()
{
        int tty_set_flag;
        tty_set_flag = tty_set();

        while(1) {

                if( kbhit() ) {
                        const int key = getchar();
                        printf("%c -----------pressed\n", key);
                        if(key == 'q')break;
                } else {
                        fprintf(stderr, "<no key detected>\n");
                }
 int i=0,j=0;///这样可以很明显的看出显示效果
 while(i++<1000)
 {
   while(j++<5000);j=0;
   while(j++<5000);j=0;
   while(j++<5000);j=0;
   while(j++<5000);}        
 }
        if(tty_set_flag == 0)
                tty_reset();
        return 0;
}

这两段程序非原创.
3)写成函数,放在单独的cpp文件中,生成被包含的头文件.
.cpp文件

#include "FeiDuSeExit.h"
bool feiDuSeExit()
{
     int tty_set_flag;
     tty_set_flag = tty_set();

     if( kbhit() )
      {
        const int key = getchar();
        printf("%c -------------------------pressed\n", key);
        if(key == 'q'){tty_reset();return 0;}
      } 
     else 
      {
       fprintf(stderr, "<no key detected>\n");
      }

     int i=0,j=0;///这样可以很明显的看出效果非阻塞
     while(i++<1000)
    {
         while(j++<5000);
     j=0;
     while(j++<5000);
     j=0;
     while(j++<5000);
     j=0;
     while(j++<5000);
    }               
     if(tty_set_flag == 0)
                tty_reset();
        return 1;
}

.h文件

#ifndef FEIDUSEEXIT_H
#define FEIDUSEEXIT_H
#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <termios.h>

static struct termios ori_attr, cur_attr;

static __inline
int tty_reset(void)
{
        if (tcsetattr(STDIN_FILENO, TCSANOW, &ori_attr) != 0)
                return -1;
        return 0;
}


static __inline
int tty_set(void)
{

        if ( tcgetattr(STDIN_FILENO, &ori_attr) )
                return -1;

        memcpy(&cur_attr, &ori_attr, sizeof(cur_attr) );
        cur_attr.c_lflag &= ~ICANON;
//        cur_attr.c_lflag |= ECHO;
        cur_attr.c_lflag &= ~ECHO;
        cur_attr.c_cc[VMIN] = 1;
        cur_attr.c_cc[VTIME] = 0;

        if (tcsetattr(STDIN_FILENO, TCSANOW, &cur_attr) != 0)
                return -1;

        return 0;
}

static __inline
int kbhit(void)
{

        fd_set rfds;
        struct timeval tv;
        int retval;

        /* Watch stdin (fd 0) to see when it has input. */
        FD_ZERO(&rfds);
        FD_SET(0, &rfds);
        /* Wait up to five seconds. */
        tv.tv_sec  = 0;
        tv.tv_usec = 0;

        retval = select(1, &rfds, NULL, NULL, &tv);
        /* Don't rely on the value of tv now! */

        if (retval == -1) {
                perror("select()");
                return 0;
        } else if (retval)
                return 1;
        /* FD_ISSET(0, &rfds) will be true. */
        else
                return 0;
        return 0;
}
bool  feiDuSeExit();
#endif

cmakelists.txt
加入类似的结构

add_executable(feiDuSeKaiHuanRoad src/FeiDuSeKaiHuanRoad.cpp src/FeiDuSeExit.cpp)

启动文件的名字:feiDuSeKaiHuanRoad 后面是包含的cpp

 target_link_libraries(feiDuSeKaiHuanRoad
   ${catkin_LIBRARIES}
 )

FeiDuSeExit.h文件放在catkin_ws的include文件里.
按键可被检测和显示返回ture,按q可以返回false.