Once the quadrotor can reliably and stably navigate the environment based on a series of desired waypoints, the quadrotor system can be used to sense and comprehend it’s surrounding environment. A map is a representation of the environment where the quadrotor is operating. To operate in the map, the quadrotor needs to know its position in the map coordinate frame. Ultimately, the goal is to develop a system that allows the quadrotor to autonomously reach a desired goal state in the map. The path from the initial state to the goal state can be a series of waypoints or actions, if a path exists.

A perfect estimate of the quadrotor’s pose needed to map creation is typically not available, especially in indoor environments. Simultaneous Localization and Mapping is the process of both making and updating a map of the environment while also estimating the system’s location in that map. This map can be saved and used later for localization and navigation without the need to rebuild the map. Typically, lasers are used to create two dimensional maps based on range measurements. However, these are often too large in size and mass or power intensive for use on small quadrotors. Instead, a RGBD sensor is used as the primary means of measuring the environment. This sensor, a Microsoft Kinect, contains stereo cameras as well as a depth sensor. The Kinect outputs point clouds which can be used by the SLAM algorithms to build the map of the environment. It is also possible to use these point clouds to emulate a 2D laser scan output.

In an ideal world, the map accurately reflects the environment, the environment is stable, and the localized estimate of the quadrotor’s pose in that map is accurate. However, in the real world, the environment is dynamic and sensor measurements are noisy. Therefore, obstacle avoidance and detection systems are developed as methods of continually updating the map. The ROS Navigation Stack combines all of these requirements into a complete sense-plan-act system.

The Navigation Stack

The ROS Navigation Stack is a 2D navigation stack that takes in information from odometry, sensor streams, and a goal pose and outputs safe velocity commands that are sent to a mobile base. The Navigation Stack will rely on the map_server package for the 2D map, the amcl package for localization in that map, sensor and odometry messages from the quadrotor, and the move_base packages to fuse all the messages in order to output a desired velocity command. This integration can be visualized below.

The Navigation Stack needs to be configured for the shape and dynamics of a robot to perform at a high level and there are several pre-requisites for navigation stack use.

• The navigation stack can only handle a differential drive and holonomic-wheeled robots. It assumes that the mobile base is controlled by sending desired velocity commands to achieve in the form of: x velocity, y velocity, theta velocity.
• The shape of the robot must either be a square or a rectangle. The Navigation Stack was developed on a square robot, so its performance will be best on robots that are nearly square or circular.
• The robot have a tf transform tree in place so that the robot publishes information about the relationships between the positions of all the joints and sensors.
• It requires a planar laser mounted somewhere on the mobile base. This laser is used for map building and localization. The robot must publish sensor data using the correct ROS Message types.

Constructing a Mapping Environment

For simulating this application, the original 3DR Kit C Gazebo model was modified to include a Kinect sensor mounted on the front arm of the quadrotor. Additionally, the WillowGarage world was loaded to provide an indoor environment suitable to mapping. The WillowGarage world and RGB output of the Kinect sensor can be seen in the Gazebo screenshot below.

Developing the 2D Map

In order to make an initial map of the environment, a series of waypoints was recorded. These waypoints maneuvered the quadrotor from its initial position outside the office complex, in and around several rooms, and returning back to the initial position. This resulted in a repeatable path with enough sensor coverage to support the development of the environment map.

In order to create the occupancy grid map to be used for future path planning, the Kinect sensor output was modified to appear like a laser scan output. The Kinect sensor provides depth information in a 3-dimensional point cloud. However, for 2D mapping, all that is necessary is a series of depth images at a single altitude. This reduces computation costs and makes navigating basic environments much simpler. In order to do this, the depthimage_to_laserscan package was used. This packages reduces computation even further by using the disparity image instead of the complete point cloud. The node takes the Kinect sensor’s disparity image input and needs to Kinect’s coordinate frame to correctly output the laser scan data. This node outputs a LaserScan message that can be used to build the 2D occupancy grid map.

