0
$\begingroup$

Rosanswers logo

Dear friends,

I have the following problem where I would love to get any hint that leads me to a solution. First of all here are my tech specs:

  • ROS melodic with Stage 4.3 simulation
    • Ubuntu 5.4.0-65.73~18.04.1-generic 5.4.78
    • Environment variables ROS_ETC_DIR=/opt/ros/melodic/etc/ros ROS_ROOT=/opt/ros/melodic/share/ros

ROS_MASTER_URI=http://localhost:11311 ROS_VERSION=1 ROS_PYTHON_VERSION=2 ROS_PACKAGE_PATH=/home/emanuel/rccar_ws/src:/home/emanuel/testnav_ws/src:/home/emanuel/floribot_ws/src/base:/home/emanuel/floribot_ws/src/plc_connection:/opt/ros/melodic/share ROSLISP_PACKAGE_DIRECTORIES=/home/emanuel/rccar_ws/devel/share/common-lisp:/home/emanuel/testnav_ws/devel/share/common-lisp:/home/emanuel/floribot_ws/devel/share/common-lisp ROS_DISTRO=melodic

I am running a ROS Stage simulation with a differential drive robot equipped with a laser scanner and odometry. The robot is to pass through a corn field. It does not know the area at the beginning but is given an empty map that is dynamically builds up as the robot proceeds. The navigation stack is set up as described in http://wiki.ros.org/navigation/Tutorials/RobotSetup with some modifications. I am using the following planners:

  • global planner: A self written planner that simply returns the goal which yields to a straight line from the current position to the goal not depending on any costmap
    • local planner: DWAPlannerROS

Now to my question: When I am sending navigation goals to the robot it can follow and reach them eventually. But I have noticed that almost every time the global and local costmap are shifted and do not match the map mapped by gmapping. Thus, the costs in the costmaps which are indicating an obstacle (here: corn plant) do not sit directly at the mapped obstacles but are shifted as it can be seen in the picture. This leads to a very unstable navigation: Sometimes the robot can reach its given goals but sometimes not. What can I change to have the costmaps lay perfectly onto the mapped map?

Unfortunately I do not have enough points to include a picture - therefore I uploaded it to this page: https://ibb.co/yXGFzqj

I appreciate any hint that could help me fixing this. Kind regards, Eman

P.S. here are my costmap and planner config files

costmap_common_params.yaml

global_frame: map
robot_base_frame: base_link
obstacle_range: 5.0
raytrace_range: 10.0
footprint: [[0.275, 0.125], [-0.275, 0.125], [-0.275, -0.125], [0.275, -0.125]]

