ROS通信编程+OpenCV

一 话题编程

1.1 实现一个发布者

代码：

#include <sstream>
#include "ros/ros.h"
#include "std_msgs/String.h"

int main(int argc, char **argv){
    //ROS节点初始化
    ros::init(argc, argv, "talker");
    //创建节点句柄
    ros::NodeHandle n;
    //创建publisher
    ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
    //设置循环的频率
    ros::Rate loop_rate(10);

    int count = 0;
    while(ros::ok()){
        std_msgs::String msg;
        std::stringstream ss;
        ss << "hello world "<< count;
        msg.data = ss.str();

        ROS_INFO("%s", msg.data.c_str());
        chatter_pub.publish(msg);

        //循环等待回调函数
        ros::spinOnce();

        loop_rate.sleep();
        ++count;
    }

    return 0;
}

1.2 实现一个订阅者

#include "ros/ros.h"
#include "std_msgs/String.h"

//接受到订阅的消息后，会进入消息回调函数
void chatterCallback(const std_msgs::String::ConstPtr& msg){
    //将接受到的消息打印出来
    ROS_INFO("I heard: [%s]", msg->data.c_str());
}

int main(int argc, char **argv){
    ros::init(argc, argv, "listener");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
    ros::spin();
    return 0;
}

1.3 修改CMakeList.txt文件编译运行

二 服务编程（自定义数据类型）

2.1 自定义数据

其中的---是上面是请求数据，下面是响应数据。ros根据这个符号解析数据。

• package.xml文件中加上：
  
  编译时需要message_generation依赖，运行时需要message_runtime依赖。作用是把msg、srv文件转化成c++,py格式可编译执行。

附：
ROS自带的消息结构：

• CMakeLists.txt文件中
  
  使用find_package（）查找包catkin的所有依赖项。
  
  添加运行时的依赖message_runtime
  
  添加自定义的文件
  
  
  之后回到catkin_ws工作空间目录下catkin_make一下。编译成功后就可以在devel/include/learning_service文件夹下看到这几个头文件
  

2.2 实现服务端

#include "ros/ros.h"
#include "learning_service/AddTwoInts.h"

bool addCallback(learning_service::AddTwoInts::Request &req,
learning_service::AddTwoInts::Response &res){
    res.sum = req.a +req.b;
    ROS_INFO("request: x = %ld, y = %ld", (long int)req.a, (long int)req.b);
    ROS_INFO("sending back response: [%ld]", (long int)res.sum);

    return true;
}

int main(int argc, char **argv){
    ros::init(argc, argv, "add_two_ints_server");
    ros::NodeHandle n;
    //创建server，等待/add_two_ints管道请求，注册回调函数
    ros::ServiceServer server = n.advertiseService("/add_two_ints", addCallback);

    ROS_INFO("Ready to add two ints.");
    ros::spin();//循环等待callback，不会return。除非ctrl+c结束服务端

    return 0;
}

2.3 实现客户端

#include <cstdlib>
#include "ros/ros.h"
#include "learning_service/AddTwoInts.h"

int main(int argc, char **argv){
    ros::init(argc, argv, "add_two_ints_client");

    //从终端命令行获取两个数
    //argc表示命令行参数的数量，argc是一个指向字符串数组的指针，通常情况下argc的值至少为1
    //因为第一个参数是程序的名称，所以还希望再输入XY
    if(argc != 3){
        ROS_INFO("usage: add_two_ints_client X Y");
        return 1;
    }

    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<learning_service::AddTwoInts>("/add_tow_ints");

    //定义发送数据值
    //atoll函数将作为参数传递给函数调用的C类型字符串转换为长整型整数。
    learning_service::AddTwoInts srv;
    srv.request.a = atoll(argv[1]);
    srv.request.b = atoll(argv[2]);

    if(client.call(srv))
    {
        ROS_INFO("Sun: %ld", (long int)srv.response.sum);
    }else{
        ROS_ERROR("Failed to call service add_tow_ints");
        return 1;
    }

    return 0;
}

2.4 修改CMakeLists.txt运行

• 开启roscore
  roscore
• 运行服务端
  rosrun learning_service server
  
