git commit -m "first commit"

navigations/sbpl_lattice_planner/launch/move_base/base_local_planner_params_close.yaml

@@ -0,0 +1,28 @@
+TrajectoryPlannerROS:
+  #Independent settings for the local costmap
+  transform_tolerance: 0.3
+  sim_time: 1.7
+  sim_granularity: 0.025
+  dwa: true
+  vx_samples: 3
+  vtheta_samples: 20
+  max_vel_x: 2.0
+  min_vel_x: 0.1
+  max_rotational_vel: 1.0
+  min_in_place_vel_theta: 0.4
+  xy_goal_tolerance: 0.1
+  yaw_goal_tolerance: 0.1
+  goal_distance_bias: 0.2
+  path_distance_bias: .9
+  occdist_scale: 0.01
+  heading_lookahead: 0.325
+  oscillation_reset_dist: 0.05
+  holonomic_robot: true
+  acc_lim_theta: 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
+  meter_scoring: true

navigations/sbpl_lattice_planner/launch/move_base/costmap_common_params.yaml

@@ -0,0 +1,21 @@
+#START VOXEL STUFF
+map_type: voxel
+origin_z: 0.0
+z_resolution: 0.2
+z_voxels: 10
+unknown_threshold: 9
+mark_threshold: 0
+#END VOXEL STUFF
+transform_tolerance: 0.3
+obstacle_range: 2.5
+max_obstacle_height: 2.0
+raytrace_range: 3.0
+footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]
+#robot_radius: 0.46
+footprint_padding: 0.01
+inflation_radius: 0.55
+cost_scaling_factor: 10.0
+lethal_cost_threshold: 100
+observation_sources: base_scan
+base_scan: {data_type: LaserScan, expected_update_rate: 0.4,
+  observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}

navigations/sbpl_lattice_planner/launch/move_base/global_costmap_params.yaml

@@ -0,0 +1,12 @@
+#Independent settings for the planner's costmap
+global_costmap: 
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  publish_frequency: 0.0
+  static_map: true
+  rolling_window: false
+  static_layer:
+    track_unknown_space: true
+  obstacle_layer:
+    track_unknown_space: true

navigations/sbpl_lattice_planner/launch/move_base/local_costmap_params_close.yaml

@@ -0,0 +1,13 @@
+local_costmap:
+  publish_voxel_map: true
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  publish_frequency: 2.0
+  static_map: false
+  rolling_window: true
+  width: 2.0
+  height: 2.0
+  resolution: 0.025
+  origin_x: 0.0
+  origin_y: 0.0

navigations/sbpl_lattice_planner/launch/move_base/move_base.xml

