机器人操作系统ROS—Arduino执行机构

发表评论

最近学习ROS，涉及底盘移动等执行机构时，发现买robot套件太贵了，又懒得从pi上直接开发，翻箱倒柜找到一个以前跟儿子一起做的arduino履带坦克，就拿它作ros机器人的action执行机构吧。

本文主要讲解如何使Arduino成为一个ROS的执行节点，与Pi上的控制模块通过ROS消息通信。

目录 隐藏
一、安装Rosserial Arduino Library
二、Arduino pub数据给ROS
三、Arduino sub来自ROS的指令并控制LED灯
四、ROS控制坦克移动

一、安装Rosserial Arduino Library

已经有人也这么想并做了一个lib库叫rosserial，它是用于非ROS设备与ROS设备进行通信的一种协议。它为非ROS设备的应用程序提供了ROS节点和服务的发布/订阅功能，使在非ROS环境中运行的应用能够通过串口或网络能够轻松地与ROS应用进行数据交互。

rosserial分为客户端和服务端两部分。rosserial客户端运行在arduino/stm32等没有ROS的终端设备中，通过串口或网络与运行在ROS环境中的rosserial服务端连接，最终通过服务端节点在ROS中发布/订阅话题。

这里我们把rosserial的arduino客户端安装到开发环境里：

安装后重启arduino，即可在“示例-Rosserial Arduino Library”里看到各种示例代码。

二、Arduino pub数据给ROS

1、参考“示例-Rosserial Arduino Library-pub/sub”，编写一个arduino pub消息到ros的代码并烧制到板子上：

vim pub.ino
/*
 * arduino ros pub示例
 */
 
#include <ros.h>
#include <std_msgs/String.h>

// ros节点句柄
ros::NodeHandle  nh;
// 传输消息定义
std_msgs::String str_msg;
// 实例化发布对象 (/tank/data为话题名称，str_msg为消息内容)
ros::Publisher pub("/tank/data", &str_msg);

char hello[13] = "I'm tank!";

void setup()
{
  // 初始化nh ROS节点
  nh.initNode();
  // nh节点使用pub发布对象来向世界中的话题发布消息
  nh.advertise(pub);
}
 
void loop()
{
  // 整理消息内容
  str_msg.data = hello;
  // 发布消息
  pub.publish( &str_msg );
  // 定频修正
  nh.spinOnce();
  delay(1000);
}

2、Pi上测试能否收到来自arduino板子的消息：

# Pi上安装ros-melodic-rosserial服务端端
sudo apt-get install -y ros-melodic-rosserial-arduino ros-melodic-rosserial

# 启动serial_node消息中转节点
roscore
rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0

# 监听arduino 发给serial_node topic的数据
rostopic echo /tank/data

# 查看节点关系图
rosrun rqt_graph rqt_graph

可以看到rostopic收到了来自arduino的消息，但是节点关系图中看不到arduino，只能看到pi上用于中转消息的serial_node节点：

三、Arduino sub来自ROS的指令并控制LED灯

1、Arduino发送数据并接受来自ROS控制led是否点亮的指令：

vim sub.ino

#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
long INFRARED_PLAY = 0x00FFA857;//红外遥控器上的PLAY键指令
long INFRARED_C = 0x00FFB04F;//红外遥控器上的C指令

// led引脚
int ledPin = 13;
// ros节点句柄
ros::NodeHandle nh;

// pub数据
std_msgs::String str_msg;
char hello[]="I'm tank!";
ros::Publisher pub("/tank/data",&str_msg);

// sub指令
void Control(const std_msgs::Float64& cmd_msg)
{
  if(long(cmd_msg.data) == 0 || long(cmd_msg.data) == INFRARED_C)
  {
      digitalWrite(ledPin, LOW); 
  }
  if(long(cmd_msg.data) == 1 || long(cmd_msg.data) == INFRARED_PLAY)
  {
      digitalWrite(ledPin, HIGH); 
  }
}
ros::Subscriber <std_msgs::Float64>  sub("/tank/cmd", &Control );

void setup()
{
    pinMode(ledPin, OUTPUT);
    nh.initNode();
    nh.advertise(pub);// publish
    nh.subscribe(sub);// subscribe
}
void loop()
{
    str_msg.data = hello;
    pub.publish(&str_msg);//publish a message
    nh.spinOnce();
    delay(1000);
}

2、Pi上测试收发arduino消息：

