上次我们通过机器人操作系统ROS—初探了解了ROS的基本概念和运行机制，并用内置的小龟模拟器进行了测试。本文我们开始学习如何编写自定义的Service/Client节点进行指定msg格式通信、尝试bag的录制和回放、并简单了解下三维可视化。
一、创建ROS msg消息和srv服务格式
温习一下，上一次我们了解到topic里的Message、ROS Services概念：
消息(msg):
msg文件就是一个描述ROS中所使用消息类型的简单文本。它们会被用来生成不同语言的源代码。
msg文件存放在package的msg目录下，内容实际上就是每行声明一个数据类型和变量名。可以使用的数据类型如下：
- int8, int16, int32, int64 (plus uint*)
- float32, float64
- string
- time, duration
- other msg files
- variable-length array[] and fixed-length array[C]
在ROS中有一个特殊的数据类型：Header，它含有时间戳和坐标系信息。在msg文件的第一行经常可以看到Header header的声明.
下面是一个示例msg文件，它使用了Header，string，和其他另外两个消息类型。
Header header string child_frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/TwistWithCovariance twist
服务(srv):
一个srv文件描述一项服务。它包含两个部分：请求和响应。
srv文件则存放在srv目录下，内容分为请求和响应两部分，由’—‘分隔。下面是一个示例srv：
int64 A int64 B --- int64 Sum
其中 A 和 B 是请求, 而Sum 是响应。
自定义一个msg消息格式:
cd ~/catkin_ws/src/beginner_tutorials
# 创建msg定义文件
mkdir msg
vim msg/Num.msg
string first_name
string last_name
uint8 age
uint32 score
# 配置msg/srv编译/运行依赖
vim package.xml
<build_depend>message_generation</build_depend> #将msg/srv文件编译为c++/python代码
<exec_depend>message_runtime</exec_depend> #执行依赖
vim CMakeLists.txt
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation #增加msg/srv编译依赖
)
catkin_package(
CATKIN_DEPENDS message_runtime #增加msg/srv运行依赖
)
add_message_files(
FILES
Num.msg #指定msg文件列表
)
generate_messages( #确保CMake调用generate_messages()函数
DEPENDENCIES
std_msgs
)
# 测试定义的msg消息类型能否被识别
# rosmsg show [message type]
rosmsg show beginner_tutorials/Num
string first_name
string last_name
uint8 age
uint32 score
rosmsg show Num #也可以不带package名直接查msgtype
[beginner_tutorials/Num]:
string first_name
string last_name
uint8 age
uint32 score
自定义一个srv服务参数/返回值:
roscd beginner_tutorials
# 创建一个srv服务
mkdir srv
# roscp [package_name] [file_to_copy_path] [copy_path]
# roscp rospy_tutorials AddTwoInts.srv srv/AddTwoInts.srv #从rospy_tutorials package中复制一个服务文件过来
vim srv/AddTwoInts.srv
int64 a
int64 b
---
int64 sum
# 配置srv编译/运行依赖
vim CMakeLists.txt
add_service_files(
FILES
AddTwoInts.srv
)
# 测试srv服务能否被识别
rossrv show beginner_tutorials/AddTwoInts
int64 a
int64 b
---
int64 sum
rossrv show AddTwoInts #跟rosmsg类似, 你也可以不指定具体的package名来查找服务文件
[rospy_tutorials/AddTwoInts]:
int64 a
int64 b
---
int64 sum编译msg/srv格式文件:
cd ~/catkin_ws
catkin_make
#所有在msg路径下的.msg文件都将转换为ROS所支持语言的源代码。生成的代码位置：
C++：~/catkin_ws/devel/include/beginner_tutorials/
Python：~/catkin_ws/devel/lib/python3/dist-packages/beginner_tutorials/msg/
lisp：~/catkin_ws/devel/share/common-lisp/ros/beginner_tutorials/msg/二、编写简单的消息发布器和订阅器
参考：Writing a Simple Publisher and Subscriber (C++)、Writing a Simple Publisher and Subscriber (Python)
C++版本：
roscd beginner_tutorials
mkdir -p src
# 新建消息pub发布器
vim src/talker.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream>
/**
* ROS消息pub测试
*/
int main(int argc, char **argv)
{
//初始化 ROS
ros::init(argc, argv, "talker");
//进程节点句柄
ros::NodeHandle n;
//告诉master我们将要在chatter topic上发布std_msgs/String消息类型的消息(缓冲区保留最近1000条消息)
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
//以 10Hz 的频率运行。
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();
//休眠以保持发布频率为10Hz
loop_rate.sleep();
++count;
}
return 0;
}
# 新建消息sub订阅器
vim src/listener.cpp
#include "ros/ros.h"
#include "std_msgs/String.h"
/**
* ROS消息sub测试
*/
//sub到消息时的回调
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;
//告诉master我们要订阅chatter话题上的消息
ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
//相当与waiting(有新消息、或需要退出时会跳过)
ros::spin();
return 0;
}
# 配置
vim CMakeLists.txt
include_directories(
#include
${catkin_INCLUDE_DIRS}
)
#不要修改代码顺序
add_executable(talker src/talker.cpp)
target_link_libraries(talker ${catkin_LIBRARIES})
add_dependencies(talker beginner_tutorials_generate_messages_cpp)
add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})
add_dependencies(listener beginner_tutorials_generate_messages_cpp)
# 编译
cd ~/catkin_ws
catkin_make #编译产出位置：~/catkin_ws/devel/lib/beginner_tutorials/talker、listener
# 测试运行
roscore
rosrun beginner_tutorials talker
rosrun beginner_tutorials listener
Python版本：
roscd beginner_tutorials
mkdir scripts
cd scripts
# 新建消息pub发布器
vim talker.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
File: talker.py
Desc: 消息pub发布
Author:yanjingang(yanjingang@mail.com)
Date: 2020/6/15 22:37
"""
import rospy
from std_msgs.msg import String
def talker():
# 通知master我们将要在chatter topic上发布std_msgs/String 消息类型的消息
pub = rospy.Publisher('chatter', String, queue_size=10) #queue_size：缓冲区保留的最近消息条数
# 初始化 ROS
rospy.init_node('talker', anonymous=True)
# 发送频率10hz
rate = rospy.Rate(10)
while not rospy.is_shutdown():
hello_str = "hello world %s" % rospy.get_time()
rospy.loginfo(hello_str)
# pub消息
pub.publish(hello_str)
# 休眠以确保10hz的频率
rate.sleep()
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException:
pass
# 新建消息sub订阅器
vim listener.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
File: listener.py
Desc: 消息sub订阅
Author:yanjingang(yanjingang@mail.com)
Date: 2020/6/15 22:37
"""
import rospy
from std_msgs.msg import String
def callback(data):
'''sub消息回调'''
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
def listener():
# ros初始化
rospy.init_node('listener', anonymous=True)
# 告诉master我们要订阅chatter话题上的消息
rospy.Subscriber("chatter", String, callback)
# 相当与waiting(有新消息、或需要退出时会跳过)
rospy.spin()
if __name__ == '__main__':
listener()
# 配置
vim CMakeLists.txt.
catkin_install_python(PROGRAMS scripts/talker.py scripts/listener.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
chmod +x talker.py listener.py
# 编译
cd ~/catkin_ws
catkin_make
# 测试运行
roscore
rosrun beginner_tutorials talker.py
rosrun beginner_tutorials listener.py
三、编写简单的Service和Client
参考：Writing a Simple Service and Client (C++)、Writing a Simple Service and Client (Python)
C++版本：
roscd beginner_tutorials
# 编写server
vim src/add_two_ints_server.cpp
#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
bool add(beginner_tutorials::AddTwoInts::Request &req,
beginner_tutorials::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;
ros::ServiceServer service = n.advertiseService("add_two_ints", add);
ROS_INFO("Ready to add two ints.");
ros::spin();
return 0;
}
# 编写client
vim src/add_two_ints_client.cpp
#include "ros/ros.h"
#include "beginner_tutorials/AddTwoInts.h"
#include <cstdlib>
int main(int argc, char **argv)
{
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;
ros::ServiceClient client = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");
beginner_tutorials::AddTwoInts srv;
srv.request.a = atoll(argv[1]);
srv.request.b = atoll(argv[2]);
if (client.call(srv)){
ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}else{
ROS_ERROR("Failed to call service add_two_ints");
return 1;
}
return 0;
}
# 配置
vim CMakeLists.txt
add_executable(add_two_ints_server src/add_two_ints_server.cpp)
target_link_libraries(add_two_ints_server ${catkin_LIBRARIES})
add_dependencies(add_two_ints_server beginner_tutorials_gencpp)
add_executable(add_two_ints_client src/add_two_ints_client.cpp)
target_link_libraries(add_two_ints_client ${catkin_LIBRARIES})
add_dependencies(add_two_ints_client beginner_tutorials_gencpp)
# 编译
cd ~/catkin_ws
catkin_make
# 测试
roscore
rosrun beginner_tutorials add_two_ints_server
[ INFO] [1592295430.720743006]: Ready to add two ints.
[ INFO] [1592295447.819885626]: request: x=1, y=3
[ INFO] [1592295447.819908758]: sending back response: [4]
rosrun beginner_tutorials add_two_ints_client 1 3
[ INFO] [1592295452.507057100]: Sum: 4Python版本：
roscd beginner_tutorials
# 新建server
vim scripts/add_two_ints_server.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
File: add_two_ints_server.py
Desc: server测试
Author:yanjingang(yanjingang@mail.com)
Date: 2020/6/15 23:12
"""
import rospy
from beginner_tutorials.srv import AddTwoInts,AddTwoIntsResponse
def handle_add_two_ints(req):
print("Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b)))
return AddTwoIntsResponse(req.a + req.b)
def add_two_ints_server():
# ROS初始化
rospy.init_node('add_two_ints_server')
# 新建名为add_two_ints的server，输入AddTwoInts，返回AddTwoIntsResponse
s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)
print("Ready to add two ints.")
rospy.spin()
if __name__ == "__main__":
add_two_ints_server()
#新建client
vim scripts/add_two_ints_client.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
File: add_two_ints_client.py
Desc: client测试
Author:yanjingang(yanjingang@mail.com)
Date: 2020/6/15 23:15
"""
import sys
import rospy
from beginner_tutorials.srv import AddTwoInts,AddTwoIntsResponse
def add_two_ints_client(x, y):
# 连接名为add_two_ints的server
rospy.wait_for_service('add_two_ints')
try:
# 初始化server对象（声明参数类型为AddTwoInts）
add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
# 调用server
resp1 = add_two_ints(x, y)
# 输出返回值
return resp1.sum
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
def usage():
return "%s [x y]"%sys.argv[0]
if __name__ == "__main__":
if len(sys.argv) == 3:
x = int(sys.argv[1])
y = int(sys.argv[2])
else:
print(usage())
sys.exit(1)
print("Requesting %s+%s"%(x, y))
print("%s + %s = %s"%(x, y, add_two_ints_client(x, y)))
# 配置
vim CMakeLists.txt
catkin_install_python(PROGRAMS scripts/talker.py scripts/listener.py scripts/add_two_ints_server.py scripts/add_two_ints_client.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
chmod +x scripts/add_two_ints_server.py
chmod +x scripts/add_two_ints_client.py
# 编译
cd ~/catkin_ws
catkin_make
# 测试
roscore
rosrun beginner_tutorials add_two_ints_server.py
Ready to add two ints.
Returning [1 + 3 = 4]
rosrun beginner_tutorials add_two_ints_client.py 1 3
Requesting 1+3
1 + 3 = 4
四、bag包的录制与回放
参考：Recording and playing back data
这里学习如何将ROS系统运行过程中的数据录制到一个.bag文件中，然后通过回放数据来重现相似的运行过程。
# 启动小龟模拟器和键盘控制节点（用于产生数据）
roscore
rosrun turtlesim turtlesim_node
rosrun turtlesim turtle_teleop_key
# 录制数据
mkdir ~/bagfiles
cd ~/bagfiles
rosbag record -a #-a表示将当前发布的所有话题数据都录制保存到一个bag文件中
或
rosbag record -O test /turtle1/cmd_vel /turtle1/pose #-O表示录制指定topic到指定名称的bag文件中
#现在用键盘控制小龟随处移动10秒钟左右，然后Ctrl-C退出rosbag record -a录制
# 查看bag包数据 （可以看到话题的名称、类型和消息数量）
rosbag info ~/bagfiles/2020-06-16-16-26-50.bag #或test.bag
path: 2020-06-16-16-26-50.bag
version: 2.0
duration: 48.7s
start: Jun 16 2020 16:26:50.47 (1592296010.47)
end: Jun 16 2020 16:27:39.14 (1592296059.14)
size: 426.8 KB
messages: 6081
compression: none [1/1 chunks]
types: geometry_msgs/Twist [9f195f881246fdfa2798d1d3eebca84a]
rosgraph_msgs/Log [acffd30cd6b6de30f120938c17c593fb]
turtlesim/Color [353891e354491c51aabe32df673fb446]
turtlesim/Pose [863b248d5016ca62ea2e895ae5265cf9]
topics: /rosout 6 msgs : rosgraph_msgs/Log (2 connections)
/turtle1/cmd_vel 17 msgs : geometry_msgs/Twist
/turtle1/color_sensor 3029 msgs : turtlesim/Color
/turtle1/pose 3029 msgs : turtlesim/Pose
# 回放bag包数据
# 先Ctrl-C退出turtle_teleop_key键盘控制节点（如果想看到一模一样的效果，需要把小龟模拟器也重启）
rosbag play ~/bagfiles/2020-06-16-16-26-50.bag #或test.bag。查看小龟模拟器能看到移动指令被重放了
五、三维仿真可视化
参考：Markers: Sending Basic Shapes (C++)、Markers: Points and Lines (C++)
gazebo和rviz是ROS里比较常见的两个3D可视化工具，两者的主要功能不同，如果做ROS开发，一般两个都得会。
rviz是三维可视化工具，强调把已有的数据可视化显示；
rviz需要已有数据。
rviz提供了很多插件，这些插件可以显示图像、模型、路径等信息，但是前提都是这些数据已经以话题、参数的形式发布，rviz做的事情就是订阅这些数据，并完成可视化的渲染，让开发者更容易理解数据的意义。
gazebo不是显示工具，强调的是仿真，它不需要数据，而是创造数据。
我们可以在gazebo中免费创建一个机器人世界，不仅可以仿真机器人的运动功能，还可以仿真机器人的传感器数据。而这些数据就可以放到rviz中显示，所以使用gazebo的时候，经常也会和rviz配合使用。当我们手上没有机器人硬件或实验环境难以搭建时，仿真往往是非常有用的利器。
下面我们试下rviz的基础用法：
# 创建ROS程序包
cd ~/catkin_ws/src
# catkin_create_pkg <package_name> [depend1] [depend2] [depend3]
catkin_create_pkg using_markers roscpp visualization_msgs
# 基础图形绘制
cd using_markers/
vim src/basic_shapes.cpp
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
/**
* 基础形状三维可视化测试
*/
int main( int argc, char** argv )
{
// 初始化ROS并新建publisher
ros::init(argc, argv, "basic_shapes");
ros::NodeHandle n;
ros::Rate r(1);
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
// 立方体
uint32_t shape = visualization_msgs::Marker::CUBE;
while (ros::ok())
{
visualization_msgs::Marker marker;
// 设置帧ID和时间戳
marker.header.frame_id = "/my_frame";
marker.header.stamp = ros::Time::now();
// 设置命名空间和id（同命名空间+id发送的数据会覆盖）
marker.ns = "basic_shapes";
marker.id = 0;
// 设置类型为立方体
marker.type = shape;
// 设置动作
marker.action = visualization_msgs::Marker::ADD;
// 设置姿态
marker.pose.position.x = 0;
marker.pose.position.y = 0;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
// 设置边界距离
marker.scale.x = 1.0;
marker.scale.y = 1.0;
marker.scale.z = 1.0;
// 设置颜色
marker.color.r = 0.0f;
marker.color.g = 1.0f;
marker.color.b = 0.0f;
marker.color.a = 1.0; //0完全透明，1完全不透明
// 设置过期时间
marker.lifetime = ros::Duration(); //没传表示用不过期
// pub maker
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
return 0;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1);
}
marker_pub.publish(marker);
// 在不同形状之间循环
switch (shape)
{
case visualization_msgs::Marker::CUBE:
shape = visualization_msgs::Marker::SPHERE;
break;
case visualization_msgs::Marker::SPHERE:
shape = visualization_msgs::Marker::ARROW;
break;
case visualization_msgs::Marker::ARROW:
shape = visualization_msgs::Marker::CYLINDER;
break;
case visualization_msgs::Marker::CYLINDER:
shape = visualization_msgs::Marker::CUBE;
break;
}
r.sleep();
}
}
# 配置
vim CMakeLists.txt
add_executable(basic_shapes src/basic_shapes.cpp)
target_link_libraries(basic_shapes ${catkin_LIBRARIES})
# 编译
cd ~/catkin_ws
catkin_make
# 运行maker绘制节点
roscore
rosrun using_markers basic_shapes
# 运行rviz渲染节点
rosmake rviz #构架rviz使其能找到maker
rosrun rviz rviz #frame填上/my_frame，如果没有maker节点就手工添加一个
# 点线绘制
roscd using_markers
vim src/points_and_lines.cpp
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <cmath>
/**
* 点/线三维可视化测试
*/
int main( int argc, char** argv )
{
// 初始化ROS并新建publisher
ros::init(argc, argv, "points_and_lines");
ros::NodeHandle n;
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
ros::Rate r(30);
float f = 0.0;
while (ros::ok())
{
// 设置点线marker
visualization_msgs::Marker points, line_strip, line_list;
points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/my_frame";
points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
points.ns = line_strip.ns = line_list.ns = "points_and_lines";
points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;
points.id = 0;
line_strip.id = 1;
line_list.id = 2;
points.type = visualization_msgs::Marker::POINTS;
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
line_list.type = visualization_msgs::Marker::LINE_LIST;
// 用x/y表示高宽
points.scale.x = 0.2;
points.scale.y = 0.2;
line_strip.scale.x = 0.1;
line_list.scale.x = 0.1;
// 设置颜色
points.color.g = 1.0f;
points.color.a = 1.0;
line_strip.color.b = 1.0;
line_strip.color.a = 1.0;
line_list.color.r = 1.0;
line_list.color.a = 1.0;
// 设置角点
for (uint32_t i = 0; i < 100; ++i)
{
float y = 5 * sin(f + i / 100.0f * 2 * M_PI);
float z = 5 * cos(f + i / 100.0f * 2 * M_PI);
geometry_msgs::Point p;
p.x = (int32_t)i - 50;
p.y = y;
p.z = z;
points.points.push_back(p);
line_strip.points.push_back(p);
line_list.points.push_back(p);
p.z += 1.0;
line_list.points.push_back(p);
}
//发布点线
marker_pub.publish(points);
marker_pub.publish(line_strip);
marker_pub.publish(line_list);
r.sleep();
f += 0.04;
}
}
# 设置
vim CMakeLists.txt
add_executable(points_and_lines src/points_and_lines.cpp)
target_link_libraries(points_and_lines ${catkin_LIBRARIES})
# 编译
cd ~/catkin_ws
catkin_make
# 运行
roscore
rosrun using_markers points_and_lines
rosrun rviz rviz #frame填上/my_frame，如果没有maker节点就手工添加一个
一个乒乓仿真的示例：
# 下载乒乓仿真节点代码
cd ~/project
git clone https://github.com/ros-visualization/visualization_tutorials
cp -r visualization_tutorials/interactive_marker_tutorials ~/catkin_ws/src/
# 编译
cd ~/catkin_ws
catkin_make
# 运行
rosrun interactive_marker_tutorials pong # 从~/catkin_ws/src/interactive_marker_tutorials/src/pong.cpp里找到frame_id，rviz接收消息用
rosrun rviz rviz #设置frame_id，添加InteractiveMarkers后选择Update Topic即可
# 测试python控制rviz
cp ~/project/visualization_tutorials/rviz_python_tutorial/ ~/catkin_ws/src/ -r
cd ~/catkin_ws
catkin_make
# 运行
rosrun rviz_python_tutorial myviz.py
好的，今天我们主要学习了怎么编写自定义的Service/Client节点进行指定msg格式通信、尝试bag的录制和回放、以及基础的三维可视化用法。下一篇我们会尝试加入硬件，实现一个简单的实例。
yan 20.6.17 23:56
参考：
