ROS Kinetic通信编程：话题、服务、动作编程 opencv学习使用

接上篇，继续学习ROS通信编程基础

一、话题编程

步骤：

• 创建发布者
  • 初始化ROS节点
  • 向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型
  • 按照一定频率循环发布消息
• 创建订阅者
  • 初始化ROS节点
  • 订阅需要的话题
  • 循环等待话题消息，接受到消息后进行回调函数
  • 回调函数中完成消息处理
• 添加编译选项
  • 设置需要编译的代码和生成的可执行文件
  • 设置链接库
  • 设置依赖
• 运行可执行程序

talker.cpp

#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，发布名为chatter的topic，消息类型为std_msgs::String
	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类型的消息
		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;
}

listener.cpp

#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节点
	ros::init(argc,argv,"listener");
	//创建节点句柄
	ros::NodeHandle n;
	//创建一个Subscriber，订阅名为chatter的topic，注册回调函数chatterCallback
	ros::Subscriber sub=n.subscribe("chatter",1000,chatterCallback);
	//循环等待回调函数
	ros::spin();
	return 0;
}

在CMakeLists.txt末尾添加编译选项

add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})

编译

cd catkin_ws
catkin_make

运行程序

# 以下是对于Ubantu 16.04的操作，其他版本的也许操作会简洁很多
roscore
#打开新终端
cd ~/catkin_ws
#下面这一步是为了保证rosrun命令能够找到相应的功能包，有可以省去这一步骤的方法，各位可以自行查找
source ~/catkin_ws/devel/setup.bash
rosrun learning_communication talker
#打开新终端
cd ~/catkin_ws
source ~/catkin_ws/devel/setup.bash
rosrun learning_communication listener

如图，发送了hello world的同时接收了hello world。

二、服务编程

定义服务请求与应答的方式

• 定义srv文件

   mkdir ~/catkin_ws/src/learning_communication/srv
   sudo nano AddTwoInts.srv
  

• AddTwoInts.srv

  int64 a
  int64 b
  ---
  int64 sum
  

• 用gedit打开package.xml，在里面添加功能包依赖

  <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>
  

• 在CMakeLists.txt添加编译选项
  
  

步骤：

• 创建服务器
  • 初始化ROS节点
  • 创建Serve实例
  • 循环等待服务请求，进入回调函数
  • 在回调函数中完成服务功能的处理，并反馈应答数据
• 创建客户端
  • 初始化ROS节点
  • 创建一个Client实例
  • 发布服务请求数据
  • 等待Serve处理之后的应答结果
• 添加编译选项
  • 设置需要编译的代码和生成的可执行文件
  • 设置链接库
  • 设置依赖
• 运行可执行程序
  server.cpp

#include<ros/ros.h>
#include"learning_communication/AddTwoInts.h"
//service回调函数，输入参数req，输出参数res
bool add(learning_communication::AddTwoInts::Request &req,learning_communication::AddTwoInts::Response &res)
{
	//将输入的参数中的请求数据相加，结果放到应答变量中
	res.sum=req.a+req.b;
	ROS_INFO("request: x=%1d,y=%1d",(long int)req.a,(long int)req.b);
	ROS_INFO("sending back response:[%1d]",(long int)res.sum);
	return true;
}
int main(int argc,char **argv)
{
	//ROS节点初始化
	ros::init(argc,argv,"add_two_ints_server");
	//创建节点句柄
	ros::NodeHandle n;
	//创建一个名为add_two_ints的server,注册回调函数add()
	ros::ServiceServer service=n.advertiseService("add_two_ints",add);
	//循环等待回调函数
	ROS_INFO("Ready to add two ints.");
	ros::spin();
	return 0;
}

client.cpp