# 启动serial_node消息中转节点
roscore
rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0
  [INFO] [1592984761.691208]: ROS Serial Python Node
  [INFO] [1592984761.702119]: Connecting to /dev/ttyACM0 at 57600 baud
  [INFO] [1592984763.806578]: Requesting topics...
  [INFO] [1592984764.331100]: Note: publish buffer size is 280 bytes
  [INFO] [1592984764.334090]: Setup publisher on /tank/data [std_msgs/String]
  [INFO] [1592984764.343140]: Note: subscribe buffer size is 280 bytes
  [INFO] [1592984764.345981]: Setup subscriber on /tank/cmd [std_msgs/UInt16]


# 监听arduino 发给serial_node topic的数据
rostopic echo /tank/data
  data: "I'm tank!"
  ---
  data: "I'm tank!"
  ---

# 向arduino发送控制led点亮指令
rostopic pub  /tank/cmd std_msgs/Float64 1   #亮灯（pub加-1参数可只发布一次消息否则会一直发）
rostopic pub  /tank/cmd std_msgs/Float64 0x00FFA857
rostopic pub  /tank/cmd std_msgs/Float64 0   #灭灯
rostopic pub  /tank/cmd std_msgs/Float64 0x00FFB04F

四、ROS控制坦克移动

1、坦克代码增加ros指令监听

vim tank.ino

#include <IRremote.h>//包含红外库
#include <ros.h>//包含ros库
#include <std_msgs/String.h>//包含ros msg类型库
#include <std_msgs/Float64.h>
int infraredPin = 3;//红外线接收器端口
long INFRARED_PLAY = 0x00FFA857;//红外遥控器上的PLAY键指令
long INFRARED_LEFT = 0x00FFE01F;//红外遥控器上的左键指令
long INFRARED_RIGHT = 0x00FF906F;//红外遥控器上的右键指令
long INFRARED_UP = 0x00FF02FD;//红外遥控器上的上键指令
long INFRARED_DOWN = 0x00FF9867;//红外遥控器上的下键指令
long INFRARED_C = 0x00FFB04F;//红外遥控器上的C指令
long INFRARED_TEST = 0x00FF22DD;//红外遥控器上的TEST键指令
long INFRARED_RETURN = 0x00FFC23D;//红外遥控器上的RETURN键指令
int ledLightPin = 4; //led常亮端口
int ledPin = 13; //led控制提醒端口
int rightMotor = 5; // 定义uno的pin 5 向 rightMotor 输出   
int rightMotor_ = 6; // 定义uno的pin 6 向 rightMotor_ 输出  
int leftMotor = 9; // 定义uno的pin 9 向 leftMotor 输出  
int leftMotor_ = 10; // 定义uno的pin 10 向 leftMotor_ 输出


IRrecv irrecv(infraredPin);//初始化红外接收器对象
decode_results results;//定义results变量为红外结果存放位置

ros::NodeHandle nh; // ros节点句柄

/* pub ros数据 */
std_msgs::String str_msg;
char hello[]="I'm tank!";
ros::Publisher pub("/tank/data",&str_msg);

/* sub ros指令 */
void control(const std_msgs::Float64& cmd_msg){
  execute(long(cmd_msg.data));  //执行指令
}
ros::Subscriber <std_msgs::Float64>  sub("/tank/cmd", &control );

