ROS1 go2 vlp16 局部避障--3 篇

本文介绍ROS1下 unitree + vlp16 + cartographer的自主定位+避障+探索

本文的基础搭建：gazebo模型及各类配置文件见https://blog.csdn.net/weixin_41469272/article/details/152049194

需要完成以上的配置的基础上，才能进行本文的配置。

0. 更新各类配置文件

unitree_guide/unitree_move_base/config/vlp_costmap_common_params.yaml

xml 复制代码

obstacle_range: 10         ##
raytrace_range: 15         ##
footprint: [[0.3, 0.4], [0.3, -0.4], [-0.35, -0.4], [-0.35, 0.4]]
# robot_radius: 0.3
inflation_radius: 0.3 
max_obstacle_height: 5.0 
min_obstacle_height: 0.0 
origin_z: 0.0 
# observation_sources: scan
# scan: {data_type: LaserScan, topic: /merged_laserscan, marking: true, clearing: true, expected_update_rate: 3.3}
observation_sources: scan
scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 10}
#observation_sources: scan
#headLaserscan: {data_type: LaserScan, topic: scan, marking: true, clearing: true, expected_update_rate: 10}

unitree_guide/unitree_move_base/config/vlp_global_costmap_params.yaml

xml 复制代码

global_costmap:
   global_frame: odom
   robot_base_frame: base
   update_frequency: 1.0 
   publish_frequency: 1.0 
   rolling_window: true
   width: 20.0
   height: 15.0
   resolution: 0.05
   transform_tolerance: 1.0 
plugins:
  - {name: static_layer, type: "costmap_2d::StaticLayer"}
  - {name: obstacles, type: "costmap_2d::ObstacleLayer"}
  - {name: inflation, type: "costmap_2d::InflationLayer"}

unitree_guide/unitree_move_base/config/vlp_local_costmap_params.yaml

xml 复制代码

local_costmap:
   global_frame: odom
   robot_base_frame: base
   update_frequency: 5.0 
   publish_frequency: 3.0 
   rolling_window: true
   width: 3
   height: 5
   inflation_radius: 0.3 
   resolution: 0.05
   transform_tolerance: 1.0 
plugins:
   - {name: obstacles, type: "costmap_2d::ObstacleLayer"}
   - {name: inflation, type: "costmap_2d::InflationLayer"}

cartographer_ros/cartographer_ros)/configuration_files/vlp_2d.lua

xml 复制代码

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.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., 
}

pose_extrapolator = { 
    pose_queue_duration = 0.5,
    imu_gravity_time_constant = 10.,
    odometry_translation_weight = 1., 
    odometry_rotation_weight = 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

unitree_guide/unitree_move_base/launch/my_carto_explore.launch

xml 复制代码

<launch>
  <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan"
      respawn="false" output="screen">
      <remap from="cloud_in" to="/velodyne_points"/>
      <!--remap from="/scan" to="/headLaserScan"/-->
      <rosparam>
          target_frame: velodyne
          transform_tolerance: 0.1
          min_height: 0.0
          max_height: 1.0

          angle_min: -3.14159
          angle_max: 3.14159
          angle_increment: 0.0175
          scan_time: 0.1
          range_min: 0.5
          range_max: 10.0
          use_inf: false

          # Concurrency level, affects number of pointclouds queued for processing and number of threads used
          # 0 : Detect number of cores
          # 1 : Single threaded
          # 2->inf : Parallelism level
          concurrency_level: 1
      </rosparam>
  </node>


  <!-- MoveBase -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find unitree_move_base)/config/vlp_costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find unitree_move_base)/config/vlp_costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find unitree_move_base)/config/vlp_global_costmap_params.yaml" command="load" />
    <rosparam file="$(find unitree_move_base)/config/vlp_local_costmap_params.yaml" command="load" />
    <rosparam file="$(find unitree_move_base)/config/vlp_teb_local_planner_params.yaml" command="load" />
    <!-- (TEB) -->
    <!--param name="base_global_planner" value="global_planner/GlobalPlanner"/-->
    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS"/>
  </node>

  <!-- AMCL -->
  <!-- <include file="$(find unitree_move_base)/launch/amcl.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 vlp_2d.lua"
      output="screen">
    <!--remap from="points2" to="/velodyne_points" /-->
    <remap from="imu" to="/trunk_imu" />
    <!--remap from="/scan" to="/headLaserScan"/-->
  </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" /-->

  <node pkg="explore_lite" type="explore" name="explore" output="screen">
    <param name="costmap_topic" value="/map"/>
    <param name="robot_base_frame" value="base"/>
    <!--param name="costmap_topic" value="/move_base/global_costmap/costmap"/-->
    <!--param name="costmap_updates_topic" value="costmap_updates"/-->
    <param name="costmap_updates_topic" value="map_updates"/>
    <param name="visualize" value="true"/>
    <param name="planner_frequency" value="0.2"/>
    <param name="progress_timeout" value="30.0"/>
    <param name="potential_scale" value="3.0"/>
    <param name="gain_scale" value="1.0"/>
    <param name="min_frontier_size" value="0.5"/>
     <param name="frontier_search_size" value="15.0" />
    launch-prefix="bash -c 'sleep 1.0; $0 $@' ">
  </node>


    <!-- rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find unitree_move_base)/config/my_cart_move_base.rviz"/>
</launch>

1. 启动gazebo仿真

bash 复制代码

roslaunch unitree_guide vlp_go2.launch

2. 启动控制模块

bash 复制代码

 ./devel/lib/unitree_guide/junior_ctrl

后输入2,让狗站起来。再进行下述操作

3. 启动move_base + cartographer + pc2scan

bash 复制代码

roslaunch unitree_move_base my_carto_explore.launch

问题

待解决问题

gazebo 加载带雷达的go2 时，狗头会被激光压歪。使得整个地图的构建也是歪的

原因分析：由于vlp16重力导致。尚未去解

折中办法：在启动move_base 之前，先让go2先站起来，后启动定位节点，减少
cartograoher的base_link找不到到source frame的映射关系，此外还有rviz 中trajecties有报错的问题

go2 模型使用的base坐标系，暂时没管。