ros实现串口通信（记录一次脱发的经历）

组长发布任务，我负责使用ros实现串口通讯。怎么说呢，头发可以在地上、桌子上甚至任何地方，除了头上~~

经过询问，任务大概分为三个点：
1、接收话题名为“config_detector”的信息，判断串口是否打开，读入串口名称
2、如果串口不打开，读入“SerialData.yml”文件中的数据，并发布话题名为“serial_recieve”的信息。然后再接收话题为“serial_sendmsg”的数据，直接打印其中的数据
3、如果串口打开，从串口中读入数据然后并发布话题名为“serial_recieve”的信息，同时，接收话题为“serial_sendmsg”的信息，然后传递给串口。

具体代码如下：

#include "../include/serial/serial.h"
#include "../include/serial/SerialPort.h"
#include "../include/serial/Protocol.h"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "opencv2/opencv.hpp"
#include <iostream>
#include <sstream>
#include <thread>
#include <mutex>
#include <time.h>
using namespace std;
using namespace cv;



static fyt_msg::DetectorParam detectorParam;
static fyt_msg::SerialRecieveMsg serialRecieveMsg;
static fyt_msg::SerialSendMsg serialSendMsg;

static int serialopen_flag; //串口标识
static string port_name = "/dev/ttyUSB0";   //初始化串口名称
SerialPort serial_port("/dev/ttyUSB0"); //定义SerialPort




//接受节点传来的装甲板参数后回调函数
void doSerialSendMsg(const fyt_msg::SerialSendMsg::ConstPtr& msg)
{
    setlocale(LC_ALL,""); 
    //ros_info可以输出中文
    //串口开启
    if(serialopen_flag == 1)
    {
        ROS_INFO("Starting passing data to serialport!");
        Protocol protocol(serial_port);
        cv::Point3f target;
        target.x = msg->distance;
        target.y = msg->pitch;
        target.z = msg->yaw;
        protocol.sendTarget(target);    //传参，打印参数

    }
    //串口不开启
    else if(serialopen_flag == 0)
    {
        ROS_INFO("msg->yaw:%lf",msg->yaw);
        ROS_INFO("msg->distance:%lf",msg->distance);
        ROS_INFO("msg->pitch:%lf",msg->pitch);
    }     
}





//接收节点config_node的信息后回调函数
void IsSerialOpen(const fyt_msg::DetectorParam::ConstPtr& msg)
{
        //串口通信打开，输出提示
        //读取串口中的数据，作为话题发布出去

    setlocale(LC_ALL,"");       //ros_info可以输出中文
    if(msg->use_serial==1)      //串口开启
    {
        serialopen_flag = 1;    //串口标识
        ROS_INFO("The use_serial is opened!");
        if(port_name.c_str() != msg->port_name.c_str())         //判断串口名称是否改变，并随之改变
        {
            serial_port.SetPortName(msg->port_name.c_str());    //改变串口名称
            serial_port.Init();                                 //初始化串口
            port_name = msg->port_name.c_str();
        }

        //将串口中接收到的信息传递给serialReceiveMsg
        serialRecieveMsg.id = (u_int8_t)mcu_data.id;//小车id
        serialRecieveMsg.enemy_color = (u_int8_t)mcu_data.color;//小车颜色
        serialRecieveMsg.state = (u_int8_t)mcu_data.state;
        serialRecieveMsg.short_speed = (u_int8_t)mcu_data.shoot_speed;
        serialRecieveMsg.gimbal_pitch = (u_int16_t)mcu_data.gimbal_pitch;
        serialRecieveMsg.gimbal_yaw = (u_int16_t)mcu_data.gimbal_yaw;

    }
    //串口不打开
    else if(msg->use_serial!=1)
    {
        serialopen_flag = 0;
        ROS_INFO("The use_serial is closed!");
        FileStorage fs_read;        //将SerialData.yml中的数据读取出来
        fs_read.open("./config_file/SerialData.yml",FileStorage::READ);
        if(!fs_read.isOpened())
        {
            ROS_INFO("The config_file/SerialData.yml can not be opened!");
            return;
        }
        else
        {
            //将信息赋值给serialReceiveMsg
            fs_read["id"] >> serialRecieveMsg.id;
            fs_read["enemy_color"] >> serialRecieveMsg.enemy_color;
            fs_read["state"] >> serialRecieveMsg.state;
            fs_read["short_speed"] >> serialRecieveMsg.short_speed;
            fs_read["gimbal_yaw"] >> serialRecieveMsg.gimbal_yaw;;
            fs_read["gimbal_pitch"] >> serialRecieveMsg.gimbal_pitch;

        }
            fs_read.release();   
        }

    }





int main(int argc,char **argv){

    setlocale(LC_ALL,"");   //可以打印中文
    ros::init(argc,argv,"serial_node");
    ros::NodeHandle n; 
    ros::Publisher pub_serialRecieveMsg;
    ros::Subscriber sub_detectorParam;
    ros::Subscriber sub_serialSendMsg;

   
    sub_detectorParam=n.subscribe<fyt_msg::DetectorParam>("config_detector",10,IsSerialOpen);
    sub_serialSendMsg=n.subscribe<fyt_msg::SerialSendMsg>("serial_sendmsg",1,doSerialSendMsg);  
    pub_serialRecieveMsg=n.advertise<fyt_msg::SerialRecieveMsg>("serial_recieve",100);
    Protocol protocol(serial_port);
    std::thread recieve(&Protocol::receiveData,&protocol);  //多线程接收串口中的数据，防止串口被堵住

    while(ros::ok())
    {
        pub_serialRecieveMsg.publish(serialRecieveMsg); //发布话题
        ros::spinOnce();    //循环之前看看还有没有需要回调的函数
    }
   
    return 0;
}

由于代码中存在许多函数是自定义的，所以这里也不过多解释，只解释学到的知识。

学习ros中的发布信息和接收信息

接收者模板

#include "ros/ros.h"
#include <sensor_msgs/LaserScan.h>

void cb(const sensor_msgs::LaserScan::ConstPtr &scan)
{
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("scan", 100, cb);
  ros::spin();
  return 0;
}

发布者模板

#include <ros/ros.h>
int main(int argc, char** argv)
{
	ros::init(argc, argv, "publish_velocity");
	ros::NodeHandle nh;
 
	ros::Publisher pub = nh.advertise <>("", 1000);
	while (ros::ok())
	{
		pub.publish();
		ros::spinOnce();
	}
	return 0;
}

接收并发布

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>

class SubsPub {
 public:
  void cb(const sensor_msgs::LaserScan::ConstPtr& scan);
  SubsPub(ros::NodeHandle n) {
    sub_ = n.subscribe("scan", 1, &SubsPub::cb, this);
    pub_ = n.advertise<sensor_msgs::PointCloud2>("cloud", 10);
  }

  ros::Subscriber sub_;
  ros::Publisher pub_;
};

void SubsPub::cb(const sensor_msgs::LaserScan::ConstPtr& scan) {
  sensor_msgs::PointCloud2 msg;
  pub_.publish(msg);
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "sp");
  ros::NodeHandle n;
  SubsPub sp(n);
  ros::spin();
  return 0;
}
//这是采用类的写法，其实也可以直接写

使用Opencv 写入、读取Yaml文件

C++11多线程 std::thread详解

ros::spin()、ros::spinOnce()：使用细节、区别