目录
- 0 专栏介绍
- 1 什么是Rviz2?
- 2 Rviz2基本界面
- 3 Rviz2基本数据类型
- 4 数据可视化案例
-
- 4.1 实例1:显示USB摄像头数据
- 4.2 实例2:显示球体
0 专栏介绍
本专栏旨在通过对ROS2的系统学习,掌握ROS2底层基本分布式原理,并具有机器人建模和应用ROS2进行实际项目的开发和调试的工程能力。
🚀详情:《ROS2从入门到精通》
1 什么是Rviz2?
机器人是复杂的机电系统,其运行过程中自身或与环境交互将产生大量数据,这些数据通常以复杂的数据结构保存在内存或磁盘中,例如栅格地图数据
shell
# This represents a 2-D grid map, in which each cell represents the probability of
# occupancy.
Header header
#MetaData for the map
MapMetaData info
# The map data, in row-major order, starting with (0,0). Occupancy
# probabilities are in the range [0,100]. Unknown is -1.
int8[] data
这种抽象的数据结构不利于开发者直观第感受数据所描述的内容,因此ROS
提供了一个三维可视化工具,用于可视化传感器的数据和状态信息------Rviz2
Rviz
很好地兼容了各种基于ROS软件框架的机器人平台。在Rviz2
中,可以使用XML
对机器人、周围物体等任何实物进行
- 尺寸
- 质量
- 位置
- 材质
- 关节
等属性的描述,并且在界面中呈现出来。同时,Rviz2
还可以通过图形化的方式,实时显示机器人传感器的信息、机器人的运动状态、周围环境的变化等。
2 Rviz2基本界面
Rviz2
已经集成在桌面完整版的ROS2
系统当中,可以通过命令行启动
shell
ros2 run rviz2 rviz2
启动成功的Rviz2
主界面如下
分为几个区域:
- 3D视图区:用于可视化显示数据,目前没有任何数据,所以显示黑色
- 工具栏:提供视角控制、目标设置、发布地点等工具
- 显示列表:左侧的显示列表是从各种话题当中选择用户所需的数据的视图的区域。单击左下方的
Add
可以添加显示插件,具体的插件参见第3节- 视图设置:右侧的视图设置可以选择多种观测视角
- Orbit:以指定的视点(在这里称为Focus)为中心旋转。这是默认情况下最常用的基本视图。
- FPS(第一人称):显示第一人称视点所看到的画面。
- ThirdPersonFollower:显示以第三人称的视点尾追特定目标的视图
- TopDownOrtho:这是Z轴的视图,与其他视图不同,以直射视图显示,而非透视法。
- XYOrbit:类似于Orbit的默认值,但焦点固定在Z轴值为0的XY平面上。
- 时间:显示当前的系统时间和ROS时间
3 Rviz2基本数据类型
类型 | 描述 | 消息类型 |
---|---|---|
Axes | 显示坐标系 | -- |
Camera | 打开一个新窗口显示机器人摄像头图像 | sensor_msgs/msg/Image sensor_msgs/msg/CameraInfo |
Grid | 显示2D或3D网格 | -- |
Grid Cells | 绘制网格的每个单元格,主要用于显示导航的costmap 中的障碍 |
nav_msgs/msg/GridCells |
Image | 打开一个新窗口显示图像信息 | sensor_msgs/msg/Image |
LaserScan | 显示激光扫描值,将传感器信息中的数据显示为世界上的点 | sensor_msgs/msg/LaserScan |
Map | 显示导航中使用的栅格地图 | nav_msgs/msg/OccupancyGrid |
Markers | 绘制各种基本形状(箭头、立方体、球体、圆柱体、点、文本等) | visualization_msgs/msg/Marker visualization_msgs/msg/MarkerArray |
Path | 显示导航中使用的机器人路径 | nav_msgs/msg/Path |
PointCloud PointCloud2 | 显示点云数据,一般用于深度相机 | sensor_msgs/msg/PointCloud sensor_msgs/msg/PointCloud2 |
Polygon | 绘制一个多边形轮廓 | geometry_msgs/msg/Polygon |
Odomerty | 显示里程计数据,例如,将随着机器人运动产生的连续路径(位置与方向)沿着时间间隔以箭头形式呈现 | nav_msgs/msg/Odometry |
RobotModel | 显示机器人模型 | -- |
TF | 显示TF树 | -- |
4 数据可视化案例
进行数据可视化的数据以对应的消息类型发布,在Rviz2
中使用相应的显示插件订阅该消息即可实现可视化。
添加完成后,Rviz2
左侧的显示列表中会列出已经添加的插件,根据每个插件属性列表的需求设置即可。其中Topic
属性用来声明该显示插件所订阅的数据来源,如果订阅成功,在中间的显示区应该会出现可视化后的数据。如果显示有问题,请检查属性区域的Status
状态。Status有四种状态:OK
、Warning
、Error
和Disabled
,如果显示的状态不是OK
,那么请查看错误信息,并详细检查数据发布是否正常。
下面介绍两个使用Rviz2
的实例。
4.1 实例1:显示USB摄像头数据
USB摄像头的基本配置可以参考文章从零开始Ubuntu16.04+ORBSLAM2+ROS实验实录(二):相机测试与标定
编写启动文件配置Rviz2
以及usb_cam
节点
py
def generate_launch_description():
# Get the launch directory
simulation_dir = os.path.abspath(os.path.join(__file__, "../../"))
# Create the launch configuration variables
rviz_config_file = LaunchConfiguration('rviz_config')
declare_rviz_config_file_cmd = DeclareLaunchArgument(
'rviz_config',
default_value=os.path.join(simulation_dir, 'rviz', 'simulation.rviz'),
description='Full path to the RVIZ config file to use')
# Launch rviz
start_rviz_cmd = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', rviz_config_file],
output='screen')
exit_event_handler = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=start_rviz_cmd,
on_exit=EmitEvent(event=Shutdown(reason='rviz exited'))))
camera_handler = Node(
package='usb_cam', executable='usb_cam_node_exe', output='screen',
name="camera1",
parameters=[os.path.join(simulation_dir, 'config', 'params.yaml')],
)
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_rviz_config_file_cmd)
# Add any conditioned actions
ld.add_action(start_rviz_cmd)
# Add other nodes and processes we need
ld.add_action(exit_event_handler)
ld.add_action(camera_handler)
return ld
其中相机参数文件params.yaml
如下所示,可以根据自己的相机情况配置
yaml
/**:
ros__parameters:
video_device: "/dev/video0"
framerate: 30.0
io_method: "mmap"
frame_id: "camera"
pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formats
av_device_format: "YUV422P"
image_width: 640
image_height: 480
camera_name: "test_camera"
camera_info_url: "package://simulation/config/camera_info.yaml"
brightness: -1
contrast: -1
saturation: -1
sharpness: -1
gain: -1
auto_white_balance: true
white_balance: 4000
autoexposure: true
exposure: 100
autofocus: false
focus: -1
USB相机的图像话题是/image_raw
,那么在Rviz2
中订阅该话题即可,如下图所示。
4.2 实例2:显示球体
第三节提到过Rviz2
中Marker
的消息类型是visualization_msgs::msg::Marker
,因此我们向话题visualization_marker
发布定义的球体数据,包括大小、颜色、位置等,接着在Rviz2
中订阅即可。
cpp
int main(int argc, char *argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<PublisherNode>();
rclcpp::Rate loop_rate(10);
while (rclcpp::ok())
{
auto sphere = visualization_msgs::msg::Marker();
sphere.header.frame_id = "map";
sphere.header.stamp = node->get_clock()->now();
sphere.ns = "rviz_lab";
sphere.id = 0;
// 声明为球体
sphere.type = visualization_msgs::msg::Marker::SPHERE;
sphere.action = visualization_msgs::msg::Marker::ADD;
// 位姿
sphere.pose.position.x = 0;
sphere.pose.position.y = 0;
sphere.pose.position.z = 0;
sphere.pose.orientation.x = 0.0;
sphere.pose.orientation.y = 0.0;
sphere.pose.orientation.z = 0.0;
sphere.pose.orientation.w = 1.0;
// 大小
sphere.scale.x = 1.0;
sphere.scale.y = 1.0;
sphere.scale.z = 1.0;
// 颜色
sphere.color.r = 0.0f;
sphere.color.g = 1.0f;
sphere.color.b = 0.0f;
sphere.color.a = 1.0;
node->publish(sphere);
RCLCPP_INFO(node->get_logger(), "Publishing SPHERE in Rviz2");
loop_rate.sleep();
}
rclcpp::shutdown();
return 0;
}
完整代码通过下方博主名片联系获取
🔥 更多精彩专栏:
👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