Merge pull request #57 from AutonomyLab/add-auto-flight

Add autonomous flight plan capabilities to driver

bebop_driver/include/bebop_driver/bebop.h

@@ -162,6 +162,9 @@ class Bebop
   void FlatTrim();
   // false: Stop, true: Start
   void NavigateHome(const bool& start_stop);
+  void StartAutonomousFlight(const std::string &filepath);
+  void PauseAutonomousFlight();
+  void StopAutonomousFlight();
   void AnimationFlip(const uint8_t& anim_id);
 
   // -1..1

bebop_driver/include/bebop_driver/bebop_driver_nodelet.h

@@ -35,6 +35,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 #include <std_msgs/Empty.h>
 #include <std_msgs/UInt8.h>
 #include <std_msgs/Bool.h>
+#include <std_msgs/String.h>
 #include <camera_info_manager/camera_info_manager.h>
 #include <image_transport/image_transport.h>
 #include <dynamic_reconfigure/server.h>
@@ -115,6 +116,9 @@ class BebopDriverNodelet : public nodelet::Nodelet
   ros::Subscriber reset_sub_;
   ros::Subscriber flattrim_sub_;
   ros::Subscriber navigatehome_sub_;
+  ros::Subscriber start_autoflight_sub_;
+  ros::Subscriber pause_autoflight_sub_;
+  ros::Subscriber stop_autoflight_sub_;
   ros::Subscriber animation_sub_;
   ros::Subscriber snapshot_sub_;
   ros::Subscriber toggle_recording_sub_;
@@ -151,6 +155,9 @@ class BebopDriverNodelet : public nodelet::Nodelet
   void EmergencyCallback(const std_msgs::EmptyConstPtr& empty_ptr);
   void FlatTrimCallback(const std_msgs::EmptyConstPtr& empty_ptr);
   void NavigateHomeCallback(const std_msgs::BoolConstPtr& start_stop_ptr);
+  void StartAutonomousFlightCallback(const std_msgs::StringConstPtr& file_path_ptr);
+  void PauseAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr);
+  void StopAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr);
   void FlipAnimationCallback(const std_msgs::UInt8ConstPtr& animid_ptr);
   void TakeSnapshotCallback(const std_msgs::EmptyConstPtr& empty_ptr);
   void ToggleRecordingCallback(const std_msgs::BoolConstPtr& toggle_ptr);

bebop_driver/src/bebop.cpp

@@ -475,6 +475,30 @@ void Bebop::NavigateHome(const bool &start_stop)
         "Navigate home failed");
 }
 
+void Bebop::StartAutonomousFlight(const std::string &filepath)
+{
+  ThrowOnInternalError("Start autonomous flight failed");
+  ThrowOnCtrlError(
+        device_controller_ptr_->common->sendMavlinkStart(device_controller_ptr_->common, const_cast<char*>(filepath.c_str()), (eARCOMMANDS_COMMON_MAVLINK_START_TYPE)0),
+        "Start autonomous flight failed");
+}
+
+void Bebop::PauseAutonomousFlight()
+{
+  ThrowOnInternalError("Pause autonomous flight failed");
+  ThrowOnCtrlError(
+        device_controller_ptr_->common->sendMavlinkPause(device_controller_ptr_->common),
+        "Pause autonomous flight failed");
+}
+
+void Bebop::StopAutonomousFlight()
+{
+  ThrowOnInternalError("Stop autonomous flight failed");
+  ThrowOnCtrlError(
+        device_controller_ptr_->common->sendMavlinkStop(device_controller_ptr_->common),
+        "Stop autonomous flight failed");
+}
+
 void Bebop::AnimationFlip(const uint8_t &anim_id)
 {
   ThrowOnInternalError("Animation failed");

bebop_driver/src/bebop_driver_nodelet.cpp

@@ -126,7 +126,10 @@ void BebopDriverNodelet::onInit()
   land_sub_ = nh.subscribe("land", 1, &BebopDriverNodelet::LandCallback, this);
   reset_sub_ = nh.subscribe("reset", 1, &BebopDriverNodelet::EmergencyCallback, this);
   flattrim_sub_ = nh.subscribe("flattrim", 1, &BebopDriverNodelet::FlatTrimCallback, this);
-  navigatehome_sub_ = nh.subscribe("navigate_home", 1, &BebopDriverNodelet::NavigateHomeCallback, this);
+  navigatehome_sub_ = nh.subscribe("autoflight/navigate_home", 1, &BebopDriverNodelet::NavigateHomeCallback, this);
+  start_autoflight_sub_ = nh.subscribe("autoflight/start", 1, &BebopDriverNodelet::StartAutonomousFlightCallback, this);
+  pause_autoflight_sub_ = nh.subscribe("autoflight/pause", 1, &BebopDriverNodelet::PauseAutonomousFlightCallback, this);
+  stop_autoflight_sub_ = nh.subscribe("autoflight/stop", 1, &BebopDriverNodelet::StopAutonomousFlightCallback, this);
   animation_sub_ = nh.subscribe("flip", 1, &BebopDriverNodelet::FlipAnimationCallback, this);
   snapshot_sub_ = nh.subscribe("snapshot", 10, &BebopDriverNodelet::TakeSnapshotCallback, this);
   toggle_recording_sub_ = nh.subscribe("record", 10, &BebopDriverNodelet::ToggleRecordingCallback, this);
@@ -304,6 +307,56 @@ void BebopDriverNodelet::NavigateHomeCallback(const std_msgs::BoolConstPtr &star
   }
 }
 
