sbpl_lattice_planner final heading error constant +45-deg

Question

I've noticed a peculiar behavior in sbpl_lattice_planner, where ~50% of actionlib requests (/map frame) result in a final heading that is consistently offset by +45-degrees. The other 50% of the time, the correct heading is obtained. In all cases, the correct (x,y) position is achieved. Thoughts?

I have verified that the correct heading is being sent (rviz markers and using /move_base_simple/goal), but the issue persists.

The pertinent launch files / parameters are below.

SBPLLatticePlanner:
  environment_type: XYThetaLattice
  planner_type: ARAPlanner
  allocated_time: 5.0
  initial_epsilon: 3.0
  forward_search: false
  lethal_obstacle: 5
  nominalvel_mpersecs: 0.1
  timetoturn45degsinplace_secs: 1.5

TrajectoryPlannerROS:
  #Independent settings for the local costmap
  transform_tolerance: 0.3
  sim_time: 1.7
  sim_granularity: 0.025
  dwa: true
  vx_samples: 20
  vtheta_samples: 20
  max_vel_x: 0.250
  #max_vel_x: 0.65
  min_vel_x: 0.1
  max_rotational_vel: 1.0
  min_in_place_rotational_vel: 0.4
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.06
  goal_distance_bias: 0.2
  path_distance_bias: .9
  occdist_scale: 0.05
  heading_lookahead: 0.325
  oscillation_reset_dist: 0.05
  holonomic_robot: true
  acc_lim_th: 3.2
  acc_lim_x: 2.5
  acc_lim_y: 2.5
  heading_scoring: false
  heading_scoring_timestep: 0.8
  holonomic_robot: true
  simple_attractor: false

launch
  node pkg="pr2_move_base" name="pr2_move_base_node" type="pr2_move_base.py" machine="c2"

  node ns="move_base_node/local_costmap" name="voxel_grid_throttle" pkg="topic_tools" type="throttle" args="messages voxel_grid 3.0 voxel_grid_throttled"

  node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen"
    remap from="odom" to="base_odometry/odom"
    remap from="cmd_vel" to="navigation/cmd_vel"
    param name="footprint_padding" value="0.10"
    param name="controller_frequency" value="10.0" 
    param name="controller_patience" value="100.0" 
    param name="base_global_planner" value="SBPLLatticePlanner" 
    param name="SBPLLatticePlanner/primitive_filename" value="$(find sbpl)/matlab/mprim/pr2.mprim" 

  /node
launch

---

Originally posted by Travis Deyle on ROS Answers with karma: 91 on 2011-04-20

Post score: 1

Answer 1

I recently started testing sbpl_lattice_planner and also had this constant 45 degree error. And as mentioned, it was also ~50% of the time. Originally I thought it was the discretization error that mphillips mentioned, until I updated sbpl_lattice_planner.cpp to display the planned orientation (including position). Turns out that it wasn't a discretization problem, but an actual planning problem. On occasions that there was the 45 degree error, the planner did, in fact, plan the final pose with that error.

To cut the story short, the error occurs in primID: 6, startangle_c: 15, endpose_c: 0 0 0 of pr2.mprim. The planner puts all the intermediate poses into the plan EXCEPT for the end pose, so in this case, the last pose in the plan has angle 0.6545rad (37.5deg). This is the heading error that we are seeing and occurs when the goal pose has 0 deg orientation.

So the simple solution is to change this 0.6545 to 0.000. Take note that for all other primitives, the end pose will not be pushed into the plan as well, but because the last intermediate pose is close to the end pose, you won't notice the difference (1/9th of the resolution). It is only this particular primID, startangle, endpose where the difference is obvious.

---

Originally posted by weiin with karma: 2268 on 2013-07-15

This answer was ACCEPTED on the original site

Post score: 2