0
$\begingroup$

Rosanswers logo

Hello,

i use ros kinetic and the husky robot (version 0.3.6). I have created a map with the GlobalPlaner, teb_local_planer and gmapping. Then I used the map together with amcl (1.14.7) and without gmapping. I can set a goal in rviz and the robot goes there without problems. When I add an obstacle in gazebo, the laser detects it, but the GlobalPlanner does not generate a new plan and the Husky drives against the obstacle. I could see that the new obstacle did not appear in the global costmap. As far as I understand the global costmap uses the created map and there the new obstacle does not exist, so it is not shown in the global costmap. I thought amcl automatically takes care to avoid such obstacles. I don't know where my mistake is...

Can someone explain to me why GlobalPlanner does not create a new path? If you need more information please let me know.

Many thanks in advance!

costmap_common:

robot_base_frame: base_link
obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[-0.24, -0.22], [-0.24, 0.22], [0.24, 0.22], [0.24, -0.22]]
footprint_padding: 0.01
transform_tolerance: 0.3
recovery_behavior_enabled: false
resolution: 0.1

obstacles_laser:
  observation_sources: scan
  scan:
    data_type: LaserScan
    topic: scan
    marking: true
    clearing: true
    inf_is_valid: true

inflation:
  cost_scaling_factor: 2.5
  inflation_radius: 0.3

static:
  map_topic: /map
  subscribe_to_updates: true

local_costmap:

local_costmap:
  global_frame: /odom
  rolling_window: true
  static_map: false
  width: 4
  height: 4
  update_frequency: 3.0
  publish_frequency: 0.0  
  plugins:
  - {name: obstacles_laser,           type: "costmap_2d::ObstacleLayer"}
  - {name: inflation,                 type: "costmap_2d::InflationLayer"}

   

global_costmap:

global_costmap:
  global_frame: /map
  static_map: true
  rolling_window: false
  update_frequency: 1.0
  publish_frequency: 0.1  
  plugins:
  - {name: static,                  type: "costmap_2d::StaticLayer"}
  - {name: inflation,               type: "costmap_2d::InflationLayer"}

teb_local_planner:

TebLocalPlannerROS:

 odom_topic: /odometry/filtered
 map_frame: /map
    
 # Trajectory
  
 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 global_plan_overwrite_orientation: True
 max_global_plan_lookahead_dist: 3.0
 feasibility_check_no_poses: 0
    
 # Robot
        
 max_vel_x: 0.4
 max_vel_x_backwards: 0.2
 max_vel_theta: 0.3
 acc_lim_x: 1.0
 acc_lim_theta: 0.2
 min_turning_radius: 0.0
 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
   type: "polygon"
   vertices: [[-0.24, -0.22], [-0.24, 0.22], [0.24, 0.22], [0.24, -0.22]] # for type "polygon"

 # GoalTolerance
    
 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
 free_goal_vel: False
    
 # Obstacles
    
 min_obstacle_dist: 0.1
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.0
 obstacle_poses_affected: 30
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # Optimization
    
 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 weight_max_vel_x: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 50
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1
 weight_obstacle: 50
 weight_dynamic_obstacle: 10 # not in use yet
 
 # Homotopy Class Planner

 enable_homotopy_class_planning: False
 enable_multithreading: True
 simple_exploration: False
 max_number_classes: 4
 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_keypoint_offset: 0.1
 obstacle_heading_threshold: 0.45
 visualize_hc_graph: False

amcl_launch file:

<?xml version="1.0"?>
<launch>
     <arg name="use_map_topic" default="true"/>
     <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <param name="laser_max_range" value="10" />
        <param name="use_map_topic" value="$(arg use_map_topic)"/>
        <param name="base_frame_id" value="base_link"/> 
        <param name="gui_publish_rate" value="5.0"/> <
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_likelihood_max_dist" value="8.0"/>
        <param name="laser_max_beams" value="60"/>
        <param name="laser_model_type" value="likelihood_field"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="max_particles" value="2000"/>
        <param name="min_particles" value="100"/>
        <param name="odom_alpha1" value="0.25"/> 
        <param name="odom_alpha2" value="0.25"/> 
        <param name="odom_alpha3" value="0.25"/> 
        <param name="odom_alpha4" value="0.25"/> 
        <param name="odom_alpha5" value="0.1"/> 
        <param name="odom_frame_id" value="odom"/>
        <param name="odom_model_type" value="diff"/>
        <param name="recovery_alpha_slow" value="0.001"/> 
        <param name="recovery_alpha_fast" value="0.1"/> 
        <param name="resample_interval" value="1"/>
        <param name="transform_tolerance" value="1.25"/> 
        <param name="update_min_a" value="0.2"/> 
        <param name="update_min_d" value="0.2"/> 
    </node>
