ROS通信编程

一、按照课件内容，分别完成话题、服务模式的ROS示例程序，并实践练习

工作空间

存放工程开发相关文件的文件夹，其中主要包括src，build，devel，install文件夹。

文件夹名称	用途
src	代码空间（Source Space )
build	编译空间（Build Space）
devel	开发空间（Development Space）
install	安装空间（Install Space）
1.创建工作空间

	mkdir -p ~/catkin_ws/src#创建文件夹
	cd ~/catkin_ws/src#进入目录
	catkin_init_workspace#初始化，使其成为ROS的工作空间

2.编译工作空间

cd ..
catkin_make

3.设置环境变量

source /home/lyy/catkin_ws/devel/setup.bash#该环境变量设置只对当前终端有效，lyy是用户名
#将上面命令放置到~/.bashrc文件中，让其对所有终端都有效
sudo nano ~/.bashrc

4.检查环境变量

echo $ROS_PACKAGE_PATH

功能包

1.创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_communication std_msgs rospy roscpp
#catkin_create_pkg 功能包名字 依赖
#std_msgs：定义的标准的数据结构
#rospy：提供python编程接口 
#roscpp：提供c++编程接口

2.编译功能包

cd ~/catkin_ws
catkin_make

说明：
在同一个工作空间下，不允许存在同名的功能包
不同工作空间，可以存在同名的功能包

ROS通信编程

1.话题编程

步骤：

• 创建发布者
  ①初始化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文件


编译

运行可执行文件

roscore
rosrun learning_communication talker
rosrun learning_communication listener

2.服务编程

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

• 定义srv文件

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

AddTwoInts.srv

int64 a
int64 b
---
int64 sum

• 在package.xml中添加功能包依赖

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

说明：部分ROS版本中exec_depend需要修改为run_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;
}

设置CMakeLists.txt文件

编译

catkin_make

运行可执行文件
先运行server，再运行client

roscore
rosrun learning_communication server
rosrun learning_communication client 整数1 整数2

3.动作编程

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

接口名称	接口含义
goal	发布任务目标
cancel	请求取消任务
status	通知客户端当前的状态
feedback	周期反馈任务运行的监控数据
result	向客户端发送任务的执行结果，只发布一次
自定义动作消息

• 定义action文件

mkdir ~/catkin_ws/src/learning_communication/action
sudo nano DoDishes.action

DoDishes.action

#定义目标信息
uint32 dishwasher_id
---
#定义结果信息
uint32 total_dishes_cleaned
---
#定义周期反馈的消息
float32 percent_complete

• 在package.xml中添加功能包依赖

<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>

• 在CMakeLists.txt添加编译选项
  
  
  步骤：
• 创建动作服务器
  ①初始化ROS节点
  ②创建动作服务器实例
  ③启动服务器，等待动作请求
  ④在回调函数中完成动作服务功能的处理，并反馈进度信息
  ⑤动作完成，发送结束信息
• 创建动作客户端
  ①初始化ROS节点
  ②创建动作客户端实例
  ③连接动作服务器
  ④发送动作目标
  ⑤根据不同类型的服务器端反馈处理回调函数
• 添加编译选项
  ①设置需要编译的代码和生成的可执行文件
  ②设置链接库
  ③设置依赖
• 运行可执行程序
  DoDishes_server.cpp

#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"
#include "learning_communication/DoDishesAction.h"
typedef actionlib::SimpleActionServer<learning_communication::DoDishesAction> Server;
// 收到action的goal后调用该回调函数
void execute(const learning_communication::DoDishesGoalConstPtr &goal, Server *as)
{
	ros::Rate r(1);
	learning_communication::DoDishesFeedback feedback;
	ROS_INFO("Dishwasher %d is working.", goal->dishwasher_id);
	// 假设洗盘子的进度，并且按照1Hz的频率发布进度feedback 
	for(int i = 1; i <= 10; i++)
	{
		feedback.percent_complete = i * 10;
		as->publishFeedback(feedback);
		r.sleep();
	}	
	// 当action完成后，向客户端返回结果
	ROS_INFO("Dishwasher %d finish working.", goal->dishwasher_id);
	as->setSucceeded();
}
int main(int argc, char **argv)
{
	ros::init(argc, argv, "do_dishes_server");
	ros::NodeHandle hNode;
	// 定义一个服务器
	Server server(hNode, "do_dishes", boost::bind(&execute, _1, &server), false);
	// 服务器开始运行
	server.start();
	ros::spin();
	return 0;
}

