目录

1.点动

2.长动

3.esp32的arduino端代码

4.效果展示

参考链接

这里的控制通过按键盘上的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.效果展示

bilibili html5 player

esp32与ros局域网话题通信IO口控制

参考链接

1.查找input_event为键盘事件时的键值:

srtruct input_event学习_To_run_away的博客-CSDN博客

2.input_event结构体事件详解(keyvalue)

input_event结构体详解_liwei405499的专栏-CSDN博客_input_event结构体

struct input_event_gao5528的博客-CSDN博客

3.键盘事件监听

Ubuntu下的一次捕捉键盘输入的实验_utt@Delimola的博客-CSDN博客

Linux下C语言检测多个按键按下状态的方法_星光-CSDN博客

4.文件读取

linux read() 函数_Cs121-CSDN博客_linux read函数

深入理解linux下write()和read()函数_hhhlizhao的博客-CSDN博客_write

5.termios库对终端的设置

Linux中不显示输入的字符的实现-hyfeng18-ChinaUnix博客

termios结构体详解_薰衣草PK向日葵的博客-CSDN博客_termios结构体