ROS地图/像素坐标描点调试【Python源码实现】

ROS python 地图描点调试工具

1. Rviz描点

1.1 需求描述

在ROS开发中，有时会加载图片文件转为地图载入move_base，这个时候调试程序的时候会有像素坐标系到map坐标系的转换，需要确认自己的程序是否正确的达到了转换的目的，那么需要这样一个工具来描点，然后消除点，可以用Debug的方式逐步运行，保证绘点和消除点的效果正确性。

1.2 visualization Marker

visualization_msgs/Marker 是 ROS 中的一种消息类型，用于在可视化工具如 RViz 中描述可视化元素的属性，如点、线、箭头、文本等。它用于向 ROS 可视化工具发送信息以在可视化工具中呈现和显示这些元素。以下是 visualization_msgs/Marker 消息的主要字段：

header：标识可视化元素所在的坐标系。
ns：命名空间，用于将多个可视化元素进行分组或区分。
id：可视化元素的唯一标识符。
type：可视化元素的类型，例如点、线、箭头等。
action：描述对可视化元素的操作，如添加、修改或删除。
pose：描述可视化元素的位姿，通常包括位置和方向。
scale：可视化元素的尺寸或大小，具体含义取决于元素的类型。
color：可视化元素的颜色。
lifetime：可视化元素的生存时间，用于控制元素在可视化工具中的显示时间。
frame_locked：一个布尔值，指示可视化元素是否相对于特定坐标系固定。
visualization_msgs/Marker 消息的类型字段 type 决定了要呈现的可视化元素类型。常见的 type 包括：

Marker.POINTS：用于表示点云数据。
Marker.LINE_STRIP 和 Marker.LINE_LIST：用于表示线段。
Marker.ARROW：用于表示箭头。
Marker.CUBE 和 Marker.CYLINDER：用于表示立方体和圆柱体等形状。
Marker.TEXT_VIEW_FACING：用于表示面向视图的文本。
等等…
通过发布 visualization_msgs/Marker 消息，在 ROS 可视化工具中呈现和控制这些可视化元素的显示、修改和删除。这对于在机器人操作和感知任务中实时可视化数据非常有用。
visualization Marker action

name	description
Marker.ADD	添加新的Marker到场景中
Marker.MODIFY	修改已存在的Marker的属性，如位置、颜色等
Marker.DELETE	删除已存在的Marker，只有具有与该消息相同namespace和ID的可视化元素将被删除
Marker.DELETEALL	rviz中添加了具有相同namespace和ID的多个Marker，并发布了一个带有Marker.DELETEALL操作的消息，那么所有这些Marker都将被删除。

1.3 工程实践

# -*-coding:utf-8-*-
# !/usr/bin/env python
# Author:Moresweet
# Date:20230907
# Blog:https://blog.csdn.net/qq_38853759?type=blogimport rospy
from rospy import Duration
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
import jsondef draw_markers_from_json_string(json_string):rospy.init_node('rviz_marker_publisher')marker_pub = rospy.Publisher('test_visual_point', Marker, queue_size=10)# Load points from JSON stringpoints = json.loads(json_string)# 消除点marker = Marker()marker.header.frame_id = "map"  # Adjust the frame_id as neededmarker.type = Marker.POINTSmarker.action = Marker.ADDmarker.scale.x = 0.1  # Point sizemarker.scale.y = 0.1marker.color.r = 1.0  # Redmarker.color.a = 1.0  # Fully opaquefor point in points:x = point["point"]["x"]y = point["point"]["y"]z = point["point"]["z"]point = Point()point.x = xpoint.y = ypoint.z = zmarker.points.append(point)marker.header.stamp = rospy.Time.now()marker_pub.publish(marker)# rate = rospy.Rate(10)  # 10 Hz# while not rospy.is_shutdown():#     marker.header.stamp = rospy.Time.now()#     marker_pub.publish(marker)#     rate.sleep()if __name__ == '__main__':try:json_string = '[{"point": {"x": 1.0, "y": 2.0, "z": 0.0}}, {"point": {"x": 3.0, "y": 4.0, "z": 0.0}}]'  # Replace with your JSON stringdraw_markers_from_json_string(json_string)marker_pub = rospy.Publisher('/test_visual_point', Marker, queue_size=10)marker = Marker()marker.header.frame_id = "map"marker.type = Marker.POINTSmarker.action = Marker.DELETEmarker.header.stamp = rospy.Time.now()marker.lifetime = Duration(1.0)marker_pub.publish(marker)except rospy.ROSInterruptException:pass

在rviz中添加Marker

设置话题
运行程序

以debug的形式，消除点
点位消失

2. 静态地图图片描点

2.1 需求描述

在图片格式的地图中，地图有自己的像素坐标系，对于转换关系在笔者之前的博客中已有提及。那么在map坐标系到像素坐标系的转换中，需要验证像素坐标系生成是否正确，这一点比较重要，像素坐标系的像素点表达了占据栅格信息。

2.2 工程实践

# -*-coding:utf-8-*-
# !/usr/bin/env python
# Author:Moresweet
# Date:20230907
# Blog:https://blog.csdn.net/qq_38853759?type=blogimport rospy
from rospy import Duration
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
import jsondef draw_markers_from_json_string(json_string):rospy.init_node('rviz_marker_publisher')marker_pub = rospy.Publisher('test_visual_point', Marker, queue_size=10)# Load points from JSON stringpoints = json.loads(json_string)# 消除点marker = Marker()marker.header.frame_id = "map"  # Adjust the frame_id as neededmarker.type = Marker.POINTSmarker.action = Marker.ADDmarker.scale.x = 0.1  # Point sizemarker.scale.y = 0.1marker.color.r = 1.0  # Redmarker.color.a = 1.0  # Fully opaquefor point in points:x = point["point"]["x"]y = point["point"]["y"]z = point["point"]["z"]point = Point()point.x = xpoint.y = ypoint.z = zmarker.points.append(point)marker.header.stamp = rospy.Time.now()marker_pub.publish(marker)# rate = rospy.Rate(10)  # 10 Hz# while not rospy.is_shutdown():#     marker.header.stamp = rospy.Time.now()#     marker_pub.publish(marker)#     rate.sleep()if __name__ == '__main__':try:json_string = '[{"point": {"x": 1.0, "y": 2.0, "z": 0.0}}, {"point": {"x": 3.0, "y": 4.0, "z": 0.0}}]'  # Replace with your JSON stringdraw_markers_from_json_string(json_string)marker_pub = rospy.Publisher('/test_visual_point', Marker, queue_size=10)marker = Marker()marker.header.frame_id = "map"marker.type = Marker.POINTSmarker.action = Marker.DELETEmarker.header.stamp = rospy.Time.now()marker.lifetime = Duration(1.0)marker_pub.publish(marker)except rospy.ROSInterruptException:pass