一、先编写一个循迹传感器的发布者
原理扫盲:
使用的传感器如下
这是一种红外循迹传感器,可以辨认出黑色轨迹的白色跑道,即可以使用两个这样的传感器达到跟随黑色轨迹行走,
因为当传感器碰到黑线时,输出的电平就会由低电平变为高电平,若在此时改变相应电机的转向,则可以达到一直跟随黑线行走的效果。
因为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 这个话题,于是传感器就可以控制电机的转动。