ros实验（1）：实现摄像头、底盘的运行状态采集并发送给任务选择模块

采用多线程采集底盘和摄像头的功能状态进行判断后广播

概述

项目背景：一个沿磁条行进的机器人，同时采用摄像头进行肉鸡的识别；
模块功能：采用多线程收集摄像头和底盘的启动信息，判定机器人状态为：底盘启动、摄像头启动然后发送给任务选择模块。

代码（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;  
}