ros2 -foxy安装cartographer建图定位-- 源码安装使用

chilian12321

已于 2022-09-03 17:43:39 修改

阅读量2.5k

点赞数

分类专栏： ros 文章标签： ros2

于 2022-09-03 17:28:02 首次发布

本文链接：https://blog.csdn.net/chilian12321/article/details/126668426

版权

ros 专栏收录该内容

19 篇文章 3 订阅

订阅专栏

foxy 的cartographer_ros 和dashing 的兼容

cd ~/xx__ws/src

安装源码

git clone https://ghproxy.com/https://github.com/ros2/cartographer.git -b foxy

git clone https://ghproxy.com/https://github.com/ros2/cartographer_ros.git -b dashing

安装依赖

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

lua文件：


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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "gyro_link",
  published_frame = "base_footprint",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  -- publish_tracked_pose=true, --发布小车位置
  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
MAP_BUILDER.num_background_threads = 2
-- TRAJECTORY_BUILDER.pure_localization_trimmer.max_submaps_to_keep = 3
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 45
TRAJECTORY_BUILDER_2D.min_range = 0.1
TRAJECTORY_BUILDER_2D.max_range = 6.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = true --是否使用imu数据

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.angular_search_window = math.rad(10.) --角度搜索框的大小
-- 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


TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.05  --运动滤波更新距离   // 如果移动距离过小, 或者时间过短, 不进行地图的更新
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.03)
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 0.5

TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.num_threads = 1
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 20  --迭代次数

POSE_GRAPH.optimization_problem.huber_scale = 1e2

POSE_GRAPH.constraint_builder.max_constraint_distance = 15.
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.constraint_builder.min_score = 0.50

POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60
POSE_GRAPH.optimize_every_n_nodes = 35

return options

建图launch:

import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory

from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource




def generate_launch_description():
    # 定位到功能包的地址
    pkg_share = FindPackageShare(package='carerobot_pkg').find('carerobot_pkg')

    scan_dir = get_package_share_directory('rplidar_ros')
    scan_launch_dir = os.path.join(scan_dir, 'launch') #雷达launch文件路径

    serial_dir = get_package_share_directory('serial_pkg')
    serial_launch_dir = os.path.join(serial_dir, 'launch') #串口launch文件路径

    remappings = [('/tf', 'tf'),
                  ('/tf_static', 'tf_static')]

    urdf = os.path.join(pkg_share, 'urdf', 'careRobot.urdf') #模型
    
    #=====================运行节点需要的配置=======================================================================
    # 是否使用仿真时间，我们用gazebo，这里设置成true
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    # 地图的分辨率
    resolution = LaunchConfiguration('resolution', default='0.01')
    # 地图的发布周期
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
    # 配置文件夹路径
    configuration_directory = LaunchConfiguration('configuration_directory',default= os.path.join(pkg_share, 'config') )
    # 配置文件
    configuration_basename = LaunchConfiguration('configuration_basename', default='careRobot_2d.lua')

    

    start_robot_state_publisher_cmd = Node(  #发布小车TF信息
        package='robot_state_publisher',
        executable='robot_state_publisher',  
        remappings=remappings,
        arguments=[urdf])

    joint_state_publisher_node = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',  
        remappings=remappings,
        arguments=[urdf]
    )


    serial_cmd = IncludeLaunchDescription(  #串口开启
        PythonLaunchDescriptionSource(os.path.join(serial_launch_dir, 'serial_pkg.launch.py')),
        launch_arguments={'namespace': '',
                          'use_namespace': 'False'}.items())

    scan_cmd = IncludeLaunchDescription(  #雷达开启
        PythonLaunchDescriptionSource(os.path.join(scan_launch_dir, 'rplidar_A1.launch.py')),
        launch_arguments={'namespace': '',
                          'use_namespace': 'False'}.items())
    #=====================声明三个节点，cartographer/occupancy_grid_node/rviz_node=================================
    cartographer_node = 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])

    occupancy_grid_node = Node(
        package='cartographer_ros',
        executable='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])

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        # arguments=['-d', rviz_config_dir],
        parameters=[{'use_sim_time': use_sim_time}],
        output='screen')

    #===============================================定义启动文件========================================================
    ld = LaunchDescription()
    ld.add_action(serial_cmd)
    ld.add_action(scan_cmd)
    
    # Add the actions to launch all of the navigation nodes
    ld.add_action(start_robot_state_publisher_cmd)
    ld.add_action(joint_state_publisher_node)  

    ld.add_action(cartographer_node)
    ld.add_action(occupancy_grid_node)
    # ld.add_action(rviz_node)  在虚拟机上面远程启动

    return ld

地图保存1:ros2 run nav2_map_server map_saver_cli -t map -f name //只是文件名 xx.yaml xx.pgm

地图保存2:

ros2 service call /write_state cartographer_ros_msgs/srv/WriteState "{filename: 'path/cartorapher.pbstream'}" //绝对路径

ros2 service call /finish_trajectory cartographer_ros_msgs/srv/FinishTrajectory "{trajectory_id: '0'}" //结束轨迹

定位launch:

import os
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory

from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource

from nav2_common.launch import RewrittenYaml

#定位 launch