To launch this package in the WillowGarag world with the appropriate control nodes, the quad_slam_laser_map.launch file was used. The depthimage_to_laserscan node portion of the launch file is reproduced below. This depicts several of the variables that were set for this implementation of the depthimage_to_laserscan node. Specifically, the coordinate frame of the sensor is set as well as the input point cloud topic name and desired output laserscan topic name.

 <!-- Fake laser -->
  <node pkg="nodelet" type="nodelet" name="laserscan_nodelet_manager" args="manager"/>
  <node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan"
        args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet laserscan_nodelet_manager">
     <param name="scan_height" value="10"/> 
     <param name="scan_time" value="0.033"/>
     <param name="range_min" value="0.45"/>
     <param name="range_max" value="10.0"/>
     <param name="min_height" value=".10"/>
     <param name="max_height" value="2.0"/>
     <param name="output_frame_id" value="quad/camera__link"/>
     <remap from="image" to="/quad/camera_/depth/disparity"/>
     <remap from="/scan" to="/sim_scan"/> 
  </node>

The image below depicts a sample laser scan measurement overlaid on top of the point cloud that it was sampled from.

Next, OpenSlam’s Gmapping algorithm was used to create the 2D map. The gmapping package contains a ROS wrapper for the Gmapping algorithm that provides laser-based SLAM. The node subscribes to the laser scan message and outputs a map in the form of a OccupancyGrid message. The gmapping node portion of the launch file is reproduced below. Note that we remapped the laserscan input to the output of the depthimage_to_laserscan node. The odometry frame value is also explicitly set.

  <!-- Run OpenSlam's Gmapping to create 2-D Occupancy Grid Map -->
  <node name="slam_gmapping" pkg="gmapping" type="slam_gmapping">
    <remap from="scan" to="/sim_scan"/>
    <param name="odom_frame" value="world"/>
  </node>

Once the environment has been sufficiently mapped, the map can be saved for later use. Specifically, the map can be used to support localization and future path planning algorithms. The map_server node is used to save dynamically generated maps to a pair of files. A YAML file describes the map meta-data, and names the image file. The image file encodes the occupancy data. The map is saved using the map_saver utility using the example code below.

rosrun map_server map_saver -f mymap

The final map of the WillowGarage environment is depicted below.

2D Localization Utilizing the Map

In order to localize in the environment, a previously recorded map is loaded with the map_server node. This node reads a map from disk and offers it via a ROS service. The current implementation of the map_server converts color values in the map image data into ternary occupancy values: free (0), occupied (100), and unknown (-1). The map is available on the map topic as a OccupancyGrid message. This node is initialized with the following excerpt from the quad_2dnav.launch file and loads the map file previously generated for the WillowGarage environment.

  <!-- Load a Map  -->
  <node name="map_server" pkg="map_server" type="map_server" args=" $(find quad_2dnav)/maps/wg_map.yaml" />

The amcl (adaptive Monte Carlo localization) package is a probabilistic localization system for a robot moving in 2D. This approach uses a particle filter to track the pose of a robot against a known map. As currently implemented, this node works only with laser scans and laser maps. The amcl node takes in a laser-based map, laser scans, and transform messages, and outputs pose estimates. The amcl node has many configuration options that can affect the performance of localization. A description of these parameters can be viewed here. These parameters are set in the amcl_quad.launch file. Portions of that file are reproduced below. Specifically, the input laserscan topic name is set as well as the coordinate frames for the quadrotor, world, and frame.

<node pkg="amcl" type="amcl" name="amcl">
  <remap from="/scan" to="/sim_scan"/>  
  <!-- Publish scans from best pose at a max of 10 Hz -->
  <param name="odom_model_type" value="omni"/>
  <param name="odom_frame_id" value="world"/>
  <param name="base_frame_id" value="base_link" />
  <param name="global_frame_id" value="map" />
</node>

