Ground-based robots are limited to 2D navigation due to their dynamics. However, since the quadrotor can also easily adjust its vertical position, 3D navigation can be implemented. Navigation in 3D enables the quadrotor more maneuverability to explore its environment, the ability to get a much more complete understanding of the environment, and also navigate the environment in more complex paths. This is especially useful when it comes to obstacle avoidance.

To demonstrate 3D navigation, the kitchen world was used in Gazebo. While 2D navigation is sufficient for empty rooms with limited obstacles, it doesn’t take full advantage of the quadrotor’s maneuverability. Therefore, the kitchen environment was selected to demonstrate 3D navigation. This environment contains obstacles of varying sizes and heights, and is more complex than the WillowGarage environment used in the 2D Mapping and Navigation section. The kitchen environment can be seen below rendered in Gazebo.

MoveIt! Package

MoveIt! is state of the art software for mobile manipulation, incorporating the latest advances in motion planning, manipulation, 3D perception, kinematics, control and navigation. It provides an easy-to-use platform for developing advanced robotics applications, evaluating new robot designs and building integrated robotics products for industrial, commercial, R&D and other domains.

The primary node used in MoveIt! is the move_group node. As shown in the system architecture image below, the move_group node integrates all of the external nodes in order to provide a set of ROS actions and services for users. These external nodes include the quadrotor’s sensors and controllers, data from the parameter server, and a user interface. A good overview of the MoveIt! concepts is found here.

The general concept is to define groups of joints and other elements to perform moving actions using motion planning algorithms. These algorithms consider a scene with objects to interact with and the joint characteristics of the group. MoveIt! provides three primary interfaces for accessing the actions and services provided by the move_group node. The move_group_interface package provides a C++ interface. The moveit_commander package supports Python, and lastly, the Motion Planning Rviz plugin provides a GUI interface.

The move_group node communicates with the quadrotor using ROS topics and actions. The node receives the current state information such as the position and orientation of the quadrotor by listening to the /joint_states topic. Therefore, it is necessary to launch a joint_state_publisher node to broadcast the state of the quadrotor. The move_group node also receives global information about the quadrotor’s pose using the ROS TF library. The TF provides the transform between the base frame of the robot and the world or map frame. In order to publish this information, a robot_state_publisher node is executed. The move_group node interacts with the quadrotor’s control system through the FollowJointTrajectoryAction interface. As described later on this page, a server on the quadrotor need to service the move_group client in order to receive control commands output by the move_group node. The FollowJointTrajectoryAction action will be modified to accommodate the multi degree of freedom (DOF) dynamics of the quadrotor. Lastly, the move_group node maintains a planning scene using the quadrotor’s sensors as inputs. The planning scene is a representative of the quadrotor’s current state and its observed environment.

Lastly, the move_group node uses the ROS param server to get the configuration information. This includes the URDF of the quadrotor, the SRDF of the quadrotor and other configuration specific information including joint limits, motion planning variables, and sensor information. The SRDF as well as the configuration files are automatically generated using the MoveIt! setup assistant described in the next section.

The MoveIt! plugin interfaces provide access to several different motion planning libraries, primarily OMPL, accessible through a ROS action or service. OMPL (Open Motion Planning Library) is an open-source motion planning library that primarily implements randomized motion planners. MoveIt! uses the motion planners from that library as its primary/default set of planners.

The user initiates a motion plan request by setting a desired goal pose for the quadrotor and the motion planner computes a trajectory. The move_group node will generate a desired trajectory that moves the quadrotor to a desired pose without crashing into any obstacles identified by the sensors or violating the velocity and acceleration constraints of the quadrotor model.

MoveIt! Setup Assistant

The MoveIt Setup Assistant is used to configure the quadrotor with the MoveIt! framework. This GUI generates the SRDF for the quadrotor was well as other necessary configuration files for use with MoveIt!. A complete tutorial is available here, but this section will describe the steps necessary for integrating the quadrotor model specifically.

The MoveIt! Setup Assistant is launched using the following command.

roslaunch moveit_setup_assistant setup_assistant.launch

