ROS2 栅格地图数据 & SLAM_toolbox仿真建图

被遗忘的旋律.

已于 2024-10-08 23:11:46 修改

阅读量672

点赞数 12

文章标签：机器人 python ubuntu

于 2024-10-07 12:21:01 首次发布

本文链接：https://blog.csdn.net/qq_56920196/article/details/142712125

版权

本文参考的是b站up主视频40.在ROS中，使用Python发布自定义地图_哔哩哔哩_bilibili，本文只是修改用在了ROS2上

版本：

ubuntu 22.04

ROS2 humble

1、消息类型

相关的具体参数可见map_server - ROS Wiki

1.1 OccupancyGrid

栅格地图消息类型如下

（详见链接：nav_msgs/OccupancyGrid Documentation (ros.org)，这里就挑几个重要的写）

# 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

1.1.1 header

std_msgs/Header Documentation (ros.org)

# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data 
# in a particular coordinate frame.

# sequence ID: consecutively increasing ID 
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library 
time stamp
#Frame this data is associated with
string frame_id

不是重点，怎么写看后面代码

1.1.2 MapMetaData info

MapMetaData info消息类型如下：

nav_msgs/MapMetaData Documentation (ros.org)

# This hold basic information about the characterists of the OccupancyGrid

# The time at which the map was loaded地图加载时间
time map_load_time
# The map resolution [m/cell]分辨率
float32 resolution
# Map width [cells]宽
uint32 width
# Map height [cells]高
uint32 height
# The origin of the map [m, m, rad].  This is the real-world pose of the
# cell (0,0) in the map.偏移量
geometry_msgs/Pose origin

重点数据是width和height获取地图的行数列数

最后一行是地图原点（左下角）的位置，即地图原点相对于实际世界原点的偏移量，具体怎么用看后面的演示。

1.1.3 data

一个一维数组，保存具体的地图数据。

假设栅格地图为m行n列，则将其按行拆分并首尾拼接成一个长度为m*n的一维数组存入data。数组中的值可以是从0~100，表示该位置有障碍物的概率。-1表示未知。

注意：地图的原点为左下角，因此最下面一行为第0行，最上面为第m-1行。data拼接数据也是从第0行开始，第m-1行结束

2、发布话题

2.1代码

ROS1代码和教程可以看开头提到的视频链接

ROS2代码：

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy                                     # ROS2 Python接口库
from rclpy.node import Node                      # ROS2 节点类
from nav_msgs.msg import OccupancyGrid           # 格栅地图数据类型
from builtin_interfaces.msg import Time          # 一会用来获取时间，设置时间戳


class PublisherNode(Node):
    
    def __init__(self, name):
        super().__init__(name)                                    # ROS2节点父类初始化
        self.pub = self.create_publisher(OccupancyGrid, "map", 10)# 创建发布者对象（消息类型、话题名、队列长度）
        self.timer = self.create_timer(1, self.timer_callback)    # 创建一个定时器（发送频率，回调函数）
        
    def timer_callback(self):                                     # 创建定时器周期执行的回调函数
        msg = OccupancyGrid()                                     # 创建消息对象
        msg.header.frame_id = "map"                               # 坐标系ID
        msg.header.stamp = self.get_clock().now().to_msg()        # 时间辍：当前时间
        msg.info.origin.position.x = 0.0                          # 偏移量
        msg.info.origin.position.y = 0.0
        msg.info.resolution = 1.0                                 # 分辨率
        msg.info.width = 4                                        # 地图尺寸
        msg.info.height = 5
        msg.data = [0]*4*5                                        # 设置地图数据
        msg.data[0] = 100
        msg.data[1] = 80
        msg.data[2] = 60
        msg.data[3] = 40
        msg.data[19] = -1
   
        self.pub.publish(msg)                                    # 发布话题消息
        self.get_logger().info('Publishing')                     # 打印信息提示
        
def main(args=None):                                 # ROS2节点主入口main函数
    rclpy.init(args=args)                            # ROS2 Python接口初始化
    node = PublisherNode("topic_helloworld_pub")     # 创建ROS2节点对象并进行初始化
    rclpy.spin(node)                                 # 循环等待ROS2退出
    node.destroy_node()                              # 销毁节点对象
    rclpy.shutdown()                                 # 关闭ROS2 Python接口

ROS2别忘了在setup.py添加依赖（改成自己的）

最后colcon build & source

2.2Rviz显示

rviz2安装：

sudo apt update
sudo apt upgrade
sudo apt install ros-humble-rviz2

这里humble改成自己的版本

启动Rviz2，add添加Axes坐标系，即确定世界地图的原点

添加map：

启动刚才写的代码

Rviz中订阅一下/map话题：

可以看到地图显示出来了：

如果将刚才说的偏移量改成x=1.0，y=-1.0，看看效果：

3、SLAM建图

建图我使用了Slam_toolbox，使用gazebo和rviz2来仿真（莫得车车用）

安装Slam_toolbox：

sudo apt install ros-humble-slam-toolbox

