ROS2 Galactic编译运行cartographer代码跑通ydlidar激光数据

目录

一. 编译和安装

1.如果已经用命令安装了cartographer需先卸载。

2. 安装cartographer依赖库

3.下载编译cartographer源码。

4. 构建

二. 修改cartorgrapher参数创建launch

1. 修改test.lua

2.launch文件修改

三.编译与运行

1. 编译cartographer_ros

2.运行启动ydlidar发布数据

3.运行cartographer_ros启动建图

四. 结果

1.运行后rviz2界面如下：

 2.保存地图

---

一. 编译和安装

1.如果已经用命令安装了cartographer需先卸载。

sudo apt remove ros-galactic-cartographer ros-galactic-cartographer-ros && sudo apt autoremove

2. 安装cartographer依赖库

sudo apt update
sudo apt install -y clang git   google-mock  libceres-dev   liblua5.3-dev  libboost-all-dev  libprotobuf-dev   protobuf-compiler  libeigen3-dev  libgflags-dev  libgoogle-glog-dev   libcairo2-dev   libsuitesparse-dev  python3-sphinx  lsb-release ninja-build stow

3.下载编译cartographer源码。

mkdir -p ~/cartoros2_ws/src
cd ~/cartoros2_ws/src
git clone https://github.com/ros2/cartographer.git
git clone https://github.com/ros2/cartographer_ros.git

#使用脚本安装abseil-cpp库

cd ~/cartoros2_ws
./src/cartographer/scripts/install_abseil.sh

4. 构建

source /opt/ros/galactic/setup.bash
cd ~/cartoros2_ws
colcon build

二. 修改cartorgrapher参数创建launch

cd /home/mfj/cartoros2_ws/src/cartographer_ros/cartographer_ros/configuration_files
cp revo_lds.lua test.lua

1. 修改test.lua

-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
--      http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "laser",--tracking_frame设为laser,与ydlidar的frame_id保持一致
  published_frame = "laser",--此处也为laser
  odom_frame = "odom",
  provide_odom_frame = false,--改为false
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = false,--改为false
  use_odometry = false,
  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.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 70--改为2*TRAJECTORY_BUILDER_2D.submaps.num_range_data
POSE_GRAPH.constraint_builder.min_score = 0.65

return options

2.launch文件修改

cd cartoros2_ws/src/cartographer_ros/cartographer_ros/launch/
cp demo_revo_lds.launch.py test.launch.py

"""
  Copyright 2018 The Cartographer Authors
  Copyright 2022 Wyca Robotics (for the ros2 conversion)

  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
"""

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node, SetRemap
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import Shutdown
import os

def generate_launch_description():
    ## ***** Launch arguments *****
    bag_filename_arg = DeclareLaunchArgument('bag_filename')

  ## ***** File paths ******
    pkg_share = FindPackageShare('cartographer_ros').find('cartographer_ros')
    urdf_dir = os.path.join(pkg_share, 'urdf')
    urdf_file = os.path.join(urdf_dir, 'backpack_2d.urdf')
    with open(urdf_file, 'r') as infp:
        robot_desc = infp.read()

    ## ***** Nodes *****
    robot_state_publisher_node = Node(
        package = 'robot_state_publisher',
        executable = 'robot_state_publisher',
        parameters=[
            {'robot_description': robot_desc},
            {'use_sim_time': True}],
        output = 'screen'
        )

    cartographer_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_node',
        parameters = [{'use_sim_time': True}],
        arguments = [
            '-configuration_directory', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files',
            '-configuration_basename', 'test.lua'],
        remappings = [
            ('scan', 'scan')],#节点映射，修改为scan
        output = 'screen'
        )

    cartographer_occupancy_grid_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_occupancy_grid_node',
        parameters = [
            {'use_sim_time': True},
            {'resolution': 0.05}],
        )

    rviz_node = Node(
        package = 'rviz2',
        executable = 'rviz2',
        on_exit = Shutdown(),
        arguments = ['-d', FindPackageShare('cartographer_ros').find('cartographer_ros') + '/configuration_files/demo_2d.rviz'],
        parameters = [{'use_sim_time': True}],
    )

    ros2_bag_play_cmd = ExecuteProcess(
        cmd = ['ros2', 'bag', 'play', LaunchConfiguration('bag_filename'), '--clock'],
        name = 'rosbag_play',
    )

    return LaunchDescription([
        # Launch arguments
        #bag_filename_arg, 注释
        # Nodes
        robot_state_publisher_node,
        cartographer_node,
        cartographer_occupancy_grid_node,
        rviz_node,
        #ros2_bag_play_cmd 注释
    ])

三.编译与运行

1. 编译cartographer_ros

colcon build --packages-up-to cartographer_ros

2.运行启动ydlidar发布数据

运行前确保frame_id 修改为laser

ros2 launch ydlidar_ros2_driver ydlidar_launch.py 

3.运行cartographer_ros启动建图

ros2 launch cartographer_ros test.launch.py 

四. 结果

1.运行后rviz2界面如下：

 2.保存地图

2.1 结束构图，停止接收新数据

ros2 service call /finish_trajectory cartographer_ros_msgs/srv/FinishTrajectory "{trajectory_id: '0'}"

2.2 序列化保存其当前状态

ros2 service call /write_state cartographer_ros_msgs/srv/WriteState "{filename: '/home/mfj/mymap.pbstream'}"

2.3 将pbstream转换为pgm和yaml

ros2 run cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=${HOME}/mymap -pbstream_filename=${HOME}/mymap.pbstream -resolution=0.05 

3.查看生成的文件和地图

3.1 mymap.yaml 文件

image: /home/mfj/mymap.pgm
resolution: 0.050000
origin: [-6.614513, -7.934558, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

3.2 mymap.pgm