Real robot navigation files

launch/amcl_navigation.launch

@@ -0,0 +1,89 @@
+<launch>
+    <!-- Kinect TF -->
+    <include file="$(find jackal_bsc)/launch/kinect_tf.launch"/>
+
+    <!-- Fake laser scan-->
+    <arg name="scan_topic" default="/jackal_laser_scan"/>
+    <node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan">
+        <param name="output_frame_id" value="kinect2_link"/>
+        <remap from="image" to="/kinect2/hd/image_depth_rect"/>
+        <remap from="camera_info" to="/kinect2/hd/camera_info"/>
+        <remap from="scan" to="$(arg scan_topic)"/>
+    </node>
+
+    <!-- Map Server -->
+    <arg name="map_file" default="$(find jackal_bsc)/maps/map.pgm"/>
+    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)"/>
+
+    <!-- AMCL Navigation -->
+    <arg name="use_map_topic" default="false"/>
+    <arg name="scan_topic" default="$(arg scan_topic)" />
+
+    <node pkg="amcl" type="amcl" name="amcl">
+        <param name="use_map_topic" value="$(arg use_map_topic)"/>
+        <!-- Publish scans from best pose at a max of 10 Hz -->
+        <param name="odom_model_type" value="diff"/>
+        <param name="odom_alpha5" value="0.1"/>
+        <param name="gui_publish_rate" value="10.0"/>
+        <param name="max_particles" value="2000"/>
+        <!-- Maximum error between the true distribution and the estimated distribution. -->
+        <param name="kld_err" value="0.05"/>
+        <param name="kld_z" value="0.99"/>
+        <param name="odom_alpha1" value="0.2"/>
+        <param name="odom_alpha2" value="0.2"/>
+        <!-- translation std dev, m -->
+        <param name="odom_alpha3" value="0.2"/>
+        <param name="odom_alpha4" value="0.2"/>
+        <param name="laser_z_hit" value="0.5"/>
+        <param name="laser_z_short" value="0.05"/>
+        <param name="laser_z_max" value="0.05"/>
+        <param name="laser_z_rand" value="0.5"/>
+        <param name="laser_sigma_hit" value="0.2"/>
+        <param name="laser_lambda_short" value="0.1"/>
+        <param name="laser_model_type" value="likelihood_field"/>
+        <!-- Maximum distance to do obstacle inflation on map, for use in likelihood_field model. -->
+        <param name="laser_likelihood_max_dist" value="2.0"/>
+        <!-- Translational movement required before performing a filter update.  -->
+        <param name="update_min_d" value="0.1"/>
+        <!--Rotational movement required before performing a filter update. -->
+        <param name="update_min_a" value="0.314"/>
+        <param name="odom_frame_id" value="odom"/>
+        <param name="base_frame_id" value="base_link"/>
+        <param name="global_frame_id" value="map"/>
+        <!-- Number of filter updates required before resampling. -->
+        <param name="resample_interval" value="1"/>
+        <!-- Increase tolerance because the computer can get quite busy -->
+        <param name="transform_tolerance" value="1.0"/>
+        <!-- Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001. -->
+        <param name="recovery_alpha_slow" value="0.0"/>
+        <!--Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1. -->
+        <param name="recovery_alpha_fast" value="0.1"/>
+        <!-- Initial pose mean -->
+        <param name="initial_pose_x" value="0.0" />
+        <param name="initial_pose_y" value="0.0" />
+        <param name="initial_pose_a" value="0.0" />
+        <!-- When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.-->
+        <param name="receive_map_topic" value="true"/>
+        <!--  When set to true, AMCL will only use the first map it subscribes to, rather than updating each time a new one is received. -->
+        <param name="first_map_only" value="false"/>
+        <remap from="scan" to="$(arg scan_topic)"/>
+    </node>
+
+    <!-- Move Base -->
+    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+
+        <rosparam file="$(find jackal_bsc)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
+        <rosparam file="$(find jackal_bsc)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
+
+        <rosparam file="$(find jackal_bsc)/params/map_nav_params/local_costmap_params.yaml" command="load" />
+        <rosparam file="$(find jackal_bsc)/params/map_nav_params/global_costmap_params.yaml" command="load" />
+
+        <rosparam file="$(find jackal_bsc)/params/base_local_planner_params.yaml" command="load" />
+        <rosparam file="$(find jackal_bsc)/params/move_base_params.yaml" command="load" />
+
+        <param name="base_global_planner" type="string" value="navfn/NavfnROS" />
+        <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"/>
+
+        <remap from="odom" to="odometry/filtered" />
+    </node>
+</launch>