plugins:
    - {name: static_layer, type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
publish_frequency: 1.0
static_layer:
    unknown_cost_value: -1
    lethal_cost_threshold: 250
    map_topic: map
    first_map_only: false
    subscribe_to_updates: false
    track_unknown_space: true
    use_maximum: false
    trinary_costmap: false
obstacle_layer:
    observation_sources: laser_scan_sensor
    laser_scan_sensor: {sensor_frame: base_laser_link, data_type: LaserScan, topic: base_scan, marking: true, clearing: true}
    track_unknown_space: true
    footprint_clearing_enabled: true
    combination_method: 0
#    origin_z: 0.0
#    z_resolution: 0.05
#    z_voxels: 10
#    unknown_threshold: 10
#    mark_threshold: 0
#    publish_voxel_map: false
inflation_layer:
    inflation_radius: 0.35 # = gap corn row / 2, so that robot runs in the very center of the row
    cost_scaling_factor: 15.0 # the higher, the quicker the exp. cost function declines

local_costmap_params.yaml

local_costmap:
  update_frequency: 10.0
  static_map: true
  rolling_window: true
  width: 3.0
  height: 3.0
  resolution: 0.05

global_costmap_params.yaml

global_costmap:
  update_frequency: 10.0
  static_map: true
  rolling_window: true
  width: 6.0
  height: 6.0
  resolution: 0.05

base_local_planner_params.yaml

DWAPlannerROS: 
    acc_lim_x: 2.5 # (double, default: 2.5) The x acceleration limit of the robot in meters/sec^2 
    acc_lim_y: 2.5 # (double, default: 2.5) The y acceleration limit of the robot in meters/sec^2 
    acc_lim_th: 1.6 # (double, default: 3.2) The rotational acceleration limit of the robot in radians/sec^2 

    max_vel_trans: 0.55 # (double, default: 0.55) The absolute value of the maximum translational velocity for the robot in m/s 
    min_vel_trans: 0.05 # (double, default: 0.1) The absolute value of the minimum translational velocity for the robot in m/s 

    max_vel_x: 0.55 # (double, default: 0.55) The maximum x velocity for the robot in m/s. 
    min_vel_x: -0.55 # (double, default: 0.0) The minimum x velocity for the robot in m/s, negative for backwards motion. 

    max_vel_y: 0.0 # (double, default: 0.1) The maximum y velocity for the robot in m/s 
    min_vel_y: 0.0 # (double, default: -0.1) The minimum y velocity for the robot in m/s 

    max_vel_theta: 1.0 # (double, default: 1.0) The absolute value of the maximum rotational velocity for the robot in radians/sec 
    min_vel_theta: 0.1 # (double, default: 0.4) The absolute value of the minimum rotational velocity for the robot in radians/sec

    yaw_goal_tolerance: 0.05 # (double, default: 0.05) The tolerance in radians for the controller in yaw/rotation when achieving its goal 
    xy_goal_tolerance: 0.10 # (double, default: 0.10) The tolerance in meters for the controller in the x & y distance when achieving a goal 
    latch_xy_goal_tolerance: false # (bool, default: false) If goal tolerance is latched, if the robot ever reaches the goal xy location it will simply rotate in place, even if it ends up outside the goal tolerance while it is doing so.

    sim_time: 1.7 # (double, default: 1.7) The amount of time to roll trajectories out for in seconds 
    sim_granularity: 0.025 # (double, default: 0.025) The granularity with which to check for collisions along each trajectory in meters
    angular_sim_granularity: 0.1 # (double, default: 0.1) The granularity with which to check for collisions for rotations in radians 
    vx_samples: 8 # (integer, default: 3) The number of samples to use when exploring the x velocity space 
    vy_samples: 10 # (integer, default: 10) The number of samples to use when exploring the y velocity space
    vth_samples: 20 # (integer, default: 20) The number of samples to use when exploring the theta velocity space   
    use_dwa: True # (bool, default: True) Use dynamic window approach to constrain sampling velocities to small window.
    controller_frequency: 20.0 # (double, default: 20.0) The frequency at which this controller will be called in Hz.

    path_distance_bias: 0.01 # (double, default: 0.6) The weight for the path distance part of the cost function
    goal_distance_bias: 1.0 # (double, default: 0.8) The weight for the goal distance part of the cost function 
    occdist_scale: 0.01 # (double, default: 0.01) The weight for the obstacle distance part of the cost function
    twirling_scale: 0.0 # (double, default: 0.0) The weight for penalizing any changes in robot heading
  
    forward_point_distance: 0.325 # (double, default: 0.325) The distance from the center point of the robot to place an additional scoring point, in meters 
    stop_time_buffer: 0.2 # (double, default: 0.2) The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds 
    scaling_speed: 0.25 # (double, default: 0.25) The absolute value of the velocity at which to start scaling the robot's footprint, in m/s  
    max_scaling_factor: 0.2 # (double, default: 0.2) The maximum factor to scale the robot's footprint by 
    publish_cost_grid: true # (bool, default: false) Whether or not to publish the cost grid that the planner will use when planning.
    oscillation_reset_dist: 0.05 # (double, default: 0.05) The distance the robot must travel before oscillation flags are reset, in meters 
    oscillation_reset_angle: 0.2 # (double, default: 0.2) The angle the robot must turn before oscillation flags are reset, in radians
    prune_plan: true # (bool, default: true) Defines whether or not to eat up the plan as the robot moves along the path. If set to true, points will fall off the end of the plan once the robot moves 1 meter past them.

overall launch file (base_global_planner_params is not used, because self written global planner is used)

 <!-- Run the map server --> 
    <node name="map_server" pkg="map_server" type="map_server" args="$(find map)/emptyMap.yaml">
     <param name="frame_id" value="map" />
    </node>

 <!-- Run gmapping -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen" >
     <remap from="scan" to="base_scan" />
     <param name="odom_frame" value="odom" />
     <param name="base_frame" value="base_link" />
     <param name="map_frame" value="map"/>
    </node>

 <!--- Run AMCL --> 
    <remap from="scan" to="base_scan" />
    <include file="$(find amcl)/examples/amcl_diff.launch" />

   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find floribot_2dnav)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find floribot_2dnav)/param/costmap_common_params.yaml" command="load" ns="local_costmap" /> 
    <rosparam file="$(find floribot_2dnav)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find floribot_2dnav)/param/global_costmap_params.yaml" command="load" /> 
    <rosparam file="$(find floribot_2dnav)/param/base_local_planner_params.yaml" command="load" />
    <rosparam file="$(find floribot_2dnav)/param/base_global_planner_params.yaml" command="load" />
    <rosparam file="$(find floribot_2dnav)/param/move_base_params.yaml" command="load" />
   </node>

