The MoveIt! package we've created is able to provide the necessary ROS services and actions in order to plan trajectories, but it isn't able to pass these trajectories to the real robot to execute. All the plans we've done were executed in an internal simulator that MoveIt! provides. In order to communicate with the real drone, it will be necessary to do some modifications to the MoveIt! package we created at the beginning of this section.

But before that, we need to learn a couple of things about how MoveIt! works.

The primary node used in MoveIt! is the move_group node. As shown in the system architecture in the following diagram, 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 sensors and controllers, data from the parameter server, and a user interface. We can read more about MoveIt! concepts here:

http://moveit.ros.org/documentation/concepts/

The system architecture appears as follows:

The move_group node communicates with the drone using ROS topics and actions. The node receives the current state information, such as the position and orientation of the drone, by listening to the /joint_states topic. Therefore, it will be necessary to launch a joint_state_publisher node to broadcast the state of the drone. The move_group node also receives global information about the quadrotor's pose using the ROS TF library. The TF provides the transformation 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. But, as we have already mentioned at the beginning of the section, the MoveIt! package was originally created with the manipulation of robot arms in mind, which is not what we are doing here. Therefore, the FollowJointTrajectoryAction action will have to be modified to accommodate the multi-DOF dynamics of the drone. This new action will be called MultiDofFollowJointTrajectory. In this system, this action is already provided, so we don't have to bother creating it. We just have to configure MoveIt! to use it. If we want to check out more information about it, we can have a look at the following repository, developed by Alessio Tonioni:

https://github.com/AlessioTonioni/Autonomous-Flight-ROS

Great! So… with all this new knowledge, let's begin doing some proper modifications in order to connect the MoveIt! package to the real drone.

First of all, we will have to create a file to define how we will control the joints of our real robot. Inside the config folder of our MoveIt! package, we will create a new file named controllers.yaml and copy the following content into it:

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

So, basically, here we are defining the action server that will be used for controlling the joints of our robot. In this case, the only joint we have on our drone is the virtual_joint. And the action server we are going to use in order to control it is the MultiDofFollowJointTrajectory, as explained previously. The name parameter defines the name of the controller, and the default parameter specifies if the controller is the primary controller chosen by MoveIt! for communicating with a particular set of joints.

Next, we'll have to create a file to define the limits of the joints of the drone. For our drone, we are going to limit its maximum velocity and acceleration. Again, inside the config folder, create a new file called joint_limits.yaml, and copy the following content into it:

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

Now, if we open smart_grasping_sandbox_moveit_controller_manager.launch.xml, which is inside the launch directory, we will find it empty and so copy the following content into it:

<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 ardrone_moveit_config)/config/controllers.yaml"/> 
</launch> 

What we are doing here is basically loading the controllers.yaml file we have just created, and the MoveItSimpleControllerManager plugin, which will allow us to send the plans calculated in MoveIt! to our real robot—in this case, the simulated drone.

Finally, we will have to create a new launch file that sets up the entire system to control our robot. So, inside the launch folder, create a new launch file called ardrone_navigation.launch:

<launch> 
  <param name="use_sim_time" value="true" /> 
  <!-- Take the name of the package in which config is stored--> 
  <arg name="config_pkg" default="$(find ardrone_moveit_config)" /> 
 
  <!-- By default, we are not in debug mode --> 
  <arg name="debug" default="false" /> 
 
  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->

  <include file="$(arg config_pkg)/launch/planning_context.launch"> 
    <arg name="load_robot_description" value="true"/> 
  </include> 
   
  <node pkg="tf" type="static_transform_publisher" name="world_to_footprint" args="0 0 0 0 0 0 world odom 5" /> 
  <node pkg="tf" type="static_transform_publisher" name="odom_to_nav" args="0 0 0 0 0 0 odom nav 5" /> 
  <node pkg="tf" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 base_footprint base_link 5" /> 
  <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 map world 5" /> 
 
  <!-- We do not have a robot connected, so publish fake joint states --> 
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"> 
    <param name="/use_gui" value="false"/> 
    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam> 
  </node> 
 
  <!-- Given the published joint states, publish tf for the robot links --> 
 <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" /> 
 
 <node name="action_controller" pkg="action_controller" type="action_controller" ></node> 
 
  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) --> 
  <include file="$(arg config_pkg)/launch/move_group.launch"> 
    <arg name="allow_trajectory_execution" value="true"/> 
    <arg name="fake_execution" value="true"/> 
    <arg name="info" value="true"/> 
    <arg name="debug" value="$(arg debug)"/> 
  </include> 
 
  <!-- Run Rviz and load the default config to see the state of the move_group node --> 
  <include file="$(arg config_pkg)/launch/moveit_rviz.launch"> 
    <arg name="config" value="true"/> 
    <arg name="debug" value="$(arg debug)"/> 
  </include> 
</launch> 

Finally, we are starting the launch files that we need in order to set up the MoveIt! environment. The most important things here are the following:

<node pkg="tf" type="static_transform_publisher" name="world_to_footprint" args="0 0 0 0 0 0 world odom 5" /> 
  <node pkg="tf" type="static_transform_publisher" name="odom_to_nav" args="0 0 0 0 0 0 odom nav 5" /> 
  <node pkg="tf" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 base_footprint   base_link 5" /> 
  <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 map world 5" /> 

Here, we are publishing some transforms that we need, since we are publishing our OctoMap in an unusual way. Also, we are connecting some links that are not connected by default. This is because, as we've been saying throughout the chapter, MoveIt! is not a tool that was initially created to navigate drones:

<node name="action_controller" pkg="action_controller" type="action_controller" ></node> 

We are starting the MultiDofFollowJointTrajectory action server that we talked about in the beginning of this section:

<include file="$(arg config_pkg)/launch/move_group.launch"> 
    <arg name="allow_trajectory_execution" value="true"/> 
    <arg name="fake_execution" value="true"/> 
    <arg name="info" value="true"/> 
    <arg name="debug" value="$(arg debug)"/> 
  </include> 

This is the main MoveIt! launch, which launches all of the MoveIt! environments.

Great! We have finished all of the MoveIt! setup in order to be able to calculate trajectories for our drone within an environment that is represented by the OctoMap. But let's do a very quick summary in order to organize all the recent information that we've received.

Typically, MoveIt! relies on predefined action files, such as FollowJointTrajectory and GripperCommand actions. Since MoveIt! wasn't developed with a drone's navigation in mind, we have defined our own action. The MultiDofFollowJointTrajectory action defines the goal, feedback, and result fields needed to enable MoveIt! to develop and transmit multi-DOF trajectories for the drone to follow. This action is already built into the system, so we don't have to worry about it.

The quadrotor is treated as a single multi-DOF joint. Thus, we defined the controllers.yaml file, in order to tell MoveIt! to use this action. This action generates trajectories in the form of a set of waypoints that the drone has to follow.

Now what we'll have to do is add some kind of code that is capable of reading these waypoints and transforming them into real movements by the drone.

We will discuss executing the trajectory with the real drone in next section, after getting familiar with ROS packages available for most popular real drones, such as Parrot and Bebop.