组长发布任务,我负责使用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;
}
//这是采用类的写法,其实也可以直接写