How to Avoid Collision (Obstacle) while navigating using rtabmap_ros and navigation stack

[RTABPK]

Hello, I am trying to perform autonomous navigation by using move_base node of navigation stack.

here is what I did,

    After mapping the environment using rtabmap_ros package, I stop the mapping session.
    start the rtabmap node again in localization mode and move the robot around.

I have two problems,

    The map is automatically loaded as soon as I start the rtabmap node in localization mode, even before localization of the robot. ideally, map should load when robot localizes itself.

    When I send the goal command using RVIZ "2D Nav Goal", It calculates the global trajectory correctly avoiding the obstacles. However, when the robot moves it collides with obstacle. and once it collides with the obstacle, it remains stuck there and does not get out of the situation.

Please guide what is that I am missing here,

here is my rtabmap launch file , https://gofile.io/?c=SoKwnx

here is my world file, https://gofile.io/?c=dsE9Tw

I spawn my husky robot into this world using a launch file here is the link to those launch files,

https://gofile.io/?c=CVNYK9

https://gofile.io/?c=MWeZWN

here is my robot description file

https://gofile.io/?c=Tmim8u

Please also note that I can locate my move_base package but there is no move_base node inside. There is only cmake and packagle.xml files inside these folders.

thanks in advance

[matlabbe]

Hi,

To avoid restarting at the last location after restarting rtabmap, set RGBD/SavedLocalizationIgnored to false. Here are some changes I've made to use also the lidar for more accurate mapping:

<param name="RGBD/SavedLocalizationIgnored"  type="string" value="true"/>
<param name="RGBD/NeighborLinkRefining"      type="string" value="true"/>
<param name="Reg/Strategy"                   type="string" value="1"/>
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="5"/> 

I saw also a problem with the scan topic, it was published by two nodes: the lidar and the fake scan from kinect. In spawn_husky.launch, change output scan name of pointcloud_to_laserscan to camera_scan. It is maybe the problem why at some point the robot collided with obstacles, if the scan from camera cleared obstacles from those added by the lidar.

cheers,
Mathieu

[RTABPK]

could you please comment on navigation method, I mean I am running move_base from inside the rtabmap launch file in localization mode and I am not using acml. Is this method correct for using navigation stack?

[matlabbe]

Hi,

Yes, you have to choose between rtabmap or amcl for localization. In that kind of environment (very symmetric), it may be challenging for visual localization. In my tests, the robot was jumping around in the map because of wrong localizations. If you want to try AMCL instead, you can use map_server's map_saver node to save the map so that it can be used by amcl (like in this tutorial). Then when starting AMCL, don't start rtabmap node at the same time.

cheers,
Mathieu

[RTABPK]

I have a 2D/3D map saved in database file located in ~/.ros directory using rtabmap. How can I run map_server's map_saver,? I mean I have following questions in mind,

1. should I open the database first?

2. how can I specify the location/directory of the database file to  map_server's map_saver so that it extracts the map? I mean which directory should I go before running map_server's map_saver?

3. where is the saved map located after running map_server's map_saver?

Thanks in advance

[matlabbe]

You can save the map just before closing rtabmap after mapping, or re-launch in localization mode (make sure RGBD/SavedLocalizationIgnored is false so that the map is loaded on start) then do (adjust map topic name if not /map):

$ rosrun map_server map_saver map:=/map
[ INFO] [1570502875.518163666]: Waiting for the map
[ INFO] [1570502875.723854641]: Received a 421 X 421 map @ 0.050 m/pix
[ INFO] [1570502875.723896028]: Writing map occupancy data to map.pgm
[ INFO] [1570502875.727056048, 50.102000000]: Writing map occupancy data to map.yaml
[ INFO] [1570502875.727141205, 50.102000000]: Done

The map is saved in the directory you launched map_saver.

The map.yaml:

