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。
最后本来应该写编译实现的,但是好像编译过程没有遇到太多的问题,那这个小练习就在这里结束啦!拜拜👋