arduino作为下位机连接VL6180X与ROS上位机进行通信的两种方式

arduino作为下位机连接VL6180X与ROS上位机进行通信的两种方式

一、通过串口直接进行通信

这种方法直接通过arduino的ros库，节点直接在下位机中就已经构建，上位机只需安装rosserial_arduino，就可以订阅到此时下位机发出的信息，但实验过程中发现该种方式只适用于Python2.7的环境，如果你的开发环境大部分处在Python3.6环境下，会对后期的数据提取造成一定困难。而且此种方式在通信时会出现数据传输不稳定的情况，以及串口等各类问题，如果你只需要简单的数据回传不需要对后续的数据进行处理可以使用该种方式。

Ubuntu环境下arduino IDE的安装以及需要环境的配置

sudo apt-get install arduino           //arduino的安装
sudo apt-get install ros-melodic-rosserial-python     //串口通讯功能安装

arduino安装完毕后还需要安装对应的ROS库文件，教程详见此博客

使用arduino IDE 将VL6180X对应的底层代码烧录至下位机中

下位机代码如下：

#include <ros.h>            //ROS&arduino通信头文件
#include <Wire.h>          //arduino通讯
#include <VL6180X.h>   //传感器对应头文件
#include <std_msgs/Int64.h>   //定义的数据类型

using namespace std;

ros::NodeHandle  nh;

VL6180X sensor;
std_msgs::Int64 str_msg;
ros::Publisher distance("distance", &str_msg);
int dis;

void setup() 
{
  Serial.begin(9600);
  Wire.begin();
  nh.initNode();
  nh.advertise(distance);
  sensor.init();
  sensor.configureDefault();
  sensor.setTimeout(100);
}

void loop() 
{ 
    Serial.print(sensor.readRangeSingleMillimeters());
  dis = sensor.readRangeSingleMillimeters();
  str_msg.data = dis;
  distance.publish( &str_msg );
  nh.spinOnce();
  delay(200);
}

通过cutecom观察此时arduino回传数据是否正确。

启动rosserial_python中的serial_node.py即可，注意串口号，如：

rosrun rosserial_python serial_node.py /dev/ttyACM0

即可看到输出的当前下位机信息。

二、使用ROS节点进行通信

此种方法不用在意串口通信的规则，也不再限制此时你的开发环境，只需要通过arduino的serial.print()函数就可以在上位机中定义相关节点获取信息。

下位机代码如下：

#include <Wire.h>
#include <VL6180X.h>

VL6180X sensor;

void setup() 
{
  Serial.begin(9600);
  Wire.begin();
  
  sensor.init();
  sensor.configureDefault();
  sensor.setTimeout(500);
}

void loop() 
{ 
  Serial.print(sensor.readRangeSingleMillimeters());
  if (sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
  Serial.println();
}

烧录完毕后在ros工作环境中创建节点

#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Empty.h>
#include <iostream>
using namespace std;
serial::Serial _serial;	
std_msgs::String result;

void distance_write_callback(const std_msgs::Int8& msg)
{
	std::string Gcode = "";
	ROS_INFO("%s", Gcode.c_str());
	_serial.write(Gcode.c_str());
	result.data = _serial.read(_serial.available());
}

int main(int argc, char** argv)
{	
	ros::init(argc, argv, "distance_find_node");
	ros::NodeHandle nh;
  ros::Subscriber sub1 = nh.subscribe("distance_topic", 1, distance_write_callback);
  ros::Publisher pub_dis = nh.advertise<std_msgs::Float64>("/distance", 10);
	// ros::spin();
  ros::Rate loop_rate(20);
	try
	{
    _serial.setPort("/dev/ttyACM0");
		_serial.setBaudrate(9600);
		_serial.setFlowcontrol(serial::flowcontrol_none);
		_serial.setParity(serial::parity_none);
		_serial.setStopbits(serial::stopbits_one);
		serial::Timeout to = serial::Timeout::simpleTimeout(1000);
		_serial.setTimeout(to);
		_serial.open();
		ROS_INFO_STREAM("Port has been open successfully");
	}
	catch (serial::IOException& e)
	{
		ROS_ERROR_STREAM("Unable to open port");
		return -1;
	}
  if (_serial.isOpen())
	{
		ROS_INFO_STREAM("Attach and wait for commands ");
	}
  while (nh.ok()){
    std::string tmp_data = _serial.readline();
//    std::cout<<tmp_data;
    std_msgs::Float64 data;
    double data_;
    stringstream stream;
    stream<<tmp_data;
    stream>>data_;
    data.data = data_;
//    data.data = atoi(tmp_data.c_str());
//    ROS_INFO("data_ = %lf, data.data = %lf", data_, data.data);
    pub_dis.publish(data);
//    ros::spinOnce();
//    loop_rate.sleep();
	}
   
}

配置完环境后编译即可完成ROS节点的订阅。