launch/odom_navigation.launch

@@ -0,0 +1,32 @@
+<launch>
+    <!-- Kinect TF -->
+    <include file="$(find jackal_bsc)/launch/kinect_tf.launch"/>
+
+    <!-- Fake laser scan-->
+    <arg name="scan_topic" default="/jackal_laser_scan"/>
+    <node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan">
+        <param name="output_frame_id" value="kinect2_link"/>
+        <remap from="image" to="/kinect2/hd/image_depth_rect"/>
+        <remap from="camera_info" to="/kinect2/hd/camera_info"/>
+        <remap from="scan" to="$(arg scan_topic)"/>
+    </node>
+
+    <!-- Move Base -->
+    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+
+        <rosparam file="$(find jackal_bsc)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
+        <rosparam file="$(find jackal_bsc)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
+
+        <rosparam file="$(find jackal_bsc)/params/odom_nav_params/global_costmap_params.yaml" command="load" />
+        <rosparam file="$(find jackal_bsc)/params/odom_nav_params/local_costmap_params.yaml" command="load" />
+
+        <rosparam file="$(find jackal_bsc)/params/base_local_planner_params.yaml" command="load" />
+        <rosparam file="$(find jackal_bsc)/params/move_base_params.yaml" command="load" />
+
+        <param name="base_global_planner" type="string" value="navfn/NavfnROS" />
+        <param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS"/>
+
+        <remap from="odom" to="odometry/filtered" />
+    </node>
+
+</launch>

launch/scan.launch

@@ -0,0 +1,9 @@
+<launch>
+    <arg name="scan_topic" default="/jackal_laser_scan"/>
+    <node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan">
+        <param name="output_frame_id" value="kinect2_link"/>
+        <remap from="image" to="/kinect2/hd/image_depth_rect"/>
+        <remap from="camera_info" to="/kinect2/hd/camera_info"/>
+        <remap from="scan" to="$(arg scan_topic)"/>
+    </node>
+</launch>

launch/teleop.launch → launch/teleop/teleop.launch

@@ -1,6 +1,6 @@
 <launch>
 	<node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen">
-		<!-- <remap from="cmd_vel" to="/jackal_bsc/key_vel"/> -->
+		 <remap from="cmd_vel" to="/jackal_bsc/key_vel"/>
 		<!--<remap from="cmd_vel" to="/cmd_vel"/>-->
 	</node>
 </launch>

maps/map.yaml