image: map.pgm
resolution: 0.050000
origin: [-10.525000, -10.525000, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

[RTABPK]

could you please elaborate on you comment "In spawn_husky.launch, change output scan name of pointcloud_to_laserscan to camera_scan". I am not sure, I get it correctly. just to make sure that its correct.

here is what I did,

<remap from="scan" to="camera_scan"/>

Thank you very much

[RTABPK]

Hi matlabbe,

I incorporated all the changes you recommended and ran the rtabmap in localization mode. but my robot never localizes itself.

Here is what happens,

my map does not appear in the beginning,  which is due to setting "RGBD/SavedLocalizationIgnored" to false.

Here is what I do,

I ran rtabamp in localization mode, open rviz, and tele-operate the robot so that it can localize itself. But the map never appears, no matter how hard I try and how many times I visit the same place. It means robot never localizes itself. And please note that I tried rtabmap with "Reg/Strategy" 0 and 1 separately but all in vain.

I think that could be the reason the robot collides with obstacles.

Please guide how can I get out of this situation.

Thanks  

[matlabbe]

The environment is very repetitive with a very low number of visual features. This is not suitable for visual localization. In your case, no visual localization seems possible. I would keep "RGBD/SavedLocalizationIgnored" to false (to show map on start) and drop a "2D pose estimate" in RVIZ on /rtabmap/localization_pose topic to manually localize it. The proximity detections with the lidar would refine that localization afterwards.

[RTABPK]

hi, matlabbe

This time I changed my world with more visual features. I used Reg/Strategy =0.

The problem is that my robot cannot localize itself without map in the beginning.  Then I tried loading the map manually and tried to localize manually.

The problem is that the robot once collides with obstacles it remains stuck there.

Is there any problem with loop Closure? localization? or move_base settings. I do not understand what is happening.

Please guide me to get out of this situation.


Thanks

[matlabbe]

For the collisions, it is more a costmap problem. Even with poor localization, the robot should never hit anything. Show the local costmap in rviz, also show the footprint of the robot. Maybe the footprint is not big enough or the sensor used to update the local costmap doesn't see the obstacle. Make sure also that footprint clearing parameter in the local costmap is set to false (use rqt_reconfigure).

Without a map, the robot cannot localize. If you did a mapping session, then reload it in a localization session, you should revisit the same locations with the same camera point of view. Go back to a known location, if not localizing, look at the terminal for possible rejection warnings.

cheers,
Mathieu

[RTABPK]

here is the snapshot of my rviz info you asked.

I dont see the footprint clearing parameter in rviz. rqt_reconfigure results "command not found". And in localization mode I revisited the same place with almost same camera point of view.  

Here is my move_base settings and note that I have already shared my robot's description file which will give an idea of size of the robot. I am using husky robot (Clearpathrobotics model)

COSTMAP_COMMON.yaml

footprint: [[-0.5, -0.33], [-0.5, 0.33], [0.5, 0.33], [0.5, -0.33]]
footprint_padding: 0.01

robot_base_frame: base_link
update_frequency: 4.0
publish_frequency: 3.0
transform_tolerance: 0.5

resolution: 0.05

obstacle_range: 7.5
raytrace_range: 6.0

#layer definitions
static:
    map_topic: /map
    subscribe_to_updates: true

obstacles_laser:
    observation_sources: laser
    laser: {data_type: LaserScan, clearing: true, marking: true, topic: scan, inf_is_valid: true}

inflation:
    inflation_radius: 3.0


COSTMAP_LOCAL.yaml

global_frame: odom
rolling_window: true

plugins:
  - {name: obstacles_laser,           type: "costmap_2d::ObstacleLayer"}
  - {name: inflation,                 type: "costmap_2d::InflationLayer"}

COSTMAP_Global_Static.yaml

global_frame: map
rolling_window: false
track_unknown_space: true

plugins:
  - {name: static,                  type: "costmap_2d::StaticLayer"}
  - {name: inflation,               type: "costmap_2d::InflationLayer"}

COSTMAP_Global_laser.yaml

global_frame: odom
rolling_window: false
track_unknown_space: true

plugins:
  - {name: obstacles_laser,           type: "costmap_2d::ObstacleLayer"}
  - {name: inflation,                 type: "costmap_2d::InflationLayer"}

COSTMAP_Exploration.yml

track_unknown_space: true
global_frame: map
rolling_window: false

plugins:
- {name: external,            type: "costmap_2d::StaticLayer"}
- {name: explore_boundary,    type: "frontier_exploration::BoundedExploreLayer"}
#Can disable sensor layer if gmapping is fast enough to update scans
- {name: obstacles_laser,     type: "costmap_2d::ObstacleLayer"}
- {name: inflation,           type: "costmap_2d::InflationLayer"}

explore_boundary:
  resize_to_boundary: false
  frontier_travel_point: middle
  #set to false for gmapping, true if re-exploring a known area
  explore_clear_space: false

Planner.yaml

controller_frequency: 5.0
recovery_behaviour_enabled: true

NavfnROS:
  allow_unknown: true # Specifies whether or not to allow navfn to create plans that traverse unknown space.
  default_tolerance: 0.1 # A tolerance on the goal point for the planner.

TrajectoryPlannerROS:
  # Robot Configuration Parameters
  acc_lim_x: 2.5
  acc_lim_theta:  3.2

  max_vel_x: 1.0
  min_vel_x: 0.0

  max_vel_theta: 1.0
  min_vel_theta: -1.0
  min_in_place_vel_theta: 0.2

  holonomic_robot: false
  escape_vel: -0.1

  # Goal Tolerance Parameters
  yaw_goal_tolerance: 0.1
  xy_goal_tolerance: 0.2
  latch_xy_goal_tolerance: false

  # Forward Simulation Parameters
  sim_time: 2.0
  sim_granularity: 0.02
  angular_sim_granularity: 0.02
  vx_samples: 6
  vtheta_samples: 20
  controller_frequency: 20.0

  # Trajectory scoring parameters
  meter_scoring: true # Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default (false).
  occdist_scale:  0.1 #The weighting for how much the controller should attempt to avoid obstacles. default 0.01
  pdist_scale: 0.75  #     The weighting for how much the controller should stay close to the path it was given . default 0.6
  gdist_scale: 1.0 #     The weighting for how much the controller should attempt to reach its local goal, also controls speed  default 0.8

  heading_lookahead: 0.325  #How far to look ahead in meters when scoring different in-place-rotation trajectories
  heading_scoring: false  #Whether to score based on the robot's heading to the path or its distance from the path. default false
  heading_scoring_timestep: 0.8   #How far to look ahead in time in seconds along the simulated trajectory when using heading scoring (double, default: 0.8)
  dwa: true #Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout
  simple_attractor: false
  publish_cost_grid_pc: true  

  # Oscillation Prevention Parameters
  oscillation_reset_dist: 0.25 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)
  escape_reset_dist: 0.1
  escape_reset_theta: 0.1

