控制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.