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