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

421
4
2020年5月6日 16时05分

前言

 

上一节:北邮智能车仿真培训(六)—— 修改模型参数

 

前面的所有教程已经完全足够你上手修改模型和自己写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

 

北邮智能车仿真培训(七)—— 使用c++编写程序插图

 

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

 

北邮智能车仿真培训(七)—— 使用c++编写程序插图(1)

 

准备

 

安装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

 

北邮智能车仿真培训(七)—— 使用c++编写程序插图(2)

 

这样就ok了

 

运行程序

rosrun buptsmartcar_cpp findLine

 

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

 

效果如下:

 

北邮智能车仿真培训(七)—— 使用c++编写程序插图(3)

 

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

 

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

 

结束

 

下一节:北邮智能车仿真培训(八)—— 两轮摄像头平衡车

发表评论

后才能评论

评论列表(4条)

  • u3oey_0531 2020年5月11日 下午5:36

    请问你的博客是哪个?想多关注一些相关的内容

  • 布剪刀石头 2020年5月7日 上午10:07

    double low_H = 0;
    double low_S = 0;
    double low_V = 221;
    这里low_v改成100过滤效果更好

  • 如梦初醒 2020年5月6日 下午4:32

    坐等更新ing