工作空间src里新建一个功能包，在功能包中创建launch文件夹，并在其中创建一个launch文件：

写入如下代码：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='slam_toolbox',
            executable='async_slam_toolbox_node',
            output='screen',
            parameters=[{
                'use_sim_time': True  # 用仿真改为True，不用仿真就False
            }]
        )
    ])

修改setup.py，增加下面这一行代码：

('share/' + package_name + '/launch', ['launch/slam_toolbox.launch.py']),

保存，colcon build，source，执行launch一条龙，启动后会发布/map话题

启动gazebo，载入机器人模型和场景（如果没有的话，我用的是下面克隆的代码，放到src里面）

git clone https://github.com/6-robot/wpr_simulation2.git

colcon build后执行：

ros2 launch wpr_simulation2 wpb_simple.launch.py

然后launch会启动gazebo并载入机器人模型。随便扔几个方块进去搭个场景：

启动rviz2：

ros2 run rviz2 rviz2

和上面的方法一样，加入一个map，选择/map话题

也可以再加入激光雷达的话题：add→LaserScan→订阅话题/Scan

看不清激光点的话可以把这个size调大一些↑

启动一个控制器发布/cmd_vel话题控制机器人

ros2 run rqt_robot_steering rqt_robot_steering

如果没有这个功能包可以用下面的指令安装，也可以用其他方法发布/cmd_vel话题

sudo apt install ros-humble-rqt-robot-steering

控制机器人进去转一圈，地图就显示到rviz上了。

地图中的灰色区域就是前面栅格地图中值为-1的未知部分，黑色的为100的部分，红色的是激光雷达的数据

最后，输入指令可以将地图文件保存：

保存到当前终端路径：

ros2 run nav2_map_server map_saver_cli -t map

保存到指定路径：

ros2 run nav2_map_server map_saver_cli -f /目/标/路/径/地图名称 -t map

得到一个地图图片和地图信息

4、Slam_toolbox话题消息类型

详细链接ROS Package: slam_toolbox

4.1 LaserScan

sensor_msgs/LaserScan Documentation (ros.org)

# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data

Header header            # timestamp in the header is the acquisition time of 
                         # the first ray in the scan.
                         #
                         # in frame frame_id, angles are measured around 
                         # the positive Z axis (counterclockwise, if Z is up)
                         # with zero angle being forward along the x axis
                         
float32 angle_min        # start angle of the scan [rad]
float32 angle_max        # end angle of the scan [rad]
float32 angle_increment  # angular distance between measurements [rad]

float32 time_increment   # time between measurements [seconds] - if your scanner
                         # is moving, this will be used in interpolating position
                         # of 3d points
float32 scan_time        # time between scans [seconds]

float32 range_min        # minimum range value [m]
float32 range_max        # maximum range value [m]

float32[] ranges         # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities    # intensity data [device-specific units].  If your
                         # device does not provide intensities, please leave
                         # the array empty.

看着很多，但都是配置参数。最有用的就最下面的ranges，一个一维数组表示雷达扫一圈返回的所有测距值

4.2 TF

common_interfaces/geometry_msgs/msg/TransformStamped.msg at humble · ros2/common_interfaces (github.com)

# This expresses a transform from coordinate frame header.frame_id
# to the coordinate frame child_frame_id at the time of header.stamp
# 在时间戳header.stemp时刻，从坐标frame_id到坐标child_frame_id的变换

# This message is mostly used by the
# <a href="https://index.ros.org/p/tf2/">tf2</a> package.
# See its documentation for more information. 更多消息参阅上述文档

# The child_frame_id is necessary in addition to the frame_id
# in the Header to communicate the full reference for the transform
# in a self contained message.

# The frame id in the header is used as the reference frame of this transform.
std_msgs/Header header

# The frame id of the child frame to which this transform points.
string child_frame_id

# Translation and rotation in 3-dimensions of child_frame_id from header.frame_id.变化
Transform transform

其中的Header内容参考1.1

其中的Transform如下：xyz以及一个四元数，表示坐标的变换common_interfaces/geometry_msgs/msg/Transform.msg at humble · ros2/common_interfaces (github.com)

# This represents the transform between two coordinate frames in free space.
Vector3 translation
Quaternion rotation

4.3 OccupancyGrid

参照1.1

4.4 PoseWithCovarianceStamped

geometry_msgs/PoseWithCovarianceStamped Documentation (ros.org)

# This expresses an estimated pose with a reference coordinate frame and timestamp
Header header
PoseWithCovariance pose

4.4.1 header

这玩意和1.1.1是一个东西

4.4.2 PoseWithCovarianceStamped

# This represents a pose in free space with uncertainty.
Pose pose

# Row-major representation of the 6x6 covariance matrix
# The orientation parameters use a fixed-axis representation.
# In order, the parameters are:
# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
float64[36] covariance

pose两部分组成，一个xyz表示位置，一个四元数表示姿态

covariance一个6*6的协方差矩阵（xyz位置以及相对xyz轴的旋转）