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

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

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

一、安装Rosserial Arduino Library

已经有人也这么想并做了一个lib库叫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/ttyUSB0

# 监听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消息中转节点
rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0
  [INFO] [1592984761.691208]: ROS Serial Python Node
  [INFO] [1592984761.702119]: Connecting to /dev/ttyUSB0 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
stty -F /dev/ttyACM0 57600  #设置为57600(代码里不要修改Serial波特率)

# 启动serial_node消息中转节点
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  #停止

测试控制效果:

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

yan 20.6.25 3:18

参考:

ROS学习之Arduino篇—rosserial_arduino包

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

ROS与Arduino学习(二)HelloWorld

发表评论

电子邮件地址不会被公开。