【三】、使用循迹传感器和超声波驱动二轮差分底盘

一、先编写一个循迹传感器的发布者

原理扫盲：
使用的传感器如下

这是一种红外循迹传感器，可以辨认出黑色轨迹的白色跑道，即可以使用两个这样的传感器达到跟随黑色轨迹行走，
因为当传感器碰到黑线时，输出的电平就会由低电平变为高电平，若在此时改变相应电机的转向，则可以达到一直跟随黑线行走的效果。

因为Bool变量的值只有 真 （true) 和假 （false），为了便于理解，先编写一个发布Bool数据类型的发布者，具体代码如下

#include "mbed.h"
#include <ros.h>
#include <std_msgs/Bool.h>

ros::NodeHandle  nh;

std_msgs::Bool tracking_msg;
ros::Publisher pub_tracking("Tracking", &tracking_msg);


Timer t;
DigitalIn sig1(p9);

int main() {
    t.start();

    nh.initNode();
    nh.advertise(pub_tracking);

    long publisher_timer = 0;

    while (1) {

        if (t.read_ms() > publisher_timer) {
            tracking_msg.data = !sig1;
            pub_tracking.publish(&tracking_msg);
            publisher_timer = t.read_ms() + 1000;
        }
        nh.spinOnce();
    }
}

二、将Bool型发布的消息改为Twist型消息

需要修改的代码如下：
①首先修改头文件
将

#include <std_msgs/Bool.h>

改为

#include <geometry_msgs/Twist.h>

因为Bool类型只能发布True和Flase两个数据，要发布小车接收者的Twist的数据类型才能驱动小车，因此就要引入Twist的数据类型的头文件。
②
将

std_msgs::Bool tracking_msg;

改为

geometry_msgs::Twist tracking_msg;

③修改发布的数据
将

tracking_msg.data = !sig1;

改为

if(sig1==1){                   //黑
                tracking_msg.linear.x = 0.5;
            }
            else if(sig1==0){  //白
                tracking_msg.linear.x = -0.5;
            }

这样就可以将红外循迹传感器接收到的信息转化成Twist数据类型，只要小车订阅了 Tracking 这个话题，小车就可以根据循迹传感器进行巡线了。

最终循迹传感器Twist数据类型的发布者代码如下

#include <ros.h>
#include <geometry_msgs/Twist.h>

ros::NodeHandle  nh;

geometry_msgs::Twist tracking_msg;
ros::Publisher pub_tracking("Tracking", &tracking_msg);


Timer t;
DigitalIn sig1(p9);

int main() {
    t.start();

    nh.initNode();
    nh.advertise(pub_tracking);

    long publisher_timer = 0;

    while (1) {

        if (t.read_ms() > publisher_timer) {
            if(sig1==1){    //黑
                tracking_msg.linear.x = 0.5;
            }
            else if(sig1==0){  //白
                tracking_msg.linear.x = -0.4;
            }
            pub_tracking.publish(&tracking_msg);
            publisher_timer = t.read_ms() + 1000;
        }
        nh.spinOnce();
    }
}

三、编写一个发布Twist消息的超声波发布者

加上超声波的初衷是怕出现小车会离开黑色轨道，然后碰到障碍物上就会一直卡死不能再动。具体代码如下：

#include "mbed.h"
#include "ultrasonic.h"
#include <ros.h>
#include <ros/time.h>
#include <geometry_msgs/Twist.h>

ros::NodeHandle  nh;

geometry_msgs::Twist ultrasonic_msg;
ros::Publisher pub_ultrasonic("ultrasonic", &ultrasonic_msg);

void dist(int distance)
{
    if(distance<100){
        ultrasonic_msg.linear.x = 0.5;
    }
    else{
        ultrasonic_msg.linear.x = -0.4;
    }
}

Timer t;    
ultrasonic mu(p9, p10, .1, 1, &dist);    //Set the trigger pin to D8 and the echo pin to D9
                                        //have updates every .1 seconds and a timeout after 1
                                        //second, and call dist when the distance changes

