Robotics StackExchange | Archived questions

Side lidar causes localization problems

I have a robot base with a 180deg side lidar (the lidar faces from the angle 0 - x direction of the robot - to the angle -180deg). When I give a 2D Nav goal in rviz, then my robot doesn't behaves well: it seems like staying at the same position (or rotating when in reality is just steering) even though in Gazebo is indeed moving. This makes me think that it is a localization problem.

When I change the urdf model so that the lidar faces forward (-90 to 90deg) though, the robot behaves perfectly with the same configuration file.

I think that the root of the problem is either the or SLAM but I can't identify it. In particular I think that maybe the fact that the -0.01deg space is unknown, while the +0.01deg known, is problematic.

Any idea on how to solve it?

I use ROS1, melodic, hectorslam, movebase.

costmapcommonparams.yaml:

map_type: costmap

# footprint: [[x0, y0], [x1, y1], ... [xn, yn]]
robot_radius: 0.5 #  footprint of the robot or the radius of the robot if it is circular
inflation_radius: 0.55 # the robot will treat all paths that stay 0.55 meters or more away from obstacles as having equal obstacle cost
transform_tolerance: 0.6


obstacle_layer:
 enabled: true
 obstacle_range: 3.0 # the robot will only update its map with information about obstacles that are within 2.5 meters of the base
 raytrace_range: 3.5 # the robot will attempt to clear out space in front of it up to 3.0 meters
 inflation_radius: 0.1
 track_unknown_space: true
 combination_method: 1

 observation_sources: laser_scan_sensor
 laser_scan_sensor: {sensor_frame: base_scan, data_type: LaserScan, topic: scan_filtered, marking: true, clearing: true} #topic: scan_filtered


inflation_layer:
  enabled: true
  observation_persistence: 0.0
  cost_scaling_factor:  10.0  # exponential rate at which the obstacle cost drops off (default: 10)
  inflation_radius:     0.8  # max. distance from an obstacle at which costs are incurred for planning paths.

static_layer:
  enabled:              true
  observation_persistence: 0.0
  map_topic:            "/map"

localcostmapparams.yaml:

local_costmap:
  global_frame: odom
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.05
  # transform_tolerance: 1.0

  plugins:
   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
   - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

globalcostmapparams.yaml:

global_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 5.0
  publish_frequency: 2.0
  # static_map: false
  static_map: true
  rolling_window: false
  track_unknown_space: true
  width: 1024
  height: 1024
  origin_x: -51.25
  origin_y: -51.25
  resolution: 0.1
  # transform_tolerance: 1.0

  plugins:
    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

hector_slam.launch:

<?xml version="1.0"?>

<launch>
  <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
  <arg name="base_frame" default="base_link"/>
  <arg name="odom_frame" default="odom"/>
  <arg name="pub_map_odom_transform" default="true"/>
  <arg name="scan_subscriber_queue_size" default="5"/>
  <arg name="scan_topic" default="scan_filtered"/> <!--scan_filtered-->
  <arg name="map_size" default="1024"/>

  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">

    <!-- Frame names -->
    <param name="map_frame" value="map" />
    <param name="base_frame" value="$(arg base_frame)" />
    <param name="odom_frame" value="$(arg odom_frame)" />

    <!-- Tf use -->
    <param name="use_tf_scan_transformation" value="true"/>
    <param name="use_tf_pose_start_estimate" value="false"/>
    <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>

    <!-- Map size / start point -->
    <param name="map_resolution" value="0.1"/>
    <param name="map_size" value="$(arg map_size)"/>
    <param name="map_start_x" value="0.5"/>
    <param name="map_start_y" value="0.5" />
    <param name="map_multi_res_levels" value="2" />

    <!-- Map update parameters -->
    <param name="update_factor_free" value="0.4"/>
    <param name="update_factor_occupied" value="0.9" />    
    <param name="map_update_distance_thresh" value="0.05"/>
    <param name="map_update_angle_thresh" value="0.06" />
    <param name="laser_z_min_value" value = "-1.0" />
    <param name="laser_z_max_value" value = "1.0" />
    <param name="map_pub_period" value="1.0"/>

    <!-- Advertising config --> 
    <param name="advertise_map_service" value="true"/>

    <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
    <param name="scan_topic" value="$(arg scan_topic)"/>

    <!-- Debug parameters -->
    <!--
      <param name="output_timing" value="false"/>
      <param name="pub_drawings" value="true"/>
      <param name="pub_debug_output" value="true"/>
    -->
    <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
  </node>
</launch>

move_base.launch:

<launch>

   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">

      <!-- Costmaps -->
      <rosparam file="$(find cb_navigation)/cfg/costmap_common_params.yaml" command="load" ns="global_costmap" /> 
      <rosparam file="$(find cb_navigation)/cfg/costmap_common_params.yaml" command="load" ns="local_costmap" />
      <rosparam file="$(find cb_navigation)/cfg/local_costmap_params.yaml" command="load" />
      <rosparam file="$(find cb_navigation)/cfg/global_costmap_params.yaml" command="load" /> 


      <!-- Global planner -->
        <param name="base_global_planner" value="global_planner/GlobalPlanner" />
        <param name="planner_frequency" value="1" />
        <param name="planner_patience" value="5.0" />

      <!-- Local Planner -->
      <!-- Teb Planner -->
      <!-- <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
      <rosparam file="$(find cb_navigation)/cfg/teb_local_planner.yaml" command="load" /> -->

      <!-- NeoLocalPlanner (For a single front wheel) -->
      <param name="base_local_planner" value="neo_local_planner/NeoLocalPlanner"/>
      <rosparam file="$(find cb_navigation)/cfg/neo_local_planner.yaml" command="load" />

      <!-- Common local planner parameters -->

      <param name="controller_frequency" value="20.0" />
        <param name="controller_patience" value="15.0" />
        <remap from="odom" to="odom_rf2o"/> <!-- To use the odom_rf2o instead of odom for the move_base. Is it needed? -->
      <param name="clearing_rotation_allowed" value="true" /> <!-- Our robot is able to rotate in place -->

      <!-- Optimized local planner parameters -->
      <param name="vx_samples" value="8.0" />
      <param name="meter_scoring" value="true" />
      <param name="sim_time" value="1.5" />
   </node>

  <include file="$(find laser2pc)/launch/laser2pc.launch"/>

</launch> 

Asked by AlexandrosNic on 2021-05-06 07:22:09 UTC

Comments

Answers