0
$\begingroup$

Rosanswers logo

Hi all
I'm implementing the simulation for a robot which has to move within an environment composed by a ground floor (which will be not used) and a first floor.
The mapping task seems to be ok, the robot can map the first floor with good accuracy.
The navigation task has to be performed at the first floor and has problems: first thing, which I don't know if it's an error, the robot odom frame seems to be under the spawned map and robot. image description
The main problem is that the local costmap seems to not mark the possible obstacles, the local costmap remains light gray as you can see in the picture about the frames if i set to "/odom" the global_frame in the local_costmap_param, so the path planner doesnt work as it should be, instead if i set to "/map" the global_frame in the local_costmap_param it seems to mark the obstacles and walls, but anyway a lot of times it fails to avoid them and to reach a goal because the path is really closed to the wall or obstacles.
These are my param files
base_local_planner.yaml

   TrajectoryPlannerROS:
  #Set the velocity limits of the robot
  max_vel_x: 0.4
  min_vel_x: -0.2 #0.1
  max_vel_theta: 0.2
  min_vel_theta: -0.2
  min_in_place_vel_theta: 0.2
  #The velocity the robot will command when trying to escape from a stuck situation
  escape_vel: -0.2
  #Set the acceleration limits of the robot
  acc_lim_theta: 0.1
  acc_lim_x: 0.1
  acc_lim_y: 0.1
  #Set the tolerance on achieving a goal
  latch_xy_goal_tolerance: false
  xy_goal_tolerance: 0.1
  #Is it holonomic?
  holonomic_robot: false
  #How long and with what granularity it'll forward simulate trajectories
  sim_time: 1.7
  sim_granularity: 0.025
  angular_sim_granularity: 0.025
  vx_samples: 8
  vtheta_samples: 20
  #Parameters for scoring trajectories
  path_distance_bias: 0.8      # 1.0   - weighting for how much it should stick to the global path plan
  goal_distance_bias: 0.8      # 0.8   - wighting for how much it should attempt to reach its goal
  gdist_scale: 0.8
  pdist_scale: 1.0
  occdist_scale: 0.5           # 0.01   - weighting for how much the controller should avoid obstacles
  heading_lookahead: 0.325

  dwa: false
  meter_scoring: true

  #How far the robot must travel before oscillation flags are reset
  oscillation_reset_dist: 0.05
  #Eat up the plan as the robot moves along it
  prune_plan: false
  #Global Frame id
  global_frame_id: map`

**costmap_common_param.yaml**
footprint: [[0.39, 0.30], [0.39, -0.30], [-0.30, -0.30], [-0.30, 0.30]] #FRIDAY
footprint_padding: 0.0
transform_tolerance : 0.5
map_type: voxel


##OBSTACLE LAYER##
obstacle_layer:
  enabled: true
  obstacle_range: 2.5
  raytrace_range: 3.0
  #expected_update_rate: 0.0
  #observation_persistence: 0.0
  #min_obstacle_height: 0.0
  max_obstacle_height: 1.0
  publish_voxel_map: false
  origin_z: 0.0
  z_voxels: 2
  z_resolution: 0.2
  unknown_threshold:    15
  mark_threshold:       0
  combination_method:   1
  track_unknown_space:  true  #true needed for disabling global path planning through unknown space
  observation_sources:  laser_scan_sensor #cliff_sensors
  laser_scan_sensor:
    data_type: LaserScan
    topic: /friday/laser/scan
    marking: true
    clearing: true
  #cloud_scan_sensor 
  #laser_scan_sensor
  #sonar_scan_sensor
  #sonar_scan_sensor: {sensor_frame: sonar_1, data_type: LaserScan, topic: /sonar_scan, marking: true, clearing: true, expected_update_rate: 100.0, min_obstacle_height: 0.1, max_obstacle_height: 0.80}
  #sonar_scan_sensor: {sensor_frame: sonar_1, data_type: LaserScan, topic: /sonar_scan, marking: true, clearing: true, expected_update_rate: 0.4, min_obstacle_height: 0.05, max_obstacle_height: 0.6}
  #cloud_scan_sensor: {sensor_frame: camera_link, data_type: LaserScan, topic: /scan_cloud, marking: true, clearing: true, expected_update_rate: #0.0, min_obstacle_height: 0.1, max_obstacle_height: 2.0}
  #laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0.4, observation_persistence: 0.0, min_obstacle_height: 0.1, max_obstacle_height: 2.0}
  #laser_scan_sensor: {sensor_frame: lidar_link, data_type: LaserScan, topic: /friday/laser/scan, marking: true, clearing: true}
  #cliff_sensors: {sensor_frame: base_link, data_type: PointCloud2, topic: /friday/sensors/cliff_points, marking: true, clearing: true}


##INFLATION LAYER##
inflation_layer:
  enabled: true
  cost_scaling_factor: 100.0
  inflation_radius: 0.15
  #transform_tolerance : 0.2


##STATIC LAYER##
static_layer:
  enabled: true


##BASE RANGE SENSOR LAYER##
base_range_layer:
  clear_threshold:    0.46
  mark_threshold:     0.98
  clear_on_max_reading: true
  topics: ["/friday/sensors/ir_f", "/friday/sensors/ir_f_l_1", "/friday/sensors/ir_f_l_2", "/friday/sensors/ir_f_l_3", "/friday/sensors/ir_f_r_1", "/friday/sensors/ir_f_r_2", "/friday/sensors/ir_f_r_3"]


##BODY RANGE SENSOR LAYER 1##
body1_range_layer:
  clear_threshold:    0.46
  mark_threshold:     0.98
  topics: ["/friday/sensors/ir_f_level1"]


##BODY RANGE SENSOR LAYER 2##
body2_range_layer:
  clear_threshold:    0.46
  mark_threshold:     0.98
  topics: ["/friday/sensors/ir_f_level2"]


##IR SR 1 RANGE SENSOR LAYER##
irsr1_range_layer:
  clear_threshold:    0.46
  mark_threshold:     0.98
  topics: ["/friday/sensors/ir_sr_level1_l", "/friday/sensors/ir_sr_level1_l"]


##IR SR 2 RANGE SENSOR LAYER##
irsr2_range_layer:
  clear_threshold:    0.46
  mark_threshold:     0.98
  topics: ["/friday/sensors/ir_sr_level2_l", "/friday/sensors/ir_sr_level2_l"]

global_costmap_param.yaml

global_costmap:
  global_frame: /map
  robot_base_frame: /base_footprint #/base_link
  update_frequency: 5.0 #4.0
  publish_frequency: 0.5 #1.0
  static_map: true
  transform_tolerance: 0.5
  #width: 1000.0
  #height: 1000.0 
  #origin_x: 0
  #origin_y: 0
  #resolution: 0.05 
  plugins:
     - {name: static_layer, type: "costmap_2d::StaticLayer"}
     - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
     - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

**local_costmap_param.yaml**
local_costmap:
  global_frame: /odom #/map
  robot_base_frame: /base_footprint #/base_link
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 4.0 #5.0
  height: 4.0 #5.0
  resolution: 0.05
  transform_tolerance: 0.5
  #publish_cost_grid: true
  inflation_radius: 0.30 #andy 0.1
  #cost_scaling_factor: 2.0
  plugins:
    - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
    #- {name: cliff_downward_layer, type: "costmap_2d::ObstacleLayer"}
    #- {name: base_range_layer, type: "range_sensor_layer::RangeSensorLayer"}
    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

**move_base_param.yaml**
shutdown_costmaps: false

controller_frequency: 5.0
controller_patience: 3.0

planner_frequency: 5.0     #10.0
planner_patience: 5.0      #4.0  

oscillation_timeout: 10.0
oscillation_distance: 0.2

max_planning_retries: 3

base_local_planner:  "base_local_planner/TrajectoryPlannerROS" #"dwa_local_planner/DWAPlannerROS" #TO CHANGE THE PLANNER

base_global_planner: "navfn/NavfnROS"                   #NEW

**navfn_global_planner_param.yaml**
NavfnROS:
  visualize_potential: false    #Publish potential for rviz as pointcloud2, not really helpful, default false
  allow_unknown: false          #Specifies whether or not to allow navfn to create plans that traverse unknown space, default true
                                #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work
  planner_window_x: 0.0         #Specifies the x size of an optional window to restrict the planner to, default 0.0
  planner_window_y: 0.0         #Specifies the y size of an optional window to restrict the planner to, default 0.0
  
  default_tolerance: 0.5        #If the goal is in an obstacle, the planer will plan to the nearest point in the radius of default_tolerance, default 0.0
                                #The area is always searched, so could be slow for big values

global_planner_param.yaml

GlobalPlanner:                                  # Also see: http://wiki.ros.org/global_planner
  old_navfn_behavior: false                     # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false
  use_quadratic: true                           # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true
  use_dijkstra: true                            # Use dijkstra's algorithm. Otherwise, A*, default true
  use_grid_path: false                          # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false
  
  allow_unknown: true                           # Allow planner to plan through unknown space, default true
                                                #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work
  planner_window_x: 0.0                         # default 0.0
  planner_window_y: 0.0                         # default 0.0
  default_tolerance: 0.0                        # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0
  
  publish_scale: 100                            # Scale by which the published potential gets multiplied, default 100
  planner_costmap_publish_frequency: 0.0        # default 0.0
  
  lethal_cost: 253                              # default 253
  neutral_cost: 50                              # default 50
  cost_factor: 3.0                              # Factor to multiply each cost from costmap by, default 3.0
  publish_potential: true                       # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true

navigation.launch

<launch>
  
  <!-- define the parameters -->
  <arg name="odom_frame_id"   default="odom"/>
  <arg name="base_frame_id"   default="base_footprint"/>
  <arg name="global_frame_id" default="map"/>
  <arg name="odom_topic" default="/friday/odom_diffdrive" />
  <arg name="laser_topic" default="/friday/laser/scan" />


  <!-- Run Amcl -->
  <node pkg="amcl" type="amcl" name="amcl" output="screen" respawn="true">
    <remap from="scan" to="$(arg laser_topic)"/>
    <param name="odom_model_type" value="diff" />
    <param name="odom_alpha5" value="0.1" />
    <param name="transform_tolerance" value="0.2" />
    <param name="gui_publish_rate" value="10.0" />
    <param name="laser_max_beams" value="30" />
    <param name="laser_max_range" value="12.0"/><!-- ADDED -->
    <param name="min_particles" value="500" />
    <param name="max_particles" value="5000" />
    <param name="kld_err" value="0.05" />
    <param name="kld_z" value="0.99" />
    <param name="odom_alpha1" value="0.2" />
    <param name="odom_alpha2" value="0.2" />
    <!-- translation std dev, m -->
    <param name="odom_alpha3" value="0.8" />
    <param name="odom_alpha4" 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="laser_sigma_hit" value="0.2" />
    <param name="laser_lambda_short" value="0.1" />
    <param name="laser_lambda_short" value="0.1" />
    <param name="laser_model_type" value="likelihood_field" />
    <!-- <param name="laser_model_type" value="beam"/> -->
    <param name="laser_likelihood_max_dist" value="2.0" />
    <param name="update_min_d" value="0.25" /> <!-- value="0.20" -->
    <param name="update_min_a" value="0.2" /> <!-- value="0.5" -->
    <param name="odom_frame_id" value="$(arg odom_frame_id)" />
    <param name="base_frame_id" value="$(arg base_frame_id)"/> <!-- Added -->
    <param name="global_frame_id" value="$(arg global_frame_id)"/> <!-- Added -->
    <param name="resample_interval" value="1" />
    <param name="transform_tolerance" value="1.0" /> <!-- value="0.1" -->
    <param name="recovery_alpha_slow" value="0.0" />
    <param name="recovery_alpha_fast" value="0.0" />

    <!--<param name="use_map_topic" value="true" />-->  

    <!-- Environment pose -->
    <param name="initial_pose_x" value="3.97"/>
    <param name="initial_pose_y" value="-0.33"/>
    <param name="initial_pose_a" value="0.0"/>

  </node>

  

  <!-- Run Move Base -->
  <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
    <rosparam file="$(find hyperlync_simulator)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find hyperlync_simulator)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find hyperlync_simulator)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find hyperlync_simulator)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find hyperlync_simulator)/param/move_base_params.yaml" command="load" />

    <!-- BASE LOCAL PLANNER PARAMAS (remember to set base_local_planner/TrajectoryPlannerROS in move_base_params) -->
    <rosparam file="$(find hyperlync_simulator)/param/base_local_planner_params.yaml" command="load"/>

    <!-- DWA PLANNER PARAMS (remember to set dwa_local_planner/DWAPlannerROS in move_base_params)
    <rosparam file="$(find hyperlync_simulator)/param/dwa_local_planner_params.yaml" command="load" />-->
    <rosparam file="$(find hyperlync_simulator)/param/global_planner_params.yaml" command="load" />
    <rosparam file="$(find hyperlync_simulator)/param/navfn_global_planner_params.yaml" command="load" />

    <!-- <remap from="/cmd_vel" to="/friday/cmd_vel"/>-->
    <remap from="odom" to="$(arg odom_topic)"/>
    <remap from="scan" to="$(arg laser_topic)"/>
  </node>

  <!-- Run the map server  -->
  <node name="map_server" pkg="map_server" type="map_server"  args="$(find hyperlync_simulator)/maps/environment.yaml"/>

</launch>

Originally posted by dottant on ROS Answers with karma: 185 on 2017-06-19

Post score: 0

Original comments

Comment by sva on 2017-06-20:
Hi, I'm encountering the same strange problem since friday. Have you found a solution? Could it depend on the value of odometrySource in the differential drive plugin?

Comment by Humpelstilzchen on 2017-06-21:
Can you add a single tf value for your odom and map transforms? e.g. "rosrun tf tf_echo /base_footprint /odom" and "rosrun tf tf_echo /base_footprint /map"

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

Thanks about the answers, but I've found the solution.
It depends on the tag odometrySource of the differential_drive_controller plugin for my robot model in Gazebo, which must be set on encoder. Previously I set it to world, that's why the odom was below the map frame.
Now everything is ok and I can set the param global_frame to "/odom" in the local_costmap_params file.

Originally posted by dottant with karma: 185 on 2017-06-21

This answer was ACCEPTED on the original site

Post score: 0

Improve this answer
$\endgroup$

Your Answer

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