int main()
{
    t.start();
    mu.startUpdates();//start mesuring the distance
    nh.initNode();
    nh.advertise(pub_ultrasonic);
    long range_time = 0;
    while(1)
    {
         //Do something else here
        mu.checkDistance();     //call checkDistance() as much as possible, as this is where
                                //the class checks if dist needs to be called.
        if (t.read_ms() >= range_time) {
            pub_ultrasonic.publish(&ultrasonic_msg);
            range_time =  t.read_ms() + 50;
        }
        nh.spinOnce();       
    }
}

四、将以上代码与电机驱动的代码整合起来

#include "mbed.h"
#include "Motor.h"
#include "ultrasonic.h"
#include <ros.h>
#include <ros/time.h>
#include <geometry_msgs/Twist.h>


DigitalIn sig1(p11);
DigitalIn sig2(p12);
Motor A(p21, p22); 
Motor B(p23, p24);
int sig3;

void messageCb(const geometry_msgs::Twist& msg)
{
  if (msg.angular.z == 0 && msg.linear.x == 0)
  {
    A.speed(0);
    B.speed(0);
  }
  else
  {
    if (msg.angular.z < 0)
    {
      A.speed(0.5);
      B.speed(-0.5);
      wait_ms(20);
      A.speed(0);
      B.speed(0);      
    }
    else if (msg.angular.z > 0)
    {
      A.speed(-0.5);
      B.speed(0.5);
      wait_ms(20);
      A.speed(0);
      B.speed(0);      
    }
    else if (msg.linear.x < 0)
    {
      A.speed(-0.5);
      B.speed(-0.5);
      wait_ms(20);
      A.speed(0);
      B.speed(0);
    }
    else if (msg.linear.x > 0)
    {
      A.speed(0.5);
      B.speed(0.5);
      wait_ms(20);
      A.speed(0);
      B.speed(0);
    }
  }
}

void dist(int distance)
{
    if(distance<100){
        sig3=0;
    }
    else{
        sig3=1;
    }
}

Timer t;
ultrasonic mu(p9, p10, .1, 1, &dist);    //Set the trigger pin to D8 and the echo pin to D9
                                        //have updates every .1 seconds and a timeout after 1
                                        //second, and call dist when the distance changes
ros::NodeHandle  nh;
geometry_msgs::Twist tracking_msg;
ros::Publisher pub_tracking("Tracking", &tracking_msg);
ros::Subscriber<geometry_msgs::Twist> sub("Tracking", &messageCb);

int main() {
  t.start();
  mu.startUpdates();//start mesuring the distance
  nh.initNode();
  nh.subscribe(sub);
  nh.advertise(pub_tracking);
  long publisher_timer = 0;

  while (1)
  {
    //Do something else here
    mu.checkDistance();     //call checkDistance() as much as possible, as this is where
                            //the class checks if dist needs to be called.
    if (t.read_ms() > publisher_timer) {
            if(sig1==0 && sig2==0){
                if(sig3==1){
                   tracking_msg.linear.x = 0.5;
                   tracking_msg.angular.z = 0.0;
                }
                else if(sig3==0){
                   tracking_msg.linear.x = 0.0;
                   tracking_msg.angular.z = 0.5;
                }
            }
            else if(sig1==1 && sig2==1){
                tracking_msg.linear.x = -0.5;
                tracking_msg.angular.z = 0.0;
            }
            else if(sig1==0 && sig2==1){
                tracking_msg.linear.x = 0.0;
                tracking_msg.angular.z = -0.5;
            }
            else if(sig1==1 && sig2==0){
                tracking_msg.linear.x = 0.0;
                tracking_msg.angular.z = 0.5;
            }
            pub_tracking.publish(&tracking_msg);
            publisher_timer = t.read_ms() + 10;
     }
     nh.spinOnce();
  }
}

运行代码：
1.roscore
2.sudo chmod 666 /dev/ttyACM0
3.rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0
运用 rqt_graph 显示当前运动的节点和话题:
rosrun rqt_graph rqt_graph
运行后得到如下图所示

其中 Tracking 是话题名称，传感器发布消息到 Tracking 话题中，然后电机的驱动订阅 Tracking 这个话题，于是传感器就可以控制电机的转动。