When the node is created, it initializes its particle filter according to the parameters provided. If no parameters are set, the initial filter state will be a moderately sized particle cloud centered around the origin. It is possible to set the initial position in RVIZ using the 2D Pose Estimate button. Alternatively, the initial x, y, and yaw can be set in the launch file. The particle cloud can be viewed as a Pose Array in RVIZ. The particle cloud (a series of red arrows) will converge around the current localized posed of the quadrotor. As the quadrotor maneuvers in the environment, the amcl node will use the laser scan data and the odometry data to continually update the particle cloud with the predicted location of the quadrotor. An example of the amcl point cloud visualization is depicted below.

2D Path Planning

The move_base node is a major component of the navigation stack that provides a ROS interface for configuring, running, and interacting with the navigation stack on a robot. The move_base node links together a global and local planner to accomplish its global navigation task. The move_base node also maintains two costmaps, one for the global planner, and one for a local planner that are used to accomplish navigation tasks. Running the move_base node on a robot that is properly configured results in a robot that will attempt to achieve a goal pose with its base to within a user-specified tolerance. The move_base node may optionally perform recovery behaviors when the robot perceives itself as stuck.  These recovery behaviors can be configured using the recovery_behaviors parameter, and disabled using the recovery_behavior_enabled parameter. For more information on configuration of the move_base node, and the navigation stack as a whole, please see the navigation setup and configuration tutorial.

The excerpt below is from the quad_2dnav.launch file. Here, the odometry topic name is modified to fit the simulation environment and the planning configuration files are loaded.

  <!-- Load Navigation Stack  -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <remap from="odom" to="/quad/ground_truth/odometry" />
    <rosparam file="$(find quad_2dnav)/resource/planning/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find quad_2dnav)/resource/planning/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find quad_2dnav)/resource/planning/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find quad_2dnav)/resource/planning/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find quad_2dnav)/resource/planning/base_local_planner_params.yaml" command="load" />
  </node>

The navigation stack stores information about obstacles in the world in two costmaps. The global costmap is used for long term planning over the entire environment whereas the local costmap is used for short term planning and obstacle avoidance. One configuration file contains options common to both costmaps while other configuration files contain options specific to the global or local costmap.

The costmap_common_params.yaml file specifies the thresholds on obstacle information put into the costmap, footprint of the robot, inflation radius for the costmap, and the list of sensors that are going to be passing information to the costmap. The global_costmap_params.yaml file defines the costmap coordinate frame, robot coordinate frame, an update frequency, and the availability of a map served by the map_server. The local_costmap_params.yaml file includes similar coordinate frame and update options. For documentation on the full range of options, please see the costmap_2d documentation.

Lastly, a configuration file for the base_local_planner is loaded. The base_local_planner is responsible for computing velocity commands to send to the quadrotor waypoint node. The base_local_planner_params.yaml file contains options to configure the base_local_planner to make it compatible with the quadrotor’s control systems. Specifically, it is necessary to set the holonomic flag so that the local planner can compute the full range of appropriate velocity commands. For documentation on the full range of options, please see the base_local_planner documentation.

RVIZ is a good tool for visualizing all the possible data processed by the navigation stack. It was previously described how RVIZ can be used to set the initial robot pose for the amcl node as well as visualize the point cloud pose estimate generated by the amcl node. Additionally, the navigation goal can be set using RVIZ. This results in a local and global plan that can be visualized. Additionally, both the local and global costmaps can be visualized to depict obstacles identified in the environment.

In the screenshot above, the quadrotor’s footprint is the red box and the global goal is the large red arrow. The global path is the green line from the quadrotor’s current position to the global goal and the blue line is the local path. The previously recorded map is shown as well. On top of the map, is the global cost function identifying all the walls as obstacles. The local cost function based on the current sensor readings is displayed slightly brighter and in the immediate vicinity of the quadrotor.

A video summarizing this section as well as displaying ROS and Gazebo footage is available below.