3D Mapping & Navigation


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.

ROS Gazebo Quadrotor Simulation Kitchen Environment


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.

ROS MoveIt! Package System Architecture Overview


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.

ROS MoveIt! Setup Assistant Collision Checking GUI
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.

ROS MoveIt! Setup Assistant Virtual Joints GUI


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.

ROS MoveIt! Setup Assistant Planning Groups GUI


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.

ROS MoveIt! Setup Assistant Passive Joints GUI


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.

ROS MoveIt! Setup Assistant Configuration Files GUI


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.

    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)

ROSE MoveIt! Perception System Architecture Overview

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.

 - 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.

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

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.

  <!-- 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" />

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.

ROS RVIZ Quadrotor Generated Octomap


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 MoveIt! Controller Configuration Overview


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.

ROS Action Interface Overview


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.

  - name: multi_dof_joint_trajectory_action
    type: MultiDofFollowJointTrajectory
    default: true
      - 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.

 <!-- 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"/>

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" />

  <!-- 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.

ROS RVIZ GUI Configuration for 3D Navigation


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.

ROS RVIZ 3D Path Planning Initialization GUI


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.

ROS RVIZ Quadrotor 3D Path Planning Simulation


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.

ROS RTAB-Map 3D Mapping Quadrotor Simulation


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.

ROS RTAB-Map Simulation Node Graph


