采用多线程采集底盘和摄像头的功能状态进行判断后广播
概述
项目背景：一个沿磁条行进的机器人，同时采用摄像头进行肉鸡的识别；
模块功能：采用多线程收集摄像头和底盘的启动信息，判定机器人状态为：底盘启动、摄像头启动然后发送给任务选择模块。
代码（1）消息采集和发送
/**
* 该例程将订阅chssis，camera，recall话题并发送state话题
*/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <actionlib/client/simple_action_client.h>
#include <boost/thread.hpp>
#include <sstream>
#include <iostream>
//定义用于状态判定的参数a，b，q；
int a;
int b;
int q = 100;
//声明状态参数改变函数
void change(int *x,int *y);
//定义一个类，里面包含所有并行工作的线程
class ch_cm_init
{
//定义一个公共类，使订阅到的参数对所有函数可见，并在收到消息时进行回调；
public:
ch_cm_init()
{
//这里有5个线程一个pub是向外广播后进行回调，一个pub是广播底盘和摄像头的状态，一个订阅是订阅这个函数的回调信息，另外两个订阅是订阅底盘和摄像头的状态；
pub_ = n_.advertise<std_msgs::String>("re_call", 1000);
pub2_ = n_.advertise<std_msgs::String>("hardware_state", 1000);
//Topic1 you want to subscribe
sub_ = n_.subscribe("chassis_state_start", 10, &ch_cm_init::ch_callback, this);
//Topic2 you want to subscribe
sub2_ = n_.subscribe("camera_state_start", 10, &ch_cm_init::cm_callback, this);
sub3_ = n_.subscribe("re_call0", 10, &ch_cm_init::re_call0, this);
}
//声明并定义底盘的状态判断函数，在每次收到底盘发送的消息时调用
void ch_callback(const std_msgs::String::ConstPtr& msg)
{
std_msgs::String ms;
std::stringstream s;
s << msg->data.c_str() ;
ms.data = s.str();
std_msgs::String ms_0;
std::stringstream s0;
s0 << "ch_st";
ms_0.data = s0.str();
if(strcmp(ms.data.c_str(),ms_0.data.c_str()) == 0)
{
std::stringstream go0;
go0 << "go";
go.data = go0.str();
pub_.publish(go);
change(&a,&q);
}
else
{
ROS_INFO("no! chassis isn't started");
}
}
//声明并定义摄像头的状态判断函数，在每次收到底盘发送的消息时调用；
void cm_callback(const std_msgs::String::ConstPtr& camera_msg)
{
std_msgs::String cms;
std::stringstream cs;
cs << camera_msg->data.c_str() ;
cms.data = cs.str();
std_msgs::String cms0;
std::stringstream cs0;
cs0 << "cm_st" ;
cms0.data = cs0.str();
if(strcmp(cms.data.c_str(),cms0.data.c_str()) == 0)
{
change(&b,&q);
}
else
{
ROS_INFO("no! camera isn't started");
}
//声明并定义回调函数，在每次收到底盘发送的消息的时候进行硬件状态判断，并发送给任务选择模块；
}
void re_call0(const std_msgs::String::ConstPtr& re_call)
{
int needle_a;
int needle_b;
needle_a = a;
needle_b = b;
if (needle_a == needle_b)
{
std::stringstream go0;
go0 << "hd_st";
go.data = go0.str();
pub2_.publish(go);
ROS_INFO("Yes hardware was started!");
}
else
{
ROS_INFO("Care for hardware!");
}
//定义一个私有类，保存肉丝启动的线程信息
}
private:
ros::NodeHandle n_;
ros::Publisher pub_;
ros::Publisher pub2_;
ros::Subscriber sub_;
ros::Subscriber sub2_;
ros::Subscriber sub3_;
std_msgs::String go;
};
//定义主函数
int main(int argc, char **argv)
{
// 初始化ROS节点
ros::init(argc, argv, "ch_cm_init");
ROS_INFO("Waiting for chassis start");
//运行上面的类，其中由五个回调的线程
ch_cm_init test;
ros::MultiThreadedSpinner s(5);
//回调函数
ros::spin(s);
return 0;
}
//定义状态判断函数
void change(int *x,int *y)
{
int needle_number;
needle_number = *y;
*x = needle_number;
}