+void BebopDriverNodelet::StartAutonomousFlightCallback(const std_msgs::StringConstPtr& filepath_ptr)
+{
+  std::string filepath;
+  if (filepath_ptr->data.empty())
+  {
+    ROS_WARN("No flight plan provided. Using default: 'flightplan.mavlink'");
+    filepath = "flightplan.mavlink";
+  }
+  else
+  {
+    filepath = filepath_ptr->data;
+  }
+
+  try
+  {
+    ROS_INFO("Starting autonomous flight path: %s", filepath.c_str());
+    bebop_ptr_->StartAutonomousFlight(filepath);
+  }
+  catch (const std::runtime_error& e)
+  {
+    ROS_ERROR_STREAM(e.what());
+  }
+}
+
+void BebopDriverNodelet::PauseAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr)
+{
+  try
+  {
+    ROS_INFO("Pausing autonomous flight");
+    bebop_ptr_->PauseAutonomousFlight();
+  }
+  catch (const std::runtime_error& e)
+  {
+    ROS_ERROR_STREAM(e.what());
+  }
+}
+
+void BebopDriverNodelet::StopAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr)
+{
+  try
+  {
+    ROS_INFO("Stopping autonomous flight");
+    bebop_ptr_->StopAutonomousFlight();
+  }
+  catch (const std::runtime_error& e)
+  {
+    ROS_ERROR_STREAM(e.what());
+  }
+}
+
 void BebopDriverNodelet::FlipAnimationCallback(const std_msgs::UInt8ConstPtr &animid_ptr)
 {
   try

bebop_tools/config/log710.yaml

@@ -66,7 +66,7 @@ teleop:
     type: topic
     message_type: "std_msgs/Empty"
     topic_name: flattrim
-    deadman_buttons: [4, 5] # RT + X
+    deadman_buttons: [0, 7] # RT + X
     axis_mappings: []
   flip:
     type: topic

docs/piloting.rst

@@ -68,7 +68,59 @@ To move Bebop's virtual camera, publish a message of type `geometry_msgs/Twist <
 GPS Navigation
 ==============
 
-.. warning:: Not fully integrated/tested yet.
+Start Flight Plan
+-----------------
+
+An autonomous flight plan consists of a series of GPS waypoints along with Bebop velocities and camera angles encoded in an XML file.
+
+Requirements that must be met before an autonomous flight can start:
+
+    * Bebop is calibrated
+    * Bebop is in outdoor mode
+    * Bebop has fixed its GPS
+
+To start an autonomous flight plan, publish a message of type `std_msgs/String <http://docs.ros.org/api/std_msgs/html/msg/String.html>`_ to `autoflight/start` topic. The ``data`` field should contain the name of the flight plan to execute, which is already stored on-board Bebop.
+
+.. note:: If an empty string is published, then the default 'flightplan.mavlink' is used.
+
+.. warning:: If not already flying, Bebop will attempt to take off upon starting a flight plan.
+
+The `Flight Plan App <https://play.google.com/store/apps/details?id=com.parrot.freeflight3>`_ allows easy construction of flight plans and saves them on-board Bebop.
+
+An FTP client can also be used to view and copy flight plans on-board Bebop. `FileZilla` is recommended:
+
+.. code-block:: bash
+
+  $ sudo apt-get install filezilla
+  $ filezilla
+
+Then open `Site Manager` (top left), click `New Site`:
+
+    * `Host`: 192.168.42.1
+    * `Protocol`: FTP
+    * `Encrpytion`: Use plain FTP
+    * `Logon Type`: Anonymous
+    * Connect.
+
+Pause Flight Plan
+-----------------
+
+To pause the execution of an autonomous flight plan, publish a message of type `std_msgs/Empty <http://docs.ros.org/api/std_msgs/html/msg/Empty.html>`_ to `autoflight/pause` topic. Bebop will then hover and await further commands.
+To resume a paused flight plan, publish the same message that was used to start the autonomous flight (ie. to the topic `autoflight/start`). Bebop will fly to the lastest waypoint reached before continuing the flight plan.
+
+.. note:: Any velocity commands sent to Bebop during an autonomous flight plan will pause the plan.
+
+Stop Flight Plan
+----------------
+
+To stop the execution of an autonomous flight plan, publish a message of type `std_msgs/Empty <http://docs.ros.org/api/std_msgs/html/msg/Empty.html>`_ to `autoflight/stop` topic. Bebop will hover and await further commands.
+
+Navigate Home
+-------------
+
+To ask Bebop to autonomously fly to it's home position, publish a message of type `std_msgs/Bool <http://docs.ros.org/api/std_msgs/html/msg/Bool.html>`_ to `autoflight/navigate_home` topic with the ``data`` field set to ``true``. To stop Bebop from navigating home, publish another message with ``data`` set to ``false``.
+
+.. warning:: The topic has changed from `navigate_home` to `autoflight/navigate_home` after version 0.5.1.
 
 Flat Trim
 =========