博客

第五讲【cartographer】11111

  • Post author:
  • Post category:其他




第五讲【cartographer】在仿真环境中 建图 纯定位

版权声明：本文为CSDN博主「WeSiGJ」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。

原文链接：

https://blog.csdn.net/wesigj/article/details/114419865



系列文章目录



第一讲【ROS-SLAM】2D激光雷达 cartographer构建地图



第二讲 【cartographer】Ubuntu16.04 kinetic 最新版cartographer安装（2020/11/4更新）



第三讲 【cartographer】 添加功能以从RVIZ为纯本地化模式设置初始姿势



第四讲 【cartographer】纯定位 纯本地化 pure_localization



第五讲【cartographer】在仿真环境中 建图 纯定位



第六讲【cartographer】纯定位参数优化（初级篇）


文章目录



前言

在众多机器人研发人员中，总会出现没有机器人使用的情况。因此，在本文中介绍了机器人仿真环境的部署与使用，让你随时随地都可以拥有无数台机器人。（IT可以的，我们也可以，话说我们属不属于IT行业，本人就属于移动机器人行业）



一、ROS仿真环境是什么？



1.1、gazebo

gazebo就相当于验证算法的MATLAB？

gazebo是三维物理仿真平台，创建一个虚拟的仿真环境，创造数据。



1.2、rviz

rviz是三维可视化工具，把已有的数据可视化显示，显示数据。



二、仿真环境快速使用步骤



2.1.引入代码


https://github.com/DroidAITech/ROS-Academy-for-Beginners/tree/master

创建一个工作空间，使用catkin_make编译，编译后添加环境路径：

source ~/.bashrc



2.2.启动仿真环境

gazebo： 

roslaunch robot_sim_demo robot_spawn.launch

键盘控制机器人节点： 

rosrun robot_sim_demo robot_keyboard_teleop.py



三、cartographer建图以及纯定位



3.1、在线建图

roslaunch cartographer_ros demo_revo_lds.launch

demo_revo_lds.launch 

<launch>
  <param name="/use_sim_time" value="true" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename revo_lds.lua"
      output="screen">
    <remap from="scan" to="scan" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>

revo_lds.lua 

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = 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 = 35
POSE_GRAPH.constraint_builder.min_score = 0.65

return options



3.2、离线建图（bag建图）

offline_backpack_2d.launch

（如果bag录制了TF，tf_static,使用下面的launch文件） 

<launch>
  <arg name="no_rviz" default="false"/>
  <param name="/use_sim_time" value="true" />

  <group unless="$(arg no_rviz)">
    <node name="rviz" pkg="rviz" type="rviz" required="true"
        args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />

    <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
        type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
  </group>

  <node name="cartographer_offline_node" pkg="cartographer_ros"
      required="$(arg no_rviz)"
      type="cartographer_offline_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basenames revo_lds.lua
          -bag_filenames $(arg bag_filenames)"
      output="screen">
    <remap from="scan" to="scan" />
  </node>
  <node name="playbag" pkg="rosbag" type="play"
      args="-r 2 -l==2  --clock $(arg bag_filenames)" />

</launch>

offline_backpack_2d.launch

（如果bag没有录制了TF，tf_static,使用下面的launch文件） 

<launch>
  <arg name="no_rviz" default="false"/>
  <param name="/use_sim_time" value="true" />

  <group unless="$(arg no_rviz)">
    <node name="rviz" pkg="rviz" type="rviz" required="true"
        args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />

    <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
        type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
  </group>

  <node name="cartographer_offline_node" pkg="cartographer_ros"
      required="$(arg no_rviz)"
      type="cartographer_offline_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basenames revo_lds.lua
          -urdf_filenames $(find cartographer_ros)/urdf/backpack_2d.urdf
          -bag_filenames $(arg bag_filenames)"
      output="screen">
    <remap from="scan" to="scan" />
  </node>
  <node name="playbag" pkg="rosbag" type="play"
      args="-r 2 -l==2  --clock $(arg bag_filenames)" />

</launch>

revo_lds.lua 

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = true,
  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 = 35
POSE_GRAPH.constraint_builder.min_score = 0.65

return options

rosbag info xxx.bag 