</launch>

The launch file that starts everything:

<launch>

  <arg name="world_name" default="$(find husky_willow_garage)worlds/willow_garage.world"/>

  <arg name="laser_enabled" default="true"/>
  <arg name="kinect_enabled" default="false"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(arg world_name)"/> <!-- world_name is wrt GAZEBO_RESOURCE_PATH environment variable -->
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <include file="$(find husky_gazebo)/launch/spawn_husky.launch">
    <arg name="laser_enabled" value="$(arg laser_enabled)"/>
    <arg name="kinect_enabled" value="$(arg kinect_enabled)"/>
  </include>

  <!-- Content of view_robot.launch -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find husky_willow_garage)/rviz/robot.rviz" output="screen"/>


   <!--- Run Move Base -->
  <include file="$(find husky_willow_garage)/launch/move_base.launch" />

  <!-- Run the map server -->
  <arg name="map_file" default="$(find husky_willow_garage)/maps/gmapping_fertig.yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)"/>
  <!--- Run AMCL -->
  <include file="$(find husky_willow_garage)/launch/amcl.launch"/>
</launch>

Originally posted by Donlouigi91 on ROS Answers with karma: 3 on 2020-06-20

Post score: 0


Original comments

Comment by gvdhoorn on 2020-06-20:
I haven't looked at the rest of your question, but:

I thought amcl automatically takes care to avoid such obstacles

AMCL is a localisation algorithm/component. It does not directly affect or take part in path planning.

Comment by Donlouigi91 on 2020-06-20:
Yes you are right, but I am not sure if amcl has influence on the GlobalPlanner.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Obstacle avoidance for variable obstacles is done by the local planner in the local costmap scope. What isnt mapped isnt taken into consideration by the global costmap and planner. It is also not ment to replan as there is no change expected. You can think of the concept analog to a car with navigation system. The navigation system is the global_planner, plans once, and gives rough directions to the driver. The driver is analog to the local_planner who takes care of not crashing into other cars, stay in the lane, etc.

I dont see where you actually upload the costap yaml files to the parameter server in the launchfiles. Also starting the map server before move_base might be better (change position in launchfile). A rosparam dump might be usefull. With all the yaml files and namespaces used to launch the files, quickly something can get messed up. And it seems everybody does it differently. I would expect move_base.launch to be the problem (usually its there where the yamls get uploaded and planners chosen). The discribed behaviour suggest a local problem with either costmap or planner. Most likely the obstacle_layer not marking. Try visualizing the local_costmap and pathplans in rviz. This should give a quick understanding of whats going wrong.


Originally posted by Dragonslayer with karma: 574 on 2020-06-21

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by Donlouigi91 on 2020-06-21:
Hi, thanks for your comment, I got it working. I just had to add the obstacle plugin to the global costmap. global_costmap:

  global_frame: /map
  static_map: true
  rolling_window: false
  update_frequency: 1.0
  publish_frequency: 0.1  
  plugins:
  - {name: static,                  type: "costmap_2d::StaticLayer"}
  - {name: inflation,               type: "costmap_2d::InflationLayer"}
  - {name: obstacles_laser,           type: "costmap_2d::ObstacleLayer"}

Now the GlobalPlanner plans a new path when a new obstacle appears.

Comment by Dragonslayer on 2020-06-21:
Nice. If you are not doing something experimental I would still consider handling variable obstacles in the local scope. Due to its smaller costmap size and more specialized planners you can safe a lot of computational resources and get better results (due to higher frequency for example). You might consider marking. Have fun.

$\endgroup$

Your Answer

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