ROS基础篇之连接DIY aduino 传感器与执行器的应用

 

说到机器人，可能我们首先想到是人形机器人，有手臂、有腿、有眼、耳朵以及大量传感器和执行器，是一个极为复杂的运动系统。

现在我们已经学会了如何在ROS里编写节点程序，并控制他们通过话题传递消息数据。接下来我们要学习如何在ROS中使用传感器和执行器，这样机器人就能与现实世界交互了。

回顾ROS的核心组件：

通信基础结构
ROS是一个分布式的进程（“节点”）框架。
传感器和执行器可以分布式部署。

机器人特定功能库
movebase
moveit
从功能库的输入输出，找到传感器与执行器的影子。

工具
rviz
传感器和执行器数据的可视化。

传感器和执行器

传感器
触碰、声呐、光电、磁感（霍尔）、陀螺仪 加速度计、激光雷达、摄像头等等
执行器 
电动机 、加热棒、扬声器、显示器
传感器和执行器元器件，需要微处理器控制，并制定通信协议。

创造传感器、执行器

传感器：矩阵键盘
原理：行列扫描确定按键
描述：按下按键时向上位机发送键值
通迅方式：串口 9600bps
通迅协议： [0-F]\n 形式的字串

代码

#include <Keypad.h>

const byte ROWS = 4; //four rows
const byte COLS = 4; //four columns
//define the cymbols on the buttons of the keypads
char hexaKeys[ROWS][COLS] = {
  {'0','1','2','3'},
  {'4','5','6','7'},
  {'8','9','A','B'},
  {'C','D','E','F'}
};
byte rowPins[ROWS] = {3, 2, 8, 0}; //connect to the row pinouts of the keypad
byte colPins[COLS] = {7, 6, 5, 4}; //connect to the column pinouts of the keypad

//initialize an instance of class NewKeypad
Keypad customKeypad = Keypad( makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS); 
int pc0=14;
int pc1=15;
int pc2=16;
int pc3=17;
int pc4=18;
int pc5=19;
void setup(){
pinMode(pc0,OUTPUT);
pinMode(pc1,OUTPUT);
pinMode(pc2,OUTPUT);
pinMode(pc3,OUTPUT);
pinMode(pc4,OUTPUT);
pinMode(pc5,OUTPUT);
digitalWrite(pc0, 1);
digitalWrite(pc1, 1);
digitalWrite(pc2, 1);
digitalWrite(pc3, 1);
digitalWrite(pc4, 1);
digitalWrite(pc5, 1);
  Serial.begin(9600);
}
  
void loop(){
  char customKey = customKeypad.getKey();
  
  if (customKey){
    byte k=customKey;
    Serial.println(customKey);
    //
    byte a=k>=65?11+k-65:k-47;
    //Serial.println(a);
    digitalWrite(pc0, ((a&0x01)>>0)?0:1);
    digitalWrite(pc1, ((a&0x02)>>1)?0:1);
    digitalWrite(pc2, ((a&0x04)>>2)?0:1);
    digitalWrite(pc3, ((a&0x08)>>3)?0:1);
   
    digitalWrite(pc4, ((a&0x10)>>4)?0:1);
    digitalWrite(pc5, ((a&0x20)>>5)?0:1);
    
  }
}

 

执行器：舵机
原理：pmw波占空比控制位置
描述：接收位置命令后，到达指定的位置
通迅方式：串口 9600bps
通迅协议： #[0000-1023]! 形式的字串

代码

#include <Servo.h> 
 
Servo myservo;  // create servo object to control a servo 
 
int potpin = 0;  // analog pin used to connect the potentiometer
int val=0;    // variable to read the value from the analog pin 
int flag=1;
int nowposi=0;
 
void setup() 
{ 
  // Open serial communications and wait for port to open:
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for Leonardo only
  }


  Serial.println("hello servo!");
  myservo.attach(9);  // attaches the servo on pin 9 to the servo object 
} 
 byte bytedata[4]={0,0,0,0};
 byte pa=0;
void loop() 
{ 
  /*val = nowposi;//analogRead(potpin);            // reads the value of the potentiometer (value between 0 and 1023) 
  val = map(val, 0, 1023, 0, 179);     // scale it to use it with the servo (value between 0 and 180) 
  myservo.write(val);                  // sets the servo position according to the scaled value 
     
  nowposi+=flag;
  if(nowposi<=0)
  flag=1;
  else if(nowposi>1023){
  flag=-1;
  }  
  delay(15); // waits for the servo to get there */
  if (Serial.available()){
    byte a=Serial.read();
    if(a=='#'||a=='!'){
      if(a=='#')
      {
        Serial.write("#");
        pa=0;
      }
      
      if(a=='!'){
        Serial.write("!");
        if(pa==4){
          int data=0;
          
          data+=(bytedata[0]-48)*1000;
          data+=(bytedata[1]-48)*100;
          data+=(bytedata[2]-48)*10;
          data+=(bytedata[3]-48);
          //Serial.write(bytedata,4);
          //*
          Serial.println((bytedata[0]-48));
          Serial.println((bytedata[1]-48));
          Serial.println((bytedata[2]-48));
          Serial.println((bytedata[3]-48));
          Serial.println((data));//*/
          val = map(data, 0, 1023, 0, 179);     // scale it to use it with the servo (value between 0 and 180) 
          //Serial.println(val);
          myservo.write(val);                  // sets the servo position according to the scaled value 
          
          
        }else{
          Serial.write("e");
        }
      }
    }else{
      if(pa<4&&a>=48&&a<=58){
      bytedata[pa]=a;
      pa++;
      }else{
      pa=0;
      }
    }
    
  }
} 