Initially, use the Create New MoveIt! Configuration Package button. Then, locate and load the kit_c_kinect.xacro file. The terminal will identify any errors or warnings found when trying to load the URDF file. You will notice the quadrotor model visualized in the panel on the right side of the GUI.

The next section generates the self-collision matrix. This can be found by clicking on the Self-Collisions pane on the left hand side of the GUI. The Default Self-Collision Matrix Generator searches for pairs of links on the quadrotor that can safely be disabled from collision checking, decreasing motion planning processing time. To run the generator, click on the Regenerate Default Collision Matrix button. The Setup Assistant will present the results of its computation in the main table. The Setup Assistant identifies 11 total links with 55 possible collisions. Accept the default 15 links that will be disabled since they are adjacent to each other.

Virtual joints are used primarily to attach the quadrotor to the world. We define only one virtual joint attaching the quadrotor’s base_link to the world frame. This virtual joint represents the motion of the base of the robot in a plane. This virtual joint will be a floating joint since the quadrotor is a multi degree of freedom object.

Planning groups are used for semantically describing different parts of the quadrotor, such as defining an end effector. For our quadrotor model, we don’t have an end effector but we define a planning group named Quad_base which contains the floating virtual_joint and the base_link. We can leave the kinematic solver set to none since we are treating the quadrotor as a simple single object.

The Setup Assistant allows you to add certain fixed poses into the configuration. This helps if, for example, you want to define a certain position as the home position but isn’t required for the quadrotor implementation since it is treated as a single object. Additionally, the Setup Assistant allows the user to designate certain planning groups as end effectors which allows some special operations to happen on them internally. This can also be skipped since the quadrotor doesn’t have an end effector.

The passive joint tab allows the user to specify any joints that the planners can’t kinematically plan for. Select the odometry sensor and IMU joints as passive joints.

The Configuration Files tab enables the user to create the file package needed to run the quadrotor with MoveIt!. It allows the user to specify individual files to be produced, but for the initial development of the quadrotor package, all the files should be generated.

Choose a location and name for the ROS package that will be generated containing your new set of configuration files. The Setup Assistant will now generate and write a set of launch and config files into the directory of your choosing. All the generated files will appear in the Generated Files/Folders tab and you can click on each of them for a description of what they contain.

The Setup Assistant creates the standard package.xml and CMakeLists.txt files for the package. It also sets up a launch folder with a series of launch files as well as a config folder with the following initial configuration files:

• quad.srdf (Representation of semantic information about the quadrotor)

• ompl_planning.yaml (Configures the OMPL planning plugin. For the planning groups defined in the SRDF, a number of planning configurations are specified)

• kinematics.yaml (Specifies which kinematic solver to use for each planning group as well as solver settings)

• joint_limits.yaml (Contains additional information about joints in the planning groups that aren’t in the URDF. This allows MoveIt! to assign realistic trajectories for the quadrotor’s controllers)

• fake_controllers.yaml (Creates dummy configurations for controllers that can be used for testing)

We want to update the joint_limits.yaml file with the relevant joint information. This includes adding both joint acceleration and velocity limits for the quadrotor. This not only makes the generated trajectories physically realistic, but also speeds up the planning process by eliminating the set of possible trajectories. Use this joint_limits.yaml as an example.

joint_limits:
  virtual_joint:
    has_velocity_limits: true
    max_velocity: 0.2
    has_acceleration_limits: true
    max_acceleration: 0.04

3D Perception Configuration

This section describes the process of configuring the quadrotor’s 3D sensors with MoveIt!. MoveIt! uses an octree based framework to represent the world around it. The Occupancy Map Updater is the primary component of MoveIt! that deals with 3D perception. The updater uses a plugin architecture to process different types of input. The currently available plugins in MoveIt! are:

• The PointCloud Occupany Map Updater: which can take as input point clouds (sensor_msgs/PointCloud2)

• The Depth Image Occupancy Map Updater: which can take as input Depth Images (sensor_msgs/Image)

This configuration is executed with the sensors_kinect.yaml configuration file. This file takes point clouds as inputs as shown below. An example file for processing depth images can be found here.

