如何使用esp32从零制作一个ROS2的teleop遥控器（cmd_vel）

Posted 2021-11-08 zhangrelay

使用课程所用镜像，硬件连线如下图所示：

使用io口34和36，对应的arduino端口如下列表：

static const uint8_t A0 = 36;
static const uint8_t A3 = 39;
static const uint8_t A4 = 32;
static const uint8_t A5 = 33;
static const uint8_t A6 = 34;
static const uint8_t A7 = 35;
static const uint8_t A10 = 4;
static const uint8_t A11 = 0;
static const uint8_t A12 = 2;
static const uint8_t A13 = 15;
static const uint8_t A14 = 13;
static const uint8_t A15 = 12;
static const uint8_t A16 = 14;
static const uint8_t A17 = 27;
static const uint8_t A18 = 25;
static const uint8_t A19 = 26;

分别是A0和A6，测试一下，是否ok。

void setup() {
  // initialize serial communication at 9600 bits per second:
  Serial.begin(9600);
}

// the loop routine runs over and over again forever:
void loop() {
  // read the input on analog pin 0:
  int sensorValue = analogRead(A0);
  // print out the value you read:
  Serial.println(sensorValue);
  delay(1);        // delay in between reads for stability
}

分别测一下A0和A6是否正常。

 

 参考如下两篇：

​​​​​​esp32(ROS2foxy)从字符串发布到速度发布turtlesim_zhangrelay的专栏-CSDN博客

esp32➡遥控篇➡turtlesim➡mobot➡turtlebot3_zhangrelay的专栏-CSDN博客

然后尝试写出如下代码：

#include <ros2arduino.h>
 
#include <WiFi.h>
#include <WiFiUdp.h>
#define SSID       "***"
#define SSID_PW    "***"
#define AGENT_IP   "***"
#define AGENT_PORT 2021 //AGENT port number
 
#define PUBLISH_FREQUENCY 2 //hz

int linx_mid = 0;
int angz_mid = 0;

int sensorlinx = 0;
int sensorangz = 0;
 
void publishVel(geometry_msgs::Twist* vel, void* arg)
{
  (void)(arg);
  static int cnt = 0;
  vel->linear.x = (sensorlinx-linx_mid)/1024.0;  //线速度
  vel->angular.z = (sensorangz-angz_mid)/1024.0; //角速度
  cnt++;
}
 
class VelPub : public ros2::Node
{
public:
  VelPub()
  : Node("esp32_cmdvel")
  {
    ros2::Publisher<geometry_msgs::Twist>* publisher_ = this->createPublisher<geometry_msgs::Twist>("turtle1/cmd_vel");
    this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishVel, nullptr, publisher_);
  }
};
 
WiFiUDP udp;
 
void setup() 
{
  WiFi.begin(SSID, SSID_PW);
  while(WiFi.status() != WL_CONNECTED);
 
  ros2::init(&udp, AGENT_IP, AGENT_PORT);
  linx_mid = analogRead(A6);
  angz_mid = analogRead(A0);
}
 
void loop() 
{
  
  sensorlinx = analogRead(A6);
  sensorangz = analogRead(A0);
  static VelPub VelNode;
  
  ros2::spin(&VelNode);
}

需要配置Micro-XRCE-DDS-Agent-1.3.0，在home文件下中新建ros/RobCode文件夹，并将编译好文件放入其中。

如果直接运行tool下工具会报错。需要安装此库。复制后即完成安装，如果不行，使用sudo make install。

正常后如下所示：

 先输入q退出，然后再输入如下命令行：

./MicroXRCEAgent udp4 --port 2021

完成配置。打开turtlesim。即可用esp32进行遥控。

---