文章目录
- 一、话题编程
- 二、服务编程
- 三、动作编程
- 四、安装opencv 图片特效示例
- 五、视频示例
- 1. 虚拟机获取摄像头权限
- 2. 播放视频
- 3. 录制视频
接上篇，继续学习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