types:       nav_msgs/Odometry     [cd5e73d190d741a2f92e81eda573aca7]
             sensor_msgs/Imu       [6a62c6daae103f4ff57a132d6f95cec2]
             sensor_msgs/LaserScan [90c7ef2dc6895d81024acba2ac42f369]
             tf2_msgs/TFMessage    [94810edda583a504dfda3829e70d7eec]
topics:      /imu          2791 msgs    : sensor_msgs/Imu      
             /odom        13993 msgs    : nav_msgs/Odometry    
             /scan         5592 msgs    : sensor_msgs/LaserScan
             /tf          46647 msgs    : tf2_msgs/TFMessage    (2 connections)
             /tf_static       1 msg     : tf2_msgs/TFMessage



3.3、纯定位

roslaunch cartographer_ros demo_backpack_2d_localization.launch


demo_backpack_2d_localization.launch
 

<launch>
  <param name="/use_sim_time" value="true" />
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d_localization.lua
          -start_trajectory_with_default_topics = false
          -load_state_filename /root/ros_workspaces/bag/mymap.pbstream" 
      output="screen">
    <remap from="scan" to="scan" />
  </node>

  <node name="set_initpose" pkg="cartographer_ros" type="set_initpose_from_rviz" output="screen"
        args="
              -configuration_directory $(find cartographer_ros)/configuration_files
              -configuration_basename backpack_2d_localization.lua
              -load_state_filename /root/ros_workspaces/bag/mymap.pbstream" >
    <remap from="scan" to="scan" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.03" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
</launch>


backpack_2d_localization.lua（1.0.0）


master和1.0.0版本的cartographer，backpack_2d.lua纯定位的参数设置不一样。 

include "backpack_2d.lua"

TRAJECTORY_BUILDER.pure_localization = true
POSE_GRAPH.optimize_every_n_nodes = 20

return options


backpack_2d_localization.lua（master）
 

include "backpack_2d.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3,
}
POSE_GRAPH.optimize_every_n_nodes = 20

return options

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",
  published_frame = "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.5,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 2e-2,
  --启用将跟踪的姿势发布为geometry_msgs / PoseStamped到主题“ tracked_pose”。
  --publish_tracked_pose_msg = true,
  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.,
  }
  --pure_localization
  POSE_GRAPH.optimize_every_n_nodes = 20
  MAP_BUILDER.num_background_threads = 4
  POSE_GRAPH.global_sampling_ratio = 0.003
  POSE_GRAPH.constraint_builder.sampling_ratio = 0.3
  POSE_GRAPH.constraint_builder.min_score = 0.85
  POSE_GRAPH.global_constraint_search_after_n_seconds = 30
  --pure_localization end
  -- POSE_GRAPH.optimization_problem.log_solver_summary = true

  MAP_BUILDER.use_trajectory_builder_2d = true
  
  TRAJECTORY_BUILDER_2D.submaps.num_range_data = 45
  TRAJECTORY_BUILDER_2D.min_range = 0.05
  TRAJECTORY_BUILDER_2D.max_range = 15.
  TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
  TRAJECTORY_BUILDER_2D.use_imu_data = true
  --重力常数
  TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 9.7883
  
  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(45.)
  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.ceres_scan_matcher.translation_weight和
  --rotation_weight。越高，将结果从先前移开，换句话说，就越昂贵：扫描匹配必须在要接受的另一个位置产生更高的分数。
  --TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 2e2
  --TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 4e2


  POSE_GRAPH.optimization_problem.huber_scale = 1e2
  --odom
  -- POSE_GRAPH.optimization_problem.initial_pose_translation_weight = 1e5
  -- POSE_GRAPH.optimization_problem.initial_pose_rotation_weight = 1e5
  -- POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e5
  -- POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e1
  

return options

需要注意的是：上面的版本是cartographer_ros 1.0.0，在最新的master分支，纯定位的backpack_2d_localization.lua是不一样的。区别在于：


master分支下的backpack_2d_localization.lua
 

include "revo_lds_2d_localization.lua"

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 5,
}
POSE_GRAPH.optimize_every_n_nodes = 20

return options



总结

以上就是今天要讲的内容，本文介绍了ROS仿真环境的使用，而ROS仿真环境提供了大量能使我们快速便捷地处理数据的函数和方法以及算法、应用的测试。

版权声明：本文为CSDN博主「WeSiGJ」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。

原文链接：

https://blog.csdn.net/wesigj/article/details/114419865