亚博microros小车-原生ubuntu支持系列:18 Cartographer建图

Cartographer简介

Cartographer是Google开源的一个ROS系统支持的2D和3D SLAM(simultaneous localization and mapping)库。基于图优化(多线程后端优化、cere构建的problem优化)的方法建图算法。可以结合来自多个传感器(比如,LIDAR、IMU 和 摄像头)的数据,同步计算传感器的位置并绘制传感器周围的环境。

cartographer的源码主要包括三个部分:cartographer、cartographer_ros和ceres-solver(后端优化)。

cartographer采用的是主流的SLAM框架,也就是特征提取、闭环检测、后端优化的三段式。由一定数量的LaserScan组成一个submap子图,一系列的submap子图构成了全局地图。用LaserScan构建submap的短时间过程累计误差不大,但是用submap构建全局地图的长时间过程就会存在很大的累计误差,所以需要利用闭环检测来修正这些submap的位置,闭环检测的基本单元是submap,闭环检测采用scan_match策略。

cartographer的重点内容就是融合多传感器数据(odometry、IMU、LaserScan等)的submap子图创建以及用于闭环检测的scan_match策略的实现。

cartographer_ros是在ROS下面运行的,可以以ROS消息的方式接受各种传感器数据,在处理过后又以消息的形式publish出去,便于调试和可视化。

https://github.com/cartographer-project/cartographer

看完这些还是没啥概念,官方文档吧,

安装:

官网的源码安装比较麻烦。推荐apt直接安装

sudo apt install ros-humble-cartographer

sudo apt install ros-humble-cartographer-ros

安装完了查看下

bohu@bohu-TM1701:~$ ros2 pkg list | grep cartographer
cartographer_ros
cartographer_ros_msgs

官网接下来又到了启动launch脚本的时候了,对于这块还想了解的,推荐看看大佬的文章:

【10天速通Navigation2】(三) :Cartographer建图算法配置:从仿真到实车,从原理到实现_cartographer navigation真实环境-CSDN博客

大佬的文章讲了很多知识点。

建图

1  启动小车代理,等小车连上

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:humble udp4 --port 8090 -v4

2 启动小车处理底层数据程序

ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py
bohu@bohu-TM1701:~/yahboomcar/yahboomcar_ws$ ros2 launch yahboomcar_bringup yahboomcar_bringup_launch.py
[INFO] [launch]: All log files can be found below /home/bohu/.ros/log/2025-01-31-17-14-56-902589-bohu-TM1701-375239
[INFO] [launch]: Default logging verbosity is set to INFO
---------------------robot_type = x3---------------------
[INFO] [complementary_filter_node-1]: process started with pid [375241]
[INFO] [ekf_node-2]: process started with pid [375243]
[INFO] [static_transform_publisher-3]: process started with pid [375245]
[INFO] [joint_state_publisher-4]: process started with pid [375247]
[INFO] [robot_state_publisher-5]: process started with pid [375249]
[INFO] [static_transform_publisher-6]: process started with pid [375251]
[static_transform_publisher-3] [WARN] [1738314897.159059362] []: Old-style arguments are deprecated; see --help for new-style arguments
[static_transform_publisher-6] [WARN] [1738314897.165546370] []: Old-style arguments are deprecated; see --help for new-style arguments
[robot_state_publisher-5] [WARN] [1738314897.574904564] [kdl_parser]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
[robot_state_publisher-5] [INFO] [1738314897.575050398] [robot_state_publisher]: got segment base_link
[robot_state_publisher-5] [INFO] [1738314897.575129250] [robot_state_publisher]: got segment imu_Link
[robot_state_publisher-5] [INFO] [1738314897.575148441] [robot_state_publisher]: got segment jq1_Link
[robot_state_publisher-5] [INFO] [1738314897.575162415] [robot_state_publisher]: got segment jq2_Link
[robot_state_publisher-5] [INFO] [1738314897.575176635] [robot_state_publisher]: got segment radar_Link
[robot_state_publisher-5] [INFO] [1738314897.575189851] [robot_state_publisher]: got segment yh_Link
[robot_state_publisher-5] [INFO] [1738314897.575203953] [robot_state_publisher]: got segment yq_Link
[robot_state_publisher-5] [INFO] [1738314897.575216727] [robot_state_publisher]: got segment zh_Link
[robot_state_publisher-5] [INFO] [1738314897.575231399] [robot_state_publisher]: got segment zq_Link
[joint_state_publisher-4] [INFO] [1738314897.812400004] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...