• 运行客户端
  rosrun learning_service client 3 5
  
  

三 动作编程

练习ROS动作编程：客户端发送一个运动坐标，模拟机器人运动到目标位置的过程。包括服务端和客户端的代码实现，要求带有实时位置反馈。同时注意学习cmakelists 的使用方法。

3.1 自定义数据

# 定义目标
float64 turtle_target_x
float64 turtle_target_y
float64 turtle_target_theta
---
# 定义结果
float64 turtle_final_x
float64 turtle_final_y
float64 turtle_final_theta
---
# 定义周期反馈的消息
float64 persent_turtle_x
float64 persent_turtle_y
float64 persent_turtle_theta

修改package.xml和CMakeLists.txt文件，和前面服务通信相同，编译后会产生这几个文件

3.2 动作服务器

需要实现的功能是让乌龟移动到指定的地方，并且一定时间反馈位置信息给客户端。结束的标志时，当距离目标<0.1时就会停止，也就是这个动作全部完成了。那么就需要知道当前位置距离终点的距离大小。获取当前位置就需要使用话题通信，同时在这个服务器中开启一个subscriber订阅Pose信息（在回调函数中处理）。所以整体代码如下：

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "learning_action/TurtleMoveAction.h"
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>

typedef actionlib::SimpleActionServer<learning_action::TurtleMoveAction> Server;
struct  Myturtle
{
    float x;
    float y;
    float theta;
} turtle_original_pose, turtle_target_pose;
//速度的发布者
ros::Publisher turtle_vel;

void poseCallback(const turtlesim::PoseConstPtr& msg){
    ROS_INFO("Turtle1_position:(%f, %f, %f)", msg->x, msg->y, msg->theta);
    turtle_original_pose.x = msg->x;
    turtle_original_pose.y = msg->y;
    turtle_original_pose.theta = msg->theta;
}

void execute(const learning_action::TurtleMoveGoalConstPtr& goal, Server* as){
    ros::Rate r(10);
    learning_action::TurtleMoveFeedback feedback;

    ROS_INFO("TurtleMove is working.");
    turtle_target_pose.x = goal->turtle_target_x;
    turtle_target_pose.y = goal->turtle_target_y;
    turtle_target_pose.theta = goal->turtle_target_theta;

    //发布速度控制指令
    geometry_msgs::Twist vel_msg;
    float break_flag;
    while(1){
        vel_msg.angular.z = 4.0 * (atan2(turtle_target_pose.y - turtle_original_pose.y,
                                        turtle_target_pose.x - turtle_original_pose.x)-turtle_original_pose.theta);
        //欧式距离计算
        vel_msg.linear.x = 0.5 * sqrt(pow(turtle_target_pose.x - turtle_original_pose.x,2)+
                                        pow(turtle_target_pose.y - turtle_original_pose.y,2));
        //当小乌龟靠近目标后就停止
        break_flag =sqrt(pow(turtle_target_pose.x - turtle_original_pose.x,2) + pow(turtle_target_pose.y - turtle_original_pose.y, 2));
        turtle_vel.publish(vel_msg);

        feedback.persent_turtle_x = turtle_original_pose.x;
        feedback.persent_turtle_y = turtle_original_pose.y;
        feedback.persent_turtle_theta = turtle_original_pose.theta;
        as->publishFeedback(feedback);
        ROS_INFO("break_flag = %f", break_flag);
        if (break_flag < 0.1) break;
        r.sleep();
    }
    as->setSucceeded();
}

int main(int argc, char** argv){
    ros::init(argc, argv, "TurtleMove_server");
    ros::NodeHandle n, turtle_node;
    //订阅位置
    ros::Subscriber sub = turtle_node.subscribe("turtle1/pose",10,&poseCallback);
    
    //发布乌龟的速度
    turtle_vel = turtle_node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);

    //定义一个服务器
    Server server(n, "TurtleMove", boost::bind(&execute, _1, &server), false);

    server.start();
    ROS_INFO("server has started.");
    ros::spin();

    return 0;
}

3.3 动作客户端

客户端需要做的时给服务端提供任务的目标，告诉服务端终点的位置，并且接收服务端的反馈信息并显示到bash中。整体代码如下：

