ROS与Arduino学习(七)小案例节点通信
ROS与Arduino学习(七)小案例节点通信
Tutorial Level:Logging日志
Next Tutorial:电机控制(基于rosserial_arduino)
本节主要用一个小的案例介绍用PC客户端对Arduino的灯进行控制。共有两个节点,一个几点在PC上将用户的数据输入,并将数据发布到话题中,然后Arduino的节点订阅话题获取消息对灯进行控制。
Tips 1 PC端节点程序
整体代码如下所示:
#include "ros/ros.h"
#include "std_msgs/Int16.h"
#include "std_msgs/String.h"
#include <cstdlib>
#include <sstream>
int main(int argc, char **argv)
{
int count;
ros::init(argc, argv, "Control2Arduino");
if (argc != 2)
{
ROS_INFO("usage: add_speed v");
return 1;
}
ros::NodeHandle n;
std::stringstream ss;
std_msgs::Int16 msg;
ros::Publisher chatter_pub = n.advertise<std_msgs::Int16>("Pc2Arduino", 1000);
ros::Rate loop_rate(10);
while (ros::ok())
{
msg.data = atoll(argv[1]);//读入数据
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
代码解释:
1、运行功能包时需要给定一个参数,我们读取argv[1],并将其作为速度值消息发布到话题中。
2、程序开始有一个参数检测,当输入为两个值时(其中一个值为回车),此时继续运行后面的代码,如果错误则返回异常。
if (argc != 2)
{
ROS_INFO("usage: add_speed v");
return 1;
}
3、读取数据
msg.data = atoll(argv[1]);//读入数据
4、发布数据
Tips 2 Arduino端节点程序
程序如下:
#include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int16.h>
ros::NodeHandle nh;
std_msgs::String str_msg;
//publish and subscribe
//
ros::Publisher pub("Arduino2PC",&str_msg);
void Control(const std_msgs::Int16& cmd_msg)
{
if(cmd_msg.data == 1)
{
digitalWrite(13, LOW);
}
if(cmd_msg.data == 0)
{
digitalWrite(13, HIGH);
}
}
ros::Subscriber <std_msgs::Int16> sub("Pc2Arduino", &Control );
char hello[]="hello world!";
void setup()
{
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
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);
}
Tips 3 测试程序
#新终端打开
$ roscore
#新终端打开
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0
#新终端打开
$ rosrun PCSystem Control2Arduino 1