sensors: 
 - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
   point_cloud_topic: /quad/camera_/depth/points
   max_range: 10.0
   frame_subsample: 1
   point_subsample: 1

Next, it is necessary to update the quad_moveit_sensor_manager.launch file in the “launch” directory of your MoveIt! configuration directory with this sensor information. This file was auto-generated by the Setup Assistant but is initially empty. The code configures the set of sensor sources for MoveIt! to use pointing to the sensors_kinect.yaml file created previously.

<launch>
<rosparam command="load" file="$(find quad_3dnav)/config/sensors_kinect.yaml" />
</launch>

Lastly, the sensor_manager.launch file is updated. The first lines of code loads the robot specific sensor manager which sets the moveit_sensor_manager ROS parameter. We then configure the Octomap monitor by adding the Octomap frame, world, and resolution parameters for the Octomap representation.

The octomap_frame parameter specifies the coordinate frame in which this representation will be stored. This frame should be a fixed frame in the world. The octomap_resolution parameter specifies the resolution at which this representation is maintained (in meters). The max_range parameter specifies the maximum range value to be applied for any sensor input to this node.

<launch>
  <!-- This file makes it easy to include the settings for sensor managers -->  
  <!-- Params for the octomap monitor -->
  <param name="octomap_frame" type="string" value="world" />
  <param name="octomap_resolution" type="double" value="0.025" />
  <param name="max_range" type="double" value="5.0" />

  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
  <arg name="moveit_sensor_manager" default="quad" />
  <include file="$(find quad_3dnav)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
</launch>

The screenshot below depicts the final Octomap representation in RVIZ after the quadrotor completed navigating a pre-defined path in the kitchen world with a Kinect motion sensor attached.

Controller Configuration

This section describes the process of configuring MoveIt! with the controllers on the quadrotor. The MoveIt! package was originally created with the manipulation of robot arms in mind. Therefore, the software defaults to supporting gripper manipulations and joint trajectories and assumes the robot has controllers that can receive these forms of control inputs successfully. Since we are adapting the MoveIt! software for a multi degree of freedom quadrotor, we will have to make some slight additions to the default MoveIt! framework. This includes modifying the MoveIt! Simple Controller Manager and adding an Action Server to interface between MoveIt! and the quadrotor’s position controllers.

ROS uses services in situations when nodes need to exchange simple task requests and feedback between each other. For more complex situations, ROS utilizes the actionlib package to create servers that execute long-running tasks. This is useful for tasks that might take a long time to execute. This allows the user or a node to cancel the task request during execution or get periodic feedback about how the request is progressing.

This client-server framework is used in MoveIt! for pre-emptable tasks such as controller arm joints and grippers. The move_group node uses an action interface to communicate with the controllers on the quadrotor. The move_group node provides an action client to talk to the controller action server on the quadrotor.

ROS uses a predefined action protocol to enable communications between the action client and server. This action protocol relies on several ROS topics in a specified ROS namespace in order to transport messages.

• The goal topic uses an autogenerated ActionGoal message to send new goals to the action server.
• The cancel topic uses actionlib_msgs/GoalID messages to enable action clients to send cancel requests to an action server.
• The status topic uses actionlib_msgs/GoalStatusArray to allow action clients to receive status information about every goal currently being tracked by the action server.
• The feedback topic uses an autogenerated ActionFeedback message to provide servers a way to send periodic updates to action clients during the processing of a goal.
• The result topic uses an autogenerated ActionResult message to provides servers a way to send information to action clients upon completion of a goal.

MultiDofFollowJointTrajectory Action

ROS uses an action specification to define the Goal, Feedback, and Result messages that the action client and server need to communicate. The action specification is defined using a .action file. The action file consists of a goal definition section, feedback section, and result section, with each section separated by 3 hyphens (---). These files are placed in a package’s ./action directory.