DWAPlannerROS:
  # Robot configuration parameters  
  acc_lim_x: 2.5
  acc_lim_y: 0
  acc_lim_th: 3.2

  max_vel_x: 0.5
  min_vel_x: 0.0
  max_vel_y: 0
  min_vel_y: 0

  max_trans_vel: 0.5
  min_trans_vel: 0.1
  max_rot_vel: 1.0
  min_rot_vel: 0.2

  # Goal Tolerance Parameters
  yaw_goal_tolerance: 0.1
  xy_goal_tolerance: 0.2
  latch_xy_goal_tolerance: false

  # # Forward Simulation Parameters
  # sim_time: 2.0
  # sim_granularity: 0.02
  # vx_samples: 6
  # vy_samples: 0
  # vtheta_samples: 20
  # penalize_negative_x: true

  # # Trajectory scoring parameters
  # path_distance_bias: 32.0 # The weighting for how much the controller should stay close to the path it was given
  # goal_distance_bias: 24.0 # The weighting for how much the controller should attempt to reach its local goal, also controls speed
  # occdist_scale: 0.01 # The weighting for how much the controller should attempt to avoid obstacles
  # forward_point_distance: 0.325 # The distance from the center point of the robot to place an additional scoring point, in meters
  # stop_time_buffer: 0.2  # The amount of time that the robot must stThe absolute value of the veolicty at which to start scaling the robot's footprint, in m/sop before a collision in order for a trajectory to be considered valid in seconds
  # scaling_speed: 0.25 # The absolute value of the veolicty at which to start scaling the robot's footprint, in m/s
  # max_scaling_factor: 0.2 # The maximum factor to scale the robot's footprint by

  # # Oscillation Prevention Parameters
  # oscillation_reset_dist: 0.25 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)

[RTABPK]

Hello there,

Anybody who can help me with collision avoidance issue. I have already shared my move_base settings.

Thank you very much in advance

[RTABPK]

Hi matlabbe,

i m using rtabmap for localization and invoke move_base from within the rtabmap launch file already attached to my previous comment.

 I get this message in the terminal where rtabmap is running in localization mode.

[pcl::IterativeClosestPoint::computeTransformation] Not enough correspondences found. Relax your threshold parameters


my ICP parameters is as follows,

[param name="Icp/CorrespondenceRatio"      type="string" value="0.1"]

does it has to do anything with poor localization and obstacle avoidance?

when I give 2D goal command using rviz , I get this message,

Got a new plan,

with following warning,

Received an empty transformed plan

please guide,


Thanks in advance

[matlabbe]

Hi,

Thx for sharing all the details, but it would be easier if you can reproduce the problem using standard launch files from husky official demos (it will take less time instead of trying to replicate your package on our side, and we don't know also what launch files are used...). For example, if you can reproduce the navigation problems with the following (using official husky launch files), it would be great:

$ roslaunch husky_gazebo husky_playpen.launch

$ roslaunch husky_navigation move_base.launch

$ rosrun rtabmap_ros rtabmap -d \
   _subscribe_rgb:=false \
   _subscribe_depth:=false \
   _subscribe_scan:=true \
   odom:=/odometry/filtered \
   grid_map:=map \
   --RGBD/NeighborLinkRefining true \
   --RGBD/ProximityPathMaxNeighbors 5

$ roslaunch husky_viz view_robot.launch 

I've sent multiple navigation goals and I have no problems. The only problem I saw is near the following obstacle, which the base cannot be detected by the lidar and the robot moved too much close of it (it is what we would expect as the lidar cannot see the base, the fix would be to increase the footprint radius in the costmap parameters so that the robot navigate farther from obstacles).