@@ -0,0 +1,7 @@
+image: map.pgm
+resolution: 0.020000
+origin: [-21.520000, -20.240000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

params/base_local_planner_params.yaml

@@ -0,0 +1,46 @@
+TrajectoryPlannerROS:
+
+  # Robot Configuration Parameters
+  acc_lim_x: 10.0
+  acc_lim_theta:  20.0
+
+  max_vel_x: 0.5
+  min_vel_x: 0.1
+
+  max_vel_theta: 1.57
+  min_vel_theta: -1.57
+  min_in_place_vel_theta: 0.314
+
+  holonomic_robot: false
+  escape_vel: -0.5
+
+  # Goal Tolerance Parameters
+  yaw_goal_tolerance: 0.157
+  xy_goal_tolerance: 0.25
+  latch_xy_goal_tolerance: false
+
+  # Forward Simulation Parameters
+  sim_time: 2.0
+  sim_granularity: 0.02
+  angular_sim_granularity: 0.02
+  vx_samples: 6
+  vtheta_samples: 20
+  controller_frequency: 20.0
+
+  # Trajectory scoring parameters
+  meter_scoring: true # Whether the gdist_scale and pdist_scale parameters should assume that goal_distance and path_distance are expressed in units of meters or cells. Cells are assumed by default (false).
+  occdist_scale:  0.1 #The weighting for how much the controller should attempt to avoid obstacles. default 0.01
+  pdist_scale: 0.75  #     The weighting for how much the controller should stay close to the path it was given . default 0.6
+  gdist_scale: 1.0 #     The weighting for how much the controller should attempt to reach its local goal, also controls speed  default 0.8
+
+  heading_lookahead: 0.325  #How far to look ahead in meters when scoring different in-place-rotation trajectories
+  heading_scoring: false  #Whether to score based on the robot's heading to the path or its distance from the path. default false
+  heading_scoring_timestep: 0.8   #How far to look ahead in time in seconds along the simulated trajectory when using heading scoring (double, default: 0.8)
+  dwa: true #Whether to use the Dynamic Window Approach (DWA)_ or whether to use Trajectory Rollout
+  simple_attractor: false
+  publish_cost_grid_pc: true
+
+  #Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.05 #How far the robot must travel in meters before oscillation flags are reset (double, default: 0.05)
+  escape_reset_dist: 0.1
+  escape_reset_theta: 0.1

params/costmap_common_params.yaml

@@ -0,0 +1,34 @@
+map_type: costmap
+origin_z: 0.0
+z_resolution: 1
+z_voxels: 2
+
+obstacle_range: 2.5
+raytrace_range: 3.0
+
+publish_voxel_map: false
+transform_tolerance: 0.5
+meter_scoring: true
+
+footprint: [[-0.21, -0.165], [-0.21, 0.165], [0.21, 0.165], [0.21, -0.165]]
+footprint_padding: 0.1
+
+plugins:
+- {name: obstacles_layer, type: "costmap_2d::ObstacleLayer"}
+- {name: inflater_layer, type: "costmap_2d::InflationLayer"}
+
+obstacles_layer:
+  observation_sources: scan
+  scan: {
+    sensor_frame: kinect2_link,
+    data_type: LaserScan,
+    topic: /jackal_laser_scan,
+    marking: true,
+    clearing: true,
+    min_obstacle_height: -2.0,
+    max_obstacle_height: 2.0,
+    obstacle_range: 2.5,
+    raytrace_range: 3.0}
+
+inflater_layer:
+ inflation_radius: 0.30

params/map_nav_params/global_costmap_params.yaml

@@ -0,0 +1,17 @@
+global_costmap:
+   global_frame: map
+   robot_base_frame: base_link
+   update_frequency: 20.0
+   publish_frequency: 5.0
+   width: 40.0
+   height: 40.0
+   resolution: 0.05
+   origin_x: -20.0
+   origin_y: -20.0
+   static_map: true
+   rolling_window: false
+
+   plugins:
+   - {name: static_layer, type: "costmap_2d::StaticLayer"}
+   - {name: obstacles_layer, type: "costmap_2d::ObstacleLayer"}
+   - {name: inflater_layer, type: "costmap_2d::InflationLayer"}

params/map_nav_params/local_costmap_params.yaml

@@ -0,0 +1,10 @@
+local_costmap:
+   global_frame: map
+   robot_base_frame: base_link
+   update_frequency: 20.0
+   publish_frequency: 5.0
+   width: 10.0
+   height: 10.0
+   resolution: 0.05
+   static_map: false
+   rolling_window: true

params/move_base_params.yaml

@@ -0,0 +1,13 @@
+shutdown_costmaps: false
+
+controller_frequency: 20.0
+controller_patience: 15.0
+
+planner_frequency: 20.0
+planner_patience: 5.0
+
+oscillation_timeout: 0.0
+oscillation_distance: 0.5
+
+recovery_behavior_enabled: true
+clearing_rotation_allowed: true

params/odom_nav_params/global_costmap_params.yaml

@@ -0,0 +1,12 @@
+global_costmap:
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 20
+  publish_frequency: 5
+  width: 40.0
+  height: 40.0
+  resolution: 0.05
+  origin_x: -20.0
+  origin_y: -20.0
+  static_map: true
+  rolling_window: false

params/odom_nav_params/local_costmap_params.yaml

@@ -0,0 +1,10 @@
+local_costmap:
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 20.0
+  publish_frequency: 5.0
+  width: 10.0
+  height: 10.0
+  resolution: 0.05
+  static_map: false
+  rolling_window: true