implementation of sendPilotingMoveBy method

bebop_driver/include/bebop_driver/bebop.h

@@ -167,6 +167,7 @@ class Bebop
   void PauseAutonomousFlight();
   void StopAutonomousFlight();
   void AnimationFlip(const uint8_t& anim_id);
+  void GoTo(const double& dx,const double& dy, const double& dz, const double& dpsi);
 
   // -1..1
   void Move(const double& roll, const double& pitch, const double& gaz_speed, const double& yaw_speed);

bebop_driver/include/bebop_driver/bebop_driver_nodelet.h

@@ -129,6 +129,8 @@ class BebopDriverNodelet : public nodelet::Nodelet
   ros::Publisher camera_joint_pub_;
   ros::Publisher gps_fix_pub_;
 
+  ros::Subscriber goto_sub_;
+
   boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_manager_ptr_;
   boost::shared_ptr<image_transport::ImageTransport> image_transport_ptr_;
   image_transport::CameraPublisher image_transport_pub_;
@@ -164,6 +166,7 @@ class BebopDriverNodelet : public nodelet::Nodelet
   void TakeSnapshotCallback(const std_msgs::EmptyConstPtr& empty_ptr);
   void SetExposureCallback(const std_msgs::Float32ConstPtr& exposure_ptr);
   void ToggleRecordingCallback(const std_msgs::BoolConstPtr& toggle_ptr);
+  void GotoCallBack(const geometry_msgs::TwistConstPtr& twist_ptr);
 
   void ParamCallback(bebop_driver::BebopArdrone3Config &config, uint32_t level);
 

bebop_driver/src/bebop.cpp

@@ -502,6 +502,13 @@ void Bebop::StopAutonomousFlight()
         "Stop autonomous flight failed");
 }
 
+void Bebop::GoTo(const double &dx,const double &dy, const double &dz, const double &dpsi){
+  ThrowOnInternalError("Navigate Go to failed");
+  ThrowOnCtrlError(
+        device_controller_ptr_->aRDrone3->sendPilotingMoveBy( device_controller_ptr_->aRDrone3, dx,dy,dz,dpsi),
+                        "Navigate Go to failed");
+}
+
 void Bebop::AnimationFlip(const uint8_t &anim_id)
 {
   ThrowOnInternalError("Animation failed");

bebop_driver/src/bebop_driver_nodelet.cpp

@@ -146,6 +146,7 @@ void BebopDriverNodelet::onInit()
   snapshot_sub_ = nh.subscribe("snapshot", 10, &BebopDriverNodelet::TakeSnapshotCallback, this);
   exposure_sub_ = nh.subscribe("set_exposure", 10, &BebopDriverNodelet::SetExposureCallback, this);
   toggle_recording_sub_ = nh.subscribe("record", 10, &BebopDriverNodelet::ToggleRecordingCallback, this);
+  goto_sub_ = nh.subscribe("goto", 1, &BebopDriverNodelet::GotoCallBack, this);
 
   odom_pub_ = nh.advertise<nav_msgs::Odometry>("odom", 30);
   camera_joint_pub_ = nh.advertise<sensor_msgs::JointState>("joint_states", 10, true);
@@ -688,4 +689,20 @@ void BebopDriverNodelet::AuxThread()
   }
 }
 
+void BebopDriverNodelet::GotoCallBack(const geometry_msgs::TwistConstPtr& twist_ptr)
+{
+  try
+  {
+    const double dx = twist_ptr->linear.x; //displacement along the front axis
+    const double dy = twist_ptr->linear.y; //displacement along the right axis
+    const double dz = twist_ptr->linear.z; //displacement along the down axis
+    const double dpsi =  twist_ptr->angular.z; // rotation of heading
+    bebop_ptr_->GoTo(dx,dy,dz,dpsi);
+  }
+  catch (const std::runtime_error& e)
+  {
+    ROS_ERROR_STREAM("[Goto] " << e.what());
+  }
+}
+
 }  // namespace bebop_driver