mphillips's profile - activity

Currently there isn't support for this type of constraint (directly). The planner tries to find a time-minimal collision free path to the goal. However, it would be possible to add this extension to the planner (and I'll do so in the next release).

Until then though an easy hack to get the planner to reach the goal on the interval (t1,t2) would be to put a "fake" dynamic obstacle at the goal location from time 0 to t1 and another fake dynamic obstacle at the goal location from time t2 to some large time. This is essentially how I will implement your feature in the next release anyway.

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>