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

1.3 工程实践

# -*-coding:utf-8-*-
# !/usr/bin/env python
# Author:Moresweet
# Date:20230907
# Blog:https://blog.csdn.net/qq_38853759?type=blog

import rospy
from rospy import Duration
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
import json


def 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 string
    points = json.loads(json_string)

    # 消除点
    marker = Marker()
    marker.header.frame_id = "map"  # Adjust the frame_id as needed
    marker.type = Marker.POINTS
    marker.action = Marker.ADD
    marker.scale.x = 0.1  # Point size
    marker.scale.y = 0.1
    marker.color.r = 1.0  # Red
    marker.color.a = 1.0  # Fully opaque

    for point in points:
        x = point["point"]["x"]
        y = point["point"]["y"]
        z = point["point"]["z"]
        point = Point()
        point.x = x
        point.y = y
        point.z = z
        marker.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 string
        draw_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.POINTS
        marker.action = Marker.DELETE
        marker.header.stamp = rospy.Time.now()
        marker.lifetime = Duration(1.0)
        marker_pub.publish(marker)
    except rospy.ROSInterruptException:
        pass

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63

在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=blog

import rospy
from rospy import Duration
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
import json


def 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 string
    points = json.loads(json_string)

    # 消除点
    marker = Marker()
    marker.header.frame_id = "map"  # Adjust the frame_id as needed
    marker.type = Marker.POINTS
    marker.action = Marker.ADD
    marker.scale.x = 0.1  # Point size
    marker.scale.y = 0.1
    marker.color.r = 1.0  # Red
    marker.color.a = 1.0  # Fully opaque

    for point in points:
        x = point["point"]["x"]
        y = point["point"]["y"]
        z = point["point"]["z"]
        point = Point()
        point.x = x
        point.y = y
        point.z = z
        marker.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 string
        draw_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.POINTS
        marker.action = Marker.DELETE
        marker.header.stamp = rospy.Time.now()
        marker.lifetime = Duration(1.0)
        marker_pub.publish(marker)
    except rospy.ROSInterruptException:
        pass

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64