####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the 
## previous tutorials:" just add the links 
## note.0= [[rtabmap_ros/TutorialsOldInterface/StereoOutdoorMapping|Stereo outdoor mapping]]
## descriptive title for the tutorial
## title = Stereo Outdoor Navigation
## multi-line description to be displayed in search 
## description = This tutorial shows how to integrate autonomous navigation with RTAB-Map in context of outdoor stereo mapping.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for 
## level= AdvancedCategory
## keywords =
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(3)>>

== 2D Navigation ==

Example of navigation with only a stereo camera:

<<Youtube(X885QsH0szo)>>

The planner used in the video is [[move_base]]. The global costmap is generated from the "/map" (<<MsgLink(nav_msgs/OccupancyGrid)>>) topic and the local costmap is updated using the "openni_points" (<<MsgLink(sensor_msgs/PointCloud2)>>) topic. Here an example of launch file with the related move_base config files. The sections below show how these topics come from.

For info and for a complete example launch file, all the snippets below were integrated together with 3D stereo mapping in [[https://github.com/introlab/rtabmap_ros/blob/master/launch/azimut3/az3_mapping_robot_stereo_nav.launch|az3_mapping_robot_stereo_nav.launch]].

 * Planner:
  {{{
   <group ns="planner">
      <remap from="openni_points" to="/planner_cloud"/>
      <remap from="map" to="/rtabmap/proj_map"/>
      <remap from="move_base_simple/goal" to="/planner_goal"/>
        
      <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
         <rosparam file="costmap_common_params.yaml" command="load" ns="global_costmap" />
      <rosparam file="costmap_common_params.yaml" command="load" ns="local_costmap" />
         <rosparam file="local_costmap_params.yaml" command="load" />
         <rosparam file="global_costmap_params.yaml" command="load" />
         <rosparam file="base_local_planner_params.yaml" command="load" />
      </node>
   </group>
}}}

 * costmap_common_params.yaml:
  {{{
obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[ 0.3,  0.3], [-0.3,  0.3], [-0.3, -0.3], [ 0.3, -0.3]]
footprint_padding: 0.03
#robot_radius: ir_of_robot
inflation_radius: 0.55
transform_tolerance: 1

controller_patience: 2.0

NavfnROS:
    allow_unknown: true

recovery_behaviors: [
    {name: conservative_clear, type: clear_costmap_recovery/ClearCostmapRecovery},
    {name: aggressive_clear, type: clear_costmap_recovery/ClearCostmapRecovery}
]

conservative_clear: 
    reset_distance: 3.00
aggressive_clear:
    reset_distance: 1.84
}}}

 * global_costmap_params.yaml:
  {{{
global_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  update_frequency: 0.5
  publish_frequency: 0.5
  static_map: true
}}}

 * local_costmap_params.yaml:
  {{{
local_costmap:
  global_frame: odom
  robot_base_frame: base_footprint
  update_frequency: 2.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 4.0
  height: 4.0
  resolution: 0.025
  origin_x: -2.0
  origin_y: -2.0

  observation_sources: point_cloud_sensor

  # assuming receiving a cloud from rtabmap_ros/obstacles_detection node
  point_cloud_sensor: {
    sensor_frame: base_footprint,
    data_type: PointCloud2, 
    topic: openni_points, 
    expected_update_rate: 0.5, 
    marking: true, 
    clearing: true,
    min_obstacle_height: -99999.0,
    max_obstacle_height: 99999.0}
}}}

 * base_local_planner_params.yaml:
  {{{
TrajectoryPlannerROS:

  # Current limits based on AZ3 standalone configuration.
  acc_lim_x:  0.75
  acc_lim_y:  0.75
  acc_lim_theta: 4.00
  max_vel_x:  0.500
  min_vel_x:  0.212
  max_rotational_vel: 0.550
  min_in_place_rotational_vel: 0.15
  escape_vel: -0.10
  holonomic_robot: false

  xy_goal_tolerance:  0.20
  yaw_goal_tolerance: 0.20

  sim_time: 1.7
  sim_granularity: 0.025
  vx_samples: 3
  vtheta_samples: 3
  vtheta_samples: 20

  goal_distance_bias: 0.8
  path_distance_bias: 0.6
  occdist_scale: 0.01
  heading_lookahead: 0.325
  dwa: true

  oscillation_reset_dist: 0.05
  meter_scoring: true
}}}
=== Global costmap ===

The [[rtabmap_ros#rtabmap|rtabmap]] node can generate a 2D occupancy grid (named "/rtabmap/proj_map") from the projection of the 3D cloud map on the ground. The empty cells are filled by projecting the ground on the xy plane. The occupied cells are filled by projecting the obstacles on the xy plane. For each point cloud, the obstacles and the ground are segmented. Since the ground is not even, the ground is segmented by normal filtering: all points with normal in the +z direction (+- fixed angle) are labelled as ground, all the others are labelled as obstacles. The images below show an example.

|| {{attachment:obs0.png|obs0|width="300"}} || {{attachment:obs1.png|obs1|width="300"}} || {{attachment:obs2.png|obs2|width="300"}} ||
|| {{attachment:obs3.png|obs3|width="300"}} || {{attachment:obs4.png|obs4|width="300"}} ||

{{{
   ...
   <remap from="map" to="/rtabmap/proj_map"/>
   ...
}}}

=== Local costmap ===

For the local costmap update, we feed it a point cloud with only the obstacles without the ground. The [[rtabmap_ros#rtabmap_ros.2BAC8-obstacles_detection|rtabmap_ros/obstacles_detection]] nodelet is used. This nodelet uses the same ground/obstacles segmentation approach described for the global costmap. Note that in local_costmap_params.yaml, because the robot can be at any height from its odom/map referentials, we set the `min_obstacle_height` and `max_obstacle_height` to high values (-99999 and 99999). The cloud used for segmentation is generated from the disparity image of stereo_image_proc using the [[rtabmap_ros#rtabmap_ros.2BAC8-point_cloud_xyz|rtabmap_ros/point_cloud_xyz]] nodelet.
{{{
   <group ns="/stereo_camera" >
      
      <node pkg="nodelet" type="nodelet" name="stereo_nodelet"  args="manager"/>

      <!-- Generate a point cloud from the disparity image -->
      <node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/point_cloud_xyz stereo_nodelet">
         <remap from="disparity/image"       to="disparity"/>
         <remap from="disparity/camera_info" to="right/camera_info_throttle"/>
         <remap from="cloud"                to="cloudXYZ"/>
         
         <param name="voxel_size" type="double" value="0.05"/>
         <param name="decimation" type="int" value="4"/>
         <param name="max_depth" type="double" value="4"/>
      </node>

      <!-- Create point cloud for the local planner -->
      <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection stereo_nodelet">
         <remap from="cloud" to="cloudXYZ"/>
         <remap from="obstacles" to="/planner_cloud"/>

         <param name="frame_id" type="string" value="base_footprint"/>
         <param name="map_frame_id" type="string" value="map"/>
         <param name="min_cluster_size" type="int" value="20"/>
         <param name="max_obstacles_height" type="double" value="0.0"/>
       </node>
   </group>
}}}