#include <actionlib/client/simple_action_client.h>
#include "learning_action/TurtleMoveAction.h"
#include <turtlesim/Pose.h>//位置
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>//topic数据

typedef actionlib::SimpleActionClient<learning_action::TurtleMoveAction> Client;
// struct  Myturtle
// {
//     float x;
//     float y;
//     float theta;
// } turtle_present_pose;

//当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state, const learning_action::TurtleMoveResultConstPtr& result)
{
    ROS_INFO("Yay! The TurtleMove is finished!");
    ros::shutdown();
}

//当action激活会会调用该回调函数
void activeCb()
{
    ROS_INFO("Goal just went active");
}

//受到feedback后调用
void feedbackCb(const learning_action::TurtleMoveFeedbackConstPtr& feedback)
{
    ROS_INFO("present_pose : x:%f y:%f theta:%f", feedback->persent_turtle_x, feedback->persent_turtle_y, feedback->persent_turtle_theta);
}

int main(int argc, char **argv){
    ros::init(argc, argv, "TurtleMove_client");

    //定义一个客户端
    Client client("TurtleMove", true);

    //等待服务器端
    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");

    //定义goal数据
    learning_action::TurtleMoveGoal goal;
    goal.turtle_target_x = 1;
    goal.turtle_target_y = 1;
    goal.turtle_target_theta = 0;

    //发送goal
    client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);

    ros::spin();
    return 0;
}

3.4 修改CMakeLists.txt以及运行

添加：

add_executable(TurtleMove_client src/TurtleMove_client.cpp)
target_link_libraries(TurtleMove_client ${catkin_LIBRARIES})
add_dependencies(TurtleMove_client ${PROJECT_NAME}_gencpp)  

add_executable(TurtleMove_server src/TurtleMove_server.cpp)
target_link_libraries(TurtleMove_server ${catkin_LIBRARIES})
add_dependencies(TurtleMove_server ${PROJECT_NAME}_gencpp) 

运行：
roscore
rosrun turtlesim turtlesim_node
rosrun learning_action TurtleMove_server
rosrun learing_action TurtleMove_client

四 Opencv

三. 每一个程序背后都站着一堆优秀的代码库。通过学习opencv图像库编程，了解如何借助第三方库函数完成一个综合程序设计。“学了opencv，妈妈再不担忧你不会图像编程啦！”。在VMware虚拟机Ubuntu系统下练习编译、安装著名的C/C++图像处理开源软件库 Opencv3.x 。安装成功后：

1. 编写一个打开图片进行特效显示的代码 test1.cpp（见opencv编程参考资料 ）； 注意gcc编译命令: gcc test1.cpp -o test1 pkg-config --cflags --libs opencv
   请解释这条编译命令，它是如何获得opencv头文件、链接lib库文件的路径的？
2. 练习使用opencv库编写打开摄像头压缩视频的程序。参考示例代码1和示例代码2。并回答：
   1）如果要求打开你硬盘上一个视频文件来播放，请问示例代码1第7行代码如何修改？
   2）在示例代码1第9行的while循环中，Mat是一个什么数据结构？ 为什么一定要加一句waitKey延时代码，删除它行不行？
   3）示例代码1代码会在while循环中一直运行，你如果试图用鼠标关闭图像显示窗口，会发现始终关不掉。需要用键盘Ctrl+C 强制中断程序，非常不友好。如何改进？

4.1 特效显示

代码如下：

#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;
int main(int argc, char** argv)
{
	CvPoint center;
    double scale = -3; 

	IplImage* image = cvLoadImage("lena.jpg");
	argc == 2? cvLoadImage(argv[1]) : 0;
	
	cvShowImage("Image", image);
	
	
	if (!image) return -1; 	center = cvPoint(image->width / 2, image->height / 2);
	for (int i = 0;i<image->height;i++)
		for (int j = 0;j<image->width;j++) {
			double dx = (double)(j - center.x) / center.x;
			double dy = (double)(i - center.y) / center.y;
			double weight = exp((dx*dx + dy*dy)*scale);
			uchar* ptr = &CV_IMAGE_ELEM(image, uchar, i, j * 3);
			ptr[0] = cvRound(ptr[0] * weight);
			ptr[1] = cvRound(ptr[1] * weight);
			ptr[2] = cvRound(ptr[2] * weight);
		}

	Mat src;Mat dst;
	src = cvarrToMat(image);
	cv::imwrite("test.png", src);

    cvNamedWindow("test",1);  	imshow("test", src);
	 cvWaitKey();
	 return 0;
}