#include<cstdlib>
#include<ros/ros.h>
#include"learning_communication/AddTwoInts.h"
int main(int argc,char **argv)
{
	//ROS节点初始化
	ros::init(argc,argv,"add_two_ints_client");
	//从终端命令行获取两个加数
	if(argc!=3)
	{
		ROS_INFO("usage:add_two_ints_client X Y");
		return 1;
	}
	//创建节点句柄
	ros::NodeHandle n;
	//创建一个client，请求add_two_ints_service
	//service消息类型是learning_communication::AddTwoInts
	ros::ServiceClient client=n.serviceClient<learning_communication::AddTwoInts>("add_two_ints");
	//创建learning_communication::AddTwoInts类型的service消息
	learning_communication::AddTwoInts srv;
	srv.request.a=atoll(argv[1]);
	srv.request.b=atoll(argv[2]);
	//发布service请求，等待加法运算的应答请求
	if(client.call(srv))
	{
		ROS_INFO("sum: %1d",(long int)srv.response.sum);
	}
	else
	{
		ROS_INFO("Failed to call service add_two_ints");
		return 1;
	}
	return 0;
}

关于编译时一直出现这样的报错，注意看是不是有些比如这个符号“_”没打。

添加编译设置

编译通过

输入指令

roscore
#打开新终端
source ~/catkin_ws/devel/setup.bash
rosrun learning_communication server
#打开新终端
source ~/catkin_ws/devel/setup.bash
rosrun learning_communication client 11 12

三、动作编程

动作是一种基于ROS消息实现的问答通信机制，它包含连续反馈，可以在任务过程中止运行。
动作（Action）的接口

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

创建工作区间

#创建功能包
cd catkin_ws/src/
catkin_create_pkg learn_action std_msgs rospy roscpp
#编译功能包
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

创建action文件夹，并在里面创建TurtleMove.action文件

# Define the goal 
float64 turtle_target_x  
# Specify Turtle's　target position 
float64 turtle_target_y 
float64 turtle_target_theta 
---
# Define the result 
float64 turtle_final_x 
float64 turtle_final_y 
float64 turtle_final_theta 
--- 
# Define a feedback message 
float64 present_turtle_x 
float64 present_turtle_y 
float64 present_turtle_theta

在learn_action的src文件夹下，创建TurtleMove_server.cpp文件和TurtleMove_client.cpp文件

TurtleMove_server.cpp

  /*   　　　此程序通过通过动作编程实现由client发布一个目标位置 　　　然后控制Turtle运动到目标位置的过程  */ 
#include <ros/ros.h> 
#include <actionlib/server/simple_action_server.h> 
#include "learn_action/TurtleMoveAction.h" 
#include <turtlesim/Pose.h>  
#include <turtlesim/Spawn.h> 
#include <geometry_msgs/Twist.h>   
typedef actionlib::SimpleActionServer<learn_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;  
 }   
 // 收到action的goal后调用该回调函数 
 void execute(const learn_action::TurtleMoveGoalConstPtr& goal, Server* as) 
 {     
 	learn_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_msgs;     
 	float break_flag;          
 	while(1)     
 	{           
 		ros::Rate r(10);                  
 		vel_msgs.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_msgs.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_msgs);
 		           
 		feedback.present_turtle_x=turtle_original_pose.x;         
 		feedback.present_turtle_y=turtle_original_pose.y;         
 		feedback.present_turtle_theta=turtle_original_pose.theta;         
 		as->publishFeedback(feedback);         
 		ROS_INFO("break_flag=%f",break_flag);         if(break_flag<0.1) break;         r.sleep();     }         // 当action完成后，向客户端返回结果         ROS_INFO("TurtleMove is finished.");         
 		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;
} 

TurtleMove_client.cpp

