Ros navigation project

why the local costmap is empty ?
why the robot moves so fast despite i made the velocity in dwaplanner very low?

global_frame: odom
rolling_window: true

plugins:
- {name: obstacles_layer,           type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer,           type: "costmap_2d::InflationLayer"}

i do not know also why the robot moves too fast and it collides with obstacles .

Hi @80601 ,

Are you sure that your /odom and /scan are working correctly? Local planner depends on these data.
Also, you need to specify window height and width for your local costmap.

Perhaphs if you post your common_costmap_params, global_costmap_params and local_costmap_params files, then I could perhaps fix your problem.

Regards,
Girish

i checked both odom and scan topics they both works as expected.

global costmap

global_frame: map
rolling_window: false
track_unknown_space: true

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

local cotsmap

global_frame: odom
rolling_window: true

plugins:
- {name: obstacles_layer,           type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer,           type: "costmap_2d::InflationLayer"}

common

robot_radius: 0.06
footprint_padding: 0.0   # padding is 0.1 by default, making difficult to pass through narrow places

robot_base_frame: base_link
update_frequency: 4.0
transform_tolerance: 0.5

#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, obstacle_range: 5.5}

inflation:
    inflation_radius: 0.001

i made some modifications on the move_base-param to reduce speed but now the robot always shows failed to produce the path


controller_frequency: 5.0
recovery_behaviour_enabled: true
conservative_reset_dist: 0.2

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: 0.5 #0.1
  acc_lim_theta: 0.5 #0.2

  max_vel_x: 1 #0.3
  min_vel_x: 0.1

  max_vel_theta: 1 #0.25
  min_vel_theta: -1 #-0.25
  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: 0.5 #0.09
  acc_lim_y: 0.5
  acc_lim_th: 0.5 #0.1

  max_vel_x: 1 #0.2
  min_vel_x:  0.1
  max_vel_y: 1
  min_vel_y: 0.1

  max_vel_trans: 0.1 #0.05
  min_vel_trans: 0.01
  max_vel_theta: 1 #0.25
  min_vel_theta: 0.05

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

Hi @80601 ,

This belongs under local_costmap. Not in global_costmap.

You have indentation problem in global_costmap and local_costmap.

= = = = = = = = = =

global_costmap

This is wrong.

This is correct.

= = = = = = = = = =

local_costmap

This is wrong.

This is correct.

= = = = = = = = = =

common_costmap

Add the two lines:

obstacle_range: 3.0
raytrace_range: 3.5

Change this line:

robot_base_frame: base_link   # wrong
robot_base_frame: base_footprint   # correct

Add these parameters:

resolution: <same value as "delta" in amcl>

cost_scaling_factor: 3.0

You need to add sensor_frame to laser in obstacle_layer:

obstacles_layer:   # obstacles_laser is spelling mistake, it must be layer, not laser.
    observation_sources: laser
    laser: {sensor_frame: base_scan, data_type: ...

Set inflation_radius between 0.3 and 1.0. 0.001 is way too less. There will be no effect. This is what is causing to visuals to appear on your local costmap on Rviz.

inflation_layer:   # name should be same as what you define in global and local costmap plugins
    inflation_radius: 0.8

= = = = = = = = = =

local_planner

From where did you get the values for acceleration and velocity for the robot?
The values you have provided are out of range for the (poor) turtlebot !
With those values, the robot would commit suicide ! :sob:
[Note: Pun indented, not suggesting anything bad.]
I hope you did not try these values on the real robot - it will smash against the wall - probably crash!
Remember: Maximum Velocities for Real Robot TurtleBot:
Linear: 0.19m/s & Angular: 0.49rad/s

Refer correct values here: turtlebot3/turtlebot3_navigation/param at master · ROBOTIS-GIT/turtlebot3 · GitHub

= = = = = = = = = =

Fix these issues, your robot will work correctly. Let me know if you still have any issues.

Regards,
Girish

great,thanks.
i did as you said and it worked .
but it still colliding with obstacles.


controller_frequency: 5.0
recovery_behaviour_enabled: true
conservative_reset_dist: 0.2

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: 0.1
  acc_lim_theta: 0.1

  max_vel_x: 0.18
  min_vel_x: 0.09

  max_vel_theta: 0.4
  min_vel_theta: -0.4
  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: 0.1
  acc_lim_y: 0.1
  acc_lim_th: 0.1 

  max_vel_x: 0.18
  min_vel_x:  0.09
  max_vel_y: 0.18
  min_vel_y: 0.09

  max_vel_trans: 0.1 
  min_vel_trans: 0.01
  max_vel_theta: 0.4 
  min_vel_theta: 0.1

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

Hi @80601 ,

Great! So, we are definitely heading in the positive direction!

For this you need to play around with controller_frequency and local_planner parameters.
Refer the course notes to understand which parameter does what, and change them respectively.

Regards,
Girish