sbpl_lattice_planner final heading error constant +45-deg.

asked 2011-04-20 05:36:07 -0500

Travis Deyle
91 ●4 ●7 ●10 http://www.hizook.com/

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

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2013-07-15 21:49:51 -0500

weiin
2268 ●26 ●46 ●65

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.

edit flag offensive delete link

add a comment

answered 2011-08-03 11:21:01 -0500

mphillips
36 ●1 ●3

updated 2011-08-03 11:23:30 -0500

Sorry I didn't respond sooner. Since the theta dimension is discretized in the planner there is the possibility for some error at the goal. However it shouldn't be any larger than 11.25 degrees, since the planner uses 22.5 degree intervals. I've run your launch file using stage and didn't notice it being off by angles as large as 45 degrees (I did observe some error closer to the 11.25). This is the launch file I used...

<launch>
  <node name="map_server" pkg="map_server" type="map_server" args="$(find sbpl_lattice_planner)/worlds/willow.pgm 0.025" />

  <node pkg="stage" type="stageros" name="stageros" args="$(find sbpl_lattice_planner)/worlds/willow.world" respawn="false" >
    <param name="base_watchdog_timeout" value="0.2"/>
  </node>

  <node name="fake_localization" pkg="fake_localization" type="fake_localization" respawn="false" />

  <node pkg="pr2_move_base" name="pr2_move_base_node" type="pr2_move_base.py" />

  <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" />
    <!-- I loaded your 2 yaml files here. -->
  </node>
</launch>

edit flag offensive delete link

add a comment

sbpl_lattice_planner final heading error constant +45-deg.

2 Answers

Question Tools

Stats

Related questions

sbpl_lattice_planner final heading error constant +45-deg. edit

2 Answers

Question Tools

Stats

Related questions

sbpl_lattice_planner final heading error constant +45-deg.