[2dnav] Use correct config files

spur_2dnav/config/base_local_planner_params.yaml

@@ -11,4 +11,4 @@ TrajectoryPlannerROS:
   acc_lim_x: 2.5
   acc_lim_y: 2.5
 
-  holonomic_robot: true
+  holonomic_robot: false  # making this true needs more tuning. So first let's turn this off for now to tune other more important configs (odom, amcl)

spur_2dnav/config/dwa_local_planner_params.yaml

@@ -0,0 +1,57 @@
+## Copied from Kobuki and tailored for SPUR.
+DWAPlannerROS:
+
+# Robot Configuration Parameters - 
+  max_vel_x: 0.45
+  min_vel_x: 0.05
+
+  max_vel_y: 0.25
+  min_vel_y: 0.05
+
+  max_trans_vel: 0.5 # choose slightly less than the base's capability
+  min_trans_vel: 0.1  # this is the min trans velocity when there is negligible rotational velocity
+  trans_stopped_vel: 0.1
+
+  # Warning!
+  #   do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
+  #   are non-negligible and small in place rotational velocities will be created.
+
+  max_rot_vel: 0.25  # choose slightly less than the base's capability
+  min_rot_vel: 0.05  # this is the min angular velocity when there is negligible translational velocity
+  rot_stopped_vel: 0.4
+
+  acc_lim_x: 2.5 # maximum is theoretically 2.0, but we 
+  acc_lim_theta: 3.2
+  acc_lim_y: 2.5
+
+# Goal Tolerance Parameters
+  yaw_goal_tolerance: 0.3  # 0.05
+  xy_goal_tolerance: 0.15  # 0.10
+  # latch_xy_goal_tolerance: false
+
+# Forward Simulation Parameters
+  sim_time: 1.0       # 1.7
+  vx_samples: 6       # 3
+  vy_samples: 1       # diff drive robot, there is only one sample
+  vtheta_samples: 20  # 20
+
+# Trajectory Scoring Parameters
+  path_distance_bias: 64.0      # 32.0   - weighting for how much it should stick to the global path plan
+  goal_distance_bias: 24.0      # 24.0   - wighting for how much it should attempt to reach its goal
+  occdist_scale: 0.5            # 0.01   - weighting for how much the controller should avoid obstacles
+  forward_point_distance: 0.325 # 0.325  - how far along to place an additional scoring point
+  stop_time_buffer: 0.2         # 0.2    - amount of time a robot must stop in before colliding for a valid traj.
+  scaling_speed: 0.25           # 0.25   - absolute velocity at which to start scaling the robot's footprint
+  max_scaling_factor: 0.2       # 0.2    - how much to scale the robot's footprint when at speed.
+
+# Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.05  # 0.05   - how far to travel before resetting oscillation flags
+
+# Debugging
+  publish_traj_pc : true
+  publish_cost_grid_pc: true
+  global_frame_id: odom
+
+
+# Differential-drive robot configuration - necessary?
+  holonomic_robot: true

spur_2dnav/config/global_planner_params.yaml

@@ -0,0 +1,20 @@
+## Copied from Turtlebot and tailored for SPUR.
+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

spur_2dnav/config/move_base_params.yaml

@@ -0,0 +1,56 @@
+## Copied from Turtlebot and tailored for SPUR.
+
+# Move base node parameters. For full documentation of the parameters in this file, please see
+#
+#  http://www.ros.org/wiki/move_base
+#
+shutdown_costmaps: false
+
+controller_frequency: 5.0
+controller_patience: 3.0
+
+planner_frequency: 1.0
+planner_patience: 5.0
+
+oscillation_timeout: 10.0
+oscillation_distance: 0.2
+
+# local planner - default is trajectory rollout
+base_local_planner: "dwa_local_planner/DWAPlannerROS"
+
+base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner
+
+#We plan to integrate recovery behaviors for turtlebot but currently those belong to gopher and still have to be adapted.
+## recovery behaviors; we avoid spinning, but we need a fall-back replanning
+#recovery_behavior_enabled: true
+
+#recovery_behaviors:
+  #- name: 'super_conservative_reset1'
+    #type: 'clear_costmap_recovery/ClearCostmapRecovery'
+  #- name: 'conservative_reset1'
+    #type: 'clear_costmap_recovery/ClearCostmapRecovery'
+  #- name: 'aggressive_reset1'
+    #type: 'clear_costmap_recovery/ClearCostmapRecovery'
+  #- name: 'clearing_rotation1'
+    #type: 'rotate_recovery/RotateRecovery'
+  #- name: 'super_conservative_reset2'
+    #type: 'clear_costmap_recovery/ClearCostmapRecovery'
+  #- name: 'conservative_reset2'
+    #type: 'clear_costmap_recovery/ClearCostmapRecovery'
+  #- name: 'aggressive_reset2'
+    #type: 'clear_costmap_recovery/ClearCostmapRecovery'
+  #- name: 'clearing_rotation2'
+    #type: 'rotate_recovery/RotateRecovery'
+
+#super_conservative_reset1:
+  #reset_distance: 3.0
+#conservative_reset1:
+  #reset_distance: 1.5
+#aggressive_reset1:
+  #reset_distance: 0.0
+#super_conservative_reset2:
+  #reset_distance: 3.0
+#conservative_reset2:
+  #reset_distance: 1.5
+#aggressive_reset2:
+  #reset_distance: 0.0