项目规划

1、功能设定 ：使按键键值0-F（0-16）在顺序上与舵机转角从0度到180度对应。

2、连接传感器和执行器到计算机

3、检查连接状态与通迅测试

4、确定节点数量、每个节点的功能以及节点间的通迅方法
      
5、编程与测试

连接计算机和传感器、执行器，创建机器人
列出设备信息

ls -o /dev/ttyUSB*
crw-rw---- 1 root 188, 0 12月  8 09:46 /dev/ttyUSB0
crw-rw---- 1 root 188, 1 12月  8 09:42 /dev/ttyUSB1

更改权限

sudo chmod 777 /dev/ttyU*

检查设备信息

ls -o /dev/ttyU*
crwxrwxrwx 1 root 188, 1 12月  8 16:23 /dev/ttyUSB1
crwxrwxrwx 1 root 188, 2 12月  8 15:29 /dev/ttyUSB2

串口测试

cutecom

测试串口下对传感器和执行器的操作

节点功能与话题消息定义

传感器节点：获取键值
串口读
协议解析  0-F字串解码为int型0-15
发布消息到话题
源码

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/UInt8.h"
#include <serial/serial.h>
//ROS已经内置了的串口包

#include <std_msgs/Empty.h>

serial::Serial ser; //声明串口对象
int hex2int(char c)
{
  if ((c >= 'A') && (c <= 'F'))
  {
    return c - 'A' + 10;
  }
  else if ((c >= 'a') && (c <= 'f'))
  {
    return c - 'a' + 10;
  }
  else if ((c >= '0') && (c <= '9'))
  {
    return c - '0';
  }
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "sread");
  ros::NodeHandle nh;
  //发布主题
  ros::Publisher read_pub = nh.advertise<std_msgs::UInt8>("pressButtonId", 10);

  try
  {
  //设置串口属性，并打开串口ros
      ser.setPort("/dev/ttyUSB0");
      ser.setBaudrate(9600);
      serial::Timeout to = serial::Timeout::simpleTimeout(1000);
      ser.setTimeout(to);
      ser.open();
  }
  catch (serial::IOException& e)
  {
      ROS_ERROR_STREAM("Unable to open port ");
      return -1;
  }

  //检测串口是否已经打开，并给出提示信息
  if(ser.isOpen())
  {
      ROS_INFO_STREAM("Serial Port initialized");
  }
  else
  {
      return -1;
  }
  ROS_INFO_STREAM("sread loop start");
  //指定循环的频率
  ros::Rate loop_rate(50);
  while(ros::ok())
  {
      if(ser.available()){
          ROS_INFO_STREAM("Reading from serial port\n");
          std::string str;
          std_msgs::UInt8 value;
          std_msgs::String result;
          //result.data = ser.read(ser.available());
          str =ser.readline();
          if(str.length()>1){
            //size_t s=str.find_last_of("\r\n");
            str=str.substr(0,1);
            value.data=hex2int(*str.c_str());
            ROS_INFO_STREAM("str="<<str<<" "<<value);
            read_pub.publish(value);
            //result.data=str.substr(0,1);
            //ROS_INFO_STREAM("Read: " << result.data);
          }
      }
      //处理ROS的信息，比如订阅消息,并调用回调函数
      ros::spinOnce();
      loop_rate.sleep();
  }
  return 0;
}

执行器节点：控制舵机
从话题接收消息
控制逻辑
通迅协议  0-15 转换为0-1024范围 输出#0000!字串
串口写
源码

#include "ros/ros.h"
#include "std_msgs/UInt8.h"
#include <serial/serial.h>

serial::Serial ser; //声明串口对象

void chatterCallback(const std_msgs::UInt8::ConstPtr& msg)
{
  ROS_INFO("recive: [%d]", msg->data);
  //0-15
  //xieyi
  //#0000!-#1023!
  int value=msg->data*(1024/16);
  char data[7]={'#',0,0,0,0,'!','\n'};
  data[1]=(value%10000)/1000+'0';
  data[2]=(value%1000)/100+'0';
  data[3]=(value%100)/10+'0';
  data[4]=value%10+'0';

   ser.write(data);   //发送串口数据
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "swrite");
  ros::NodeHandle nh;
  try
  {
  //设置串口属性，并打开串口
      ser.setPort("/dev/ttyUSB1");
      ser.setBaudrate(9600);
      serial::Timeout to = serial::Timeout::simpleTimeout(1000);
      ser.setTimeout(to);
      ser.open();
  }
  catch (serial::IOException& e)
  {
      ROS_ERROR_STREAM("Unable to open port ");
      return -1;
  }

  //检测串口是否已经打开，并给出提示信息
  if(ser.isOpen())
  {
      ROS_INFO_STREAM("Serial Port initialized");
  }
  else
  {
      return -1;
  }
  ros::Subscriber sub = nh.subscribe("pressButtonId", 10, chatterCallback);
  ros::spin();

  return 0;
}

小结

1机器人中使用传感器和执行器是非常重要的，因为这是和现实世界进行互动的唯一途径。

2我们用简单的示例演示了如何配置、检查、使用传感器和执行器。

3学习了ros内置串口库的编程方法。通过实例，我们能更好的理解ROS中节点、话题、消息等概念，并能在实际项目中灵活运用进行数据交互。