编译文件：

gcc test1.cpp -o test1 `pkg-config --cflags --libs opencv`

使用pkg-config工具获取opencv库，这个命令会返回包含opencv库路径、头文件路径和依赖的库文件路径等信息的字符串。
gcc + 文件名 + -o＋输出文件流名称＋支持包g++＋ 文件名 + -o＋输出文件流名称 + 支持包
其中pkg-config是一个linux下的命令，用于获得某一个库/模块的所有编译相关的信息(包括头文件和库的所有信息)—cflags是用来指定程序在编译时所需要头文件所在的目录
—libs则是指定程序在链接时所需要的动态链接库的目录

4.2 摄像头压缩视频

• 示例代码1：

#include <opencv2/opencv.hpp>
using namespace cv;
int main()
{
	//从摄像头读取视频
	VideoCapture capture("man.mp4");
	//循环显示每一帧
	while(1){
		Mat frame;//定义一个Mat变量，用于存储每一帧的图像
		capture >> frame;//读取当前帧
		if(frame.empty())//播放完毕，退出
			break;
		imshow("读取视频帧",frame);//显示当前帧
		waitKey(30);//掩饰30ms
	}
	system("pause");
	return 0;
}

1. 如果要打开硬盘上一个视频文件播放需要在路径前面加上..表示上一层目录。
2. Mat是点阵（矩阵）。waitKey(30)的作用不断刷新图像，如果没有这个就无法实现画面的实时显示，即看不到画面的变化。
3. 可以增加一个判断语句，判断键盘输入的字符，如果符合就break。

4.3 摄像头

/*********************************************************************
打开电脑摄像头，空格控制视频录制，ESC退出并保存视频RecordVideo.avi
*********************************************************************/
#include<iostream>
#include <opencv2/opencv.hpp>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
using namespace cv;
using namespace std;

int main()
{
	//打开电脑摄像头
	VideoCapture cap(0);
	if (!cap.isOpened())
	{
		cout << "error" << endl;
		waitKey(0);
		return 0;
	}

	//获得cap的分辨率
	int w = static_cast<int>(cap.get(CV_CAP_PROP_FRAME_WIDTH));
	int h = static_cast<int>(cap.get(CV_CAP_PROP_FRAME_HEIGHT));
	Size videoSize(w, h);
	VideoWriter writer("RecordVideo.avi", CV_FOURCC('M', 'J', 'P', 'G'), 25, videoSize);
	
	Mat frame;
	int key;//记录键盘按键
	char startOrStop = 1;//0  开始录制视频； 1 结束录制视频
	char flag = 0;//正在录制标志 0-不在录制； 1-正在录制

	while (1)
	{
		cap >> frame;
		key = waitKey(100);
		if (key == 32)//按下空格开始录制、暂停录制   可以来回切换
		{
			startOrStop = 1 - startOrStop;
			if (startOrStop == 0)
			{
				flag = 1;
			}
		}
		if (key == 27)//按下ESC退出整个程序，保存视频文件到磁盘
		{
			break;
		}

		if (startOrStop == 0 && flag==1)
		{
			writer << frame;
			cout << "recording" << endl;
		}
		else if (startOrStop == 1)
		{
			flag = 0;
			cout << "end recording" << endl;
			
		}
		imshow("picture", frame);
	}
	cap.release();
	writer.release();
	destroyAllWindows();
	return 0;
}

五 总结

进一步了解了OpenCV，走进了计算机视觉的世界。通过运行程序进一步了解了ROS通信机制的原理。如何采用代码控制通信操作等。

参考资料

1. ROS kinetic 动作编程: 客户端发送一个运动目标，模拟机器人运动到目标位置的过程
2. GCC背后的故事&OpenCV相识何必曾相逢
3. Ubuntu系统下基于opencv完成图像程序编程【入门篇】