#include <actionlib/client/simple_action_client.h> 
#include "learn_action/TurtleMoveAction.h" 
#include <turtlesim/Pose.h>  
#include <turtlesim/Spawn.h> 
#include <geometry_msgs/Twist.h>   
typedef actionlib::SimpleActionClient<learn_action::TurtleMoveAction> Client;   
struct Myturtle 
{     
	float x;     
	float y;   
	float theta; 
}turtle_present_pose;   
// 当action完成后会调用该回调函数一次 
void doneCb(const actionlib::SimpleClientGoalState& state,         const learn_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 learn_action::TurtleMoveFeedbackConstPtr& feedback) 
{     
	ROS_INFO(" present_pose : %f  %f  %f", feedback->present_turtle_x,                    feedback->present_turtle_y,feedback->present_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.");      
	 // 创建一个action的goal     
	 learn_action::TurtleMoveGoal goal;    
	 goal.turtle_target_x = 1;     
	 goal.turtle_target_y = 1;     
	 goal.turtle_target_theta = 0;       
	 // 发送action的goal给服务器端，并且设置回调函数     
	 client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);       
	 ros::spin();      
	 return 0; 
} 

在package.xml里面添加依赖

<build_depend>message_generation</build_depend>  
<build_depend>actionlib</build_depend>  
<build_depend>actionlib_msgs</build_depend>
<exec_depend>message_runtime</exec_depend>  
<exec_depend>actionlib</exec_depend>  
<exec_depend>actionlib_msgs</exec_depend> 

添加完就是这样

修改learn_action里面的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
＃开一个新终端窗口
source ./devel/setup.bash
rosrun turtlesim turtlesim.node
＃新终端
source ./devel/setup.bash
rosrun learn_action TurtleMove_server
＃新终端
source ./devel/setup.bash
rosrun learn_action TurtleMove_client

运行结果如下

四、安装opencv 图片特效示例

官网下载Sources版本opencv-3.4.1，将下载的压缩包复制到home目录下并解压、配置

unzip opencv-3.4.1.zip

#进入文件夹
cd opencv-3.4.1

#安装依赖库和cmake ，如果提醒需要apt-get update，那就先sudo su进入root权限，再sudo apt-get update，然后在执行下面命令
sudo apt-get install cmake  
sudo apt-get install build-essential libgtk2.0-dev libavcodec-dev libavformat-dev libjpeg.dev libtiff5.dev libswscale-dev libjasper-dev  

#安装完cmake之后执行命令 ,创建编译文件夹
mkdir my_build_dir

#进入文件夹进行配置
cd my_build_dir

cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local ..

sudo make

sudo make install

sudo make install 执行完毕后OpenCV编译过程就结束了，接下来就需要配置一些OpenCV的编译环境首先将OpenCV的库添加到路径，从而可以让系统找到

sudo gedit /etc/ld.so.conf.d/opencv.conf 

执行了这个命令可能会出现一个空白文档，在里面写上

/usr/local/lib  

保存回到命令行界面
执行命令使得刚才的配置路径生效

sudo ldconfig  

配置bash

sudo gedit /etc/bash.bashrc  

在最末尾添加

PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/lib/pkgconfig  
export PKG_CONFIG_PATH  

保存，执行如下命令使得配置生效

source /etc/bash.bashrc  

更新，完成配置。

sudo updatedb  

在opencv-3.4.1下新建文件夹mytest

cd opencv-3.4.1
mkdir mytest

在这个文件下新建空白文档test.cpp

test.cpp

#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;
}

保存并编译

g++ test.cpp -o test `pkg-config --cflags --libs opencv`

如果报错可以把g++换成gcc

gcc test1.cpp -o test1：这部分是常见的使用 GCC 编译器的命令，指定编译的源代码文件为 “test1.cpp”，并将生成的可执行文件命名为 “test1”。

pkg-config --cflags --libs opencv：这部分使用了反引号（``）将命令包围起来，表示它是一个命令替换。pkg-config 是一个常用的工具，用于获取库的编译选项和链接选项。–cflags 参数告诉 pkg-config 输出编译选项，–libs 参数告诉它输出链接选项。“opencv” 参数表示要获取 OpenCV 库的选项。

通过执行 pkg-config --cflags --libs opencv 命令，pkg-config 将返回 OpenCV 库的编译选项和链接选项。这些选项包括头文件路径、链接库路径和需要链接的库文件等信息。在编译命令中使用这些选项，可以确保编译器能够找到所需的头文件和库文件，并正确链接它们。
运行