</launch>

tf tree https://ibb.co/2K1kKkV


Originally posted by MoritzEmanuel on ROS Answers with karma: 3 on 2021-03-02

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Hi,

can you provide your config for the costmap and the planners? (mostly the frames they operate in are interesting) furthermore please provide a tf tree of your setup.

My guess is, that your odom frame isn't sitting at the same spot as your map frame, which maybe will offset your costmaps if their global_frame is odom. Therefore please check the global_frame of your costmaps.

The misalignment probably comes from the mapping and localization procedure. Thats why your global_frame of the costmap imo should be map. Can you check the position of your global costmap in rviz? For this set fixed frame to map. Does the position move over time?

Hope this helps


Originally posted by Tristan9497 with karma: 220 on 2021-03-02

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by MoritzEmanuel on 2021-03-05:
Hi Tristan,

thank you for your answer. I added the costmap and planner config files in addition to the tf tree and my launch file into the description.

  • I have set the global_frame for the local and global costmap to map, but the problem still exists
  • The fixed frame in rviz is set to map. Yes, the position of the global costmap moves when the robot is moving -> this leads to the shift that I have encountered (see another image here https://ibb.co/HXrKPsB)

Do you have an idea where the error may come from?

Kind regards,

Eman

Comment by MoritzEmanuel on 2021-03-05:
I think there is something wrong with odom or gmapping, because

  • at the beginning odom tf matches map tf perfectly but later on odom tf moves away from map tf while the robot is moving (map tf is fixed frame in rviz)
  • after the robot has moved and the costmap does not overlay with the static map, the laserscan matches the obstacles in the costmap but not the one in the static map

Here is an image that shows what I mean: https://ibb.co/HK4tdfS

Does that help somehow to find the errror causing this behaviour

Comment by Tristan9497 on 2021-03-05:
Ok, thanks for the further information. I can't tell you exactly what is causing the behavior but i can point out things that seem weird in your setup.

Check the global_costmap_params.yaml and local_costmap_params.yaml here you define you want the maps to be static meaning not moving relative to the global_frame. Diretly after that you define them as rolling window meaning moving with your base frame.

This certainly is wrong the costmap can either be rolling or static, not both. Take a look at the parameter part in the Documentation

My proposal here is the general setup for the costmaps:

global:
  static_map: true
  rolling_window: false
  global_frame: map

local:
  static_map: false
  rolling_window: true
  global_frame: odom

Comment by Tristan9497 on 2021-03-05:
I am actually unsure if i would include the static layer in the local costmap. I would probably feed only live data to it so it can clear obstacles that have been previously in the map but aren't there anymore.

Comment by MoritzEmanuel on 2021-03-10:
Hello Tristan, thank you for your help! together with a friend we have found the solution: Disabling the amcl did the trick! Probably slam_gmapping and amcl were competing against each other and causing jumps in the tf between map-->odom. Further more I set both global_frame to map:

global_costmap:
  static_map: true
  rolling_window: false
  global_frame: map

local_costmap:
  static_map: false
  rolling_window: true
  global_frame: map

It is so cool to have you guys helping out! Thank you, Eman

Comment by Tristan9497 on 2021-03-10:
Hey Eman, thanks for the update i am happy that i was able to help you.

What you are saying makes totally sense, since both gmapping and amcl have a localization they both publish tf transforms.

Yea i love this community here as well. I was totally lost, when i started with ros.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.