然后,启动rviz,可视化建图,终端输入,

ros2 launch yahboomcar_nav display_launch.py

此时还没运行建图节点,所以没有数据。接下来运行建图节点,终端输入,

ros2 launch yahboomcar_nav map_cartographer_launch.py

启动键盘控制

ros2 run yahboomcar_ctrl yahboom_keyboard

然后控制小车,缓慢的走完需要建图的区域

建图完毕后,输入以下指令保存地图,终端输入,

ros2 launch yahboomcar_nav save_map_launch.py
bohu@bohu-TM1701:~/yahboomcar/yahboomcar_ws$ ros2 launch yahboomcar_nav save_map_launch.py
[INFO] [launch]: All log files can be found below /home/bohu/.ros/log/2025-01-31-17-45-15-428783-bohu-TM1701-377017
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [map_saver_cli-1]: process started with pid [377019]
[map_saver_cli-1] [INFO] [1738316715.838488933] [map_saver]: 
[map_saver_cli-1] 	map_saver lifecycle node launched. 
[map_saver_cli-1] 	Waiting on external lifecycle transitions to activate
[map_saver_cli-1] 	See https://design.ros2.org/articles/node_lifecycle.html for more information.
[map_saver_cli-1] [INFO] [1738316715.838780993] [map_saver]: Creating
[map_saver_cli-1] [INFO] [1738316715.839511570] [map_saver]: Configuring
[map_saver_cli-1] [INFO] [1738316715.846012025] [map_saver]: Saving map from 'map' topic to '/home/bohu/yahboomcar/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map' file
[map_saver_cli-1] [WARN] [1738316715.846072515] [map_saver]: Free threshold unspecified. Setting it to default value: 0.250000
[map_saver_cli-1] [WARN] [1738316715.846095065] [map_saver]: Occupied threshold unspecified. Setting it to default value: 0.650000
[map_saver_cli-1] [WARN] [1738316715.865849280] [map_io]: Image format unspecified. Setting it to: pgm
[map_saver_cli-1] [INFO] [1738316715.865932329] [map_io]: Received a 208 X 213 map @ 0.05 m/pix
[map_saver_cli-1] [INFO] [1738316715.927583001] [map_io]: Writing map occupancy data to /home/bohu/yahboomcar/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map.pgm
[map_saver_cli-1] [INFO] [1738316715.929668988] [map_io]: Writing map metadata to /home/bohu/yahboomcar/yahboomcar_ws/src/yahboomcar_nav/maps/yahboom_map.yaml
[map_saver_cli-1] [INFO] [1738316715.929823753] [map_io]: Map saved
[map_saver_cli-1] [INFO] [1738316715.929836633] [map_saver]: Map saved successfully
[map_saver_cli-1] [INFO] [1738316715.932021514] [map_saver]: Destroying
[INFO] [map_saver_cli-1]: process has finished cleanly [pid 377019]

会有两个文件生成,一个是yahboom_map.pgm,一个是yahboom_map.yaml,看下yaml的内容,

image: yahboom_map.pgm

mode: trinary

resolution: 0.05

origin: [-5.56, -1.67, 0]

negate: 0

occupied_thresh: 0.65

free_thresh: 0.25

  • image:表示地图的图片,也就是yahboom_map.pgm

  • mode:该属性可以是trinary、scale或者raw之一,取决于所选择的mode,trinary模式是默认模式

  • resolution:地图的分辨率, 米/像素

  • 地图左下角的 2D 位姿(x,y,yaw), 这里的yaw是逆时针方向旋转的(yaw=0 表示没有旋转)。目前系统中的很多部分会忽略yaw值。

  • negate:是否颠倒 白/黑 、自由/占用 的意义(阈值的解释不受影响)

  • occupied_thresh:占用概率大于这个阈值的的像素,会被认为是完全占用。

  • free_thresh:占用概率小于这个阈值的的像素,会被认为是完全自由。

节点通讯图 

TF树

源码

src/yahboomcar_nav/launch/display_launch.py 

from ament_index_python.packages import get_package_share_path

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, LaunchConfiguration

from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue

def generate_launch_description():
    package_path = get_package_share_path('yahboomcar_nav')
    default_rviz_config_path = package_path / 'rviz/view.rviz'
    rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=str(default_rviz_config_path),
                                     description='Absolute path to rviz config file')

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', LaunchConfiguration('rvizconfig')],
    )

    return LaunchDescription([
        rviz_arg,
        rviz_node
    ])

src/yahboomcar_nav/launch/map_cartographer_launch.py

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node


def generate_launch_description():
    package_launch_path =os.path.join(get_package_share_directory('yahboomcar_nav'), 'launch')

    cartographer_launch = IncludeLaunchDescription(PythonLaunchDescriptionSource(
        [package_launch_path, '/cartographer_launch.py'])
    )
    base_link_to_laser_tf_node = Node(
     package='tf2_ros',
     executable='static_transform_publisher',
     name='base_link_to_base_laser',
     arguments=['-0.0046412', '0' , '0.094079','0','0','0','base_link','laser_frame']
    ) 
    return LaunchDescription([cartographer_launch,base_link_to_laser_tf_node])

src/yahboomcar_nav/launch/cartographer_launch.py

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import ThisLaunchFileDir


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    package_path = get_package_share_directory('yahboomcar_nav')
    configuration_directory = LaunchConfiguration('configuration_directory', default=os.path.join(
                                                  package_path, 'params'))
    configuration_basename = LaunchConfiguration('configuration_basename', default='lds_2d.lua')

    resolution = LaunchConfiguration('resolution', default='0.05')
    publish_period_sec = LaunchConfiguration(
        'publish_period_sec', default='1.0')

    return LaunchDescription([
        DeclareLaunchArgument(
            'configuration_directory',
            default_value=configuration_directory,
            description='Full path to config file to load'),
        DeclareLaunchArgument(
            'configuration_basename',
            default_value=configuration_basename,
            description='Name of lua file for cartographer'),
        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),

        Node(
            package='cartographer_ros',
            executable='cartographer_node',
            name='cartographer_node',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=['-configuration_directory', configuration_directory,
                       '-configuration_basename', configuration_basename],
            remappings=[('/odom','/odom')]
                       ),

        DeclareLaunchArgument(
            'resolution',
            default_value=resolution,
            description='Resolution of a grid cell in the published occupancy grid'),

        DeclareLaunchArgument(
            'publish_period_sec',
            default_value=publish_period_sec,
            description='OccupancyGrid publishing period'),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [ThisLaunchFileDir(), '/occupancy_grid_launch.py']),
            launch_arguments={'use_sim_time': use_sim_time, 'resolution': resolution,
                              'publish_period_sec': publish_period_sec}.items(),
        ),
    ])

src/yahboomcar_nav/launch/occupancy_grid_launch.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    resolution = LaunchConfiguration('resolution', default='0.05')
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')

    return LaunchDescription([
        DeclareLaunchArgument(
            'resolution',
            default_value=resolution,
            description='Resolution of a grid cell in the published occupancy grid'),

        DeclareLaunchArgument(
            'publish_period_sec',
            default_value=publish_period_sec,
            description='OccupancyGrid publishing period'),

        DeclareLaunchArgument(
            'use_sim_time',
            default_value='false',
            description='Use simulation (Gazebo) clock if true'),

        Node(
            package='cartographer_ros',
            executable='cartographer_occupancy_grid_node',
            name='occupancy_grid_node',
            output='screen',
            parameters=[{'use_sim_time': use_sim_time}],
            arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec]),
    ])

cartographer_launch.py 里面调用了lds_2d.lua 脚本,里面很多参数

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_footprint",
  published_frame = "odom",--发布map到odom之间的位姿态
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_odometry = true,--使用里程计数据
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}


MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.use_imu_data = false  --是否使用IMU数据
TRAJECTORY_BUILDER_2D.min_range = 0.10 --激光的最近有效距离
TRAJECTORY_BUILDER_2D.max_range = 3.5  --激光最远的有效距离
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.  --无效激光数据设置距离为该数值
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true --是否使用在线相关扫描匹配
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)  --运动敏感度

POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7

return options

以下参数来自大佬:【10天速通Navigation2】(三) :Cartographer建图算法配置:从仿真到实车,从原理到实现_cartographer navigation真实环境-CSDN博客

