From 8e1a5701fd642440520eabaa61df9f5a32fd51b9 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Fri, 31 Mar 2017 15:02:47 +0200 Subject: [PATCH 01/25] Added cmd_move_by command. --- .../cfg/autogenerated/BebopArdrone3.cfg | 2 +- .../autogenerated/ardrone3_state_callbacks.h | 8 +++++++- bebop_driver/include/bebop_driver/bebop.h | 1 + .../bebop_driver/bebop_driver_nodelet.h | 2 ++ bebop_driver/scripts/meta/generate.py | 2 +- .../scripts/meta/templates/cfg.mustache | 2 +- bebop_driver/src/bebop.cpp | 14 ++++++++++++++ bebop_driver/src/bebop_driver_nodelet.cpp | 18 ++++++++++++++++++ bebop_driver/src/bebop_video_decoder.cpp | 6 +++--- 9 files changed, 48 insertions(+), 7 deletions(-) diff --git a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg index 137258d..f017465 100755 --- a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg +++ b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python2 # BebopArdrone3.cfg # auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/e73425074471c58561d04c85da4a6400b638779d/xml/ardrone3.xml diff --git a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h index 6be19a5..b3867f7 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h +++ b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h @@ -870,6 +870,12 @@ class Ardrone3PilotingStateAltitudeChanged : public AbstractState }; // Ardrone3PilotingStateAltitudeChanged +// Relative move ended.\n Informs about the move that the drone managed to do and why it stopped. +class Ardrone3RelativeMoveEnded : public AbstractState +{ + +}; // Ardrone3RelativeMoveEnded + // Drones location changed.\n This event is meant to replace [PositionChanged](#1-4-4). class Ardrone3PilotingStateGpsLocationChanged : public AbstractState @@ -1991,4 +1997,4 @@ class Ardrone3PROStateFeatures : public AbstractState } // namespace cb } // namespace bebop_driver -#endif // BEBOP_AUTONOMY_AUTOGENERATED_ardrone3_STATE_CALLBACKS_H \ No newline at end of file +#endif // BEBOP_AUTONOMY_AUTOGENERATED_ardrone3_STATE_CALLBACKS_H diff --git a/bebop_driver/include/bebop_driver/bebop.h b/bebop_driver/include/bebop_driver/bebop.h index 1381ff9..b103fc6 100644 --- a/bebop_driver/include/bebop_driver/bebop.h +++ b/bebop_driver/include/bebop_driver/bebop.h @@ -169,6 +169,7 @@ class Bebop // -1..1 void Move(const double& roll, const double& pitch, const double& gaz_speed, const double& yaw_speed); + void MoveBy(const float& dX, const float& dY, const float& dZ, const float& dPsi); void MoveCamera(const double& tilt, const double& pan); void TakeSnapshot(); diff --git a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h index c0dbdb4..279bd55 100644 --- a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h +++ b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h @@ -110,6 +110,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_; @@ -149,6 +150,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); diff --git a/bebop_driver/scripts/meta/generate.py b/bebop_driver/scripts/meta/generate.py index e4d97b5..8d93a86 100755 --- a/bebop_driver/scripts/meta/generate.py +++ b/bebop_driver/scripts/meta/generate.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python2 import datetime from copy import deepcopy diff --git a/bebop_driver/scripts/meta/templates/cfg.mustache b/bebop_driver/scripts/meta/templates/cfg.mustache index 4af784d..db94ee7 100644 --- a/bebop_driver/scripts/meta/templates/cfg.mustache +++ b/bebop_driver/scripts/meta/templates/cfg.mustache @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python2 # {{cfg_filename}} # auto-generated from {{url}} diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp index 7285318..a52d196 100644 --- a/bebop_driver/src/bebop.cpp +++ b/bebop_driver/src/bebop.cpp @@ -541,6 +541,20 @@ void Bebop::Move(const double &roll, const double &pitch, const double &gaz_spee } } + +void Bebop::MoveBy(const float& dX, const float& dY, const float& dZ, const float& 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) { diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index e0a32a2..87e70aa 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/bebop_driver/src/bebop_driver_nodelet.cpp @@ -121,6 +121,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); @@ -219,6 +220,23 @@ 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 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) From 23fb05ede6902accb74e0c691ad132f69fe90ed6 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Fri, 31 Mar 2017 15:29:16 +0200 Subject: [PATCH 02/25] Removed Arch Linux specific python changes. --- bebop_driver/cfg/autogenerated/BebopArdrone3.cfg | 4 ++-- bebop_driver/scripts/meta/generate.py | 2 +- bebop_driver/scripts/meta/templates/cfg.mustache | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg index f017465..81f74ca 100755 --- a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg +++ b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python # BebopArdrone3.cfg # auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/e73425074471c58561d04c85da4a6400b638779d/xml/ardrone3.xml @@ -135,4 +135,4 @@ gpssettings_group_gen.add("GPSSettingsHomeTypeType", int_t, 0, "The type of the gpssettings_group_gen.add("GPSSettingsReturnHomeDelayDelay", int_t, 0, "Delay in second", 0 , 0, 120) -exit(gen.generate(PACKAGE, "bebop_driver_nodelet", "BebopArdrone3")) \ No newline at end of file +exit(gen.generate(PACKAGE, "bebop_driver_nodelet", "BebopArdrone3")) diff --git a/bebop_driver/scripts/meta/generate.py b/bebop_driver/scripts/meta/generate.py index 8d93a86..e4d97b5 100755 --- a/bebop_driver/scripts/meta/generate.py +++ b/bebop_driver/scripts/meta/generate.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python import datetime from copy import deepcopy diff --git a/bebop_driver/scripts/meta/templates/cfg.mustache b/bebop_driver/scripts/meta/templates/cfg.mustache index db94ee7..6a25386 100644 --- a/bebop_driver/scripts/meta/templates/cfg.mustache +++ b/bebop_driver/scripts/meta/templates/cfg.mustache @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python # {{cfg_filename}} # auto-generated from {{url}} @@ -32,4 +32,4 @@ gen = ParameterGenerator() {{/cfg_class}} -exit(gen.generate(PACKAGE, "bebop_driver_nodelet", "Bebop{{project}}")) \ No newline at end of file +exit(gen.generate(PACKAGE, "bebop_driver_nodelet", "Bebop{{project}}")) From b7636d664d5d4d5bc374c1b41c4bec90d90e7273 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Tue, 11 Apr 2017 15:54:45 +0200 Subject: [PATCH 03/25] Added SetPictureFormat --- bebop_driver/include/bebop_driver/bebop.h | 6 ++++++ .../include/bebop_driver/bebop_driver_nodelet.h | 2 ++ bebop_driver/src/bebop.cpp | 12 ++++++++++++ bebop_driver/src/bebop_driver_nodelet.cpp | 15 +++++++++++++++ 4 files changed, 35 insertions(+) diff --git a/bebop_driver/include/bebop_driver/bebop.h b/bebop_driver/include/bebop_driver/bebop.h index 4ef5910..2211229 100644 --- a/bebop_driver/include/bebop_driver/bebop.h +++ b/bebop_driver/include/bebop_driver/bebop.h @@ -174,6 +174,12 @@ class Bebop 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 7bf950f..773111e 100644 --- a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h +++ b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h @@ -123,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_; @@ -164,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/src/bebop.cpp b/bebop_driver/src/bebop.cpp index ab241e1..6591db3 100644 --- a/bebop_driver/src/bebop.cpp +++ b/bebop_driver/src/bebop.cpp @@ -621,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 e91b373..6c9aeaa 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 @@ -145,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); @@ -410,6 +412,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 %f", 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 From ecaf843226fb45db3cb7d72d984dc508bb262499 Mon Sep 17 00:00:00 2001 From: Mani Monajjemi Date: Mon, 12 Jun 2017 22:39:37 -0700 Subject: [PATCH 04/25] Add maintainers to readme, docs and manifest (closes #115) --- README.md | 8 +++++++- bebop_autonomy/package.xml | 4 ++++ bebop_description/package.xml | 4 ++++ bebop_driver/package.xml | 4 ++++ bebop_msgs/package.xml | 4 ++++ bebop_tools/package.xml | 4 ++++ docs/index.rst | 3 +-- 7 files changed, 28 insertions(+), 3 deletions(-) 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/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_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/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: From 81d01cccd3d6466519387811220225e7de0b892c Mon Sep 17 00:00:00 2001 From: Mani Monajjemi Date: Mon, 26 Jun 2017 23:27:17 -0700 Subject: [PATCH 05/25] Remove the Release build flag from the documentation Relates to #93 #107 #111 --- docs/installation.rst | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 From 6e51598e800f7e9eaa6fb59643cb5599696ed05d Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Fri, 31 Mar 2017 15:02:47 +0200 Subject: [PATCH 06/25] Added cmd_move_by command. --- bebop_driver/cfg/autogenerated/BebopArdrone3.cfg | 2 +- bebop_driver/scripts/meta/generate.py | 2 +- bebop_driver/scripts/meta/templates/cfg.mustache | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg index 81f74ca..03de398 100755 --- a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg +++ b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python2 # BebopArdrone3.cfg # auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/e73425074471c58561d04c85da4a6400b638779d/xml/ardrone3.xml diff --git a/bebop_driver/scripts/meta/generate.py b/bebop_driver/scripts/meta/generate.py index e4d97b5..8d93a86 100755 --- a/bebop_driver/scripts/meta/generate.py +++ b/bebop_driver/scripts/meta/generate.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python2 import datetime from copy import deepcopy diff --git a/bebop_driver/scripts/meta/templates/cfg.mustache b/bebop_driver/scripts/meta/templates/cfg.mustache index 6a25386..85c61b6 100644 --- a/bebop_driver/scripts/meta/templates/cfg.mustache +++ b/bebop_driver/scripts/meta/templates/cfg.mustache @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python2 # {{cfg_filename}} # auto-generated from {{url}} From ff19906f9d2783fa45a0fc60d8fd1638012d81a1 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Fri, 31 Mar 2017 15:29:16 +0200 Subject: [PATCH 07/25] Removed Arch Linux specific python changes. --- bebop_driver/cfg/autogenerated/BebopArdrone3.cfg | 2 +- bebop_driver/scripts/meta/generate.py | 2 +- bebop_driver/scripts/meta/templates/cfg.mustache | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg index 03de398..81f74ca 100755 --- a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg +++ b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python # BebopArdrone3.cfg # auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/e73425074471c58561d04c85da4a6400b638779d/xml/ardrone3.xml diff --git a/bebop_driver/scripts/meta/generate.py b/bebop_driver/scripts/meta/generate.py index 8d93a86..e4d97b5 100755 --- a/bebop_driver/scripts/meta/generate.py +++ b/bebop_driver/scripts/meta/generate.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python import datetime from copy import deepcopy diff --git a/bebop_driver/scripts/meta/templates/cfg.mustache b/bebop_driver/scripts/meta/templates/cfg.mustache index 85c61b6..6a25386 100644 --- a/bebop_driver/scripts/meta/templates/cfg.mustache +++ b/bebop_driver/scripts/meta/templates/cfg.mustache @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python # {{cfg_filename}} # auto-generated from {{url}} From 44353d1a1d23f720f0d12641ca183773e8e05fb6 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Wed, 12 Jul 2017 10:24:47 +0200 Subject: [PATCH 08/25] Reverted changes to autogenerated file. --- bebop_driver/cfg/autogenerated/BebopArdrone3.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg index 81f74ca..137258d 100755 --- a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg +++ b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg @@ -135,4 +135,4 @@ gpssettings_group_gen.add("GPSSettingsHomeTypeType", int_t, 0, "The type of the gpssettings_group_gen.add("GPSSettingsReturnHomeDelayDelay", int_t, 0, "Delay in second", 0 , 0, 120) -exit(gen.generate(PACKAGE, "bebop_driver_nodelet", "BebopArdrone3")) +exit(gen.generate(PACKAGE, "bebop_driver_nodelet", "BebopArdrone3")) \ No newline at end of file From a5dfd32742213a10b2bdf9ca244f07d26d32c25b Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Wed, 12 Jul 2017 12:25:37 +0200 Subject: [PATCH 09/25] Revert changes in an autogenerated file. --- .../bebop_driver/autogenerated/ardrone3_state_callbacks.h | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h index b3867f7..6be19a5 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h +++ b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h @@ -870,12 +870,6 @@ class Ardrone3PilotingStateAltitudeChanged : public AbstractState }; // Ardrone3PilotingStateAltitudeChanged -// Relative move ended.\n Informs about the move that the drone managed to do and why it stopped. -class Ardrone3RelativeMoveEnded : public AbstractState -{ - -}; // Ardrone3RelativeMoveEnded - // Drones location changed.\n This event is meant to replace [PositionChanged](#1-4-4). class Ardrone3PilotingStateGpsLocationChanged : public AbstractState @@ -1997,4 +1991,4 @@ class Ardrone3PROStateFeatures : public AbstractState } // namespace cb } // namespace bebop_driver -#endif // BEBOP_AUTONOMY_AUTOGENERATED_ardrone3_STATE_CALLBACKS_H +#endif // BEBOP_AUTONOMY_AUTOGENERATED_ardrone3_STATE_CALLBACKS_H \ No newline at end of file From 3ddc996e0170b3394fe3caba8a05053a21dba050 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Wed, 12 Jul 2017 12:30:23 +0200 Subject: [PATCH 10/25] Reverted changes in a template. --- bebop_driver/scripts/meta/templates/cfg.mustache | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bebop_driver/scripts/meta/templates/cfg.mustache b/bebop_driver/scripts/meta/templates/cfg.mustache index 6a25386..4af784d 100644 --- a/bebop_driver/scripts/meta/templates/cfg.mustache +++ b/bebop_driver/scripts/meta/templates/cfg.mustache @@ -32,4 +32,4 @@ gen = ParameterGenerator() {{/cfg_class}} -exit(gen.generate(PACKAGE, "bebop_driver_nodelet", "Bebop{{project}}")) +exit(gen.generate(PACKAGE, "bebop_driver_nodelet", "Bebop{{project}}")) \ No newline at end of file From dc373e73ceef0aea7847a83a24194b37744eb453 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Wed, 12 Jul 2017 12:55:43 +0200 Subject: [PATCH 11/25] Changed floats to doubles in MoveBy signature. --- bebop_driver/include/bebop_driver/bebop.h | 2 +- bebop_driver/src/bebop.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/bebop_driver/include/bebop_driver/bebop.h b/bebop_driver/include/bebop_driver/bebop.h index 2211229..3fbe7c3 100644 --- a/bebop_driver/include/bebop_driver/bebop.h +++ b/bebop_driver/include/bebop_driver/bebop.h @@ -170,7 +170,7 @@ class Bebop // -1..1 void Move(const double& roll, const double& pitch, const double& gaz_speed, const double& yaw_speed); - void MoveBy(const float& dX, const float& dY, const float& dZ, const float& dPsi); + void MoveBy(const double& dX, const double& dY, const double& dZ, const double& dPsi); void MoveCamera(const double& tilt, const double& pan); void TakeSnapshot(); diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp index 6591db3..854c0b4 100644 --- a/bebop_driver/src/bebop.cpp +++ b/bebop_driver/src/bebop.cpp @@ -550,7 +550,7 @@ void Bebop::Move(const double &roll, const double &pitch, const double &gaz_spee } -void Bebop::MoveBy(const float& dX, const float& dY, const float& dZ, const float& dPsi) +void Bebop::MoveBy(const double& dX, const double& dY, const double& dZ, const double& dPsi) { ThrowOnInternalError("MoveBy failure"); From 4c990b0aa75688ffe2a4f6c6f35b7baf255da09b Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Wed, 12 Jul 2017 13:01:11 +0200 Subject: [PATCH 12/25] Removed blank line. --- bebop_driver/src/bebop_driver_nodelet.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index 6c9aeaa..c21950e 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/bebop_driver/src/bebop_driver_nodelet.cpp @@ -240,7 +240,6 @@ void BebopDriverNodelet::CmdMoveByCallback(const geometry_msgs::TwistConstPtr& t try { const geometry_msgs::Twist& bebop_twist_ = *twist_ptr; - bebop_ptr_->MoveBy(bebop_twist_.linear.x, bebop_twist_.linear.y, bebop_twist_.linear.z, From 20c57aadd7de48ac940734cec61e45799d2d00e6 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Wed, 12 Jul 2017 13:07:14 +0200 Subject: [PATCH 13/25] Corrcted type for unit8_t. --- bebop_driver/src/bebop_driver_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index c21950e..7d6b520 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/bebop_driver/src/bebop_driver_nodelet.cpp @@ -415,7 +415,7 @@ void BebopDriverNodelet::SetPictureFormatCallback(const std_msgs::UInt8ConstPtr& { try { - ROS_INFO("Setting picture format to %f", format_ptr->data); + ROS_INFO("Setting picture format to %u", format_ptr->data); bebop_ptr_->SetPictureFormat(format_ptr->data); } catch (const std::runtime_error& e) From 1cf2eecebf81896c96d3132a08d283f8422ed6bf Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Wed, 12 Jul 2017 13:14:10 +0200 Subject: [PATCH 14/25] Added entry in the contribute list. --- docs/contribute.rst | 3 +++ 1 file changed, 3 insertions(+) diff --git a/docs/contribute.rst b/docs/contribute.rst index dbec0ef..d465294 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 command `_ + Acknowledgments ================ From 15089f9103d3f44a148f264baec54aec0f8b4e4b Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Wed, 12 Jul 2017 14:15:08 +0200 Subject: [PATCH 15/25] Added documentation for the command move by. --- docs/piloting.rst | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/docs/piloting.rst b/docs/piloting.rst index 9aed49b..37d140a 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 ========================= From 19b5ad04b5264166a87aab9239d770cd5ede714f Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Wed, 12 Jul 2017 15:26:36 +0200 Subject: [PATCH 16/25] Added documentation of the set_picture_format command. --- docs/piloting.rst | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/docs/piloting.rst b/docs/piloting.rst index 37d140a..8cc2421 100644 --- a/docs/piloting.rst +++ b/docs/piloting.rst @@ -187,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 =================== From 90035f9af750799833686cb40f1dbc288fe5f973 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Wed, 12 Jul 2017 15:30:04 +0200 Subject: [PATCH 17/25] Adapted entry in the contribute list. --- docs/contribute.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/contribute.rst b/docs/contribute.rst index d465294..7c578da 100644 --- a/docs/contribute.rst +++ b/docs/contribute.rst @@ -45,8 +45,8 @@ List of Contributers - `Sepehr Mohaimenianpour `_ -- `Andreas Ziegler `_ - - `Added wrapper to the move by command `_ +- `Andreas Ziegler `_ + - `Added wrapper to the move by and the set picture format commands `_ Acknowledgments ================ From ee7381264a03c74c072d09ea950adceda7441c25 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Thu, 13 Jul 2017 08:48:32 +0200 Subject: [PATCH 18/25] Added unit test for move by command. --- bebop_driver/test/bebop_itl_test.cpp | 226 +++++++++++++++++++++++++++ 1 file changed, 226 insertions(+) diff --git a/bebop_driver/test/bebop_itl_test.cpp b/bebop_driver/test/bebop_itl_test.cpp index caf55c7..2bb82fa 100644 --- a/bebop_driver/test/bebop_itl_test.cpp +++ b/bebop_driver/test/bebop_itl_test.cpp @@ -195,6 +195,7 @@ 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( @@ -471,6 +472,231 @@ TEST_F(BebopInTheLoopTest, Piloting) } +TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) +{ + 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); + + // 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) < -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) + , "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 ..."); + StopBebop(); + + 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.1), + "Measuring the forward and lateral velocities from odom ..."); + + StopBebop(); + + 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(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); + + 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.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; + 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.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; + 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.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; + 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.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; + + 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..." + ); + + // emergency state is transient (unlike ardrone), we may miss the signal +// ROS_WARN("Emergency ..."); +// reset_pub.publish(em); + +// TIMED_ASSERT(5.0, +// flying_state->IsActive() && flying_state->GetMsgCopy().state == +// bebop_msgs::Ardrone3PilotingStateFlyingStateChanged::state_emergency, +// "Waiting for reset to happen..." +// ); + + ASSERT_GE(bat_percent - bat_state->GetMsgCopy().percent, 0); +} + + // TODO(mani-monaj): Add value tests TEST_F(BebopInTheLoopTest, StaticTFTest) { From ffed1bdd1fe63e2dcf84219f098a3e0bfb6ce2c7 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Mon, 17 Jul 2017 12:14:55 +0200 Subject: [PATCH 19/25] Adapted generate script to use the current (3.12.6) message definitions. --- bebop_driver/scripts/meta/generate.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/bebop_driver/scripts/meta/generate.py b/bebop_driver/scripts/meta/generate.py index e4d97b5..eda8db9 100755 --- a/bebop_driver/scripts/meta/generate.py +++ b/bebop_driver/scripts/meta/generate.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python2 import datetime from copy import deepcopy @@ -12,9 +12,9 @@ import subprocess import urllib2 -# SDK 3.11.0: https://github.com/Parrot-Developers/arsdk_manifests/blob/a6acc778f9d4d852985cccba91b854630921768e/release.xml +# SDK 3.12.6: https://github.com/Parrot-Developers/arsdk_manifests/blob/ARSDK3_version_3_12_6/release.xml LIBARCOMMANDS_GIT_OWNER = "Parrot-Developers" -LIBARCOMMANDS_GIT_HASH = "e73425074471c58561d04c85da4a6400b638779d" +LIBARCOMMANDS_GIT_HASH = "ab28dab91845cd36c4d7002b55f70805deaff3c8" # From XML types to ROS primitive types ROS_TYPE_MAP = { From f92e45ffd259cd676b3be7dbb4f34daceb51ff87 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Mon, 17 Jul 2017 12:19:18 +0200 Subject: [PATCH 20/25] Updated autogenerated files with the adapted gnerate python script. --- .../cfg/autogenerated/BebopArdrone3.cfg | 2 +- .../autogenerated/common_state_callbacks.h | 6 ++-- bebop_driver/scripts/meta/last_build_info | 6 ++-- ...one3AccessoryStateConnectedAccessories.msg | 22 ++++++++++++++ .../Ardrone3PilotingStatemoveToChanged.msg | 30 +++++++++++++++++++ ...ightPlanStateComponentStateListChanged.msg | 9 +++--- .../CommonMavlinkStateMissionItemExecuted.msg | 12 ++++++++ 7 files changed, 76 insertions(+), 11 deletions(-) create mode 100644 bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg create mode 100644 bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg create mode 100644 bebop_msgs/msg/autogenerated/CommonMavlinkStateMissionItemExecuted.msg diff --git a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg index 137258d..f017465 100755 --- a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg +++ b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python2 # BebopArdrone3.cfg # auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/e73425074471c58561d04c85da4a6400b638779d/xml/ardrone3.xml diff --git a/bebop_driver/include/bebop_driver/autogenerated/common_state_callbacks.h b/bebop_driver/include/bebop_driver/autogenerated/common_state_callbacks.h index f0267dc..2387cfe 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/common_state_callbacks.h +++ b/bebop_driver/include/bebop_driver/autogenerated/common_state_callbacks.h @@ -1227,7 +1227,7 @@ class CommonMavlinkStateMissonItemExecuted : public AbstractState public: CommonMavlinkStateMissonItemExecuted(::ros::NodeHandle& nh, ::ros::NodeHandle& priv_nh, const ::std::string& topic) - : AbstractState(ARCONTROLLER_DICTIONARY_KEY_COMMON_MAVLINKSTATE_MISSONITEMEXECUTED) + : AbstractState(ARCONTROLLER_DICTIONARY_KEY_COMMON_MAVLINKSTATE_MISSIONITEMEXECUTED) { if (priv_nh.getParam("states/enable_mavlinkstate_missonitemexecuted", pub_enabled_) && pub_enabled_) { @@ -1257,7 +1257,7 @@ class CommonMavlinkStateMissonItemExecuted : public AbstractState arg = NULL; - HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_MAVLINKSTATE_MISSONITEMEXECUTED_IDX, arg); + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_MAVLINKSTATE_MISSIONITEMEXECUTED_IDX, arg); if (arg) { msg_ptr->idx = arg->value.U32; @@ -2484,4 +2484,4 @@ class CommonRunStateRunIdChanged : public AbstractState } // namespace cb } // namespace bebop_driver -#endif // BEBOP_AUTONOMY_AUTOGENERATED_common_STATE_CALLBACKS_H \ No newline at end of file +#endif // BEBOP_AUTONOMY_AUTOGENERATED_common_STATE_CALLBACKS_H diff --git a/bebop_driver/scripts/meta/last_build_info b/bebop_driver/scripts/meta/last_build_info index 6c76209..5083f88 100644 --- a/bebop_driver/scripts/meta/last_build_info +++ b/bebop_driver/scripts/meta/last_build_info @@ -1,5 +1,5 @@ Last auto-generation build info: -- Source hash e73425074471c58561d04c85da4a6400b638779d -- Date 2017-02-10 13:22:17.661636 -- Generator generate.py @ adc1afe +- Source hash ab28dab91845cd36c4d7002b55f70805deaff3c8 +- Date 2017-07-17 12:04:26.902273 +- Generator generate.py @ ee73812 diff --git a/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg b/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg new file mode 100644 index 0000000..a7f828f --- /dev/null +++ b/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg @@ -0,0 +1,22 @@ +# Ardrone3AccessoryStateConnectedAccessories +# auto-generated from up stream XML files at +# github.com/Parrot-Developers/libARCommands/tree/master/Xml +# To check upstream commit hash, refer to last_build_info file +# Do not modify this file by hand. Check scripts/meta folder for generator files. +# +# SDK Comment: List of all connected accessories. This event presents the list of all connected accessories. To actually use the component, use the component dedicated feature. + +Header header + +# Id of the accessory for the session. +uint8 id +# Accessory type +uint8 accessory_type_sequoia=0 # Parrot Sequoia (multispectral camera for agriculture) +uint8 accessory_type_unknownaccessory_1=1 # UNKNOWNACCESSORY_1 camera (thermal+rgb camera) +uint8 accessory_type +# Unique Id of the accessory. This id is unique by accessory_type. +string uid +# Software Version of the accessory. +string swVersion +# List entry attribute Bitfield. 0x01: First: indicate its the first element of the list. 0x02: Last: indicate its the last element of the list. 0x04: Empty: indicate the list is empty (implies First/Last). All other arguments should be ignored. 0x08: Remove: This value should be removed from the existing list. +uint8 list_flags diff --git a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg new file mode 100644 index 0000000..4ba4ccd --- /dev/null +++ b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg @@ -0,0 +1,30 @@ +# Ardrone3PilotingStatemoveToChanged +# auto-generated from up stream XML files at +# github.com/Parrot-Developers/libARCommands/tree/master/Xml +# To check upstream commit hash, refer to last_build_info file +# Do not modify this file by hand. Check scripts/meta folder for generator files. +# +# SDK Comment: The drone moves or moved to a given location. + +Header header + +# Latitude of the location (in degrees) to reach +float64 latitude +# Longitude of the location (in degrees) to reach +float64 longitude +# Altitude above sea level (in m) to reach +float64 altitude +# Orientation mode of the move to +uint8 orientation_mode_NONE=0 # The drone wont change its orientation +uint8 orientation_mode_TO_TARGET=1 # The drone will make a rotation to look in direction of the given location +uint8 orientation_mode_HEADING_START=2 # The drone will orientate itself to the given heading before moving to the location +uint8 orientation_mode_HEADING_DURING=3 # The drone will orientate itself to the given heading while moving to the location +uint8 orientation_mode +# Heading (relative to the North in degrees). This value is only used if the orientation mode is HEADING_START or HEADING_DURING +float32 heading +# Status of the move to +uint8 status_RUNNING=0 # The drone is actually flying to the given position +uint8 status_DONE=1 # The drone has reached the target +uint8 status_CANCELED=2 # The move to has been canceled, either by a new moveTo command or by a CancelMoveTo command. +uint8 status_ERROR=3 # The move to has not been finished or started because of an error. +uint8 status diff --git a/bebop_msgs/msg/autogenerated/CommonFlightPlanStateComponentStateListChanged.msg b/bebop_msgs/msg/autogenerated/CommonFlightPlanStateComponentStateListChanged.msg index edefd5e..bafa4d7 100644 --- a/bebop_msgs/msg/autogenerated/CommonFlightPlanStateComponentStateListChanged.msg +++ b/bebop_msgs/msg/autogenerated/CommonFlightPlanStateComponentStateListChanged.msg @@ -9,10 +9,11 @@ Header header # Drone FlightPlan component id (unique) -uint8 component_GPS=0 # GPS for Drone FlightPlan -uint8 component_Calibration=1 # Calibration for Drone FlightPlan -uint8 component_Mavlink_File=2 # Mavlink file for Drone FlightPlan -uint8 component_TakeOff=3 # Take off +uint8 component_GPS=0 # Drone GPS component. State is 0 when the drone needs a GPS fix. +uint8 component_Calibration=1 # Drone Calibration component. State is 0 when the sensors of the drone needs to be calibrated. +uint8 component_Mavlink_File=2 # Mavlink file component. State is 0 when the mavlink file is missing or contains error. +uint8 component_TakeOff=3 # Drone Take off component. State is 0 when the drone cannot take-off. +uint8 component_WaypointsBeyondGeofence=4 # Component for waypoints beyond the geofence. State is 0 when one or more waypoints are beyond the geofence. uint8 component # State of the FlightPlan component (1 FlightPlan component OK, otherwise 0) uint8 State diff --git a/bebop_msgs/msg/autogenerated/CommonMavlinkStateMissionItemExecuted.msg b/bebop_msgs/msg/autogenerated/CommonMavlinkStateMissionItemExecuted.msg new file mode 100644 index 0000000..6c2b22e --- /dev/null +++ b/bebop_msgs/msg/autogenerated/CommonMavlinkStateMissionItemExecuted.msg @@ -0,0 +1,12 @@ +# CommonMavlinkStateMissionItemExecuted +# auto-generated from up stream XML files at +# github.com/Parrot-Developers/libARCommands/tree/master/Xml +# To check upstream commit hash, refer to last_build_info file +# Do not modify this file by hand. Check scripts/meta folder for generator files. +# +# SDK Comment: Mission item has been executed. + +Header header + +# Index of the mission item. This is the place of the mission item in the list of the items of the mission. Begins at 0. +uint32 idx From 0aba6ddccc35f4a735280e1f80e44d3bc83344d0 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Tue, 18 Jul 2017 06:12:23 +0200 Subject: [PATCH 21/25] Revert "Updated autogenerated files with the adapted gnerate python script." This reverts commit f92e45ffd259cd676b3be7dbb4f34daceb51ff87. --- .../cfg/autogenerated/BebopArdrone3.cfg | 2 +- .../autogenerated/common_state_callbacks.h | 6 ++-- bebop_driver/scripts/meta/last_build_info | 6 ++-- ...one3AccessoryStateConnectedAccessories.msg | 22 -------------- .../Ardrone3PilotingStatemoveToChanged.msg | 30 ------------------- ...ightPlanStateComponentStateListChanged.msg | 9 +++--- .../CommonMavlinkStateMissionItemExecuted.msg | 12 -------- 7 files changed, 11 insertions(+), 76 deletions(-) delete mode 100644 bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg delete mode 100644 bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg delete mode 100644 bebop_msgs/msg/autogenerated/CommonMavlinkStateMissionItemExecuted.msg diff --git a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg index f017465..137258d 100755 --- a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg +++ b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python # BebopArdrone3.cfg # auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/e73425074471c58561d04c85da4a6400b638779d/xml/ardrone3.xml diff --git a/bebop_driver/include/bebop_driver/autogenerated/common_state_callbacks.h b/bebop_driver/include/bebop_driver/autogenerated/common_state_callbacks.h index 2387cfe..f0267dc 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/common_state_callbacks.h +++ b/bebop_driver/include/bebop_driver/autogenerated/common_state_callbacks.h @@ -1227,7 +1227,7 @@ class CommonMavlinkStateMissonItemExecuted : public AbstractState public: CommonMavlinkStateMissonItemExecuted(::ros::NodeHandle& nh, ::ros::NodeHandle& priv_nh, const ::std::string& topic) - : AbstractState(ARCONTROLLER_DICTIONARY_KEY_COMMON_MAVLINKSTATE_MISSIONITEMEXECUTED) + : AbstractState(ARCONTROLLER_DICTIONARY_KEY_COMMON_MAVLINKSTATE_MISSONITEMEXECUTED) { if (priv_nh.getParam("states/enable_mavlinkstate_missonitemexecuted", pub_enabled_) && pub_enabled_) { @@ -1257,7 +1257,7 @@ class CommonMavlinkStateMissonItemExecuted : public AbstractState arg = NULL; - HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_MAVLINKSTATE_MISSIONITEMEXECUTED_IDX, arg); + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_COMMON_MAVLINKSTATE_MISSONITEMEXECUTED_IDX, arg); if (arg) { msg_ptr->idx = arg->value.U32; @@ -2484,4 +2484,4 @@ class CommonRunStateRunIdChanged : public AbstractState } // namespace cb } // namespace bebop_driver -#endif // BEBOP_AUTONOMY_AUTOGENERATED_common_STATE_CALLBACKS_H +#endif // BEBOP_AUTONOMY_AUTOGENERATED_common_STATE_CALLBACKS_H \ No newline at end of file diff --git a/bebop_driver/scripts/meta/last_build_info b/bebop_driver/scripts/meta/last_build_info index 5083f88..6c76209 100644 --- a/bebop_driver/scripts/meta/last_build_info +++ b/bebop_driver/scripts/meta/last_build_info @@ -1,5 +1,5 @@ Last auto-generation build info: -- Source hash ab28dab91845cd36c4d7002b55f70805deaff3c8 -- Date 2017-07-17 12:04:26.902273 -- Generator generate.py @ ee73812 +- Source hash e73425074471c58561d04c85da4a6400b638779d +- Date 2017-02-10 13:22:17.661636 +- Generator generate.py @ adc1afe diff --git a/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg b/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg deleted file mode 100644 index a7f828f..0000000 --- a/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg +++ /dev/null @@ -1,22 +0,0 @@ -# Ardrone3AccessoryStateConnectedAccessories -# auto-generated from up stream XML files at -# github.com/Parrot-Developers/libARCommands/tree/master/Xml -# To check upstream commit hash, refer to last_build_info file -# Do not modify this file by hand. Check scripts/meta folder for generator files. -# -# SDK Comment: List of all connected accessories. This event presents the list of all connected accessories. To actually use the component, use the component dedicated feature. - -Header header - -# Id of the accessory for the session. -uint8 id -# Accessory type -uint8 accessory_type_sequoia=0 # Parrot Sequoia (multispectral camera for agriculture) -uint8 accessory_type_unknownaccessory_1=1 # UNKNOWNACCESSORY_1 camera (thermal+rgb camera) -uint8 accessory_type -# Unique Id of the accessory. This id is unique by accessory_type. -string uid -# Software Version of the accessory. -string swVersion -# List entry attribute Bitfield. 0x01: First: indicate its the first element of the list. 0x02: Last: indicate its the last element of the list. 0x04: Empty: indicate the list is empty (implies First/Last). All other arguments should be ignored. 0x08: Remove: This value should be removed from the existing list. -uint8 list_flags diff --git a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg deleted file mode 100644 index 4ba4ccd..0000000 --- a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg +++ /dev/null @@ -1,30 +0,0 @@ -# Ardrone3PilotingStatemoveToChanged -# auto-generated from up stream XML files at -# github.com/Parrot-Developers/libARCommands/tree/master/Xml -# To check upstream commit hash, refer to last_build_info file -# Do not modify this file by hand. Check scripts/meta folder for generator files. -# -# SDK Comment: The drone moves or moved to a given location. - -Header header - -# Latitude of the location (in degrees) to reach -float64 latitude -# Longitude of the location (in degrees) to reach -float64 longitude -# Altitude above sea level (in m) to reach -float64 altitude -# Orientation mode of the move to -uint8 orientation_mode_NONE=0 # The drone wont change its orientation -uint8 orientation_mode_TO_TARGET=1 # The drone will make a rotation to look in direction of the given location -uint8 orientation_mode_HEADING_START=2 # The drone will orientate itself to the given heading before moving to the location -uint8 orientation_mode_HEADING_DURING=3 # The drone will orientate itself to the given heading while moving to the location -uint8 orientation_mode -# Heading (relative to the North in degrees). This value is only used if the orientation mode is HEADING_START or HEADING_DURING -float32 heading -# Status of the move to -uint8 status_RUNNING=0 # The drone is actually flying to the given position -uint8 status_DONE=1 # The drone has reached the target -uint8 status_CANCELED=2 # The move to has been canceled, either by a new moveTo command or by a CancelMoveTo command. -uint8 status_ERROR=3 # The move to has not been finished or started because of an error. -uint8 status diff --git a/bebop_msgs/msg/autogenerated/CommonFlightPlanStateComponentStateListChanged.msg b/bebop_msgs/msg/autogenerated/CommonFlightPlanStateComponentStateListChanged.msg index bafa4d7..edefd5e 100644 --- a/bebop_msgs/msg/autogenerated/CommonFlightPlanStateComponentStateListChanged.msg +++ b/bebop_msgs/msg/autogenerated/CommonFlightPlanStateComponentStateListChanged.msg @@ -9,11 +9,10 @@ Header header # Drone FlightPlan component id (unique) -uint8 component_GPS=0 # Drone GPS component. State is 0 when the drone needs a GPS fix. -uint8 component_Calibration=1 # Drone Calibration component. State is 0 when the sensors of the drone needs to be calibrated. -uint8 component_Mavlink_File=2 # Mavlink file component. State is 0 when the mavlink file is missing or contains error. -uint8 component_TakeOff=3 # Drone Take off component. State is 0 when the drone cannot take-off. -uint8 component_WaypointsBeyondGeofence=4 # Component for waypoints beyond the geofence. State is 0 when one or more waypoints are beyond the geofence. +uint8 component_GPS=0 # GPS for Drone FlightPlan +uint8 component_Calibration=1 # Calibration for Drone FlightPlan +uint8 component_Mavlink_File=2 # Mavlink file for Drone FlightPlan +uint8 component_TakeOff=3 # Take off uint8 component # State of the FlightPlan component (1 FlightPlan component OK, otherwise 0) uint8 State diff --git a/bebop_msgs/msg/autogenerated/CommonMavlinkStateMissionItemExecuted.msg b/bebop_msgs/msg/autogenerated/CommonMavlinkStateMissionItemExecuted.msg deleted file mode 100644 index 6c2b22e..0000000 --- a/bebop_msgs/msg/autogenerated/CommonMavlinkStateMissionItemExecuted.msg +++ /dev/null @@ -1,12 +0,0 @@ -# CommonMavlinkStateMissionItemExecuted -# auto-generated from up stream XML files at -# github.com/Parrot-Developers/libARCommands/tree/master/Xml -# To check upstream commit hash, refer to last_build_info file -# Do not modify this file by hand. Check scripts/meta folder for generator files. -# -# SDK Comment: Mission item has been executed. - -Header header - -# Index of the mission item. This is the place of the mission item in the list of the items of the mission. Begins at 0. -uint32 idx From 75be564fdd27bda8eafc5f9bffc3d26168c97e73 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Tue, 18 Jul 2017 06:12:54 +0200 Subject: [PATCH 22/25] Revert "Adapted generate script to use the current (3.12.6) message definitions." This reverts commit ffed1bdd1fe63e2dcf84219f098a3e0bfb6ce2c7. --- bebop_driver/scripts/meta/generate.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/bebop_driver/scripts/meta/generate.py b/bebop_driver/scripts/meta/generate.py index eda8db9..e4d97b5 100755 --- a/bebop_driver/scripts/meta/generate.py +++ b/bebop_driver/scripts/meta/generate.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python2 +#!/usr/bin/env python import datetime from copy import deepcopy @@ -12,9 +12,9 @@ import subprocess import urllib2 -# SDK 3.12.6: https://github.com/Parrot-Developers/arsdk_manifests/blob/ARSDK3_version_3_12_6/release.xml +# SDK 3.11.0: https://github.com/Parrot-Developers/arsdk_manifests/blob/a6acc778f9d4d852985cccba91b854630921768e/release.xml LIBARCOMMANDS_GIT_OWNER = "Parrot-Developers" -LIBARCOMMANDS_GIT_HASH = "ab28dab91845cd36c4d7002b55f70805deaff3c8" +LIBARCOMMANDS_GIT_HASH = "e73425074471c58561d04c85da4a6400b638779d" # From XML types to ROS primitive types ROS_TYPE_MAP = { From 035106c68b0e6773e09fe173ce812825983b463c Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Tue, 18 Jul 2017 07:00:26 +0200 Subject: [PATCH 23/25] Moved state definitions into test class as they will be used by multiple tests. --- bebop_driver/test/bebop_itl_test.cpp | 294 +++++++++++++-------------- 1 file changed, 140 insertions(+), 154 deletions(-) diff --git a/bebop_driver/test/bebop_itl_test.cpp b/bebop_driver/test/bebop_itl_test.cpp index 2bb82fa..c7fc2a8 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() @@ -201,6 +225,28 @@ class BebopInTheLoopTest: public ::testing::Test 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(); @@ -249,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); @@ -290,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 @@ -310,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..."); @@ -335,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(); @@ -346,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(); @@ -358,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; @@ -377,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; @@ -397,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; @@ -418,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; @@ -433,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; @@ -453,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..." ); @@ -468,42 +484,12 @@ 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) { - 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); @@ -515,16 +501,16 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) 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 @@ -535,21 +521,21 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) ROS_WARN("Moving 0.5m forwared ..."); 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), + 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..."); @@ -560,8 +546,8 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) 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), + 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(); @@ -571,8 +557,8 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) 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.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(); @@ -583,18 +569,18 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) tw.angular.z = 0.0; ROS_WARN("Moving 0.5m right ..."); 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.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.5; @@ -602,19 +588,19 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) ROS_WARN("Ascending for 0.5m ..."); cmdmoveby_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.5; @@ -622,18 +608,18 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) ROS_WARN("Descending for 0.5m ..."); cmdmoveby_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; @@ -643,14 +629,14 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) // 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; @@ -658,14 +644,14 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) 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.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; @@ -678,7 +664,7 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) 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..." ); @@ -693,7 +679,7 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) // "Waiting for reset to happen..." // ); - ASSERT_GE(bat_percent - bat_state->GetMsgCopy().percent, 0); + ASSERT_GE(bat_percent - bat_state_->GetMsgCopy().percent, 0); } From b93ec8a08dce08fe2a5127e3d72de10c175c1ea4 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Tue, 18 Jul 2017 10:25:48 +0200 Subject: [PATCH 24/25] Removed StopBebop() at some places. Added speed check after end of a movement. Adapted some values. All changes are related the piloting with move by command unit test. --- bebop_driver/test/bebop_itl_test.cpp | 72 +++++++++++++++++++--------- 1 file changed, 50 insertions(+), 22 deletions(-) diff --git a/bebop_driver/test/bebop_itl_test.cpp b/bebop_driver/test/bebop_itl_test.cpp index c7fc2a8..88a0762 100644 --- a/bebop_driver/test/bebop_itl_test.cpp +++ b/bebop_driver/test/bebop_itl_test.cpp @@ -293,6 +293,7 @@ class BebopInTheLoopTest: public ::testing::Test * * odom on the other hand is REP-103 compatible */ +/* TEST_F(BebopInTheLoopTest, Piloting) { ros::Publisher takeoff_pub = nh_.advertise("takeoff", 1); @@ -455,6 +456,7 @@ TEST_F(BebopInTheLoopTest, Piloting) 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; @@ -486,6 +488,7 @@ TEST_F(BebopInTheLoopTest, Piloting) ASSERT_GE(bat_percent - bat_state_->GetMsgCopy().percent, 0); } +*/ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) @@ -521,11 +524,10 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) ROS_WARN("Moving 0.5m forwared ..."); cmdmoveby_pub_.publish(tw); - TIMED_ASSERT(3.0, angles::to_degrees(att_state_->GetMsgCopy().pitch) < -2.5 , "Measuring pitch ..."); + 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 ..."); - StopBebop(); TIMED_ASSERT(5.0, (fabs(speed_state_->GetMsgCopy().speedX) < 0.05) && (fabs(speed_state_->GetMsgCopy().speedY) < 0.05) && @@ -549,30 +551,63 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) 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.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.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.1), + 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.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(3.0, angles::to_degrees(att_state_->GetMsgCopy().roll) > 2.5, "Measuring roll ..."); + 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 @@ -583,19 +618,17 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) 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.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.5) && - ((odom_->GetMsgCopy().pose.pose.position.z - alt_start_odom) >= 0.5) && + ((alt_state_->GetMsgCopy().altitude - alt_start) >= 0.3) && + ((odom_->GetMsgCopy().pose.pose.position.z - alt_start_odom) >= 0.3) && (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); @@ -603,19 +636,17 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) 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.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.5) && - ((odom_->GetMsgCopy().pose.pose.position.z - alt_start_odom) <= -0.5) && + ((alt_state_->GetMsgCopy().altitude - alt_start) <= -0.3) && + ((odom_->GetMsgCopy().pose.pose.position.z - alt_start_odom) <= -0.3) && (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); @@ -623,7 +654,7 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) tw.linear.x = 0.0; tw.linear.y = 0.0; tw.linear.z = 0.0; - tw.angular.z = -0.5 * 3.141596; + tw.angular.z = 0.5 * 3.141596; ROS_WARN("Rotating CW for 90 degrees ..."); cmdmoveby_pub_.publish(tw); @@ -632,7 +663,6 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) 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); @@ -640,15 +670,13 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) tw.linear.x = 0.0; tw.linear.y = 0.0; tw.linear.z = 0.0; - tw.angular.z = 0.5 * 3.141596; + 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.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; From 0051e05b04bace36be0f56efeb81eccc8e5b21b1 Mon Sep 17 00:00:00 2001 From: Andreas Ziegler <062.127@gmail.com> Date: Tue, 18 Jul 2017 11:48:32 +0200 Subject: [PATCH 25/25] Adapted values in PilotingWithMoveBy unit test. PilotingWithMoveBy unit test ist now working. --- bebop_driver/test/bebop_itl_test.cpp | 25 ++++++------------------- 1 file changed, 6 insertions(+), 19 deletions(-) diff --git a/bebop_driver/test/bebop_itl_test.cpp b/bebop_driver/test/bebop_itl_test.cpp index 88a0762..a66febb 100644 --- a/bebop_driver/test/bebop_itl_test.cpp +++ b/bebop_driver/test/bebop_itl_test.cpp @@ -293,7 +293,6 @@ class BebopInTheLoopTest: public ::testing::Test * * odom on the other hand is REP-103 compatible */ -/* TEST_F(BebopInTheLoopTest, Piloting) { ros::Publisher takeoff_pub = nh_.advertise("takeoff", 1); @@ -456,7 +455,6 @@ TEST_F(BebopInTheLoopTest, Piloting) 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; @@ -488,7 +486,6 @@ TEST_F(BebopInTheLoopTest, Piloting) ASSERT_GE(bat_percent - bat_state_->GetMsgCopy().percent, 0); } -*/ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) @@ -623,8 +620,8 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) ROS_WARN("Ascending for 0.5m ..."); cmdmoveby_pub_.publish(tw); TIMED_ASSERT(10.0, - ((alt_state_->GetMsgCopy().altitude - alt_start) >= 0.3) && - ((odom_->GetMsgCopy().pose.pose.position.z - alt_start_odom) >= 0.3) && + ((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 ..."); @@ -641,8 +638,8 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) ROS_WARN("Descending for 0.5m ..."); cmdmoveby_pub_.publish(tw); TIMED_ASSERT(10.0, - ((alt_state_->GetMsgCopy().altitude - alt_start) <= -0.3) && - ((odom_->GetMsgCopy().pose.pose.position.z - alt_start_odom) <= -0.3) && + ((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 ..."); @@ -660,7 +657,7 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) // 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.45 * 3.141596, "Measuring Yaw"); @@ -674,7 +671,7 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) 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.5 * 3.141596, + 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) */ @@ -697,16 +694,6 @@ TEST_F(BebopInTheLoopTest, PilotingWithMoveBy) "Waiting for land to finish..." ); - // emergency state is transient (unlike ardrone), we may miss the signal -// ROS_WARN("Emergency ..."); -// reset_pub.publish(em); - -// TIMED_ASSERT(5.0, -// flying_state->IsActive() && flying_state->GetMsgCopy().state == -// bebop_msgs::Ardrone3PilotingStateFlyingStateChanged::state_emergency, -// "Waiting for reset to happen..." -// ); - ASSERT_GE(bat_percent - bat_state_->GetMsgCopy().percent, 0); }