From 6f1d37af457170654877d0350e95fdd6c3965165 Mon Sep 17 00:00:00 2001 From: pousseur_hug Date: Thu, 21 Mar 2019 14:16:49 +0100 Subject: [PATCH] implement sendPilotingMoveBy method --- bebop_driver/include/bebop_driver/bebop.h | 1 + .../include/bebop_driver/bebop_driver_nodelet.h | 3 +++ bebop_driver/src/bebop.cpp | 7 +++++++ bebop_driver/src/bebop_driver_nodelet.cpp | 17 +++++++++++++++++ 4 files changed, 28 insertions(+) diff --git a/bebop_driver/include/bebop_driver/bebop.h b/bebop_driver/include/bebop_driver/bebop.h index 2aa8fb9..04c7964 100644 --- a/bebop_driver/include/bebop_driver/bebop.h +++ b/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); diff --git a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h index 03cbebc..e7c1211 100644 --- a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h +++ b/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 cinfo_manager_ptr_; boost::shared_ptr 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); diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp index d2664c1..caeb59f 100644 --- a/bebop_driver/src/bebop.cpp +++ b/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"); diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index 1deca22..c325a6a 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/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("odom", 30); camera_joint_pub_ = nh.advertise("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