39 thoughts on “3D Mapping & Navigation

  1. Hello,
    I need some help. When I load the mentioned urdf into movite assistant manager, I can see only the kinect but not the drone body. Obviously there is some error. Can you help me with this problem?

      1. Thanks for your reply wil. It helped. I could like the xacro files. I sould have gone throught the post thoroughly. But now I have another issue. I dont seem to understand where the MultiDofFollowJointTrajectory.action files goes. When I run quad_3dnav.launch, I get the error
        [ERROR] [1498202167.465073750]: Unknown controller type: ‘MultiDofFollowJointTrajectory’

        Can you help?

        Thanks for your valuible time again.

  2. Hi!
    I have two queries.
    1. When I try to launch the quad_3dnav.launch I get the following error.
    [ERROR] [1491480854.038203647, 1.613000000]: Group ‘Quad_base’ is not a chain
    [ERROR] [1491480854.038343206, 1.613000000]: Kinematics solver of type ‘kdl_kinematics_plugin/KDLKinematicsPlugin’ could not be initialized for group ‘Quad_base’
    [ERROR] [1491480854.039022315, 1.613000000]: Kinematics solver could not be instantiated for joint group Quad_base.

    2. I want run this simulation using moveit with Parrot AR Drone 2.0 for path planning. How can modify the kit_c_kinect.xacro file for doing this simulation on a AR Drone.


    1. Ajay,

      1) Try running the MoveIt! Setup Assistant with the kinematic solver set to none since we are treating the quadrotor as a simple single object. I think the repo files were outdated and I left a kinematic solver in there to generate those.

      2) In the kit_c_kinect.xacro file, you could update the parameters of the IMU and GPS sensor, remove the Kinect since the AR doesn’t have that, and add a plain camera. If you want to make the simulated quadrotor’s dimensions match the AR, you need to update kit_c.xacro. Lastly, if you want the visualization to match the AR, you would need to find that meshfile. For example, my simulation uses the hummingbird.dae file, as referenced in the kit_c.xacro file.

  3. I’m getting this error when running roslaunch quad_3dnav quad_3dnav.launch:

    [ERROR] [1491878808.141111216]: Client [/move_group] wants topic /multi_dof_joint_trajectory_action/feedback to have datatype/md5sum [action_controller/MultiDofFollowJointTrajectoryActionFeedback/bb90301ebf9ac17d9c3bcafa7a141cba], but our version has [action_controller/MultiDofFollowJointTrajectoryActionFeedback/0611ea4a62fe3c6e8470a86482016c70]. Dropping connection.
    [ERROR] [1491878808.243564351]: Client [/action_controller] wants topic /multi_dof_joint_trajectory_action/goal to have datatype/md5sum [action_controller/MultiDofFollowJointTrajectoryActionGoal/762be92da0a19418188f1bfa9dab9fe5], but our version has [action_controller/MultiDofFollowJointTrajectoryActionGoal/1292bbe002476592a0f3166376533ba1]. Dropping connection.

    RViz says “NO PLANNING LIBRARY LOADED” in the Context tab under Planning Library, and the scene is empty except for the robot in the center. Any ideas here?

    It’s my first time working with ROS, help is greatly appreciated!

    1. Austin,

      For the error message, make sure you have the most up to date package versions. I had that same issue recently and solved it with:
      sudo apt-get update
      sudo apt-get upgrade

      When you first run the quad_3dnav.launch file, it should call the following files in descending order. Ensure you have all these as well:

      1. Awesome, I did that and remade the workspace, and now I’ve got the planning libraries loaded. One more problem, if you don’t mind me bothering you some more. In RViz, now I’m getting, under Global Status, “Global Status: Error > Fixed Frame > Fixed Frame [world] does not exist”. Any ideas here?

        1. Austin,

          Have you checked out your TF tree? It should have a connection “world” broadcast by Gazebo and connected to “base_link”. This is defined in the component_snippets.xacro file in the odometry_plugin_macro. Specifically, the parent_frame_id=”world” line. This should match the same line in the kit_c_base.xacro file as well.

          Make sure that in quad_3dnav.launch you are spawning the quadrotor model with the file spawn_quad_kinect_kitchen.launch. In spawn_quad_kinect_kitchen.launch, make sure the enable_ground_truth parameter is set to “true”. The spawn_quad_kinect_kitchen launch file then loads the kit_c_base.xacro file which also loads the component_snippets.xacro file.

      2. I don’t know if there exists anyone other than me who tries to get up and running this project on ROS Kinetic. However, I have encountered the exact same problem and the solution was not simply an update&upgrade sequence.

        Actually, this problem directly relates to action_controller package in which the custom message depends on moveit_msgs/MultiDOFJointTrajectory. If one tries to compile this on ROS Kinetic, no errors will be encountered. Although, the error Austin has posted occurs in run-time. The reason is, since ROS Kinetic, MultiDOFJointTrajectory and similar messages are moved to trajectory_msgs package. Therefore, changing every occurence of moveit_msgs with trajectory_msgs in the message definition of action_controller package has resolved the problem for me.

        I can also push this simple fix onto the project repository for a decent migration to Kinetic.

  4. Hi, thanks for your code.
    I have met a problem:
    when i use ‘roslaunch quad_3dnav quad_3dnav.launch’ to run the code.
    everything seems to go well.
    when i plan the robot, it succeed.
    but when i pressed the execution in rviz, the program crashed. and it shows:
    [move_group-13] process has died [pid 29634, exit code -11, cmd /opt/ros/indigo/lib/moveit_ros_move_group/move_group __name:=move_group __log:=/home/rain/.ros/log/c4085b46-265f-11e7-a16d-64006a25a949/move_group-13.log].
    log file: /home/rain/.ros/log/c4085b46-265f-11e7-a16d-64006a25a949/move_group-13*.log

    so could you give me some advices?

    1. Pyni,

      That error is pretty cryptic. Is there more information you can provide from the log file it pointed you to?

        1. So you were able to create the octomap with the pre-planned waypoint mission followed by planning the route and visualizing it? The problem only happened when you hit the execute button? Sounds like it might be an issue with the action controller. Check and make sure controllers.yaml has type: MultiDofFollowJointTrajectory and that you have the MultiDofFollowJointTrajectory files found in the action_controller package.

          1. Thanks for the response!
            Yes I have created the octomap with waypoints and then moveit plans the path. However error occurs when executing. Here, you may see the screenshots of Rviz, Gazebo and my node graph: http://imgur.com/a/QGYBX

            the controllers.yaml file has;
            – name: multi_dof_joint_trajectory_action
            type: MultiDofFollowJointTrajectory
            default: true
            – virtual_joint

            I built the source that you provide on github so, the MultiDofFollowJointTrajectory files are there.

            Do I need to go over all the steps and regenerate action_controller and Moveit Setup or building the source avaliable is enough?

            Thanks again, it is so critical for my research. Waiting for your suggestions.

          2. I updated all my packages, remade the repo, and rebuilt the MoveIt! Configuration files and am having the exact same error. I tried looking around for others that have that error and didn’t see any clear solution. I would suggest completing one of the basic MoveIt! tutorials to see if that works. It may be a problem with the custom action file, but since the error looks like it is from a MoveIt! library file, I think something broke while updating the MoveIt! packages from when I originally created this software.

            I’m sorry I can’t be more helpful. Please do let me know if you find a solution and I will do the same.

          3. I’ve implemented the solution found by Troy and updated the repository with the new trajectory_execution.launch.xml file. The system now plans and executes trajectories on my local machine. If you are using my joystick interface, you need to enable 3D autonomous control with the RB button on an Xbox controller.

          4. Sorry, it won’t let me comment at the end of this thread though I was hoping one of you had solved this I have just encountered this today. To add to kolormatik’s comment the backtrace gives me the following, I will dig a bit further later.

            Thread 12 “move_group” received signal SIGSEGV, Segmentation fault.
            [Switching to Thread 0x7fffb7cc8700 (LWP 15707)]
            0x00007ffff4f85704 in trajectory_execution_manager::TrajectoryExecutionManager::validate(trajectory_execution_manager::TrajectoryExecutionManager::TrajectoryExecutionContext const&) const ()
            from /home/cor231/catkin_ws/devel/lib/libmoveit_trajectory_execution_manager.so.0.9.8
            (gdb) where
            #0 0x00007ffff4f85704 in trajectory_execution_manager::TrajectoryExecutionManager::validate(trajectory_execution_manager::TrajectoryExecutionManager::TrajectoryExecutionContext const&) const ()
            from /home/cor231/catkin_ws/devel/lib/libmoveit_trajectory_execution_manager.so.0.9.8
            #1 0x00007ffff4f89d43 in trajectory_execution_manager::TrajectoryExecutionManager::execute(boost::function const&, boost::function const&, bool) () from /home/cor231/catkin_ws/devel/lib/libmoveit_trajectory_execution_manager.so.0.9.8
            #2 0x00007ffff4f89e4c in trajectory_execution_manager::TrajectoryExecutionManager::execute(boost::function const&, bool) ()
            from /home/cor231/catkin_ws/devel/lib/libmoveit_trajectory_execution_manager.so.0.9.8
            #3 0x00007fffb7d808b6 in move_group::MoveGroupExecuteTrajectoryAction::executePathCallback_Execute(boost::shared_ptr<moveit_msgs::ExecuteTrajectoryGoal_<std::allocator > const> const&, moveit_msgs::ExecuteTrajectoryResult_<std::allocator >&) () from /home/cor231/catkin_ws/devel/lib/libmoveit_move_group_default_capabilities.so
            #4 0x00007fffb7d80dd1 in move_group::MoveGroupExecuteTrajectoryAction::executePathCallback(boost::shared_ptr<moveit_msgs::ExecuteTrajectoryGoal_<std::allocator > const> const&) ()
            from /home/cor231/catkin_ws/devel/lib/libmoveit_move_group_default_capabilities.so
            #5 0x00007fffb7d9192c in actionlib::SimpleActionServer<moveit_msgs::ExecuteTrajectoryAction_<std::allocator > >::executeLoop() ()
            from /home/cor231/catkin_ws/devel/lib/libmoveit_move_group_default_capabilities.so
            #6 0x00007ffff32f45d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
            #7 0x00007ffff64f36ba in start_thread (arg=0x7fffb7cc8700) at pthread_create.c:333
            #8 0x00007ffff57af82d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

          5. Thank you for this code
            sorry, it won’t let me post at the end of the thread but I have asked this question of the moveit git repo and a partial fix has been provided https://github.com/ros-planning/moveit/issues/539#issuecomment-311376563.

            This has got me up and running I am now seeing that the trajectories only have translation and not velocity and acceleration. As part of my project, I would like to implement these. Is this a flag that I need to set in moveit? or a change that needs to be made in the controller.

            thank you for any help

          6. Troy,

            Thank you for discovering that work around. Code works on my local machine and I updated the repository.

            I only implemented a position controller for the quadrotor which then outputs desired attitude and thrust for the lower level attitude and angular rate controllers. Therefore, I don’t have a need for velocity or acceleration controllers. I know labs like UPenn have executed some pretty incredible trajectories using acceleration controllers so feel free to fork and implement those more advanced control systems.

            All of my controllers are written as separate nodes in the quad_control package.

            Thanks again for the assistance!

  5. How to create that map? You say that once the map is created when the quad finishes a predefined path in the kitchen scene with a camera. but how do i get that working?
    Also, I don’t have a joystick, could you please point me to some resources allowing me to build a keyboard controller for the drone.

    1. Ridhwan,

      For 2D maps the quad_slam_laser_map.launch file was used.OpenSlam’s Gmapping algorithm was used to create the 2D map.The node subscribes to the laser scan message and outputs a map in the form of a OccupancyGrid message. The map_server node is used to save dynamically generated maps to a pair of files. This is all detailed in the 2D Navigation section.

      For 3D occupancy grid maps, 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 configuration is executed with the sensors_kinect.yaml configuration file. 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.

      For the keyboard input, check out turtlebot_key.cpp as a good template.

  6. Hi
    I got everything working. MoveIt is planning trajectories too! But sometimes the trajectories have states where the quadrotor has to do 360 degree flip. How do i change this so that the planner just gives normal paths towards a goal point.

    1. Glad to hear you got up and running! I haven’t seen that behavior before, that’s interesting. The first place I would look is the joint_limits.yaml file. Specific joint properties can be changed with the keys max_position, min_position, max_velocity, max_acceleration.

  7. wiselby,can I use my own urdf file with the Moveit setup assistant to achieve 3D navigation,my robot is a Multi-rotor UAS equipped with a multi-joint manipulator arm,and I wonder if I can use your way to my own robot?

  8. Hello there,

    I am very new to ROS, during the catkin_make i received a lot of error regarding the glog_catkin. It shows the following error messages:

    glog_catkin/CMakeFiles/glog_src.dir/build.make:101: recipe for target ‘glog_catkin/glog_src-prefix/src/glog_src-stamp/glog_src-patch’ failed
    make[2]: *** [glog_catkin/glog_src-prefix/src/glog_src-stamp/glog_src-patch] Error 2
    make[2]: *** Waiting for unfinished jobs….
    CMakeFiles/Makefile2:450: recipe for target ‘glog_catkin/CMakeFiles/glog_src.dir/all’ failed
    make[1]: *** [glog_catkin/CMakeFiles/glog_src.dir/all] Error 2
    Makefile:138: recipe for target ‘all’ failed
    make: *** [all] Error 2

    and i tried to change the CMakeLists.txt on the glog, I changed the github.com in the 13th line to but still no luck. any idea? thank you

  9. Hello,
    I am able to start up the things and mapped the required xbox controller keys to keyboard. Now how I could save the 3d octomap created so that I can reuse every time. Also I want to write a script that will give the points to start planning and execute the path. How I can do that?

    1. Hello!

      I have implemented Waypoint control so that you can pre-record a series of waypoints and have the quadrotor navigate to those waypoints in the prescribed order. You can see more info here.

      I built the Octomap each time but you can see this discussion for assistance on saving and re-loading an Octomap.

  10. Hello,

    I install every package, and I am trying to run quad_3dnav. the problem is try can make a plan, then execute it, but the robot not move. You said that “If the quadrotor is operating in the 3D navigation mode, it will receive the trajectory”. My question is, how to turn on 3D navigation mode? I don’t have Xbox controller, but I try to publish message:

    rostopic pub /quad/motors mav_msgs/Motopeed “header:
    seq: 0
    secs: 0
    nsecs: 0
    frame_id: ”
    – 1″

    the drone still not move

    1. When using the Xbox controller, I set it up so that a button needs to be pressed to enable 3D Navigation mode. I copied the line here below so you will need some way of changing this variable.

      pnh.param(“button_ctrl_3dnav”, buttons_.ctrl_enable_3dnav, 4);

      After that, I would check the MoveIt! output. It should be Twist messages. These then get turned into waypoints for the quadrotor to follow.

  11. Greetings,
    Dear Professor Wil Selby, I am a computer science student from UK, working on an autonomous navigation project.
    I have modified the actionController.cpp in order to achieve the path planning for the kinetic branch of rotorS simulator . However, when I started the launch file, I get the error message follows:
    Action client not connected: multi_dof_joint_trajectory_action,
    and I did not see any action client was called within the actionController.cpp
    only simple_action_client.h was included implicitly within moveit_simple_controller_manager.cpp.
    I would like to know Does the actionController suppose to work without any action client configuration, or there are more modifications about action client need to be done ?

    Thank you very much for your help.

  12. Dear Professor Wil Selly,

    First of all thanks for this code, it helped me a lot to understand ROS and “octomapping” concepts.

    The thing that I’m tracking now is how to modify the quad orientation. I noticed that while I’m using the goal tool from the RVIz plugin, the robot executes the correct trajectory but sadly it appers that it don’t give care about the orientation just the position.

    For example if I set the goal position with the same X,Y and Z and just execute a 180 degree turn, the robot quad keeps in the same position, it don’t execute the turn.

    this is not so good because the aim purpose we loose the capacity to map the environment.

    I dont know if i did something wrong about the controllers, I followed exactly this tutorial.

    Thanks in advance.

  13. Could you please help me to solve the following issue:
    Unable to parse XACRO file:
    It has been downloaded “https://raw.githubusercontent.com/wilselby/ROS_quadrotor_simulator/master/quad_description/urdf/kit_c_kinect.xacro”
    thanks in advance

  14. Hello wilselby, thank you for documenting and sharing the amazing work. I just wanted to know if urdf file in your repository is possible to use with VREP ROS and Moveit!?
    I have 3D octomap from octomap_server ros package and i wanted to apply collision avoidance and control but moveit require urdf which i can’t find.
    Thank you again for the wonderful work!

Comments are closed.