北邮智能车仿真培训（七）—— 使用c++编写程序

前言

上一节：北邮智能车仿真培训（六）—— 修改模型参数

前面的所有教程已经完全足够你上手修改模型和自己写Python脚本来驱动小车了，这里因为单片机主要还是支持c语言，所以这一节讲下用c++来驱动。

注意！这一章节由于我debug过程略微有点曲折，可能中间细节没记录，大家有问题在评论区留言，我会提供解决方案！！！

创建cpp程序包

cd ~/smartcar_ws/src
catkin_create_pkg buptsmartcar_cpp roscpp geometry_msgs cv_bridge image_transport

Tips:

·   程序包名，buptsmartcar_cpp；依赖消息类型roscpp和geometry_msgs,cv_bridge,image_transport

·   这里可以看到src下多了个文件夹buptsmartcar_cpp：

准备

安装opencv

sudo apt-get install libopencv-dev

修改CMakeList.txt文件（这就是用c++的麻烦之处）

sudo gedit ~/smartcar_ws/src/buptsmartcar_cpp/CMakeLists.txt

CMakeLists.txt如下(这里我把所有的注释都删了，实在太长了)你们把他复制进去替换你的内容：

cmake_minimum_required(VERSION 2.8.3)
project(buptsmartcar_cpp)
set(OpenCV_DIR /opt/ros/kinetic/share/OpenCV-3.3.1-dev/)
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  OpenCV
  roscpp
  cv_bridge
  image_transport
)
catkin_package(
)
include_directories(
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)
add_executable(findLine src/findLine.cpp)
target_link_libraries(findLine 
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
)
add_dependencies(findLine findLine_tutorials_generate_messages_cpp)

创建cpp巡线文件

gedit ~/smartcar_ws/src/buptsmartcar_cpp/src/findLine.cpp

findLine.cpp如下（解释见代码注释）：

#include <ros/ros.h>
#include <stdio.h>
#include "std_msgs/Float64.h"
#include "sensor_msgs/Image.h"
#include "geometry_msgs/Twist.h"
#include <opencv-3.3.1-dev/opencv2/core.hpp>
#include <opencv-3.3.1-dev/opencv/highgui.h>
#include <opencv-3.3.1-dev/opencv2/opencv.hpp>
#include "cv_bridge/cv_bridge.h"
#include<sensor_msgs/image_encodings.h>
#include<image_transport/image_transport.h>
using namespace cv;//opencv的名字空间

//巡线的类
class Follower{

  public:
	ros::NodeHandle node;//ros节点
	static ros::Publisher cmdpub;//发布速度
	static ros::Subscriber imageSub;//订阅传感器图片
	
	//图片处理的回调函数
	static void image_callback(const sensor_msgs::ImageConstPtr& msg);
	//速度控制函数
	static void speed_contrl(float speed_car,float angular_car);
	//类的构造函数
	Follower();
};

//初始化静态成员变量
ros::Publisher Follower::cmdpub;
ros::Subscriber Follower::imageSub;
//main
int main(int argc, char* argv[])
{
	//初始化节点
	ros::init(argc,argv,"follower_cpp");
	//调用构造函数
	Follower f;
	//ros机制，程序不退出一直执行回调函数
	ros::spin();	
}

//传感器图片回调函数
void Follower::image_callback(const sensor_msgs::ImageConstPtr& msg)
{
	//定义从ros图片转成opencv图片的指针
	cv_bridge::CvImagePtr cv_ptr;
		//捕获异常
        try
        {
        	//将ros图片转换成opencv支持的类型
            cv_ptr =  cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); 
        }
        catch(cv_bridge::Exception& e)//处理异常
        {
            ROS_ERROR("cv_bridge exception: %s", e.what());
            return;
	}
	//定义两个Mat变量来使用
	Mat hsv = cv_ptr->image.clone();
	Mat mask = cv_ptr->image.clone();
	//原图转成hsv图
	cvtColor(cv_ptr->image, hsv, COLOR_BGR2HSV);
	//hsv参数
    double low_H = 0;
    double low_S = 0;
    double low_V = 221;
    double high_H = 180;
    double high_S = 30;
    double high_V = 255;
    //从hsv中提取白色
	inRange(hsv, Scalar(low_H, low_S, low_V), Scalar(high_H, high_S, high_V), mask);

	//------------！！！这里写你的图像处理函数！！！------------//

	//处理结果输出给速度控制
	speed_contrl(0.2,0.2);
    
    //就是想show一下
	imshow("mask",mask);
	waitKey(3);
}

//速度控制函数
void Follower::speed_contrl(float speed_car,float angular_car)
{
		//配置参数
		geometry_msgs::Twist twist;
        geometry_msgs::Vector3 linear;
        linear.x=speed_car;
        linear.y=0;
        linear.z=0;
        geometry_msgs::Vector3 angular;
        angular.x=0;
        angular.y=0;
        angular.z=angular_car;
        twist.linear=linear;
        twist.angular=angular;
        //发布速度
        cmdpub.publish(twist);
}
//构造函数
Follower::Follower()
{
	//给静态成员赋值
	cmdpub = node.advertise<geometry_msgs::Twist>("cmd_vel", 10, true);
	imageSub = node.subscribe("camera/image_raw",10,image_callback);
}

编译工程

这里要非常非常注意了！！！！，cpp文件和Python不一样，写c++程序你做了一点修改你就必须去编译一下

cd ~/smartcar_ws
catkin_make
source ./devel/setup.bash

这样就ok了

运行程序

rosrun buptsmartcar_cpp findLine

这里也要注意了，findLine是不用加.cpp的哈

效果如下：

这很明显是我的hsv参数设置的不太好，这里就是个演示效果就不去细细调整了。

上面写的程序就是让小车原地旋转哈，没有加上巡线的代码，就把之前的python脚本翻译了一下，大家可以自己扩展。

结束

下一节：北邮智能车仿真培训（八）—— 两轮摄像头平衡车