Typically, MoveIt! relies on pre-defined action files such as FollowJointTrajectory and GripperCommand actions. Since MoveIt! wasn’t developed with quadrotor navigation in mind, we will define our own action file. The MultiDofFollowJointTrajectory.action file defines the goal, feedback, and result fields needed to enable MoveIt! to develop and transmit multi degree of freedom trajectories for the quadrotor to follow. The quadrotor is treated as a single multi degree of freedom joint. The action file defines the goal as a trajectory_msgs/MultiDOFJointTrajectory message, lists several result codes, and provides feedback in the form of the quadrotor’s starting pose, desired goal pose, and current pose along the trajectory. When the action file is built, 6 messages are automatically generated for client and server communication. These files can be viewed here.

MoveIt! Controller Configuration

Next, we must create a controllers.yaml file and save it in the MoveIt! config directory that was created with the MoveIt! Setup Assistant. This will specify the controller configuration for the quadrotor. The name parameter defines the name of the controller. The action_ns parameter is optional and defines the namespace for the controller. The type parameter defines the type of action being used by the controller. This will be the MultiDofFollowJointTrajectory action. The default parameter specifies if the controller is the primary controller chosen by MoveIt! for communicating with a particular set of joints. Lastly, the joint parameter names of all the joints that are being addressed by this interface. For the quadrotor, this is simply the virtual_joint.

controller_list:
  - name: multi_dof_joint_trajectory_action
    type: MultiDofFollowJointTrajectory
    default: true
    joints:
      - virtual_joint

Next, create the controller launch file called quad_moveit_controller_manager.launch in the launch directory created by the MoveIt! Setup Assistant. The code below loads the MoveIt!SimpleControllerManager node and the list of controllers previously defined in the controllers.yaml file.

<launch>
 <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
 <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
 <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
 <!-- load controller_list -->
 <rosparam file="$(find quad_3dnav)/config/controllers.yaml"/>
</launch>

MoveIt! Simple Controller Manager

As described previously, MoveIt! does not currently come with the ability to develop multi degree of freedom trajectories required to take full advantage of the quadrotor’s dynamics. Instead, we will use a modified version of the Moveit_simple_controller_manger to handle the MultiDofFollowJointTrajectoryAction to interface with the Action Server we will develop in the next section. This package was developed by Alessio Tonioni. The package can be downloaded via GitHub and more information is available in his thesis. The package modifies the moveit_simple_controller_manager.cpp to recognize the MultiDofFollowJointTrajectory action type. The package also include the multi_dof_follow_joint_trajectory_handle.h file which uses the action_controller/MultiDofFollowJointTrajectoryAction interface.

Action Server

Lastly, we develop a simple action controller to translate the multi degree of freedom trajectories produced by MoveIt! into a series of desired CommandTrajectory messages for the quadrotor. This action server interfaces with the MoveIt! action client with the MultiDofFollowJointTrajectory.action. The majority of this package was developed by Alessio Tonioni. The package can be downloaded via GitHub. Modifications of the action server code were made to support the quadrotor’s specific position controller architecture.

The action server, actionController.cpp, receives the trajectory output from the move_group node that will successfully navigate the quadrotor from its initial pose to the desired pose. This trajectory takes the form of a trajectory_msgs/MultiDOFJointTrajectory message. This message consists of a MultiDOFJointTrajectoryPoint[] points message that represent a multi degree of freedom trajectory as a series of points along the trajectory. Each point is a 6 degree of freedom geometry_msgs/Transform message that includes an array of positions, velocities, and accelerations. It also indicates the order of that point along the trajectory with a duration time_from_start value. The action server receives the entire trajectory from the move_group node and then publishes each transform as a desired waypoint that is sent to the waypoint_publisher node. The action server maintains a timer and sends each waypoint once the duration value associated with each point on the trajectory is reached. A button mapped on the user’s joystick switched the quadrotor’s position control system into 3D navigation mode and the quadrotor executes the desired trajectory.

Launching MoveIt!

As discussed previously in the MoveIt! Setup Assistant section, the Setup Assistant creates default launch files in addition to configuration files. The primary launch files is the move_group.launch file. This file allows the user to set variables for each of the subsequent launch files called by move_group.launch.There are a few variables that we want to be aware of. Setting the variable allow_trajectory_execution to true enables the execution of paths. Also, setting fake_execution to false results in our quadrotor controller manager is launched and not the default fake one.