def generate_launch_description():
    # 定位到功能包的地址
    pkg_share = FindPackageShare(package='carerobot_pkg').find('carerobot_pkg')

    scan_dir = get_package_share_directory('rplidar_ros')
    scan_launch_dir = os.path.join(scan_dir, 'launch') #雷达launch文件路径

    serial_dir = get_package_share_directory('serial_pkg')
    serial_launch_dir = os.path.join(serial_dir, 'launch') #串口launch文件路径

    remappings = [('/tf', 'tf'),
                  ('/tf_static', 'tf_static')]

    urdf = os.path.join(pkg_share, 'urdf', 'careRobot.urdf') #模型
    
    #=====================运行节点需要的配置=======================================================================
    # 是否使用仿真时间，我们用gazebo，这里设置成true
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    # 地图的分辨率
    resolution = LaunchConfiguration('resolution', default='0.01')
    # 地图的发布周期
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
    # 配置文件夹路径
    configuration_directory = LaunchConfiguration('configuration_directory',default= os.path.join(pkg_share, 'config') )
    # 配置文件
    configuration_basename = LaunchConfiguration('configuration_basename', default='careRobot_2d.lua')

    autostart = LaunchConfiguration('autostart', default='True')
    #配置地图
    configuration_map = LaunchConfiguration('configuration_map', default=os.path.join(pkg_share, 'maps', 'careRobot.pbstream'))

    map_yaml_file = LaunchConfiguration('configuration_map', default=os.path.join(pkg_share, 'maps', 'careRobot.yaml'))
    #配置导航文件
    configuration_nav_params =  LaunchConfiguration('configuration_nav_params', default=os.path.join(pkg_share, 'params', 'careRobot_nav.yaml'))
    

    default_bt_xml_filename = LaunchConfiguration('default_bt_xml_filename', 
        default=os.path.join(get_package_share_directory('nav2_bt_navigator'),
        'behavior_trees', 'navigate_w_replanning_and_recovery.xml'))


    map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local', default='True')

    namespace = LaunchConfiguration('namespace', default='')

    lifecycle_nodes = ['controller_server',
                       'planner_server',
                       'recoveries_server',
                       'bt_navigator',
                       'waypoint_follower']

   


    serial_cmd = IncludeLaunchDescription(  #串口开启
        PythonLaunchDescriptionSource(os.path.join(serial_launch_dir, 'serial_pkg.launch.py')),
        launch_arguments={'namespace': '',
                          'use_namespace': 'False'}.items())

    scan_cmd = IncludeLaunchDescription(  #雷达开启
        PythonLaunchDescriptionSource(os.path.join(scan_launch_dir, 'rplidar_A1.launch.py')),
        launch_arguments={'namespace': '',
                          'use_namespace': 'False'}.items())
    #=====================声明三个节点，cartographer/occupancy_grid_node/rviz_node=================================
 
    start_robot_state_publisher_cmd = Node(  #发布小车TF信息
        package='robot_state_publisher',
        executable='robot_state_publisher',  
        remappings=remappings,
        arguments=[urdf])

    joint_state_publisher_node = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',  
        remappings=remappings,
        arguments=[urdf]
    )
    cartographer_node = Node(
        package = 'cartographer_ros',
        executable = 'cartographer_node',
        name='cartographer_node',
        parameters = [{'use_sim_time': True}],
        arguments = [
            '-configuration_directory',configuration_directory,     #配置文件路径
            '-configuration_basename', configuration_basename,      #配置文件名
            '-load_state_filename', configuration_map               #pbstream地图
            ],
        # remappings = [
        #     ('echoes', 'horizontal_laser_2d')],
        output = 'screen'
    )


    occupancy_grid_node = Node(
        package='cartographer_ros',
        executable='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])

    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        # arguments=['-d', rviz_config_dir],
        parameters=[{'use_sim_time': use_sim_time}],
        output='screen')


    param_substitutions = {
        'use_sim_time': use_sim_time,
        'default_bt_xml_filename': default_bt_xml_filename,
        'autostart': autostart,
        'map_subscribe_transient_local': map_subscribe_transient_local}

    configured_params = RewrittenYaml(
            source_file=configuration_nav_params,
            root_key=namespace,
            param_rewrites=param_substitutions,
            convert_types=True)

    nav2_controller_node= Node( #导航控制器
            package='nav2_controller',
            executable='controller_server',
            output='screen',
            parameters=[configured_params],
            remappings=remappings)

    nav2_planner_node=Node( #导航归化器
        package='nav2_planner',
        executable='planner_server',
        name='planner_server',
        output='screen',
        parameters=[configured_params],
        remappings=remappings)

    nav2_recoveries_node=Node( #导航恢复
        package='nav2_recoveries',
        executable='recoveries_server',
        name='recoveries_server',
        output='screen',
        parameters=[configured_params],
        remappings=remappings)

    nav2_bt_navigator_node=Node( #行为树
        package='nav2_bt_navigator',
        executable='bt_navigator',
        name='bt_navigator',
        output='screen',
        parameters=[configured_params],
        remappings=remappings)

    nav2_waypoint_follower_node=Node(#点跟随
        package='nav2_waypoint_follower',
        executable='waypoint_follower',
        name='waypoint_follower',
        output='screen',
        parameters=[configured_params],
        remappings=remappings)

    nav2_lifecycle_manager_node=Node( #生命周期
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_navigation',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time},
                    {'autostart': autostart},
                    {'node_names': lifecycle_nodes}])

    #===============================================定义启动文件========================================================
    ld = LaunchDescription()
    ld.add_action(serial_cmd)
    ld.add_action(scan_cmd)
    
    # Add the actions to launch all of the navigation nodes
    ld.add_action(start_robot_state_publisher_cmd)
    ld.add_action(joint_state_publisher_node)  

    ld.add_action(cartographer_node)
    ld.add_action(occupancy_grid_node)

    ld.add_action(nav2_controller_node)
    ld.add_action(nav2_planner_node)
    ld.add_action(nav2_recoveries_node)
    ld.add_action(nav2_bt_navigator_node)
    ld.add_action(nav2_waypoint_follower_node)
    ld.add_action(nav2_lifecycle_manager_node)

    #ld.add_action(rviz_node)

    return ld