ros练习：小车巡线【2】

2.小车制作

在完成地图的绘制之后我们来做小车，小车比较好做，基本上书里对于urdf,xacro,gazebo都会有详细的描述，源码里一般都有一个小车，我就照葫芦画瓢搞了个，并且加了一个摄像头。但是，我一开始把摄像头装在了车的尾部，这就给巡线带来了很大的困难，建议大家把摄像头还是装在车头为好。
在小车做好之后我就试验了一下，写了个简单的cmd_vel/发布器看看车能不能跑，用rqt_image_view看看小车能不能看见，都没问题（如果发现图像没有朝地面，改一改相关的joint即可）

3.源码实现

黑底寻白线的程序其实还是挺好写的（或者我给自己设定了一个很简单的情景），先把源码贴出来吧：

#include "ros/ros.h"
#include"std_msgs/Float64MultiArray.h"
#include "std_msgs/String.h"
#include "sensor_msgs/Image.h"
#include"geometry_msgs/Twist.h"
using namespace std;

class usage
{
public:
    ros::Publisher pub;
    int left;
    int right;
    int down;
    int up;
    geometry_msgs::Twist pub_msg;
    usage()
    {
        ros::NodeHandle nh;
        pub=nh.advertise<geometry_msgs::Twist>("/cmd_vel",1000);
    }
    void callback(const sensor_msgs::ImageConstPtr&msg)
{ 
    ROS_INFO("up I get :[%d]",msg->data[320800]);//
    ROS_INFO("down I get :[%d]",msg->data[320000]);
     left=msg->data[320800];
     right=msg->data[639600];
     up=msg->data[400];
     down=msg->data[480400];
    if(left==0&&right==0)
    {
        pub_msg.linear.x=0;
        pub_msg.angular.z=-1;
    }
     else if(left==255&&right==0)
    {
        pub_msg.linear.x=0;
        pub_msg.angular.z=1;
    }
     else if(left==0&&right==255)
    {
        pub_msg.linear.x=0;
        pub_msg.angular.z=-1;
    }
     else if(left==255&&right==255)
    {
        pub_msg.linear.x=0.1;
        pub_msg.angular.z=0.0;
    }

      if(up==0&&down==255)
    {
        pub_msg.linear.x=0;
        pub_msg.angular.z=-1;
    }
     if(up==255&&down==0)
    {
        pub_msg.linear.x=0;
        pub_msg.angular.z=-1;
    }
    
    pub.publish(pub_msg);
}
private:
    /* data */
};


int main(int argc, char *argv[])
{
    ros::init(argc, argv, "listener_demo");
    ros::NodeHandle n;
    usage us;
    ros::Subscriber sub=n.subscribe("/mybot/camera1/image_raw",1,&usage::callback,&us);
    ros::spin();
    return 0;
}

camera返回的数据属于sensor_msgs::image，它的数据成员长这样

data是一个数组，里面是图像像素的灰度值，msg->data[320800]我就可以得到我需要的灰度值。之后的逻辑就很简单了，遇到白色走，遇到黑色转，这样就可以完成简单的巡线。
由于我需要在回调函数中使用publisher，所以我使用了在类中声明回调函数的方法，这样我就可以在类的实例化的过程中利用构造函数产生publisher。

最后本来应该写编译实现的，但是好像编译过程没有遇到太多的问题，那这个小练习就在这里结束啦！拜拜👋