DoDishes_client.cpp

#include "ros/ros.h"
#include "actionlib/client/simple_action_client.h"
#include "learning_communication/DoDishesAction.h"
typedef actionlib::SimpleActionClient<learning_communication::DoDishesAction> Client;
// 当action完成后会调用该回调函数一次
void doneCallback(const actionlib::SimpleClientGoalState &state
	, const learning_communication::DoDishesResultConstPtr &result)
{
	ROS_INFO("Yay! The dishes are now clean");
	ros::shutdown();
}
// 当action激活后会调用该回调函数一次
void activeCallback()
{
	ROS_INFO("Goal just went active");
}
// 收到feedback后调用该回调函数
void feedbackCallback(const learning_communication::DoDishesFeedbackConstPtr &feedback)
{
	ROS_INFO("percent_complete : %f", feedback->percent_complete);
}
int main(int argc, char **argv)
{
	ros::init(argc, argv, "do_dishes_client");
	// 定义一个客户端
	Client client("do_dishes", true);
	// 等待服务器端
	ROS_INFO("Waiting for action server to start.");
	client.waitForServer();
	ROS_INFO("Action server started, sending goal.");
	// 创建一个 action 的 goal
	learning_communication::DoDishesGoal goal;
	goal.dishwasher_id = 1;
	// 发送action的goal给服务端，并且设置回调函数
	client.sendGoal(goal, &doneCallback, &activeCallback, &feedbackCallback);
	ros::spin();
	return 0;
}

设置CMakeLists.txt文件

编译


运行可执行文件

roscore
rosrun learning_communication DoDishes_client
rosrun learning_communication DoDishes_server

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

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

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

typedef actionlib::SimpleActionServer<learning_communication::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_communication::TurtleMoveGoalConstPtr& goal, Server* as){
    ros::Rate r(10);
    learning_communication::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;
}、

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

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

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

//当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state, const learning_communication::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_communication::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_communication::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;
}

设置CMakeLists.txt文件

编译


运行可执行文件

roscore
rosrun turtlesim turtlesim_node
rosrun learning_communication TurtleMove_server
rosrun learning_communication TurtleMove_client

三. 每一个程序背后都站着一堆优秀的代码库。通过学习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 强制中断程序，非常不友好。如何改进？

1.编写一个打开图片进行特效显示的代码

1.1撰写代码

创建一个文件夹用于存放代码文件code，并创建一个test1.cpp文件

mkdir code
cd code
gedit test1.cpp

test1.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;
}

1.2编译文件

gcc编译器：gcc + 文件名 + -o + 输出文件流名称 +` 支持包

在code文件夹下准备一张图片，我在这里命名为lena.jpg

输出结果执行以下命令：

./test1

code文件夹下有test.png

1.3关于编译的命令

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

pkg-config

1. 检查库的版本号。如果所需要的库的版本不满足要求，它会打印出错误信息，避免链接错误版本的库文件。
2. 获得编译预处理参数，如宏定义，头文件的位置。
3. 获得链接参数，如库及依赖的其它库的位置，文件名及其它一些连接参数。
4. 自动加入所依赖的其它库的设置。

--cflags
用来指定程序编译时所需要的头文件所在目录

--lib
指定程序所需要的动态链接库目录

2.使用opencv库编写打开摄像头压缩视频的程序

2.1虚拟机获取摄像头权限

使用快捷键Win+R，输入services.msc，确定
找到VMware USB Arbitration S…

在虚拟机中设置，USB控制器兼容性设置为USB3.0。

单击右下角摄像头图标

连接

1.2播放视频

创建test2.cpp
test2.cpp

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

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

在code文件夹下放置一个视频man.mp4
编译test2.cpp文件

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

运行

./test2

播放man.mp4视频