options: 这是一个表(table),包含了Cartographer算法的各种配置选项。

  • map_builder: 指定用于构建地图的类。
  • trajectory_builder: 指定用于构建轨迹的类。
  • map_frame: 地图的坐标系。
    tracking_frame: 跟踪的坐标系,通常是机器人底盘的坐标系。
    published_frame: 发布的坐标系,通常是用于导航的坐标系。注意这里用于设置cartographer是否发布从tracking_frame到odom_frame之间的tf树

    odom_frame: 里程计坐标系。
    provide_odom_frame: 是否提供里程计坐标系。
    publish_frame_projected_to_2d: 是否将3D数据投影到2D进行发布。
    use_pose_extrapolator: 是否使用姿态外推器。
    ==use_odometry: 是否使用里程计数据。==通常我们需要引入odom或者IMU用于辅助定位,否则会出现偏移和无法闭环的问题(后面我们会讲到)
    use_nav_sat: 是否使用导航卫星数据。
    use_landmarks: 是否使用地标数据。
    num_laser_scans: 使用的激光扫描仪数量。
    num_multi_echo_laser_scans: 使用多回声激光扫描仪的数量。
    num_subdivisions_per_laser_scan: 每个激光扫描数据的细分数量。
    num_point_clouds: 使用的点云数据数量。
    lookup_transform_timeout_sec: 查找变换的超时时间。
    submap_publish_period_sec: 发布子地图的周期。
    pose_publish_period_sec: 发布姿态的周期。
    trajectory_publish_period_sec: 发布轨迹的周期。
    rangefinder_sampling_ratio: 激光采样比率。
    odometry_sampling_ratio: 里程计采样比率。
    fixed_frame_pose_sampling_ratio: 固定帧姿态采样比率。
    imu_sampling_ratio: IMU采样比率。
    landmarks_sampling_ratio: 地标采样比率。

 

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:/a/962722.html

如若内容造成侵权/违法违规/事实不符,请联系我们进行投诉反馈qq邮箱809451989@qq.com,一经查实,立即删除!

相关文章

SpringBoot或SpringAI对接DeekSeek大模型

今日除夕夜,deepseek可是出尽了风头,但是我看网上还没有这方面的内容对接,官网也并没有,故而本次对接是为了完成这个空缺 我看很多的博客内容是流式请求虽然返回时正常的,但是他并不是实时返回,而是全部响应…

嵌入式C语言:什么是共用体?

在嵌入式C语言编程中,共用体(Union)是一种特殊的数据结构,它允许在相同的内存位置存储不同类型的数据。意味着共用体中的所有成员共享同一块内存区域,因此,在任何给定时间,共用体只能有效地存储…

Unet 改进:在encoder和decoder间加入TransformerBlock

目录 1. TransformerBlock 2. Unet 改进 3. 完整代码 Tips:融入模块后的网络经过测试,可以直接使用,设置好输入和输出的图片维度即可 1. TransformerBlock TransformerBlock是Transformer模型架构的基本组件,广泛应用于机器翻译、文本摘要和情感分析等自然语言处理任务…

音标-- 01--音标

提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档 文章目录 国际音标1.元音音标单元音双元音常见单词 2.辅音音标清辅音,浊辅音清辅音(不振动)和 浊辅音(振动)是成对…

使用Pygame制作“打砖块”游戏

1. 前言 打砖块(Breakout / Arkanoid) 是一款经典街机游戏,玩家控制一个可左右移动的挡板,接住并反弹球,击碎屏幕上方的砖块。随着砖块被击碎,不仅能获得分数,还可以体验到不断加速或复杂的反弹…

【AI绘画】MidJourney关键词{Prompt}全面整理

AI绘画整理,MidJourney关键词。喜欢AI绘画的朋友必备,建议收藏,后面用到时供查阅使用。 1、光线与影子篇 中 英 闪耀的霓虹灯 shimmeringneon lights 黑暗中的影子 shadows in the dark 照亮城市的月光 moonlightilluminatingthe cit…

嵌入式系统|DMA和SPI

文章目录 DMA(直接内存访问)DMA底层原理1. 关键组件2. 工作机制3. DMA传输模式 SPI(串行外设接口)SPI的基本原理SPI连接示例 DMA与SPI的共同作用 DMA(直接内存访问) 类型:DMA是一种数据传输接口…

