0
$\begingroup$

Rosanswers logo

I have a multi robot system where they all have a version of the navstack with different namespaces. They all share the same costmap parameters but again in a different namespace.

The problem is that when I give a 2d nav goal to a robot, it moves somewhat biasedly in a straight line, and runs straight into another robot. There is plenty of space to navigate around it, but it seems it does not percieve it as an obstacle, even though it is visible that it shows up on the laserscan.

Please take a look at the linked video. https://imgur.com/a/6d81brc I apologize for the bad quality, I tried to use kazam, but it just recorded a black screen of rviz.


Here are the costmap and planner parameters:

Global costmap parameter:

global_costmap:
  global_frame: map
  robot_base_frame: base_footprint

  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 0.5

  static_map: true

Local costmap parameter:

local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint

  update_frequency: 10.0
  publish_frequency: 10.0
  transform_tolerance: 0.5  

  static_map: false  
  rolling_window: true
  width: 3
  height: 3
  resolution: 0.05

Costmap common parameter:

obstacle_range: 3.0
raytrace_range: 3.5

footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]]
#robot_radius: 0.105

inflation_radius: 1.75 #default 1.0
cost_scaling_factor: 20 #default 3.0

map_type: costmap
observation_sources: scan
scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

Global planner parameter:

GlobalPlanner:                                  
  old_navfn_behavior: false                     
  use_quadratic: true                           
  use_dijkstra: true                            
  use_grid_path: false                          
  
  allow_unknown: true                           
  planner_window_x: 0.0                         # default 0.0
  planner_window_y: 0.0                         # default 0.0
  default_tolerance: 0.0                        
  
  publish_scale: 100                            
  planner_costmap_publish_frequency: 0.0        # default 0.0
  
  lethal_cost: 253                              # default 253
  neutral_cost: 50                              # default 50
  cost_factor: 3.0                              # default 3.0
  publish_potential: true                     

DWA local planner parameter:

DWAPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.22
  min_vel_x: -0.22

  max_vel_y: 0.0
  min_vel_y: 0.0

# The velocity when robot is moving in a straight line
  max_vel_trans:  0.22
  min_vel_trans:  0.11

  max_vel_theta: 5.0 #Default 2.75
  min_vel_theta: 2.5 #default 1.37

  acc_lim_x: 2.5
  acc_lim_y: 0.0
  acc_lim_theta: 3.2 

# Goal Tolerance Parametes
  xy_goal_tolerance: 0.05
  yaw_goal_tolerance: 0.17
  latch_xy_goal_tolerance: false

# Forward Simulation Parameters
  sim_time: 4.5 #Default 1.5
  vx_samples: 20
  vy_samples: 0
  vth_samples: 40
  controller_frequency: 10.0

# Trajectory Scoring Parameters
  path_distance_bias: 32.0
  goal_distance_bias: 8.0 #Default 20.0
  occdist_scale: 0.00 #Default 0.02
  forward_point_distance: 0.0 #Default 0.325
  stop_time_buffer: 0.2
  scaling_speed: 0.25
  max_scaling_factor: 0.2

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05

# Debugging
  publish_traj_pc : true
  publish_cost_grid_pc: true
 

Originally posted by WarTurtle on ROS Answers with karma: 170 on 2021-11-18

Post score: 3


Original comments

Comment by miura on 2021-11-18:
Are odom, base_footprint, and base_scan the same as in the current tf? I think it is possible that base_scan does not exist. It may work if you change sensor_frame: base_scan to sensor_frame: base_footprint.

Comment by WarTurtle on 2021-11-24:
It worked by adding a namespace before base_scan and scan:

scan: {sensor_frame: Bot1/base_scan, data_type: LaserScan, topic: Bot1/scan, marking: true, clearing: true}

Comment by miura on 2021-11-24:
@WarTurtle Congratulations on the resolution. You can create your own answer and make it resolved.

Comment by WarTurtle on 2021-11-25:
@miura Yes, I will do that when I figure out how to make it more general e.g.

scan: {sensor_frame: $(arg namespace)/base_scan, data_type: LaserScan, topic: $(arg namespace)/scan, marking: true, clearing: true}
$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

Very cool video. Thank you for sharing all the pertinent information.

Here is a link of Navigation Guide, please read when you have a chance as it explains steps to solve navigation problems in the future: https://kaiyuzheng.me/documents/navguide.pdf

From the video, it looks the obstacle is detected but the path generated is too close to the obstacle, so it ends up hitting the obstacle so I'd start by taking a look at your cost function.

The cost function used to score each trajectory is in the following form:

cost = path_distance_bias * (distance to path from the endpoint of the trajectory in meters) + goal_distance_bias * (distance to local goal from the endpoint of the trajectory in meters) + occdist_scale * (maximum obstacle cost along the trajectory in obstacle cost (0-254))

Source: http://wiki.ros.org/dwa_local_planner

In this case your occdist_scale is 0.00 so basically you are not adding weight to avoid obstacles.


Originally posted by osilva with karma: 1650 on 2021-11-18

This answer was NOT ACCEPTED on the original site

Post score: 1

$\endgroup$
0
$\begingroup$

Rosanswers logo

Thank you, @fergs , @osilva , and @miura for your help. The problem was in costmap common parameter, specifically in

scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

I could not work out to remap it from the launch file when it is inside the dictionary. The solution was to add n costmap_common_params.yaml files for n robots with names specific to the namespace of the robot, e.g. costmap_common_params_Bot1.yaml would contain

scan: {sensor_frame: Bot1/base_scan, data_type: LaserScan, topic: Bot1/scan, marking: true, clearing: true}

I could then call these in the launch file using:

<rosparam file="$(find deploy)/param/costmap_common_params_$(arg namespace).yaml"
command="load" ns="global_costmap" />

<rosparam file="$(find deploy)/param/costmap_common_params_$(arg namespace).yaml"
command="load" ns="local_costmap" />

Hope this can help anyone else having problems with this.


Originally posted by WarTurtle with karma: 170 on 2021-12-09

This answer was ACCEPTED on the original site

Post score: 2

$\endgroup$

Your Answer

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