The move_group.launch file is incorporated into the quad_3dnav.launch file. This file launches the quadrotor mode, gazebo, and the control systems as described previously. Here we also launch the action_controller node that runs our action server. After launching the move_group node, we launch that RVIZ with the MoveIt! GUI plugin to interact with the move_group node.

<!--rosrun action_controller action_controller -->
  <node name="action_controller" pkg="action_controller" type="action_controller" ></node>

  <!-- Run the main MoveIt executable -->
  <include file="$(find quad_3dnav)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="false"/>
    <arg name="info" value="false"/>
    <arg name="debug" value="$(arg debug)"/>
    <arg name="db" default="true" />
  </include>

  <!-- Visualization RVIZ -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find quad_3dnav)/resource/moveit.rviz" output="screen"/>

RVIZ GUI Manipulation

MoveIt! comes with a plugin for RVIZ allows users to setup scenes in which the quadrotor will work, generate plans, visualize the output and interact directly with a visualized quadrotor. The quad_3dnav.launch file will load the moveit.rviz configuration file which is pre-configured with the MoveIt! plugin. If not, you can find the Motion Planning plugin in the Disaplys tab under the moveit_ros_visualization folder, labeled as  “MotionPlanning.”

Before interacting with the GUI, the following parameters need to be set:

• In the “Global Options” tab of the “Displays” subwindow, set the Fixed Frame field to “world”

Now, you can start configuring the Plugin for your quadrotor. Click on “MotionPlanning” in “Displays”

• Make sure the Robot Description field is set to “robot_description”
• Make sure the Planning Scene Topic field is set to “monitored_planning_scene”.
• In Planning Request, change the Planning Group to “Quad_base”.
• Set the Trajectory Topic in the Planned Path tab to “/move_group/display_planned_path”

There are additional visualizations specific to the path planning and the planning scene. These can be toggled using the checkboxes.

• The start state for path planning using the “Query Start State” checkbox in the “Planning Request” tab.
• The goal state for path planning using the “Query Goal State” checkbox in the “Planning Request” tab.
• The quadrotor’s configuration in the planning scene/planning environment using the “Show Scene Robot” checkbox in the “Scene Robot” tab.
• The planned path using the “Show Robot Visual” checkbox in the “Planned Path” tab.

Next, specify the planning algorithm that you want to use. This can be selected in the OMPL dropdown menu from the Context tab in the GUI. After the algorithm is specified, you can select the desired start and end states for the trajectory by manipulating the interactive markers. You can drag these markers around to set the desired start and goal poses for the quadrotor.

Now, you can start motion planning with the quadrotor in the MoveIt! Rviz Plugin.

• Move the Start State to a desired location (optional).
• Move the Goal State to another desired location.
• Make sure both states are not in collision with the quadrotor itself.
• Make sure the Planned Path is being visualized. Also check the “Show Trail” checkbox in the Planned Path tab.
• In the Planning tab, press the Plan button. You should be able to see a visualization of the quadrotor moving and a trail.

Below is another example of a trajectory developed to allow the quadrotor to reach a desired goal pose without colliding with the kitchen island obstacle.

Once the user is satisfied with the computed trajectory, the user can select the Execution button to send the trajectory to the quadrotor. If the quadrotor is operating in the 3D navigation mode, it will receive the trajectory as a series of waypoints and follow them to the goal state.

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

RTAB-Map Package

The Real-Time Appearance-Based Mapping package is a RGB-D SLAM approach based on a global loop closure detector with real-time constraints. This package can be used to generate a 3D point clouds of the environment and/or to create a 2D occupancy grid map for navigation. The tutorials and demos show some examples of mapping with RTAB-Map and it can be installed via the ros-indigo-rtabmap-ros package.

The RTAB-Mapviz node starts the visualization interface of RTAB-Map. It is a wrapper of the RTAB-Map GUI library. It has the same purpose as rviz but with specific options for RTAB-Map. The screenshot below depicts the final map in the RTAB-Map visualizer after the quadrotor navigated the WillowGarage world.

The graph below depicts the nodes and topics required for the integration of RTAB-Map and Octomap and the development of a map of the WillowGarage environment.