/* 指令执行 */
bool cmd_keep = true;//是否保持末次指令一直执行
void execute(long cmd){
  if (cmd == INFRARED_TEST) {//遥控器按下了TEST键，不保持末次指令
    cmd_keep = false;
  }else if (cmd == INFRARED_RETURN) {//遥控器按下了返回键，保持末次指令一直执行
    cmd_keep = true;
  }else if (cmd == INFRARED_PLAY || cmd == INFRARED_UP) {//遥控器按下了PLAY/UP键，前进
    digitalWrite(ledPin, HIGH);
    //forward 向前转  
    digitalWrite(rightMotor,HIGH); //给高电平  
    digitalWrite(rightMotor_,LOW);  //给低电平  
    digitalWrite(leftMotor,HIGH); //给高电平  
    digitalWrite(leftMotor_,LOW);  //给低电平
  }else if (cmd == INFRARED_DOWN ) {//遥控器按下了－键，后退
    digitalWrite(ledPin, HIGH);
    //back 向后转  
    digitalWrite(rightMotor,LOW);  
    digitalWrite(rightMotor_,HIGH);    
    digitalWrite(leftMotor,LOW);  
    digitalWrite(leftMotor_,HIGH); 
  }else if (cmd == INFRARED_LEFT ) {//遥控器按下了左键，左转
    digitalWrite(ledPin, HIGH);
    //left 左转 
    digitalWrite(leftMotor,LOW);  
    digitalWrite(leftMotor_,LOW); 
    digitalWrite(rightMotor,HIGH);  
    digitalWrite(rightMotor_,LOW);
  }else if (cmd == INFRARED_RIGHT ) {//遥控器按下了右键，右转
    digitalWrite(ledPin, HIGH);
    //right 右转 
    digitalWrite(rightMotor,LOW);  
    digitalWrite(rightMotor_,LOW);    
    digitalWrite(leftMotor,HIGH);  
    digitalWrite(leftMotor_,LOW); 
  }else if (cmd == INFRARED_C || cmd_keep == false) {//遥控器按下了C键或非持续类指令，停止
    digitalWrite(ledPin, LOW);
    //stop 停止  
    digitalWrite(rightMotor,LOW);  
    digitalWrite(rightMotor_,LOW);    
    digitalWrite(leftMotor,LOW);  
    digitalWrite(leftMotor_,LOW); 
  }
}

/* 履带坦克 */
void setup() {
  //红外接收器端口初始化
  pinMode(infraredPin, INPUT);
  //初始化电机IO,模式为OUTPUT 输出模式  
  pinMode(rightMotor,OUTPUT);  
  pinMode(rightMotor_,OUTPUT);  
  pinMode(leftMotor,OUTPUT);  
  pinMode(leftMotor_,OUTPUT);

  //led端口初始化
  pinMode(ledPin, OUTPUT);
  pinMode(ledLightPin, OUTPUT);
  digitalWrite(ledLightPin, HIGH);
  //波特率57600
  //Serial.begin(57600);
  //启动红外解码
  irrecv.enableIRIn();
  
  //ROS节点初始化
  nh.initNode();
  nh.advertise(pub);// publish
  nh.subscribe(sub);// subscribe
}

void loop() {
  //检测红外信号
  if (irrecv.decode(&results)) {//是否接收到解码数据,把接收到的数据存储在变量results中
    //Serial.println(results.value, HEX);//接收到的数据以16进制的方式在串口输出
    //执行指令
    execute(results.value);

    //继续等待接收下一组信号
    irrecv.resume();

    //led熄灭
    delay(200);
    digitalWrite(ledPin, LOW);
  }

  /*
  //pub ros消息
  str_msg.data = hello;
  pub.publish(&str_msg);//publish a message
  delay(10);
  */
  nh.spinOnce();
}

 

2、Pi上测试控制坦克：

# 查看arduino板子波特率
stty -F /dev/ttyACM0 57600  #设置为57600（代码里不要修改Serial波特率）

# 启动serial_node消息中转节点
roscore
rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0

# 向arduino发送控制坦克控制指令
rostopic pub  /tank/cmd std_msgs/Float64 0x00FF22DD  #切换为单次执行
#rostopic pub  /tank/cmd std_msgs/Float64 0x00FFC23D  #恢复为持续执行
rostopic pub  /tank/cmd std_msgs/Float64 0x00FF02FD  #前进 或 0x00FFA857
#rostopic pub  /tank/cmd std_msgs/Float64 0x00FF9867  #后退
#rostopic pub  /tank/cmd std_msgs/Float64 0x00FFE01F  #左转
#rostopic pub  /tank/cmd std_msgs/Float64 0x00FF906F  #右转
rostopic pub  /tank/cmd std_msgs/Float64 0x00FFB04F  #停止
*或写ros代码pub相关指令，因与rosserial无关不再赘述

# 查看节点关系
rqt_graph

rostopic指令节点发出cmd消息，rosserial server node将消息转发给ttyACM0的底盘rosserial客户端节点（转发部分在graph上看不到）

测试控制效果：

今天就先到这里，下一篇我们会把RGB-D深度相机的3维点云部分测试好后加到这个坦克上做为它的眼睛来感知环境。

yan 20.6.25 3:18

参考：

ROS学习之Arduino篇—rosserial_arduino包

ROS与Arduino学习（八）电机控制(基于rosserial_arduino)

ROS与Arduino学习（二）HelloWorld

rosserial

rosserial简介

Short introduction – pyserial

欢迎关注下方“非著名资深码农“公众号进行交流～

发表评论

邮箱地址不会被公开。