@@ -0,0 +1,44 @@
+<!-- <launch>
+
+  <node ns="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">
+    <param name="footprint_padding" value="0.01" />
+    <param name="controller_frequency" value="10.0" />
+    <param name="controller_patience" value="100.0" />
+    <param name="base_global_planner" value="SBPLLatticePlanner" />
+    <param name="base_local_planner" value="DWAPlannerROS" />
+    <param name="SBPLLatticePlanner/primitive_filename" value="$(find sbpl_lattice_planner)/matlab/mprim/pr2.mprim" />
+    <rosparam file="$(find sbpl_lattice_planner)/launch/move_base/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find sbpl_lattice_planner)/launch/move_base/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find sbpl_lattice_planner)/launch/move_base/local_costmap_params_close.yaml" command="load" />
+    <rosparam file="$(find sbpl_lattice_planner)/launch/move_base/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find sbpl_lattice_planner)/launch/move_base/sbpl_global_params.yaml" command="load" />
+    <rosparam file="$(find sbpl_lattice_planner)/launch/move_base/base_local_planner_params_close.yaml" command="load" />
+  </node>
+</launch> -->
+
+<launch>
+    <!--node ns="local_costmap" name="voxel_grid_throttle" pkg="topic_tools" type="throttle" args="messages voxel_grid 3.0 voxel_grid_throttled" /-->
+    <arg name="local_planner" default="dwb" doc="Local planner can be either dwa, base, teb or pose"/>
+    <arg name="with_virtual_walls" default="true" doc="Enables usage of virtual walls when set. Set to false when running SLAM." />
+    <arg name="prefix" default="" doc="Prefix used for robot tf frames" /> <!-- used in the config files -->
+
+    <node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen" clear_params="true">
+        <param name="SBPLLatticePlanner/primitive_filename" value="$(find sbpl_lattice_planner)/matlab/mprim/unicycle_highcost_5cm.mprim" />
+        <rosparam file="$(find sbpl_lattice_planner)/config/move_base_common_params.yaml" command="load" />
+        <rosparam file="$(find sbpl_lattice_planner)/config/sbpl_global_params.yaml" command="load" />
+        <rosparam file="$(find sbpl_lattice_planner)/config/$(arg local_planner)_local_planner_params.yaml" command="load" />
+        <!-- global costmap params -->
+        <rosparam file="$(find sbpl_lattice_planner)/config/costmap_common_params.yaml" command="load" ns="global_costmap" subst_value="true" />
+        <rosparam file="$(find sbpl_lattice_planner)/config/costmap_global_params.yaml" command="load" />
+        <rosparam file="$(find sbpl_lattice_planner)/config/costmap_global_params_plugins_virtual_walls.yaml" command="load" if="$(arg with_virtual_walls)" />
+        <rosparam file="$(find sbpl_lattice_planner)/config/costmap_global_params_plugins_no_virtual_walls.yaml" command="load" unless="$(arg with_virtual_walls)" />
+        <!-- local costmap params -->
+        <rosparam file="$(find sbpl_lattice_planner)/config/costmap_common_params.yaml" command="load" ns="local_costmap" subst_value="true" />
+        <rosparam file="$(find sbpl_lattice_planner)/config/costmap_local_params.yaml" command="load" subst_value="true" />
+        <rosparam file="$(find sbpl_lattice_planner)/config/costmap_local_params_plugins_virtual_walls.yaml" command="load" if="$(arg with_virtual_walls)" />
+        <rosparam file="$(find sbpl_lattice_planner)/config/costmap_local_params_plugins_no_virtual_walls.yaml" command="load" unless="$(arg with_virtual_walls)" />
+        <!-- <remap from="map" to="/map" /> -->
+        <!-- <remap from="marker" to="move_base_node/DWBLocalPlanner/markers" /> -->
+    </node>
+</launch>

navigations/sbpl_lattice_planner/launch/move_base/nav_view_sbpl.xml

@@ -0,0 +1,10 @@
+<launch>
+  <node name="nav_view" pkg="rqt_nav_view" type="rqt_nav_view" respawn="false">
+    <remap from="goal" to="move_base_simple/goal" />
+    <remap from="obstacles" to="move_base_node/local_costmap/obstacles" />
+    <remap from="inflated_obstacles" to="move_base_node/local_costmap/inflated_obstacles" />
+    <remap from="global_plan" to="move_base_node/SBPLLatticePlanner/plan" />
+    <remap from="local_plan" to="move_base_node/TrajectoryPlannerROS/local_plan" />
+    <remap from="robot_footprint" to="move_base_node/local_costmap/robot_footprint"/>
+  </node>
+</launch>

navigations/sbpl_lattice_planner/launch/move_base/sbpl_global_params.yaml

@@ -0,0 +1,18 @@
+# SBPLLatticePlanner:
+#   environment_type: XYThetaLattice
+#   planner_type: ARAPlanner
+#   allocated_time: 5.0
+#   initial_epsilon: 3.0
+#   forward_search: false
+#   allow_unknown: true
+
+base_global_planner: SBPLLatticePlanner
+SBPLLatticePlanner:
+  environment_type: XYThetaLattice
+  planner_type: ARAPlanner
+  allocated_time: 10.0
+  initial_epsilon: 15.0
+  forward_search: false
+  nominalvel_mpersecs: 0.8
+  timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
+  allow_unknown: true