diff --git a/README.md b/README.md index 09951fd..cd52d2c 100644 --- a/README.md +++ b/README.md @@ -5,9 +5,15 @@ * Documentation: http://bebop-autonomy.readthedocs.io/ * ROS wiki page: http://wiki.ros.org/bebop_autonomy * Support: [ROS Q&A (tag: bebop_autonomy)](http://answers.ros.org/questions/scope:all/sort:activity-desc/tags:bebop_autonomy/page:1/) -* Developer Forum: https://trello.com/b/C6rNl8Ux * Code API: http://docs.ros.org/indigo/api/bebop_autonomy/html + +## Development Team + * Author: [Mani Monajjemi](http://mani.im) ([Autonomy Lab](http://autonomylab.org), [Simon Fraser University](http://www.sfu.ca)) + [other contributers](http://bebop-autonomy.readthedocs.io/en/latest/contribute.html#list-of-contributers) +* Maintainers: + * [Sepehr MohaimenianPour](http://sepehr.im/) ([Autonomy Lab](http://autonomylab.org), [Simon Fraser University](http://www.sfu.ca)) + * Thomas Bamford ([Dynamic Systems Lab](http://www.dynsyslab.org), [University of Toronto](https://www.utoronto.ca/)) + * [Tobias Naegeli](https://ait.ethz.ch/people/naegelit/) ([Advanced Interactive Technologies Lab](http://www.ait.ethz.ch/), [ETH Zürich](http://www.ethz.ch/)) ## Build Status diff --git a/bebop_autonomy/package.xml b/bebop_autonomy/package.xml index 1ad5df5..265808a 100644 --- a/bebop_autonomy/package.xml +++ b/bebop_autonomy/package.xml @@ -5,6 +5,10 @@ bebop_autonomy is a ROS driver for Parrot Bebop drone, based on Parrot’s official ARDroneSDK3 Mani Monajjemi + Sepehr MohaimenianPour + Thomas Bamford + Tobias Naegeli + BSD http://wiki.ros.org/bebop_autonomy diff --git a/bebop_description/package.xml b/bebop_description/package.xml index 8f08cc3..06b4fb4 100644 --- a/bebop_description/package.xml +++ b/bebop_description/package.xml @@ -5,6 +5,10 @@ URDF robot description for Parrot Bebop Drones Mani Monajjemi + Sepehr MohaimenianPour + Thomas Bamford + Tobias Naegeli + BSD http://wiki.ros.org/bebop_description diff --git a/bebop_driver/include/bebop_driver/bebop.h b/bebop_driver/include/bebop_driver/bebop.h index 2aa8fb9..3fbe7c3 100644 --- a/bebop_driver/include/bebop_driver/bebop.h +++ b/bebop_driver/include/bebop_driver/bebop.h @@ -170,9 +170,16 @@ class Bebop // -1..1 void Move(const double& roll, const double& pitch, const double& gaz_speed, const double& yaw_speed); + void MoveBy(const double& dX, const double& dY, const double& dZ, const double& dPsi); void MoveCamera(const double& tilt, const double& pan); void TakeSnapshot(); + + /** + * @brief Set the format of the taken pictures + * @param format 0: Raw image, 1: 4:3 jpeg photo, 2: 16:9 snapshot from camera, 3: jpeg fisheye image only + */ + void SetPictureFormat(const int& format); // exposure should be between -3.0 and +3.0 void SetExposure(const float& exposure); // true: start, false: stop diff --git a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h index 03cbebc..773111e 100644 --- a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h +++ b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h @@ -111,6 +111,7 @@ class BebopDriverNodelet : public nodelet::Nodelet geometry_msgs::Twist prev_camera_twist_; ros::Subscriber cmd_vel_sub_; + ros::Subscriber cmd_move_by; ros::Subscriber camera_move_sub_; ros::Subscriber takeoff_sub_; ros::Subscriber land_sub_; @@ -122,6 +123,7 @@ class BebopDriverNodelet : public nodelet::Nodelet ros::Subscriber stop_autoflight_sub_; ros::Subscriber animation_sub_; ros::Subscriber snapshot_sub_; + ros::Subscriber setpictureformat_sub; ros::Subscriber exposure_sub_; ros::Subscriber toggle_recording_sub_; @@ -151,6 +153,7 @@ class BebopDriverNodelet : public nodelet::Nodelet void AuxThread(); void CmdVelCallback(const geometry_msgs::TwistConstPtr& twist_ptr); + void CmdMoveByCallback(const geometry_msgs::TwistConstPtr& twist_ptr); void CameraMoveCallback(const geometry_msgs::TwistConstPtr& twist_ptr); void TakeoffCallback(const std_msgs::EmptyConstPtr& empty_ptr); void LandCallback(const std_msgs::EmptyConstPtr& empty_ptr); @@ -162,6 +165,7 @@ class BebopDriverNodelet : public nodelet::Nodelet 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 SetPictureFormatCallback(const std_msgs::UInt8ConstPtr &format_ptr); void SetExposureCallback(const std_msgs::Float32ConstPtr& exposure_ptr); void ToggleRecordingCallback(const std_msgs::BoolConstPtr& toggle_ptr); diff --git a/bebop_driver/package.xml b/bebop_driver/package.xml index b66cb85..158660e 100644 --- a/bebop_driver/package.xml +++ b/bebop_driver/package.xml @@ -5,6 +5,10 @@ ROS driver for Parrot Bebop drone, based on Parrot’s official ARDroneSDK3 Mani Monajjemi + Sepehr MohaimenianPour + Thomas Bamford + Tobias Naegeli + BSD http://wiki.ros.org/bebop_driver diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp index d2664c1..854c0b4 100644 --- a/bebop_driver/src/bebop.cpp +++ b/bebop_driver/src/bebop.cpp @@ -549,6 +549,20 @@ void Bebop::Move(const double &roll, const double &pitch, const double &gaz_spee } } + +void Bebop::MoveBy(const double& dX, const double& dY, const double& dZ, const double& dPsi) +{ + ThrowOnInternalError("MoveBy failure"); + + ThrowOnCtrlError( + device_controller_ptr_->aRDrone3->sendPilotingMoveBy( + device_controller_ptr_->aRDrone3, + static_cast(dX), + static_cast(dY), + static_cast(dZ), + static_cast(dPsi))); +} + // in degrees void Bebop::MoveCamera(const double &tilt, const double &pan) { @@ -607,6 +621,18 @@ void Bebop::TakeSnapshot() device_controller_ptr_->aRDrone3)); } +/** + * @brief Set the format of the taken pictures + * @param format 0: Raw image, 1: 4:3 jpeg photo, 2: 16:9 snapshot from camera, 3: jpeg fisheye image only + */ +void Bebop::SetPictureFormat(const int& format) +{ + ThrowOnInternalError("Failed to set picture format"); + ThrowOnCtrlError( + device_controller_ptr_->aRDrone3->sendPictureSettingsPictureFormatSelection( + device_controller_ptr_->aRDrone3, (eARCOMMANDS_ARDRONE3_PICTURESETTINGS_PICTUREFORMATSELECTION_TYPE)format)); +} + void Bebop::SetExposure(const float& exposure) { ThrowOnInternalError("Failed to set exposure"); diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index 1deca22..7d6b520 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/bebop_driver/src/bebop_driver_nodelet.cpp @@ -31,6 +31,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI #include #include #include +#include #include #include @@ -133,6 +134,7 @@ void BebopDriverNodelet::onInit() } cmd_vel_sub_ = nh.subscribe("cmd_vel", 1, &BebopDriverNodelet::CmdVelCallback, this); + cmd_move_by = nh.subscribe("cmd_move_by", 1, &BebopDriverNodelet::CmdMoveByCallback, this); camera_move_sub_ = nh.subscribe("camera_control", 1, &BebopDriverNodelet::CameraMoveCallback, this); takeoff_sub_ = nh.subscribe("takeoff", 1, &BebopDriverNodelet::TakeoffCallback, this); land_sub_ = nh.subscribe("land", 1, &BebopDriverNodelet::LandCallback, this); @@ -144,6 +146,7 @@ void BebopDriverNodelet::onInit() 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); + setpictureformat_sub = nh.subscribe("set_picture_format", 1, &BebopDriverNodelet::SetPictureFormatCallback, this); exposure_sub_ = nh.subscribe("set_exposure", 10, &BebopDriverNodelet::SetExposureCallback, this); toggle_recording_sub_ = nh.subscribe("record", 10, &BebopDriverNodelet::ToggleRecordingCallback, this); @@ -232,6 +235,22 @@ void BebopDriverNodelet::CmdVelCallback(const geometry_msgs::TwistConstPtr& twis } } +void BebopDriverNodelet::CmdMoveByCallback(const geometry_msgs::TwistConstPtr& twist_ptr) +{ + try + { + const geometry_msgs::Twist& bebop_twist_ = *twist_ptr; + bebop_ptr_->MoveBy(bebop_twist_.linear.x, + bebop_twist_.linear.y, + bebop_twist_.linear.z, + bebop_twist_.angular.z); + } + catch (const std::runtime_error& e) + { + ROS_ERROR_STREAM(e.what()); + } +} + void BebopDriverNodelet::TakeoffCallback(const std_msgs::EmptyConstPtr& empty_ptr) { try @@ -392,6 +411,19 @@ void BebopDriverNodelet::TakeSnapshotCallback(const std_msgs::EmptyConstPtr &emp } } +void BebopDriverNodelet::SetPictureFormatCallback(const std_msgs::UInt8ConstPtr& format_ptr) +{ + try + { + ROS_INFO("Setting picture format to %u", format_ptr->data); + bebop_ptr_->SetPictureFormat(format_ptr->data); + } + catch (const std::runtime_error& e) + { + ROS_ERROR_STREAM(e.what()); + } +} + void BebopDriverNodelet::SetExposureCallback(const std_msgs::Float32ConstPtr& exposure_ptr) { try diff --git a/bebop_driver/src/bebop_video_decoder.cpp b/bebop_driver/src/bebop_video_decoder.cpp index 63f0a82..9e2e500 100644 --- a/bebop_driver/src/bebop_video_decoder.cpp +++ b/bebop_driver/src/bebop_video_decoder.cpp @@ -133,7 +133,7 @@ bool VideoDecoder::ReallocateBuffers() boost::lexical_cast(codec_ctx_ptr_->width) + " x " + boost::lexical_cast(codec_ctx_ptr_->width)); - const uint32_t num_bytes = avpicture_get_size(PIX_FMT_RGB24, codec_ctx_ptr_->width, codec_ctx_ptr_->width); + const uint32_t num_bytes = avpicture_get_size(AV_PIX_FMT_RGB24, codec_ctx_ptr_->width, codec_ctx_ptr_->width); frame_rgb_ptr_ = av_frame_alloc(); ThrowOnCondition(!frame_rgb_ptr_, "Can not allocate memory for frames!"); @@ -143,12 +143,12 @@ bool VideoDecoder::ReallocateBuffers() std::string("Can not allocate memory for the buffer: ") + boost::lexical_cast(num_bytes)); ThrowOnCondition(0 == avpicture_fill( - reinterpret_cast(frame_rgb_ptr_), frame_rgb_raw_ptr_, PIX_FMT_RGB24, + reinterpret_cast(frame_rgb_ptr_), frame_rgb_raw_ptr_, AV_PIX_FMT_RGB24, codec_ctx_ptr_->width, codec_ctx_ptr_->height), "Failed to initialize the picture data structure."); img_convert_ctx_ptr_ = sws_getContext(codec_ctx_ptr_->width, codec_ctx_ptr_->height, codec_ctx_ptr_->pix_fmt, - codec_ctx_ptr_->width, codec_ctx_ptr_->height, PIX_FMT_RGB24, + codec_ctx_ptr_->width, codec_ctx_ptr_->height, AV_PIX_FMT_RGB24, SWS_FAST_BILINEAR, NULL, NULL, NULL); } catch (const std::runtime_error& e) diff --git a/bebop_driver/test/bebop_itl_test.cpp b/bebop_driver/test/bebop_itl_test.cpp index caf55c7..a66febb 100644 --- a/bebop_driver/test/bebop_itl_test.cpp +++ b/bebop_driver/test/bebop_itl_test.cpp @@ -178,12 +178,36 @@ class BebopInTheLoopTest: public ::testing::Test ros::Publisher land_pub_; ros::Publisher cmdvel_pub_; + ros::Publisher cmdmoveby_pub_; std_msgs::Empty em; geometry_msgs::Twist tw; boost::shared_ptr > image_; + boost::shared_ptr > + flying_state_; + + boost::shared_ptr > + speed_state_; + + boost::shared_ptr > + alt_state_; + + boost::shared_ptr > + att_state_; + + // The problem with the battery state is that it is only updated when its values changes, + // this is more likely to happen when flying than sitting on the ground + boost::shared_ptr > + bat_state_; + + boost::shared_ptr > + odom_; + + boost::shared_ptr > + gps_; + // boost::shared_ptr > attitude_state_; virtual void SetUp() @@ -195,11 +219,34 @@ class BebopInTheLoopTest: public ::testing::Test // Common Publishers land_pub_= nh_.advertise("land", 1); cmdvel_pub_ = nh_.advertise("cmd_vel", 1); + cmdmoveby_pub_ = nh_.advertise("cmd_move_by", 1); // Common Subs image_.reset(new util::ASyncSub( nh_, "image_raw", 10)); + flying_state_.reset(new util::ASyncSub( + nh_, "states/ardrone3/PilotingState/FlyingStateChanged", 10)); + + speed_state_.reset(new util::ASyncSub( + nh_, "states/ardrone3/PilotingState/SpeedChanged", 10)); + + alt_state_.reset(new util::ASyncSub( + nh_, "states/ardrone3/PilotingState/AltitudeChanged", 10)); + + att_state_.reset(new util::ASyncSub( + nh_, "states/ardrone3/PilotingState/AttitudeChanged", 10)); + + bat_state_.reset(new util::ASyncSub( + nh_, "states/common/CommonState/BatteryStateChanged", 10)); + + odom_.reset(new util::ASyncSub( + nh_, "odom", 10)); + + gps_.reset(new util::ASyncSub( + nh_, "fix", 10)); + + spinner_ptr_.reset(new ros::AsyncSpinner(4)); spinner_ptr_->start(); @@ -248,36 +295,6 @@ class BebopInTheLoopTest: public ::testing::Test */ TEST_F(BebopInTheLoopTest, Piloting) { - boost::shared_ptr > - flying_state(new util::ASyncSub( - nh_, "states/ardrone3/PilotingState/FlyingStateChanged", 10)); - - boost::shared_ptr > - speed_state(new util::ASyncSub( - nh_, "states/ardrone3/PilotingState/SpeedChanged", 10)); - - boost::shared_ptr > - alt_state(new util::ASyncSub( - nh_, "states/ardrone3/PilotingState/AltitudeChanged", 10)); - - boost::shared_ptr > - att_state(new util::ASyncSub( - nh_, "states/ardrone3/PilotingState/AttitudeChanged", 10)); - - // The problem with the battery state is that it is only updated when its values changes, - // this is more likely to happen when flying than sitting on the ground - boost::shared_ptr > - bat_state(new util::ASyncSub( - nh_, "states/common/CommonState/BatteryStateChanged", 10)); - - boost::shared_ptr > - odom(new util::ASyncSub( - nh_, "odom", 10)); - - boost::shared_ptr > - gps(new util::ASyncSub( - nh_, "fix", 10)); - ros::Publisher takeoff_pub = nh_.advertise("takeoff", 1); ros::Publisher reset_pub = nh_.advertise("reset", 1); @@ -289,16 +306,16 @@ TEST_F(BebopInTheLoopTest, Piloting) ROS_WARN("Taking off ..."); takeoff_pub.publish(em); TIMED_ASSERT(10.0, - flying_state->IsActive() && flying_state->GetMsgCopy().state == + flying_state_->IsActive() && flying_state_->GetMsgCopy().state == bebop_msgs::Ardrone3PilotingStateFlyingStateChanged::state_hovering, "Waiting for takeoff to finish ..." ); - TIMED_ASSERT(5.0, speed_state->IsActive(), "Waiting for Speed measurements ..."); - TIMED_ASSERT(5.0, alt_state->IsActive(), "Waiting for alt measurements ..."); - TIMED_ASSERT(5.0, att_state->IsActive(), "Waiting for attitude measurements ..."); - TIMED_ASSERT(5.0, odom->IsActive(), "Waiting for odom ..."); - TIMED_ASSERT(5.0, gps->IsActive(), "Waiting for GPS ..."); + TIMED_ASSERT(5.0, speed_state_->IsActive(), "Waiting for Speed measurements ..."); + TIMED_ASSERT(5.0, alt_state_->IsActive(), "Waiting for alt measurements ..."); + TIMED_ASSERT(5.0, att_state_->IsActive(), "Waiting for attitude measurements ..."); + TIMED_ASSERT(5.0, odom_->IsActive(), "Waiting for odom ..."); + TIMED_ASSERT(5.0, gps_->IsActive(), "Waiting for GPS ..."); // TODO(mani-monaj): Use proper values for pitch/roll test thresholds based on cmd_vel @@ -309,21 +326,21 @@ TEST_F(BebopInTheLoopTest, Piloting) ROS_WARN("Moving forwared ..."); cmdvel_pub_.publish(tw); - TIMED_ASSERT(3.0, angles::to_degrees(att_state->GetMsgCopy().pitch) < -2.5 , "Measuring pitch ..."); - TIMED_ASSERT(3.0, (odom->GetMsgCopy().twist.twist.linear.x > 0.2), + TIMED_ASSERT(3.0, angles::to_degrees(att_state_->GetMsgCopy().pitch) < -2.5 , "Measuring pitch ..."); + TIMED_ASSERT(3.0, (odom_->GetMsgCopy().twist.twist.linear.x > 0.2), "Measuring the forward and lateral velocities from odom ..."); StopBebop(); TIMED_ASSERT(5.0, - (fabs(speed_state->GetMsgCopy().speedX) < 0.05) && - (fabs(speed_state->GetMsgCopy().speedY) < 0.05) && - (fabs(speed_state->GetMsgCopy().speedZ) < 0.05) + (fabs(speed_state_->GetMsgCopy().speedX) < 0.05) && + (fabs(speed_state_->GetMsgCopy().speedY) < 0.05) && + (fabs(speed_state_->GetMsgCopy().speedZ) < 0.05) , "Measuring the speed vector ..."); TIMED_ASSERT(5.0, - (fabs(odom->GetMsgCopy().twist.twist.linear.x) < 0.05) && - (fabs(odom->GetMsgCopy().twist.twist.linear.y) < 0.05) && - (fabs(odom->GetMsgCopy().twist.twist.linear.z) < 0.05) + (fabs(odom_->GetMsgCopy().twist.twist.linear.x) < 0.05) && + (fabs(odom_->GetMsgCopy().twist.twist.linear.y) < 0.05) && + (fabs(odom_->GetMsgCopy().twist.twist.linear.z) < 0.05) , "Measuring the speed vector from odom..."); @@ -334,8 +351,8 @@ TEST_F(BebopInTheLoopTest, Piloting) ROS_WARN("Moving Backward ..."); cmdvel_pub_.publish(tw); - TIMED_ASSERT(3.0, angles::to_degrees(att_state->GetMsgCopy().pitch) > 2.5, "Measuring pitch ..."); - TIMED_ASSERT(3.0, (odom->GetMsgCopy().twist.twist.linear.x < -0.2), + TIMED_ASSERT(3.0, angles::to_degrees(att_state_->GetMsgCopy().pitch) > 2.5, "Measuring pitch ..."); + TIMED_ASSERT(3.0, (odom_->GetMsgCopy().twist.twist.linear.x < -0.2), "Measuring the forward and lateral velocities from odom ..."); StopBebop(); @@ -345,8 +362,8 @@ TEST_F(BebopInTheLoopTest, Piloting) tw.angular.z = 0.0; ROS_WARN("Moving left ..."); cmdvel_pub_.publish(tw); - TIMED_ASSERT(3.0, angles::to_degrees(att_state->GetMsgCopy().roll) < -2.5, "Measuring roll ..."); - TIMED_ASSERT(3.0, (odom->GetMsgCopy().twist.twist.linear.y > 0.1), + TIMED_ASSERT(3.0, angles::to_degrees(att_state_->GetMsgCopy().roll) < -2.5, "Measuring roll ..."); + TIMED_ASSERT(3.0, (odom_->GetMsgCopy().twist.twist.linear.y > 0.1), "Measuring the forward and lateral velocities from odom ..."); StopBebop(); @@ -357,18 +374,18 @@ TEST_F(BebopInTheLoopTest, Piloting) tw.angular.z = 0.0; ROS_WARN("Moving right ..."); cmdvel_pub_.publish(tw); - TIMED_ASSERT(3.0, angles::to_degrees(att_state->GetMsgCopy().roll) > 2.5, "Measuring roll ..."); - TIMED_ASSERT(3.0, (odom->GetMsgCopy().twist.twist.linear.y < -0.1), + TIMED_ASSERT(3.0, angles::to_degrees(att_state_->GetMsgCopy().roll) > 2.5, "Measuring roll ..."); + TIMED_ASSERT(3.0, (odom_->GetMsgCopy().twist.twist.linear.y < -0.1), "Measuring the forward and lateral velocities from odom ..."); StopBebop(); double alt_start, yaw_start, alt_start_odom, yaw_start_odom; // Make sure altitude is fresh - ASSERT_LT(alt_state->GetFreshness().toSec(), 0.5); - ASSERT_LT(odom->GetFreshness().toSec(), 0.5); + ASSERT_LT(alt_state_->GetFreshness().toSec(), 0.5); + ASSERT_LT(odom_->GetFreshness().toSec(), 0.5); - alt_start = alt_state->GetMsgCopy().altitude; - alt_start_odom = odom->GetMsgCopy().pose.pose.position.z; + alt_start = alt_state_->GetMsgCopy().altitude; + alt_start_odom = odom_->GetMsgCopy().pose.pose.position.z; tw.linear.x = 0.0; tw.linear.y = 0.0; tw.linear.z = 0.2; @@ -376,19 +393,19 @@ TEST_F(BebopInTheLoopTest, Piloting) ROS_WARN("Ascending for 0.5m ..."); cmdvel_pub_.publish(tw); TIMED_ASSERT(10.0, - ((alt_state->GetMsgCopy().altitude - alt_start) >= 0.5) && - ((odom->GetMsgCopy().pose.pose.position.z - alt_start_odom) >= 0.5) && - (speed_state->GetMsgCopy().speedZ < -0.05) && - (odom->GetMsgCopy().twist.twist.linear.z > 0.05), + ((alt_state_->GetMsgCopy().altitude - alt_start) >= 0.5) && + ((odom_->GetMsgCopy().pose.pose.position.z - alt_start_odom) >= 0.5) && + (speed_state_->GetMsgCopy().speedZ < -0.05) && + (odom_->GetMsgCopy().twist.twist.linear.z > 0.05), "Measuring altitude, speed.z and vertical velocity from odom ..."); StopBebop(); // Make sure altitude is fresh - ASSERT_LT(alt_state->GetFreshness().toSec(), 0.5); - ASSERT_LT(odom->GetFreshness().toSec(), 0.5); - alt_start = alt_state->GetMsgCopy().altitude; - alt_start_odom = odom->GetMsgCopy().pose.pose.position.z; + ASSERT_LT(alt_state_->GetFreshness().toSec(), 0.5); + ASSERT_LT(odom_->GetFreshness().toSec(), 0.5); + alt_start = alt_state_->GetMsgCopy().altitude; + alt_start_odom = odom_->GetMsgCopy().pose.pose.position.z; tw.linear.x = 0.0; tw.linear.y = 0.0; tw.linear.z = -0.2; @@ -396,18 +413,18 @@ TEST_F(BebopInTheLoopTest, Piloting) ROS_WARN("Descending for 0.5m ..."); cmdvel_pub_.publish(tw); TIMED_ASSERT(10.0, - ((alt_state->GetMsgCopy().altitude - alt_start) <= -0.5) && - ((odom->GetMsgCopy().pose.pose.position.z - alt_start_odom) <= -0.5) && - (speed_state->GetMsgCopy().speedZ > 0.05) && - (odom->GetMsgCopy().twist.twist.linear.z < -0.05), + ((alt_state_->GetMsgCopy().altitude - alt_start) <= -0.5) && + ((odom_->GetMsgCopy().pose.pose.position.z - alt_start_odom) <= -0.5) && + (speed_state_->GetMsgCopy().speedZ > 0.05) && + (odom_->GetMsgCopy().twist.twist.linear.z < -0.05), "Measuring altitude, speed.z and vertical velocity from odom ..."); StopBebop(); // Make sure the attitude is fresh - ASSERT_LT(att_state->GetFreshness().toSec(), 0.5); - ASSERT_LT(odom->GetFreshness().toSec(), 0.5); - yaw_start = att_state->GetMsgCopy().yaw; + ASSERT_LT(att_state_->GetFreshness().toSec(), 0.5); + ASSERT_LT(odom_->GetFreshness().toSec(), 0.5); + yaw_start = att_state_->GetMsgCopy().yaw; tw.linear.x = 0.0; tw.linear.y = 0.0; tw.linear.z = 0.0; @@ -417,14 +434,14 @@ TEST_F(BebopInTheLoopTest, Piloting) // TODO(mani-monaj): Add yaw TIMED_ASSERT(10.0, - angles::normalize_angle(att_state->GetMsgCopy().yaw - yaw_start) >= 0.5 * 3.141596, + angles::normalize_angle(att_state_->GetMsgCopy().yaw - yaw_start) >= 0.5 * 3.141596, "Measuring Yaw"); StopBebop(); // Make sure the attitude is fresh - ASSERT_LT(att_state->GetFreshness().toSec(), 0.5); - yaw_start = att_state->GetMsgCopy().yaw; + ASSERT_LT(att_state_->GetFreshness().toSec(), 0.5); + yaw_start = att_state_->GetMsgCopy().yaw; tw.linear.x = 0.0; tw.linear.y = 0.0; tw.linear.z = 0.0; @@ -432,14 +449,14 @@ TEST_F(BebopInTheLoopTest, Piloting) ROS_WARN("Rotating CCW for 90 degrees ..."); cmdvel_pub_.publish(tw); TIMED_ASSERT(10.0, - angles::normalize_angle(att_state->GetMsgCopy().yaw - yaw_start) <= -0.5 * 3.141596, + angles::normalize_angle(att_state_->GetMsgCopy().yaw - yaw_start) <= -0.5 * 3.141596, "Measuring Yaw"); StopBebop(); /* By this time, battery state must have been changed (even on Bebop 2) */ - TIMED_ASSERT(20.0, bat_state->IsActive(), "Measuring battery ..."); - const uint8_t bat_percent = bat_state->GetMsgCopy().percent; + TIMED_ASSERT(20.0, bat_state_->IsActive(), "Measuring battery ..."); + const uint8_t bat_percent = bat_state_->GetMsgCopy().percent; tw.linear.x = 0.0; tw.linear.y = 0.0; @@ -452,7 +469,7 @@ TEST_F(BebopInTheLoopTest, Piloting) land_pub_.publish(em); TIMED_ASSERT(10.0, - flying_state->IsActive() && flying_state->GetMsgCopy().state == + flying_state_->IsActive() && flying_state_->GetMsgCopy().state == bebop_msgs::Ardrone3PilotingStateFlyingStateChanged::state_landed, "Waiting for land to finish..." ); @@ -467,7 +484,217 @@ TEST_F(BebopInTheLoopTest, Piloting) // "Waiting for reset to happen..." // ); - ASSERT_GE(bat_percent - bat_state->GetMsgCopy().percent, 0); + ASSERT_GE(bat_percent - bat_state_->GetMsgCopy().percent, 0); +} + + +TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) +{ + ros::Publisher takeoff_pub = nh_.advertise("takeoff", 1); + ros::Publisher reset_pub = nh_.advertise("reset", 1); + + // Wait 5s time for connections to establish + TIMED_ASSERT(5.0, takeoff_pub.getNumSubscribers() > 0, "Waiting for takeoff subscription ..."); + TIMED_ASSERT(5.0, cmdvel_pub_.getNumSubscribers() > 0, "Waiting for cmd_vel subscription ..."); + TIMED_ASSERT(5.0, reset_pub.getNumSubscribers() > 0, "Waiting for reset subscription ..."); + + ROS_WARN("Taking off ..."); + takeoff_pub.publish(em); + TIMED_ASSERT(10.0, + flying_state_->IsActive() && flying_state_->GetMsgCopy().state == + bebop_msgs::Ardrone3PilotingStateFlyingStateChanged::state_hovering, + "Waiting for takeoff to finish ..." + ); + + TIMED_ASSERT(5.0, speed_state_->IsActive(), "Waiting for Speed measurements ..."); + TIMED_ASSERT(5.0, alt_state_->IsActive(), "Waiting for alt measurements ..."); + TIMED_ASSERT(5.0, att_state_->IsActive(), "Waiting for attitude measurements ..."); + TIMED_ASSERT(5.0, odom_->IsActive(), "Waiting for odom ..."); + TIMED_ASSERT(5.0, gps_->IsActive(), "Waiting for GPS ..."); + + // TODO(mani-monaj): Use proper values for pitch/roll test thresholds based on cmd_vel + + tw.linear.x = 0.5; + tw.linear.y = 0.0; + tw.linear.z = 0.0; + tw.angular.z = 0.0; + ROS_WARN("Moving 0.5m forwared ..."); + cmdmoveby_pub_.publish(tw); + + TIMED_ASSERT(3.0, angles::to_degrees(att_state_->GetMsgCopy().pitch) < -1.5 , "Measuring pitch ..."); + TIMED_ASSERT(3.0, (odom_->GetMsgCopy().twist.twist.linear.x > 0.2), + "Measuring the forward and lateral velocities from odom ..."); + + TIMED_ASSERT(5.0, + (fabs(speed_state_->GetMsgCopy().speedX) < 0.05) && + (fabs(speed_state_->GetMsgCopy().speedY) < 0.05) && + (fabs(speed_state_->GetMsgCopy().speedZ) < 0.05) + , "Measuring the speed vector ..."); + + TIMED_ASSERT(5.0, + (fabs(odom_->GetMsgCopy().twist.twist.linear.x) < 0.05) && + (fabs(odom_->GetMsgCopy().twist.twist.linear.y) < 0.05) && + (fabs(odom_->GetMsgCopy().twist.twist.linear.z) < 0.05) + , "Measuring the speed vector from odom..."); + + + tw.linear.x = -0.5; + tw.linear.y = 0.0; + tw.linear.z = 0.0; + tw.angular.z = 0.0; + ROS_WARN("Moving 0.5m backward ..."); + cmdmoveby_pub_.publish(tw); + + TIMED_ASSERT(3.0, angles::to_degrees(att_state_->GetMsgCopy().pitch) > 2.5, "Measuring pitch ..."); + TIMED_ASSERT(3.0, (odom_->GetMsgCopy().twist.twist.linear.x < -0.2), + "Measuring the forward and lateral velocities from odom ..."); + TIMED_ASSERT(5.0, + (fabs(speed_state_->GetMsgCopy().speedX) < 0.025) && + (fabs(speed_state_->GetMsgCopy().speedY) < 0.025) && + (fabs(speed_state_->GetMsgCopy().speedZ) < 0.025) + , "Measuring the speed vector ..."); + + TIMED_ASSERT(5.0, + (fabs(odom_->GetMsgCopy().twist.twist.linear.x) < 0.025) && + (fabs(odom_->GetMsgCopy().twist.twist.linear.y) < 0.025) && + (fabs(odom_->GetMsgCopy().twist.twist.linear.z) < 0.025) + , "Measuring the speed vector from odom..."); + + tw.linear.x = 0.0; + tw.linear.y = -0.5; + tw.linear.z = 0.0; + tw.angular.z = 0.0; + ROS_WARN("Moving 0.5m left ..."); + cmdmoveby_pub_.publish(tw); + + TIMED_ASSERT(3.0, angles::to_degrees(att_state_->GetMsgCopy().roll) < -2.5, "Measuring roll ..."); + TIMED_ASSERT(3.0, (odom_->GetMsgCopy().twist.twist.linear.y > 0.05), + "Measuring the forward and lateral velocities from odom ..."); + + StopBebop(); + TIMED_ASSERT(5.0, + (fabs(speed_state_->GetMsgCopy().speedX) < 0.025) && + (fabs(speed_state_->GetMsgCopy().speedY) < 0.025) && + (fabs(speed_state_->GetMsgCopy().speedZ) < 0.025) + , "Measuring the speed vector ..."); + + TIMED_ASSERT(5.0, + (fabs(odom_->GetMsgCopy().twist.twist.linear.x) < 0.025) && + (fabs(odom_->GetMsgCopy().twist.twist.linear.y) < 0.025) && + (fabs(odom_->GetMsgCopy().twist.twist.linear.z) < 0.025) + , "Measuring the speed vector from odom..."); + + tw.linear.x = 0.0; + tw.linear.y = 0.5; + tw.linear.z = 0.0; + tw.angular.z = 0.0; + ROS_WARN("Moving 0.5m right ..."); + cmdmoveby_pub_.publish(tw); + TIMED_ASSERT(4.0, angles::to_degrees(att_state_->GetMsgCopy().roll) > 1.5, "Measuring roll ..."); + TIMED_ASSERT(3.0, (odom_->GetMsgCopy().twist.twist.linear.y < -0.1), + "Measuring the forward and lateral velocities from odom ..."); + StopBebop(); + TIMED_ASSERT(5.0, + (fabs(speed_state_->GetMsgCopy().speedX) < 0.025) && + (fabs(speed_state_->GetMsgCopy().speedY) < 0.025) && + (fabs(speed_state_->GetMsgCopy().speedZ) < 0.025) + , "Measuring the speed vector ..."); + + TIMED_ASSERT(5.0, + (fabs(odom_->GetMsgCopy().twist.twist.linear.x) < 0.025) && + (fabs(odom_->GetMsgCopy().twist.twist.linear.y) < 0.025) && + (fabs(odom_->GetMsgCopy().twist.twist.linear.z) < 0.025) + , "Measuring the speed vector from odom..."); + + double alt_start, yaw_start, alt_start_odom, yaw_start_odom; + // Make sure altitude is fresh + ASSERT_LT(alt_state_->GetFreshness().toSec(), 0.5); + ASSERT_LT(odom_->GetFreshness().toSec(), 0.5); + + alt_start = alt_state_->GetMsgCopy().altitude; + alt_start_odom = odom_->GetMsgCopy().pose.pose.position.z; + tw.linear.x = 0.0; + tw.linear.y = 0.0; + tw.linear.z = -0.5; + tw.angular.z = 0.0; + ROS_WARN("Ascending for 0.5m ..."); + cmdmoveby_pub_.publish(tw); + TIMED_ASSERT(10.0, + ((alt_state_->GetMsgCopy().altitude - alt_start) >= 0.35) && + ((odom_->GetMsgCopy().pose.pose.position.z - alt_start_odom) >= 0.35) && + (speed_state_->GetMsgCopy().speedZ < -0.05) && + (odom_->GetMsgCopy().twist.twist.linear.z > 0.05), + "Measuring altitude, speed.z and vertical velocity from odom ..."); + + // Make sure altitude is fresh + ASSERT_LT(alt_state_->GetFreshness().toSec(), 0.5); + ASSERT_LT(odom_->GetFreshness().toSec(), 0.5); + alt_start = alt_state_->GetMsgCopy().altitude; + alt_start_odom = odom_->GetMsgCopy().pose.pose.position.z; + tw.linear.x = 0.0; + tw.linear.y = 0.0; + tw.linear.z = 0.5; + tw.angular.z = 0.0; + ROS_WARN("Descending for 0.5m ..."); + cmdmoveby_pub_.publish(tw); + TIMED_ASSERT(10.0, + ((alt_state_->GetMsgCopy().altitude - alt_start) <= -0.35) && + ((odom_->GetMsgCopy().pose.pose.position.z - alt_start_odom) <= -0.35) && + (speed_state_->GetMsgCopy().speedZ > 0.05) && + (odom_->GetMsgCopy().twist.twist.linear.z < -0.05), + "Measuring altitude, speed.z and vertical velocity from odom ..."); + + // Make sure the attitude is fresh + ASSERT_LT(att_state_->GetFreshness().toSec(), 0.5); + ASSERT_LT(odom_->GetFreshness().toSec(), 0.5); + yaw_start = att_state_->GetMsgCopy().yaw; + tw.linear.x = 0.0; + tw.linear.y = 0.0; + tw.linear.z = 0.0; + tw.angular.z = 0.5 * 3.141596; + ROS_WARN("Rotating CW for 90 degrees ..."); + cmdmoveby_pub_.publish(tw); + + // TODO(mani-monaj): Add yaw + TIMED_ASSERT(10.0, + angles::normalize_angle(att_state_->GetMsgCopy().yaw - yaw_start) >= 0.45 * 3.141596, + "Measuring Yaw"); + + + // Make sure the attitude is fresh + ASSERT_LT(att_state_->GetFreshness().toSec(), 0.5); + yaw_start = att_state_->GetMsgCopy().yaw; + tw.linear.x = 0.0; + tw.linear.y = 0.0; + tw.linear.z = 0.0; + tw.angular.z = -0.5 * 3.141596; + ROS_WARN("Rotating CCW for 90 degrees ..."); + cmdmoveby_pub_.publish(tw); + TIMED_ASSERT(10.0, + angles::normalize_angle(att_state_->GetMsgCopy().yaw - yaw_start) <= -0.45 * 3.141596, + "Measuring Yaw"); + + /* By this time, battery state must have been changed (even on Bebop 2) */ + TIMED_ASSERT(20.0, bat_state_->IsActive(), "Measuring battery ..."); + const uint8_t bat_percent = bat_state_->GetMsgCopy().percent; + + tw.linear.x = 0.0; + tw.linear.y = 0.0; + tw.linear.z = 0.0; + tw.angular.z = 0.0; + ROS_WARN("Stop ..."); + cmdvel_pub_.publish(tw); + + ROS_WARN("Landing ..."); + land_pub_.publish(em); + + TIMED_ASSERT(10.0, + flying_state_->IsActive() && flying_state_->GetMsgCopy().state == + bebop_msgs::Ardrone3PilotingStateFlyingStateChanged::state_landed, + "Waiting for land to finish..." + ); + + ASSERT_GE(bat_percent - bat_state_->GetMsgCopy().percent, 0); } diff --git a/bebop_msgs/package.xml b/bebop_msgs/package.xml index 4d36c06..b74c10e 100644 --- a/bebop_msgs/package.xml +++ b/bebop_msgs/package.xml @@ -5,6 +5,10 @@ Common message definitions for bebop_autonomy Mani Monajjemi + Sepehr MohaimenianPour + Thomas Bamford + Tobias Naegeli + BSD http://wiki.ros.org/bebop_msgs http://github.com/AutonomyLab/bebop_autonomy diff --git a/bebop_tools/package.xml b/bebop_tools/package.xml index e9d3739..0f59d8e 100644 --- a/bebop_tools/package.xml +++ b/bebop_tools/package.xml @@ -5,6 +5,10 @@ Miscellaneous tools for bebop_autonomy metapackage Mani Monajjemi + Sepehr MohaimenianPour + Thomas Bamford + Tobias Naegeli + BSD http://wiki.ros.org/bebop_tools diff --git a/docs/contribute.rst b/docs/contribute.rst index dbec0ef..7c578da 100644 --- a/docs/contribute.rst +++ b/docs/contribute.rst @@ -45,6 +45,9 @@ List of Contributers - `Sepehr Mohaimenianpour `_ +- `Andreas Ziegler `_ + - `Added wrapper to the move by and the set picture format commands `_ + Acknowledgments ================ diff --git a/docs/index.rst b/docs/index.rst index 3b35906..45e559e 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -2,13 +2,12 @@ bebop_autonomy - ROS Driver for Parrot Bebop Drone (quadrocopter) 1.0 & 2.0 **************************************************************************** -*bebop_autonomy* is a :abbr:`ROS (Robot Operating System)` driver for `Parrot Bebop 1.0 `_ and `2.0 `_ drones (quadrocopters), based on Parrot's official `ARDroneSDK3 `_. This driver has been developed in `Autonomy Lab `_ of `Simon Fraser University `_ by `Mani Monajjemi `_ and other contributers (:ref:`sec-contribs`). +*bebop_autonomy* is a :abbr:`ROS (Robot Operating System)` driver for `Parrot Bebop 1.0 `_ and `2.0 `_ drones (quadrocopters), based on Parrot's official `ARDroneSDK3 `_. This driver has been developed in `Autonomy Lab `_ of `Simon Fraser University `_ by `Mani Monajjemi `_ and other contributers (:ref:`sec-contribs`). This software is maintained by `Sepehr MohaimenianPour `_ (AutonomyLab, Simon Fraser University), `Thomas Bamford <#>`_ (Dynamic Systems Lab, University of Toronto) and `Tobias Naegeli `_ (Advanced Interactive Technologies Lab, ETH Zürich). [`Source Code `_] [`ROS wiki page `_] [`Support `_] [`Bug Tracker `_] -[`Developer Forum `_] .. _sec-roadmap: diff --git a/docs/installation.rst b/docs/installation.rst index 4c6aec6..1a4676e 100644 --- a/docs/installation.rst +++ b/docs/installation.rst @@ -29,6 +29,5 @@ To compile from source, you need to clone the source code in a new or existing ` $ rosdep update $ rosdep install --from-paths src -i # Build the workspace - $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo - + $ catkin build diff --git a/docs/piloting.rst b/docs/piloting.rst index 9aed49b..8cc2421 100644 --- a/docs/piloting.rst +++ b/docs/piloting.rst @@ -62,6 +62,19 @@ The ``linear.z`` part of this message controls the **vertical velocity** of the ver_vel_m_per_s = linear.z * max_vert_speed rot_vel_deg_per_s = angular.z * max_rot_speed +A second possibility to pilot the Bebop is by publishing messages of type to `geometry_msgs/Twist `_ to `cmd_move_by` topic while Bebop is flying. The effect of each field of the message on Bebop's movement is listed below: + +.. code-block:: text + + linear.x (+) Distance (in meters) to translate forward + (-) Distance (in meters) to translate backward + linear.y (+) Distance (in meters) to translate to left + (-) Distance (in meters) to translate to right + linear.z (+) Distance (in meters) to ascend + (-) Distance (in meters) to descend + angular.z (+) Angle (in rad) to rotate counter clockwise + (-) Angle (in rad) to rotate clockwise + Moving the Virtual Camera ========================= @@ -174,6 +187,18 @@ To take a high resolution on-board snapshot, publish a ``std_msgs/Empty`` messag * Username: ``anonymous`` * Password: ** +Set picture format +================== + +To change the format of the snapshots, publish a ``std_msgs/UInt8`` message on ``set_picture_format`` topic. The format will then be changed according to the following settings: + +.. code-block:: text + + 0 Raw image + 1 4:3 jpeg image + 2 16:9 Snapshot from camera + 3 jpeg fisheye image only + Set camera exposure ===================