./test

输出结果如上图所示，图片lena.jpg我是在home目录和mytest目录下都存了的。

五、视频示例

1. 虚拟机获取摄像头权限

使用快捷键 Win + R ，输入 services.msc ，并回车。
找到 VMware USB Arbitration S… 服务，确保启动了。

点击 “ 虚拟机 ” ，然后点击 “ 设置(S)… ”。
选择 “ USB控制器 ” ，将 “ USB兼容性 ” 设置为 “ USB 3.x ” ，并点击确定。

选择 “ 虚拟机 ” ，再选择 “ 可移动设备 ” ，再选择最下面那个 ，最后点击 “ 连接 ” ，再弹出的窗口内点击 “ 确定 ” 。
最右边那个摄像头图标有个小绿点，就表示连接成功了。

2. 播放视频

创建文件test2.cpp

#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;
}

代码分析
VideoCapture capture(0)，后面的参数设置为 0 ，则从摄像头读取视频并循环显示每一帧；如果设置为一个视频的文件名，比如：man.mp4 ，则会将视频读取并循环显示每一帧。
while 循环体中的 Mat 数据结构其实是一个点阵，对应图像上的每一个点，点的集合形成了一帧图像，有关 Mat 详解请看：OpenCV中Mat数据结构
语句：waitKey(30) ，中的参数单位是 ms 毫秒，也就是每一帧间隔 30 ms ，该语句时不能删除的，否则会执行错误，无法播放视频或录制视频。

要打开硬盘上的一个视频文件来播放，可以修改示例代码中的第7行代码。将视频文件的路径作为参数传递给 VideoCapture 构造函数即可

// 从硬盘上读取视频文件
    VideoCapture capture("path/to/your/video/file.mp4");

示例代码1代码会在while循环中一直运行，你如果试图用鼠标关闭图像显示窗口，会发现始终关不掉。需要用键盘Ctrl+C 强制中断程序，非常不友好。如何改进？

#include <opencv2/opencv.hpp>
using namespace cv;

int main()
{
    // 从硬盘上读取视频文件
    VideoCapture capture("path/to/your/video/file.mp4");

    // 检查视频文件是否成功打开
    if (!capture.isOpened()) {
        std::cout << "无法打开视频文件" << std::endl;
        return -1;
    }

    // 创建窗口用于显示视频帧
    namedWindow("读取视频帧", WINDOW_NORMAL);

    // 循环显示每一帧
    while (1) {
        Mat frame; // 定义一个Mat变量，用于存储每一帧的图像
        capture >> frame; // 读取当前帧
        if (frame.empty()) // 播放完毕，退出
            break;
        imshow("读取视频帧", frame); // 显示当前帧

        // 等待键盘输入或检测到窗口关闭事件
        char key = waitKey(30);
        if (key == 'q' || key == 'Q' || key == 27) // 按下 'q'、'Q' 或 Esc 键退出
            break;
    }

    // 关闭窗口
    destroyAllWindows();

    return 0;
}

在改进后的代码中，我们使用 namedWindow 创建了一个可以调整大小的窗口，并在循环中使用 waitKey 等待键盘输入或窗口关闭事件。如果用户按下 ‘q’、‘Q’ 键或 Esc 键，即可退出循环，从而正常关闭程序。同时，在视频文件无法打开时，会输出错误消息并返回一个错误码。

可以将 “path/to/your/video/file.mp4” 替换为实际视频文件的路径。运行改进后的代码，可以通过按下 ‘q’、‘Q’ 键或 Esc 键来关闭图像显示窗口，从而更友好地结束程序。

准备一个小视频命名为 man.mp4 。

3. 录制视频

创建一个 test3.cpp 。

/*********************************************************************
打开电脑摄像头，空格控制视频录制，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;
}

编译并运行。

g++ test3.cpp -o test3 `pkg-config --cflags --libs opencv`
./test3