目录
1.点动
2.长动
3.esp32的arduino端代码
4.效果展示
参考链接
2121-10-07
这里的控制通过按键盘上的wsad达到小车前后左右的效果。
长动，顾名思义，就是按一下w，小车就开始前进，知道输入停止按键，小车就停止。
与之对应的是点动，就是按住w按键，小车开始前进，直到松开按键，小车就停止。
我们通过/command这个话题来传递命令消息，点动与长动只有上位机（pc）上的程序不同，esp32端的程序是一样的。
1.点动
直接看代码，点动的话，当我们在键盘终端敲击一下w这个按键，则程序会往/command话题发送一个话题消息，esp32接受到消息之后，解析消息数据从而判断命令，再驱动电机控制小车动作。
//读取键盘输入
#include <ros/ros.h>
#include <std_msgs/UInt16.h>
#include <stdio.h>
#include <unistd.h>
#include <linux/input.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termio.h>
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"teleop_controlCar");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<std_msgs::UInt16>("/command",1000);
std_msgs::UInt16 command_car;
command_car.data=0;
ros::Rate r(50);
printf("----Control panel----\n");
printf("Please input command!\n");
printf(" w \n");
printf("a s d\n");
//设置终端参数,关闭终端回显
struct termios new_settings;
tcgetattr(0,&new_settings);
new_settings.c_lflag &= (~ECHO);
new_settings.c_lflag &= (~ICANON);
new_settings.c_cc[VTIME] = 0;
new_settings.c_cc[VMIN] = 1;
tcsetattr(0,TCSANOW,&new_settings);
//以下为键盘输入检测
int fd_kb;
int flag_ke=1;//0:按下 1:释放;
struct input_event event_kb;
fd_kb = open("/dev/input/event2", O_RDONLY); //键盘输入
if (fd_kb <= 0)
{
printf("open device error\n");
return 0;
}
while (ros::ok())
{
if (read(fd_kb, &event_kb, sizeof(event_kb)) == sizeof(event_kb)) //event_kb有24个字节
{
if (event_kb.type == EV_KEY)
{
if (event_kb.value == 1) //1表示按下，0表示释放
{
if(flag_ke==1)
{
switch(event_kb.code)
{
case KEY_W:command_car.data=1;pub.publish(command_car);break;
case KEY_A:command_car.data=3;pub.publish(command_car);break;
case KEY_S:command_car.data=2;pub.publish(command_car);break;
case KEY_D:command_car.data=4;pub.publish(command_car);break;
case KEY_KP1:command_car.data=5;pub.publish(command_car);break;
case KEY_KP2:command_car.data=6;pub.publish(command_car);break;
case KEY_KP4:command_car.data=7;pub.publish(command_car);break;
case KEY_KP5:command_car.data=8;pub.publish(command_car);break;
case KEY_KP7:command_car.data=9;pub.publish(command_car);break;
case KEY_KP8:command_car.data=10;pub.publish(command_car);break;
case KEY_Q:
new_settings.c_lflag &= ECHO;
new_settings.c_lflag &= ICANON;
tcsetattr(0,TCSANOW,&new_settings);
printf("Finished!");
close(fd_kb);
return 0;
default:command_car.data=0;pub.publish(command_car);
}
flag_ke=0;
}
else
flag_ke=0;
}
else if(event_kb.value == 0)
{
if(flag_ke==0)
{
command_car.data=0;
pub.publish(command_car);
flag_ke=1;
}
else
flag_ke=1;
}
}
}
}
close(fd_kb);
return 0;
}
2.长动
长动的作用机制，当我们按下键盘上的w这个按键的时候，程序会往/command这个话题发送一个消息，数据内容为1（即前进），这个时候esp32就会受到话题消息，然后控制小车前进，当我们松开键盘，则程序立即往/command话题发布停止的消息，esp32就立即控制小车停止。
#include <ros/ros.h>
#include <std_msgs/UInt16.h>
#include <termio.h>
#include <stdio.h>
#define KEYCODE_W 0x77 //command 1 forward
#define KEYCODE_A 0x61 //command 3 left
#define KEYCODE_S 0x73 //command 2 backward
#define KEYCODE_D 0x64 //command 4 right
#define KEYCODE_1 0x31 //command 5 motor1 right
#define KEYCODE_2 0x32 //command 6 motor1 left
#define KEYCODE_4 0x34 //command 7 motor2 right
#define KEYCODE_5 0x35 //command 8 motor2 left
#define KEYCODE_7 0x37 //command 9 motor3 right
#define KEYCODE_8 0x38 //command 10 motor3 left
int scanKeyboard()
{
int in;
struct termios new_settings;
struct termios stored_settings;
//设置终端参数
tcgetattr(0,&stored_settings);
new_settings = stored_settings;
new_settings.c_lflag &= (~ECHO);
new_settings.c_lflag &= (~ICANON);
new_settings.c_cc[VTIME] = 0;
tcgetattr(0,&stored_settings);
new_settings.c_cc[VMIN] = 1;
tcsetattr(0,TCSANOW,&new_settings);
in = getchar();
tcsetattr(0,TCSANOW,&stored_settings);
return in;
}
//测试函数
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"teleop_controlCar");
ros::NodeHandle nh;
//command 0:stop 1:forward 2:backward 3:left 4:right 5:motor1 6:motor2 7:motor3
ros::Publisher pub = nh.advertise<std_msgs::UInt16>("/command",1000);
std_msgs::UInt16 command_car;
command_car.data=0;
ros::Rate r(50);
while(ros::ok()){
switch(scanKeyboard())
{
case KEYCODE_W:command_car.data=1;break;
case KEYCODE_A:command_car.data=3;break;
case KEYCODE_S:command_car.data=2;break;
case KEYCODE_D:command_car.data=4;break;
case KEYCODE_1:command_car.data=5;break;
case KEYCODE_2:command_car.data=6;break;
case KEYCODE_4:command_car.data=7;break;
case KEYCODE_5:command_car.data=8;break;
case KEYCODE_7:command_car.data=9;break;
case KEYCODE_8:command_car.data=10;break;
default:command_car.data=0;
}
pub.publish(command_car);
r.sleep();
}
}
3.esp32的arduino端代码
#include <WiFi.h>
#include <ros.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/String.h>
#define LED_BUILTIN 2
#define delay_flash 5
//电机引脚定义
#define MOTO_1_PIN_1 14 //前后左右
#define MOTO_1_PIN_2 15
#define MOTO_2_PIN_1 13
#define MOTO_2_PIN_2 12
#define MOTO_3_PIN_1 2 //抬起下落
#define MOTO_3_PIN_2 0
#define MOTO_4_PIN_1 4
#define MOTO_4_PIN_2 16
#define MOTO_5_PIN_1 17 //吊臂弯曲伸直
#define MOTO_5_PIN_2 5
#define MOTO_6_PIN_1 18
#define MOTO_6_PIN_2 19
#define MOTO_7_PIN_1 27 //挖斗挖掘放下
#define MOTO_7_PIN_2 26
#define MOTO_8_PIN_1 25
#define MOTO_8_PIN_2 33
//esp32作为sta模式,接入局域网路由器WiFi名和密码
const char* ssid = "Zhitong";
const char* password = "95359897";
IPAddress server(192, 168, 191, 2); // ROS server的IP
ros::NodeHandle nh;
std_msgs::String str_msg;
std_msgs::UInt16 command_car;
ros::Publisher chatter("chatter", &str_msg);
char hello[20] = "ESP32 wifi alive!";
WiFiClient client;
class WiFiHardware {
public:
WiFiHardware() {};
void init() {
// do your initialization here. this probably includes TCP server/client setup
client.connect(server, 11411);
}
// read a byte from the serial port. -1 = failure
int read() {
// implement this method so that it reads a byte from the TCP connection and returns it
// you may return -1 is there is an error; for example if the TCP connection is not open
return client.read(); //will return -1 when it will works
}
// write data to the connection to ROS
void write(uint8_t* data, int length) {
// implement this so that it takes the arguments and writes or prints them to the TCP connection
for(int i=0; i<length; i++)
client.write(data[i]);
}
// returns milliseconds since start of program
unsigned long time() {
return millis(); // easy; did this one for you
}
};
//初始化WiFi
void setupWiFi()
{
WiFi.begin(ssid, password);
Serial.print("\nConnecting to "); Serial.println(ssid);
uint8_t i = 0;
while (WiFi.status() != WL_CONNECTED) {
i++;
delay(500);
Serial.print(".");
if (i > 120) { //60秒后如果还是连接不上，就判定为连接超时
Serial.print("连接超时！请检查网络环境");
break;
}
}
if(i == 21){
Serial.print("Could not connect to"); Serial.println(ssid);
while(1) delay(500);
}
Serial.print("Ready! Use ");
Serial.print(WiFi.localIP());
Serial.println(" to access client");
}
void init_motorPin(){
pinMode(MOTO_1_PIN_1,OUTPUT);
pinMode(MOTO_1_PIN_2,OUTPUT);
pinMode(MOTO_2_PIN_1,OUTPUT);
pinMode(MOTO_2_PIN_2,OUTPUT);
pinMode(MOTO_3_PIN_1,OUTPUT);
pinMode(MOTO_3_PIN_2,OUTPUT);
pinMode(MOTO_4_PIN_1,OUTPUT);
pinMode(MOTO_4_PIN_2,OUTPUT);
pinMode(MOTO_5_PIN_1,OUTPUT);
pinMode(MOTO_5_PIN_2,OUTPUT);
pinMode(MOTO_6_PIN_1,OUTPUT);
pinMode(MOTO_6_PIN_2,OUTPUT);
pinMode(MOTO_7_PIN_1,OUTPUT);
pinMode(MOTO_7_PIN_2,OUTPUT);
pinMode(MOTO_8_PIN_1,OUTPUT);
pinMode(MOTO_8_PIN_2,OUTPUT);
delay(delay_flash);
stop_car();
}
void forward_car(){
digitalWrite(MOTO_1_PIN_1,HIGH);
digitalWrite(MOTO_1_PIN_2,LOW);
digitalWrite(MOTO_2_PIN_1,HIGH);
digitalWrite(MOTO_2_PIN_2,LOW);
digitalWrite(MOTO_3_PIN_1,HIGH);
digitalWrite(MOTO_3_PIN_2,LOW);
digitalWrite(MOTO_4_PIN_1,HIGH);
digitalWrite(MOTO_4_PIN_2,LOW);
delay(delay_flash);
digitalWrite(MOTO_1_PIN_1,HIGH);
digitalWrite(MOTO_1_PIN_2,LOW);
digitalWrite(MOTO_2_PIN_1,HIGH);
digitalWrite(MOTO_2_PIN_2,LOW);
digitalWrite(MOTO_3_PIN_1,HIGH);
digitalWrite(MOTO_3_PIN_2,LOW);
digitalWrite(MOTO_4_PIN_1,HIGH);
digitalWrite(MOTO_4_PIN_2,LOW);
}
void backward_car(){
digitalWrite(MOTO_1_PIN_1,LOW);
digitalWrite(MOTO_1_PIN_2,HIGH);
digitalWrite(MOTO_2_PIN_1,LOW);
digitalWrite(MOTO_2_PIN_2,HIGH);
digitalWrite(MOTO_3_PIN_1,LOW);
digitalWrite(MOTO_3_PIN_2,HIGH);
digitalWrite(MOTO_4_PIN_1,LOW);
digitalWrite(MOTO_4_PIN_2,HIGH);
delay(delay_flash);
digitalWrite(MOTO_1_PIN_1,LOW);
digitalWrite(MOTO_1_PIN_2,HIGH);
digitalWrite(MOTO_2_PIN_1,LOW);
digitalWrite(MOTO_2_PIN_2,HIGH);
digitalWrite(MOTO_3_PIN_1,LOW);
digitalWrite(MOTO_3_PIN_2,HIGH);
digitalWrite(MOTO_4_PIN_1,LOW);
digitalWrite(MOTO_4_PIN_2,HIGH);
}
void left_car(){
digitalWrite(MOTO_1_PIN_1,LOW);
digitalWrite(MOTO_1_PIN_2,HIGH);
digitalWrite(MOTO_2_PIN_1,HIGH);
digitalWrite(MOTO_2_PIN_2,LOW);
digitalWrite(MOTO_3_PIN_1,HIGH);
digitalWrite(MOTO_3_PIN_2,LOW);
digitalWrite(MOTO_4_PIN_1,LOW);
digitalWrite(MOTO_4_PIN_2,HIGH);
delay(delay_flash);
digitalWrite(MOTO_1_PIN_1,LOW);
digitalWrite(MOTO_1_PIN_2,HIGH);
digitalWrite(MOTO_2_PIN_1,HIGH);
digitalWrite(MOTO_2_PIN_2,LOW);
digitalWrite(MOTO_3_PIN_1,HIGH);
digitalWrite(MOTO_3_PIN_2,LOW);
digitalWrite(MOTO_4_PIN_1,LOW);
digitalWrite(MOTO_4_PIN_2,HIGH);
}
void right_car(){
digitalWrite(MOTO_1_PIN_1,HIGH);
digitalWrite(MOTO_1_PIN_2,LOW);
digitalWrite(MOTO_2_PIN_1,LOW);
digitalWrite(MOTO_2_PIN_2,HIGH);
digitalWrite(MOTO_3_PIN_1,LOW);
digitalWrite(MOTO_3_PIN_2,HIGH);
digitalWrite(MOTO_4_PIN_1,HIGH);
digitalWrite(MOTO_4_PIN_2,LOW);
delay(delay_flash);
digitalWrite(MOTO_1_PIN_1,HIGH);
digitalWrite(MOTO_1_PIN_2,LOW);
digitalWrite(MOTO_2_PIN_1,LOW);
digitalWrite(MOTO_2_PIN_2,HIGH);
digitalWrite(MOTO_3_PIN_1,LOW);
digitalWrite(MOTO_3_PIN_2,HIGH);
digitalWrite(MOTO_4_PIN_1,HIGH);
digitalWrite(MOTO_4_PIN_2,LOW);
}
void motor5_right(){
digitalWrite(MOTO_5_PIN_1,HIGH);
digitalWrite(MOTO_5_PIN_2,LOW);
delay(delay_flash);
digitalWrite(MOTO_5_PIN_1,HIGH);
digitalWrite(MOTO_5_PIN_2,LOW);
}
void motor5_left(){
digitalWrite(MOTO_5_PIN_1,LOW);
digitalWrite(MOTO_5_PIN_2,HIGH);
delay(delay_flash);
digitalWrite(MOTO_5_PIN_1,LOW);
digitalWrite(MOTO_5_PIN_2,HIGH);
}
void motor6_right(){
digitalWrite(MOTO_6_PIN_1,HIGH);
digitalWrite(MOTO_6_PIN_2,LOW);
delay(delay_flash);
digitalWrite(MOTO_6_PIN_1,HIGH);
digitalWrite(MOTO_6_PIN_2,LOW);
}
void motor6_left(){
digitalWrite(MOTO_6_PIN_1,LOW);
digitalWrite(MOTO_6_PIN_2,HIGH);
delay(delay_flash);
digitalWrite(MOTO_6_PIN_1,LOW);
digitalWrite(MOTO_6_PIN_2,HIGH);
}
void motor7_right(){
digitalWrite(MOTO_7_PIN_1,HIGH);
digitalWrite(MOTO_7_PIN_2,LOW);
delay(delay_flash);
digitalWrite(MOTO_7_PIN_1,HIGH);
digitalWrite(MOTO_7_PIN_2,LOW);
//pinMode(LED_BUILTIN,OUTPUT);
//digitalWrite(LED_BUILTIN,HIGH);
}
void motor7_left(){
digitalWrite(MOTO_7_PIN_1,LOW);
digitalWrite(MOTO_7_PIN_2,HIGH);
delay(delay_flash);
digitalWrite(MOTO_7_PIN_1,LOW);
digitalWrite(MOTO_7_PIN_2,HIGH);
//pinMode(LED_BUILTIN,OUTPUT);
//digitalWrite(LED_BUILTIN,LOW);
}
void stop_car(){
digitalWrite(MOTO_1_PIN_1,LOW);
digitalWrite(MOTO_1_PIN_2,LOW);
digitalWrite(MOTO_2_PIN_1,LOW);
digitalWrite(MOTO_2_PIN_2,LOW);
digitalWrite(MOTO_3_PIN_1,LOW);
digitalWrite(MOTO_3_PIN_2,LOW);
digitalWrite(MOTO_4_PIN_1,LOW);
digitalWrite(MOTO_4_PIN_2,LOW);
digitalWrite(MOTO_5_PIN_1,LOW);
digitalWrite(MOTO_5_PIN_2,LOW);
digitalWrite(MOTO_6_PIN_1,LOW);
digitalWrite(MOTO_6_PIN_2,LOW);
digitalWrite(MOTO_7_PIN_1,LOW);
digitalWrite(MOTO_7_PIN_2,LOW);
digitalWrite(MOTO_8_PIN_1,LOW);
digitalWrite(MOTO_8_PIN_2,LOW);
}
void control_car(const std_msgs::UInt16 &msg)
{
switch(msg.data)
{
case 0:stop_car();break;
case 1:forward_car();break;
case 2:backward_car();break;
case 3:left_car();break;
case 4:right_car();break;
case 5:motor5_right();break;
case 6:motor5_left();break;
case 7:motor6_right();break;
case 8:motor6_left();break;
case 9:motor7_right();break;
case 10:motor7_left();break;
default:stop_car();
}
}
ros::Subscriber<std_msgs::UInt16> sub("/command", &control_car);
void setup() {
Serial.begin(115200);
setupWiFi();
init_motorPin();
nh.getHardware()->setConnection(server);
nh.initNode();
nh.advertise(chatter);
nh.subscribe(sub);
}
void loop() {
//str_msg.data=hello;
//chatter.publish( &str_msg );
nh.spinOnce();
}
4.效果展示
esp32与ros局域网话题通信IO口控制
参考链接
1.查找input_event为键盘事件时的键值：
2.input_event结构体事件详解（keyvalue）
3.键盘事件监听
4.文件读取
5.termios库对终端的设置
Linux中不显示输入的字符的实现-hyfeng18-ChinaUnix博客