AVL树介绍

一、介绍 高度平衡的搜索二叉树,保证每个节点的左右子树高度差不超过1,降低搜索树的高度以提高搜索效率。 通过平衡因子和旋转来保证左右子树高度差不超过1 二、插入节点 1、插入规则 (1)搜按索树规则插入节点 (…

win11 sourcetree安装问题

win11 sourcetree安装出现msys-2.0.dll 问题,需要从win10的以下路径复制出 msys-2.0.dll来加入到win11中 C:\Users\kz121468\AppData\Local\Atlassian\SourceTree\git_local\usr\bin\ 复制到 win11的 C:\Users\kz121468\AppData\Local\Atlassian\SourceTree\git_lo…

Contrastive Imitation Learning

机器人模仿学习中对比解码的一致性采样 摘要 本文中,我们在机器人应用的对比模仿学习中,利用一致性采样来挖掘演示质量中的样本间关系。通过在排序后的演示对比解码过程中,引入相邻样本间的一致性机制,我们旨在改进用于机器人学习…

Spring Web MVC基础第一篇

目录 1.什么是Spring Web MVC? 2.创建Spring Web MVC项目 3.注解使用 3.1RequestMapping(路由映射) 3.2一般参数传递 3.3RequestParam(参数重命名) 3.4RequestBody(传递JSON数据) 3.5Pa…

DeepSeek的使用技巧介绍

DeepSeek是一款由杭州深度求索人工智能技术有限公司开发的AI工具,结合了自然语言处理和深度学习技术,能够完成多种任务,如知识问答、数据分析、文案创作、代码开发等。以下将从使用技巧、核心功能及注意事项等方面详细介绍DeepSeek的使用方法…

创新创业计划书|建筑垃圾资源化回收

目录 第1部分 公司概况........................................................................ 1 第2部分 产品/服务...................................................................... 3 第3部分 研究与开发.................................................…

为AI聊天工具添加一个知识系统 之80 详细设计之21 符号逻辑 之1

本文要点 要点 前面我们讨论了本项目中的正则表达式。现在我们将前面讨论的正则表达式视为狭义的符号文本及其符号规则rule(认识的原则--认识上认识对象的约束),进而在更广泛的视角下将其视为符号逻辑及其符号原则principle(知识…

Spring Boot 热部署实现指南

在开发 Spring Bot 项目时,热部署功能能够显著提升开发效率,让开发者无需频繁重启服务器就能看到代码修改后的效果。下面为大家详细介绍一种实现 Spring Boot 热部署的方法,同时也欢迎大家补充其他实现形式。 步骤一、开启 IDEA 自动编译功能…

ARM嵌入式学习--第十一天(中断处理 , ADC)

--中断的概念 中断是指计算机运行过程中,出现某些意外情况需主机干预时,机器能自动停止正在运行的程序并转入处理新情况的程序,处理完毕后又返回被暂停的程序继续运行 --CPU处理事情的方式 -轮询方式 不断查询是否有事情需要处理&#xff0c…

ARM嵌入式学习--第十天(UART)

--UART介绍 UART(Universal Asynchonous Receiver and Transmitter)通用异步接收器,是一种通用串行数据总线,用于异步通信。该总线双向通信,可以实现全双工传输和接收。在嵌入式设计中,UART用来与PC进行通信,包括与监控…

socket实现HTTP请求,参考HttpURLConnection源码解析

背景 有台服务器,网卡绑定有2个ip地址,分别为: A:192.168.111.201 B:192.168.111.202 在这台服务器请求目标地址 C:192.168.111.203 时必须使用B作为源地址才能访问目标地址C,在这台服务器默认…

漏洞扫描工具之xray

下载地址:https://github.com/chaitin/xray/releases 1.9.11 使用文档:https://docs.xray.cool/tools/xray/Scanning 与burpsuite联动: https://xz.aliyun.com/news/7563 参考:https://blog.csdn.net/lza20001103/article/details…

正月初三特殊的一天

在我们河南豫东地区,初三这一天一般情况下可以在家休息,不需要串门走亲戚,给亲戚的长辈或比自己辈份长的拜年。 特殊的正月初三 还有两种情况,正月初三这一天必须去走亲戚。一种是有去世的亲戚没有过三周年,正月初三这…