spur_2dnav/config/navfn_global_planner_params.yaml

@@ -0,0 +1,10 @@
+## Copied from Kobuki and tailored for SPUR.
+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.0        #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

spur_2dnav/launch/amcl.launch

@@ -3,15 +3,25 @@
 -->
 <launch>
   <arg name="custom_param_file" default="$(find spur_2dnav)/config/dummy.yaml"/>
+  <arg name="gui" default="true"/>
+  <arg name="headless" default="false"/>
   <arg name="sim" default="false" />
   <arg name="map_file" default="$(find turtlebot_navigation)/maps/willow-2010-02-18-0.10.yaml"/>
   <arg name="run_rviz" default="true" />
+  <arg name="world_file" default="worlds/willowgarage.world" />
 
   <group if="$(arg sim)">
     <!-- Run Gazebo -->
     <include file="$(find spur_gazebo)/launch/spur_world.launch">
+      <arg name="gui" value="$(arg gui)" />
+      <arg name="headless" value="$(arg headless)"/>
       <arg name="visualize_laser" value="true" />
+      <arg name="world_file" value="$(arg world_file)"/>
     </include>
+
+    <!-- If using willow map then locate the robot at the specific init pose. -->
+    <node pkg="rostopic" type="rostopic" name="robot_initialpose_publisher"
+          args="pub -1 initialpose geometry_msgs/PoseWithCovarianceStamped '{ header:{ seq: 0, stamp: { secs: 91, nsecs: 316000000 }, frame_id: map }, pose: { pose: { position: { x: 27.5855789185, y: 22.4980049133, z: 0.0 }, orientation: { x: 0.0, y: 0.0, z: -0.832933327565, w: 0.553373356633 }}, covariance: [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]}}'" />
   </group>
 
   <!-- Run RViz -->

spur_2dnav/launch/includes/move_base.launch.xml

@@ -11,17 +11,19 @@
   <arg name="odom_topic" default="/robot_pose_ekf/odom_combined" />
   <arg name="laser_topic" default="/spur/laser/scan" />
   <arg name="twist_topic" default="/spur/cmd_vel" />
-  <arg name="custom_param_file" default="$(find turtlebot_navigation)/param/dummy.yaml"/>
+  <arg name="custom_param_file" default="$(find spur_2dnav)/config/dummy.yaml"/>
 
   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
-    <rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
-    <rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />   
-    <rosparam file="$(find turtlebot_navigation)/param/local_costmap_params.yaml" command="load" />   
-    <rosparam file="$(find turtlebot_navigation)/param/global_costmap_params.yaml" command="load" />
-    <rosparam file="$(find turtlebot_navigation)/param/dwa_local_planner_params.yaml" command="load" />
-    <rosparam file="$(find turtlebot_navigation)/param/move_base_params.yaml" command="load" />
-    <rosparam file="$(find turtlebot_navigation)/param/global_planner_params.yaml" command="load" />
-    <rosparam file="$(find turtlebot_navigation)/param/navfn_global_planner_params.yaml" command="load" />
+    <rosparam file="$(find spur_2dnav)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find spur_2dnav)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />   
+    <rosparam file="$(find spur_2dnav)/config/local_costmap_params.yaml" command="load" />   
+    <rosparam file="$(find spur_2dnav)/config/global_costmap_params.yaml" command="load" />
+<!--    <rosparam file="$(find spur_2dnav)/config/base_local_planner_params.yaml" command="load" /> -->
+    <rosparam file="$(find spur_2dnav)/config/move_base_params.yaml" command="load" />
+    <rosparam file="$(find spur_2dnav)/config/global_planner_params.yaml" command="load" />
+    <rosparam file="$(find spur_2dnav)/config/navfn_global_planner_params.yaml" command="load" />
+    <rosparam file="$(find spur_2dnav)/config/dwa_local_planner_params.yaml" command="load" />
+
     <!-- external params file that could be loaded into the move_base namespace -->
     <rosparam file="$(arg custom_param_file)" command="load" />
 

