为算法控制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[];
struct input_event t;
keys_fd=open(DEV_PATH, O_RDWR);
if(keys_fd <= )
{
printf("open /dev/input/event3 device error!\n");
return -;
}
while()
{
static int temp=;
//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== || t.value==)
{
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 ;
}
可以检测并显示 按键的按下和松开.
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) != )
return -;
return ;
}
static __inline
int tty_set(void)
{
if ( tcgetattr(STDIN_FILENO, &ori_attr) )
return -;
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] = ;
cur_attr.c_cc[VTIME] = ;
if (tcsetattr(STDIN_FILENO, TCSANOW, &cur_attr) != )
return -;
return ;
}
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(, &rfds);
/* Wait up to five seconds. */
tv.tv_sec = ;
tv.tv_usec = ;
retval = select(, &rfds, NULL, NULL, &tv);
/* Don't rely on the value of tv now! */
if (retval == -) {
perror("select()");
return ;
} else if (retval)
return ;
/* FD_ISSET(0, &rfds) will be true. */
else
return ;
return ;
}
int main()
{
int tty_set_flag;
tty_set_flag = tty_set();
while() {
if( kbhit() ) {
const int key = getchar();
printf("%c -----------pressed\n", key);
if(key == 'q')break;
} else {
fprintf(stderr, "<no key detected>\n");
}
int i=,j=;///这样可以很明显的看出显示效果
while(i++<)
{
while(j++<);j=;
while(j++<);j=;
while(j++<);j=;
while(j++<);}
}
if(tty_set_flag == )
tty_reset();
return ;
}
这两段程序非原创.
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 ;}
}
else
{
fprintf(stderr, "<no key detected>\n");
}
int i=,j=;///这样可以很明显的看出效果非阻塞
while(i++<)
{
while(j++<);
j=;
while(j++<);
j=;
while(j++<);
j=;
while(j++<);
}
if(tty_set_flag == )
tty_reset();
return ;
}
.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) != )
return -;
return ;
}
static __inline
int tty_set(void)
{
if ( tcgetattr(STDIN_FILENO, &ori_attr) )
return -;
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] = ;
cur_attr.c_cc[VTIME] = ;
if (tcsetattr(STDIN_FILENO, TCSANOW, &cur_attr) != )
return -;
return ;
}
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(, &rfds);
/* Wait up to five seconds. */
tv.tv_sec = ;
tv.tv_usec = ;
retval = select(, &rfds, NULL, NULL, &tv);
/* Don't rely on the value of tv now! */
if (retval == -) {
perror("select()");
return ;
} else if (retval)
return ;
/* FD_ISSET(0, &rfds) will be true. */
else
return ;
return ;
}
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.
![ROS基础四之roscpp/rospy节点编写[图]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)