spur_description/launch/spur.rviz

@@ -395,76 +395,74 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        odom:
-          base_footprint:
-            base_link:
-              bl_rotation_link:
-                bl_wheel_link:
-                  bl_wheel_tire_l_link:
-                    {}
-                  bl_wheel_tire_r_link:
-                    {}
-              body:
-                {}
-              br_rotation_link:
-                br_wheel_link:
-                  br_wheel_tire_l_link:
-                    {}
-                  br_wheel_tire_r_link:
-                    {}
-              fl_rotation_link:
-                fl_wheel_link:
-                  fl_wheel_tire_l_link:
-                    {}
-                  fl_wheel_tire_r_link:
-                    {}
-              fr_rotation_link:
-                fr_wheel_link:
-                  fr_wheel_tire_l_link:
-                    {}
-                  fr_wheel_tire_r_link:
-                    {}
-              hokuyo_sensor_link:
-                {}
-              larm_base_link:
-                larm_shoulder_p_link:
-                  larm_shoulder_r_link:
-                    larm_shoulder_y_link:
-                      larm_elbow_p_link:
-                        larm_elbow_p_body:
-                          larm_elbow_p_body_1:
-                            {}
-                          larm_elbow_p_body_2:
-                            {}
-                        larm_wrist_p_link:
-                          larm_wrist_r_link:
+        map:
+          odom:
+            base_footprint:
+              base_link:
+                bl_rotation_link:
+                  bl_wheel_link:
+                    bl_wheel_tire_l_link:
+                      {}
+                    bl_wheel_tire_r_link:
+                      {}
+                body:
+                  {}
+                br_rotation_link:
+                  br_wheel_link:
+                    br_wheel_tire_l_link:
+                      {}
+                    br_wheel_tire_r_link:
+                      {}
+                fl_rotation_link:
+                  fl_wheel_link:
+                    fl_wheel_tire_l_link:
+                      {}
+                    fl_wheel_tire_r_link:
+                      {}
+                fr_rotation_link:
+                  fr_wheel_link:
+                    fr_wheel_tire_l_link:
+                      {}
+                    fr_wheel_tire_r_link:
+                      {}
+                hokuyo_sensor_link:
+                  {}
+                larm_base_link:
+                  larm_shoulder_p_link:
+                    larm_shoulder_r_link:
+                      larm_shoulder_y_link:
+                        larm_elbow_p_link:
+                          larm_elbow_p_body:
+                            larm_elbow_p_body_1:
+                              {}
+                            larm_elbow_p_body_2:
+                              {}
+                          larm_wrist_p_link:
+                            larm_wrist_r_link:
+                              {}
+                        larm_shoulder_y_body:
+                          larm_shoulder_y_body_1:
                             {}
-                      larm_shoulder_y_body:
-                        larm_shoulder_y_body_1:
-                          {}
-                        larm_shoulder_y_body_2:
-                          {}
-              rarm_base_link:
-                rarm_shoulder_p_link:
-                  rarm_shoulder_r_link:
-                    rarm_shoulder_y_link:
-                      rarm_elbow_p_link:
-                        rarm_elbow_p_body:
-                          rarm_elbow_p_body_1:
+                          larm_shoulder_y_body_2:
                             {}
-                          rarm_elbow_p_body_2:
+                rarm_base_link:
+                  rarm_shoulder_p_link:
+                    rarm_shoulder_r_link:
+                      rarm_shoulder_y_link:
+                        rarm_elbow_p_link:
+                          rarm_elbow_p_body:
+                            rarm_elbow_p_body_1:
+                              {}
+                            rarm_elbow_p_body_2:
+                              {}
+                          rarm_wrist_p_link:
+                            rarm_wrist_r_link:
+                              {}
+                        rarm_shoulder_y_body:
+                          rarm_shoulder_y_body_1:
                             {}
-                        rarm_wrist_p_link:
-                          rarm_wrist_r_link:
+                          rarm_shoulder_y_body_2:
                             {}
-                      rarm_shoulder_y_body:
-                        rarm_shoulder_y_body_1:
-                          {}
-                        rarm_shoulder_y_body_2:
-                          {}
-        map:
-          odom:
-            {}
       Update Interval: 0
       Value: true
     - Angle Tolerance: 0.1
@@ -538,22 +536,22 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 6.93232
+      Distance: 39.5249
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
+        X: 23.4349
+        Y: 15.0015
+        Z: -21.9805
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.465398
+      Pitch: 1.2604
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.885398
+      Yaw: 1.1154
     Saved: ~
 Window Geometry:
   Displays: