From 10b6b8658cd527a18de92a21821b1dc1f0fb816f Mon Sep 17 00:00:00 2001 From: Thomas Bamford Date: Tue, 3 Jul 2018 08:04:50 -0400 Subject: [PATCH 01/11] Add support for SDK 3.14.0 --- README.md | 2 +- bebop_autonomy/CHANGELOG.rst | 3 + bebop_autonomy/package.xml | 2 +- bebop_description/CHANGELOG.rst | 3 + bebop_description/package.xml | 2 +- bebop_driver/CHANGELOG.rst | 5 + .../cfg/autogenerated/BebopArdrone3.cfg | 2 +- .../ardrone3_setting_callback_includes.h | 2 +- .../ardrone3_setting_callbacks.h | 2 +- .../ardrone3_state_callback_includes.h | 52 ++- .../autogenerated/ardrone3_state_callbacks.h | 304 +++++++++++++++++- .../autogenerated/callbacks_common.h | 2 +- .../common_state_callback_includes.h | 2 +- .../autogenerated/common_state_callbacks.h | 4 +- bebop_driver/package.xml | 6 +- bebop_driver/scripts/meta/generate.py | 4 +- bebop_driver/scripts/meta/last_build_info | 6 +- bebop_msgs/CHANGELOG.rst | 5 + .../Ardrone3AccessoryStateBattery.msg | 16 + ...one3AccessoryStateConnectedAccessories.msg | 4 +- ...rdrone3PilotingStateFlyingStateChanged.msg | 2 +- .../Ardrone3PilotingStateMotionState.msg | 14 + .../Ardrone3PilotingStatePilotedPOI.msg | 22 ++ ...PilotingStateReturnHomeBatteryCapacity.msg | 16 + .../Ardrone3PilotingStatemoveToChanged.msg | 2 +- .../Ardrone3SoundStateAlertSound.msg | 14 + bebop_msgs/msg/autogenerated/last_build_info | 6 +- bebop_msgs/package.xml | 2 +- bebop_tools/CHANGELOG.rst | 3 + bebop_tools/package.xml | 2 +- .../autogenerated/ardrone3_settings_param.rst | 2 +- .../ardrone3_states_param_topic.rst | 87 ++++- .../common_states_param_topic.rst | 2 +- docs/dev.rst | 33 +- docs/index.rst | 4 +- 35 files changed, 598 insertions(+), 41 deletions(-) create mode 100644 bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateBattery.msg create mode 100644 bebop_msgs/msg/autogenerated/Ardrone3PilotingStateMotionState.msg create mode 100644 bebop_msgs/msg/autogenerated/Ardrone3PilotingStatePilotedPOI.msg create mode 100644 bebop_msgs/msg/autogenerated/Ardrone3PilotingStateReturnHomeBatteryCapacity.msg create mode 100644 bebop_msgs/msg/autogenerated/Ardrone3SoundStateAlertSound.msg diff --git a/README.md b/README.md index 1c0d2d3..bc3a7e5 100644 --- a/README.md +++ b/README.md @@ -19,4 +19,4 @@ [![Build Status (ROS I,J,K) - TravisCI](https://travis-ci.org/AutonomyLab/bebop_autonomy.svg?branch=indigo-devel)](https://travis-ci.org/AutonomyLab/bebop_autonomy) [![Build Status (ROS I,J) - Semaphore](https://semaphoreci.com/api/v1/projects/11786233-d505-4d79-b27c-80c2742243a4/537552/badge.svg)](https://semaphoreci.com/mani_monaj/bebop_autonomy) -Built against [parrot_arsdk](https://github.com/AutonomyLab/parrot_arsdk) 3.12.6p1 +Built against [parrot_arsdk](https://github.com/AutonomyLab/parrot_arsdk) 3.14.0p0 diff --git a/bebop_autonomy/CHANGELOG.rst b/bebop_autonomy/CHANGELOG.rst index d5ffd1d..847985d 100644 --- a/bebop_autonomy/CHANGELOG.rst +++ b/bebop_autonomy/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bebop_autonomy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.0 (2018-07-03) +------------------ + 0.7.0 (2017-07-29) ------------------ diff --git a/bebop_autonomy/package.xml b/bebop_autonomy/package.xml index 610dca4..ed5097c 100644 --- a/bebop_autonomy/package.xml +++ b/bebop_autonomy/package.xml @@ -1,7 +1,7 @@ bebop_autonomy - 0.7.0 + 0.8.0 bebop_autonomy is a ROS driver for Parrot Bebop drone, based on Parrot’s official ARDroneSDK3 Mani Monajjemi diff --git a/bebop_description/CHANGELOG.rst b/bebop_description/CHANGELOG.rst index e889325..c45e78b 100644 --- a/bebop_description/CHANGELOG.rst +++ b/bebop_description/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bebop_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.0 (2018-07-03) +------------------ + 0.7.0 (2017-07-29) ------------------ diff --git a/bebop_description/package.xml b/bebop_description/package.xml index 301fa3b..fff87ec 100644 --- a/bebop_description/package.xml +++ b/bebop_description/package.xml @@ -1,7 +1,7 @@ bebop_description - 0.7.0 + 0.8.0 URDF robot description for Parrot Bebop Drones Mani Monajjemi diff --git a/bebop_driver/CHANGELOG.rst b/bebop_driver/CHANGELOG.rst index 0dd8cca..50b4a46 100644 --- a/bebop_driver/CHANGELOG.rst +++ b/bebop_driver/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bebop_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.0 (2018-07-03) +------------------ +* SDK 3.14.0 support +* Contributors: Thomas Bamford + 0.7.0 (2017-07-29) ------------------ * SDK 3.12.6 support (except 64bit Ubuntu Xenial, working on fix) diff --git a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg index f4b2763..a31702d 100755 --- a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg +++ b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg @@ -1,7 +1,7 @@ #!/usr/bin/env python # BebopArdrone3.cfg -# auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml +# auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml # Do not modify this file by hand. Check scripts/meta folder for generator files. PACKAGE = "bebop_driver" diff --git a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callback_includes.h b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callback_includes.h index cce9163..5084f93 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callback_includes.h +++ b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callback_includes.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * Ardrone3_setting_callback_includes.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ diff --git a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callbacks.h b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callbacks.h index f47b1ab..a064585 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callbacks.h +++ b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callbacks.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * Ardrone3_setting_callbacks.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ #ifndef BEBOP_AUTONOMY_AUTOGENERATED_Ardrone3_SETTING_CALLBACKS_H diff --git a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callback_includes.h b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callback_includes.h index 8d04c7e..5375fb4 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callback_includes.h +++ b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callback_includes.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * ardrone3_state_callback_includes.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ @@ -48,6 +48,9 @@ namespace cb class Ardrone3PilotingStateLandingStateChanged; class Ardrone3PilotingStateAirSpeedChanged; class Ardrone3PilotingStatemoveToChanged; + class Ardrone3PilotingStateMotionState; + class Ardrone3PilotingStatePilotedPOI; + class Ardrone3PilotingStateReturnHomeBatteryCapacity; class Ardrone3NetworkStateWifiScanListChanged; class Ardrone3NetworkStateAllWifiScanChanged; class Ardrone3NetworkStateWifiAuthChannelListChanged; @@ -66,6 +69,8 @@ namespace cb class Ardrone3GPSStateHomeTypeChosenChanged; class Ardrone3PROStateFeatures; class Ardrone3AccessoryStateConnectedAccessories; + class Ardrone3AccessoryStateBattery; + class Ardrone3SoundStateAlertSound; } // namespace cb #endif // FORWARD_DECLARATIONS @@ -107,6 +112,12 @@ boost::shared_ptr ardrone3_pilotingstate_airspeedchanged_ptr; boost::shared_ptr ardrone3_pilotingstate_movetochanged_ptr; +boost::shared_ptr + ardrone3_pilotingstate_motionstate_ptr; +boost::shared_ptr + ardrone3_pilotingstate_pilotedpoi_ptr; +boost::shared_ptr + ardrone3_pilotingstate_returnhomebatterycapacity_ptr; boost::shared_ptr ardrone3_networkstate_wifiscanlistchanged_ptr; boost::shared_ptr @@ -143,6 +154,10 @@ boost::shared_ptr ardrone3_prostate_features_ptr; boost::shared_ptr ardrone3_accessorystate_connectedaccessories_ptr; +boost::shared_ptr + ardrone3_accessorystate_battery_ptr; +boost::shared_ptr + ardrone3_soundstate_alertsound_ptr; #endif // DEFINE_SHARED_PTRS #ifdef UPDTAE_CALLBACK_MAP @@ -201,6 +216,15 @@ ardrone3_pilotingstate_airspeedchanged_ptr.reset( ardrone3_pilotingstate_movetochanged_ptr.reset( new cb::Ardrone3PilotingStatemoveToChanged( nh, priv_nh, "states/ardrone3/PilotingState/moveToChanged")); +ardrone3_pilotingstate_motionstate_ptr.reset( + new cb::Ardrone3PilotingStateMotionState( + nh, priv_nh, "states/ardrone3/PilotingState/MotionState")); +ardrone3_pilotingstate_pilotedpoi_ptr.reset( + new cb::Ardrone3PilotingStatePilotedPOI( + nh, priv_nh, "states/ardrone3/PilotingState/PilotedPOI")); +ardrone3_pilotingstate_returnhomebatterycapacity_ptr.reset( + new cb::Ardrone3PilotingStateReturnHomeBatteryCapacity( + nh, priv_nh, "states/ardrone3/PilotingState/ReturnHomeBatteryCapacity")); ardrone3_networkstate_wifiscanlistchanged_ptr.reset( new cb::Ardrone3NetworkStateWifiScanListChanged( nh, priv_nh, "states/ardrone3/NetworkState/WifiScanListChanged")); @@ -255,6 +279,12 @@ ardrone3_prostate_features_ptr.reset( ardrone3_accessorystate_connectedaccessories_ptr.reset( new cb::Ardrone3AccessoryStateConnectedAccessories( nh, priv_nh, "states/ardrone3/AccessoryState/ConnectedAccessories")); +ardrone3_accessorystate_battery_ptr.reset( + new cb::Ardrone3AccessoryStateBattery( + nh, priv_nh, "states/ardrone3/AccessoryState/Battery")); +ardrone3_soundstate_alertsound_ptr.reset( + new cb::Ardrone3SoundStateAlertSound( + nh, priv_nh, "states/ardrone3/SoundState/AlertSound")); // Add all wrappers to the callback map callback_map_.insert(std::pair >( @@ -329,6 +359,18 @@ callback_map_.insert(std::pairGetCommandKey(), ardrone3_pilotingstate_movetochanged_ptr)); // Add all wrappers to the callback map +callback_map_.insert(std::pair >( + ardrone3_pilotingstate_motionstate_ptr->GetCommandKey(), + ardrone3_pilotingstate_motionstate_ptr)); +// Add all wrappers to the callback map +callback_map_.insert(std::pair >( + ardrone3_pilotingstate_pilotedpoi_ptr->GetCommandKey(), + ardrone3_pilotingstate_pilotedpoi_ptr)); +// Add all wrappers to the callback map +callback_map_.insert(std::pair >( + ardrone3_pilotingstate_returnhomebatterycapacity_ptr->GetCommandKey(), + ardrone3_pilotingstate_returnhomebatterycapacity_ptr)); +// Add all wrappers to the callback map callback_map_.insert(std::pair >( ardrone3_networkstate_wifiscanlistchanged_ptr->GetCommandKey(), ardrone3_networkstate_wifiscanlistchanged_ptr)); @@ -400,4 +442,12 @@ callback_map_.insert(std::pair >( ardrone3_accessorystate_connectedaccessories_ptr->GetCommandKey(), ardrone3_accessorystate_connectedaccessories_ptr)); +// Add all wrappers to the callback map +callback_map_.insert(std::pair >( + ardrone3_accessorystate_battery_ptr->GetCommandKey(), + ardrone3_accessorystate_battery_ptr)); +// Add all wrappers to the callback map +callback_map_.insert(std::pair >( + ardrone3_soundstate_alertsound_ptr->GetCommandKey(), + ardrone3_soundstate_alertsound_ptr)); #endif // UPDTAE_CALLBACK_MAP 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 87c63cc..442ac91 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h +++ b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * ardrone3_state_callbacks.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ @@ -57,6 +57,9 @@ extern "C" #include "bebop_msgs/Ardrone3PilotingStateLandingStateChanged.h" #include "bebop_msgs/Ardrone3PilotingStateAirSpeedChanged.h" #include "bebop_msgs/Ardrone3PilotingStatemoveToChanged.h" +#include "bebop_msgs/Ardrone3PilotingStateMotionState.h" +#include "bebop_msgs/Ardrone3PilotingStatePilotedPOI.h" +#include "bebop_msgs/Ardrone3PilotingStateReturnHomeBatteryCapacity.h" #include "bebop_msgs/Ardrone3NetworkStateWifiScanListChanged.h" #include "bebop_msgs/Ardrone3NetworkStateAllWifiScanChanged.h" #include "bebop_msgs/Ardrone3NetworkStateWifiAuthChannelListChanged.h" @@ -75,6 +78,8 @@ extern "C" #include "bebop_msgs/Ardrone3GPSStateHomeTypeChosenChanged.h" #include "bebop_msgs/Ardrone3PROStateFeatures.h" #include "bebop_msgs/Ardrone3AccessoryStateConnectedAccessories.h" +#include "bebop_msgs/Ardrone3AccessoryStateBattery.h" +#include "bebop_msgs/Ardrone3SoundStateAlertSound.h" namespace bebop_driver { @@ -1147,6 +1152,180 @@ class Ardrone3PilotingStatemoveToChanged : public AbstractState }; // Ardrone3PilotingStatemoveToChanged +// Motion state.\n If [MotionDetection](#1-6-16) is disabled, motion is steady.\n This information is only valid when the drone is not flying. +class Ardrone3PilotingStateMotionState : public AbstractState +{ +private: + ::bebop_msgs::Ardrone3PilotingStateMotionState::Ptr msg_ptr; + +public: + + Ardrone3PilotingStateMotionState(::ros::NodeHandle& nh, ::ros::NodeHandle& priv_nh, const ::std::string& topic) + : AbstractState(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_MOTIONSTATE) + { + if (priv_nh.getParam("states/enable_pilotingstate_motionstate", pub_enabled_) && pub_enabled_) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB" , "[STATES] Enabling %s", topic.c_str()); + ros_pub_ = nh.advertise(topic, 10, true); + } // pub_enabled_ is false + } + + ::bebop_msgs::Ardrone3PilotingStateMotionState::ConstPtr GetDataCstPtr() const + { + ::boost::lock_guard lock(mutex_); + return msg_ptr; + } + + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", "Ardrone3PilotingStateMotionState::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + msg_ptr.reset(new ::bebop_msgs::Ardrone3PilotingStateMotionState()); + msg_ptr->header.stamp = t; + msg_ptr->header.frame_id = "base_link"; + + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_MOTIONSTATE_STATE, arg); + if (arg) + { + msg_ptr->state = arg->value.I32; + } + + if (pub_enabled_) ros_pub_.publish(msg_ptr); + } + +}; // Ardrone3PilotingStateMotionState + + +// Piloted POI state. +class Ardrone3PilotingStatePilotedPOI : public AbstractState +{ +private: + ::bebop_msgs::Ardrone3PilotingStatePilotedPOI::Ptr msg_ptr; + +public: + + Ardrone3PilotingStatePilotedPOI(::ros::NodeHandle& nh, ::ros::NodeHandle& priv_nh, const ::std::string& topic) + : AbstractState(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_PILOTEDPOI) + { + if (priv_nh.getParam("states/enable_pilotingstate_pilotedpoi", pub_enabled_) && pub_enabled_) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB" , "[STATES] Enabling %s", topic.c_str()); + ros_pub_ = nh.advertise(topic, 10, true); + } // pub_enabled_ is false + } + + ::bebop_msgs::Ardrone3PilotingStatePilotedPOI::ConstPtr GetDataCstPtr() const + { + ::boost::lock_guard lock(mutex_); + return msg_ptr; + } + + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", "Ardrone3PilotingStatePilotedPOI::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + msg_ptr.reset(new ::bebop_msgs::Ardrone3PilotingStatePilotedPOI()); + msg_ptr->header.stamp = t; + msg_ptr->header.frame_id = "base_link"; + + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_PILOTEDPOI_LATITUDE, arg); + if (arg) + { + msg_ptr->latitude = arg->value.Double; + } + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_PILOTEDPOI_LONGITUDE, arg); + if (arg) + { + msg_ptr->longitude = arg->value.Double; + } + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_PILOTEDPOI_ALTITUDE, arg); + if (arg) + { + msg_ptr->altitude = arg->value.Double; + } + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_PILOTEDPOI_STATUS, arg); + if (arg) + { + msg_ptr->status = arg->value.I32; + } + + if (pub_enabled_) ros_pub_.publish(msg_ptr); + } + +}; // Ardrone3PilotingStatePilotedPOI + + +// Battery capacity status to return home. +class Ardrone3PilotingStateReturnHomeBatteryCapacity : public AbstractState +{ +private: + ::bebop_msgs::Ardrone3PilotingStateReturnHomeBatteryCapacity::Ptr msg_ptr; + +public: + + Ardrone3PilotingStateReturnHomeBatteryCapacity(::ros::NodeHandle& nh, ::ros::NodeHandle& priv_nh, const ::std::string& topic) + : AbstractState(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_RETURNHOMEBATTERYCAPACITY) + { + if (priv_nh.getParam("states/enable_pilotingstate_returnhomebatterycapacity", pub_enabled_) && pub_enabled_) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB" , "[STATES] Enabling %s", topic.c_str()); + ros_pub_ = nh.advertise(topic, 10, true); + } // pub_enabled_ is false + } + + ::bebop_msgs::Ardrone3PilotingStateReturnHomeBatteryCapacity::ConstPtr GetDataCstPtr() const + { + ::boost::lock_guard lock(mutex_); + return msg_ptr; + } + + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", "Ardrone3PilotingStateReturnHomeBatteryCapacity::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + msg_ptr.reset(new ::bebop_msgs::Ardrone3PilotingStateReturnHomeBatteryCapacity()); + msg_ptr->header.stamp = t; + msg_ptr->header.frame_id = "base_link"; + + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_RETURNHOMEBATTERYCAPACITY_STATUS, arg); + if (arg) + { + msg_ptr->status = arg->value.I32; + } + + if (pub_enabled_) ros_pub_.publish(msg_ptr); + } + +}; // Ardrone3PilotingStateReturnHomeBatteryCapacity + + // Wifi scan results.\n Please note that the list is not complete until you receive the event [WifiScanEnded](#1-14-1). class Ardrone3NetworkStateWifiScanListChanged : public AbstractState { @@ -2143,12 +2322,135 @@ class Ardrone3AccessoryStateConnectedAccessories : public AbstractState msg_ptr->swVersion = arg->value.String; } +/** arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_CONNECTEDACCESSORIES_LIST_FLAGS, arg); + if (arg) + { + msg_ptr->list_flags = arg->value.U8; + } +**/ if (pub_enabled_) ros_pub_.publish(msg_ptr); } }; // Ardrone3AccessoryStateConnectedAccessories +// Connected accessories battery. +class Ardrone3AccessoryStateBattery : public AbstractState +{ +private: + ::bebop_msgs::Ardrone3AccessoryStateBattery::Ptr msg_ptr; + +public: + + Ardrone3AccessoryStateBattery(::ros::NodeHandle& nh, ::ros::NodeHandle& priv_nh, const ::std::string& topic) + : AbstractState(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_BATTERY) + { + if (priv_nh.getParam("states/enable_accessorystate_battery", pub_enabled_) && pub_enabled_) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB" , "[STATES] Enabling %s", topic.c_str()); + ros_pub_ = nh.advertise(topic, 10, true); + } // pub_enabled_ is false + } + + ::bebop_msgs::Ardrone3AccessoryStateBattery::ConstPtr GetDataCstPtr() const + { + ::boost::lock_guard lock(mutex_); + return msg_ptr; + } + + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", "Ardrone3AccessoryStateBattery::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + msg_ptr.reset(new ::bebop_msgs::Ardrone3AccessoryStateBattery()); + msg_ptr->header.stamp = t; + msg_ptr->header.frame_id = "base_link"; + + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_BATTERY_ID, arg); + if (arg) + { + msg_ptr->id = arg->value.U8; + } + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_BATTERY_BATTERYLEVEL, arg); + if (arg) + { + msg_ptr->batteryLevel = arg->value.U8; + } +/** + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_BATTERY_LIST_FLAGS, arg); + if (arg) + { + msg_ptr->list_flags = arg->value.U8; + } +**/ + if (pub_enabled_) ros_pub_.publish(msg_ptr); + } + +}; // Ardrone3AccessoryStateBattery + + +// Alert sound state. +class Ardrone3SoundStateAlertSound : public AbstractState +{ +private: + ::bebop_msgs::Ardrone3SoundStateAlertSound::Ptr msg_ptr; + +public: + + Ardrone3SoundStateAlertSound(::ros::NodeHandle& nh, ::ros::NodeHandle& priv_nh, const ::std::string& topic) + : AbstractState(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SOUNDSTATE_ALERTSOUND) + { + if (priv_nh.getParam("states/enable_soundstate_alertsound", pub_enabled_) && pub_enabled_) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB" , "[STATES] Enabling %s", topic.c_str()); + ros_pub_ = nh.advertise(topic, 10, true); + } // pub_enabled_ is false + } + + ::bebop_msgs::Ardrone3SoundStateAlertSound::ConstPtr GetDataCstPtr() const + { + ::boost::lock_guard lock(mutex_); + return msg_ptr; + } + + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", "Ardrone3SoundStateAlertSound::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + msg_ptr.reset(new ::bebop_msgs::Ardrone3SoundStateAlertSound()); + msg_ptr->header.stamp = t; + msg_ptr->header.frame_id = "base_link"; + + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SOUNDSTATE_ALERTSOUND_STATE, arg); + if (arg) + { + msg_ptr->state = arg->value.I32; + } + + if (pub_enabled_) ros_pub_.publish(msg_ptr); + } + +}; // Ardrone3SoundStateAlertSound + + } // namespace cb } // namespace bebop_driver #endif // BEBOP_AUTONOMY_AUTOGENERATED_ardrone3_STATE_CALLBACKS_H diff --git a/bebop_driver/include/bebop_driver/autogenerated/callbacks_common.h b/bebop_driver/include/bebop_driver/autogenerated/callbacks_common.h index e67f3b7..fa9e636 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/callbacks_common.h +++ b/bebop_driver/include/bebop_driver/autogenerated/callbacks_common.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * bebop_common_commands.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ #ifndef BEBOP_COMMON_COMMANDS_H diff --git a/bebop_driver/include/bebop_driver/autogenerated/common_state_callback_includes.h b/bebop_driver/include/bebop_driver/autogenerated/common_state_callback_includes.h index cba9ea9..a6b592b 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/common_state_callback_includes.h +++ b/bebop_driver/include/bebop_driver/autogenerated/common_state_callback_includes.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * common_state_callback_includes.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/common.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/common.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ 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 a5d7527..6e74f8c 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/common_state_callbacks.h +++ b/bebop_driver/include/bebop_driver/autogenerated/common_state_callbacks.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * common_state_callbacks.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/common.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/common.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ @@ -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/package.xml b/bebop_driver/package.xml index 1c0a3cb..09f156b 100644 --- a/bebop_driver/package.xml +++ b/bebop_driver/package.xml @@ -1,7 +1,7 @@ bebop_driver - 0.7.0 + 0.8.0 ROS driver for Parrot Bebop drone, based on Parrot’s official ARDroneSDK3 Mani Monajjemi @@ -18,7 +18,7 @@ catkin - parrot_arsdk + parrot_arsdk bebop_msgs bebop_description pkg-config @@ -36,7 +36,7 @@ roslint rostest - parrot_arsdk + parrot_arsdk bebop_msgs bebop_description nodelet diff --git a/bebop_driver/scripts/meta/generate.py b/bebop_driver/scripts/meta/generate.py index bf4d647..b1ccd71 100755 --- a/bebop_driver/scripts/meta/generate.py +++ b/bebop_driver/scripts/meta/generate.py @@ -12,9 +12,9 @@ import subprocess import urllib2 -# SDK 3.12.6: https://github.com/Parrot-Developers/arsdk_manifests/blob/d7640c80ed7147971995222d9f4655932a904aa8/release.xml +# SDK 3.14.0: https://github.com/Parrot-Developers/arsdk_manifests/blob/1ff5bdc5458627c12eb22e1dd1814cff25778f31/release.xml LIBARCOMMANDS_GIT_OWNER = "Parrot-Developers" -LIBARCOMMANDS_GIT_HASH = "ab28dab91845cd36c4d7002b55f70805deaff3c8" +LIBARCOMMANDS_GIT_HASH = "6faa46898d94fa207fd1b039643d356d14b7db42" # From XML types to ROS primitive types ROS_TYPE_MAP = { diff --git a/bebop_driver/scripts/meta/last_build_info b/bebop_driver/scripts/meta/last_build_info index 17c91d4..733c8cc 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-06-27 13:53:57.790920 -- Generator generate.py @ 8801830 +- Source hash 6faa46898d94fa207fd1b039643d356d14b7db42 +- Date 2018-07-03 00:25:08.348220 +- Generator generate.py @ e00904c diff --git a/bebop_msgs/CHANGELOG.rst b/bebop_msgs/CHANGELOG.rst index 5cd6dc4..148f36d 100644 --- a/bebop_msgs/CHANGELOG.rst +++ b/bebop_msgs/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bebop_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.0 (2018-07-03) +------------------ +* Update to Parrot SDK 3.14.0 (from 3.12.6) +* Contributors: Thomas Bamford + 0.7.0 (2017-07-29) ------------------ * Update to Parrot SDK 3.12.6 (from 3.10.1) diff --git a/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateBattery.msg b/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateBattery.msg new file mode 100644 index 0000000..e94e12a --- /dev/null +++ b/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateBattery.msg @@ -0,0 +1,16 @@ +# Ardrone3AccessoryStateBattery +# 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: Connected accessories battery. + +Header header + +# Id of the accessory for the session. +uint8 id +# Battery level in percentage. +uint8 batteryLevel +# 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/Ardrone3AccessoryStateConnectedAccessories.msg b/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg index 11d0602..00524a9 100644 --- a/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg +++ b/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg @@ -12,9 +12,11 @@ Header header 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_flir=1 # FLIR 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/Ardrone3PilotingStateFlyingStateChanged.msg b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateFlyingStateChanged.msg index 384e9d3..d58288c 100644 --- a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateFlyingStateChanged.msg +++ b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateFlyingStateChanged.msg @@ -16,6 +16,6 @@ uint8 state_flying=3 # Flying state uint8 state_landing=4 # Landing state uint8 state_emergency=5 # Emergency state uint8 state_usertakeoff=6 # User take off state. Waiting for user action to take off. -uint8 state_motor_ramping=7 # Motor ramping state (for fixed wings). +uint8 state_motor_ramping=7 # Motor ramping state. uint8 state_emergency_landing=8 # Emergency landing state. Drone autopilot has detected defective sensor(s). Only Yaw argument in PCMD is taken into account. All others flying commands are ignored. uint8 state diff --git a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateMotionState.msg b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateMotionState.msg new file mode 100644 index 0000000..1b1e678 --- /dev/null +++ b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateMotionState.msg @@ -0,0 +1,14 @@ +# Ardrone3PilotingStateMotionState +# 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: Motion state.\n If [MotionDetection](#1-6-16) is disabled, motion is steady.\n This information is only valid when the drone is not flying. + +Header header + +# Motion state +uint8 state_steady=0 # Drone is steady +uint8 state_moving=1 # Drone is moving +uint8 state diff --git a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatePilotedPOI.msg b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatePilotedPOI.msg new file mode 100644 index 0000000..7f7076c --- /dev/null +++ b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatePilotedPOI.msg @@ -0,0 +1,22 @@ +# Ardrone3PilotingStatePilotedPOI +# 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: Piloted POI state. + +Header header + +# Latitude of the location (in degrees) to look at. This information is only valid when the state is pending or running. +float64 latitude +# Longitude of the location (in degrees) to look at. This information is only valid when the state is pending or running. +float64 longitude +# Altitude above sea level (in m) to look at. This information is only valid when the state is pending or running. +float64 altitude +# Status of the move to +uint8 status_UNAVAILABLE=0 # The piloted POI is not available +uint8 status_AVAILABLE=1 # The piloted POI is available +uint8 status_PENDING=2 # Piloted POI has been requested. Waiting to be in state that allow the piloted POI to start +uint8 status_RUNNING=3 # Piloted POI is running +uint8 status diff --git a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateReturnHomeBatteryCapacity.msg b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateReturnHomeBatteryCapacity.msg new file mode 100644 index 0000000..2a8bb61 --- /dev/null +++ b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateReturnHomeBatteryCapacity.msg @@ -0,0 +1,16 @@ +# Ardrone3PilotingStateReturnHomeBatteryCapacity +# 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: Battery capacity status to return home. + +Header header + +# Status of battery to return home +uint8 status_OK=0 # The battery is full enough to do a return home +uint8 status_WARNING=1 # The battery is about to be too discharged to do a return home +uint8 status_CRITICAL=2 # The battery level is too low to return to the home position +uint8 status_UNKNOWN=3 # Battery capacity to do a return home is unknown. This can be either because the home is unknown or the position of the drone is unknown, or the drone has not enough information to determine how long it takes to fly home. +uint8 status diff --git a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg index 4ba4ccd..73d962d 100644 --- a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg +++ b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg @@ -25,6 +25,6 @@ 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_CANCELED=2 # The move to has been canceled, either by a CancelMoveTo command or when a disconnection appears. 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/Ardrone3SoundStateAlertSound.msg b/bebop_msgs/msg/autogenerated/Ardrone3SoundStateAlertSound.msg new file mode 100644 index 0000000..3981bab --- /dev/null +++ b/bebop_msgs/msg/autogenerated/Ardrone3SoundStateAlertSound.msg @@ -0,0 +1,14 @@ +# Ardrone3SoundStateAlertSound +# 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: Alert sound state. + +Header header + +# State of the alert sound +uint8 state_stopped=0 # Alert sound is not playing +uint8 state_playing=1 # Alert sound is playing +uint8 state diff --git a/bebop_msgs/msg/autogenerated/last_build_info b/bebop_msgs/msg/autogenerated/last_build_info index 17c91d4..733c8cc 100644 --- a/bebop_msgs/msg/autogenerated/last_build_info +++ b/bebop_msgs/msg/autogenerated/last_build_info @@ -1,5 +1,5 @@ Last auto-generation build info: -- Source hash ab28dab91845cd36c4d7002b55f70805deaff3c8 -- Date 2017-06-27 13:53:57.790920 -- Generator generate.py @ 8801830 +- Source hash 6faa46898d94fa207fd1b039643d356d14b7db42 +- Date 2018-07-03 00:25:08.348220 +- Generator generate.py @ e00904c diff --git a/bebop_msgs/package.xml b/bebop_msgs/package.xml index 406daee..be1c58c 100644 --- a/bebop_msgs/package.xml +++ b/bebop_msgs/package.xml @@ -1,7 +1,7 @@ bebop_msgs - 0.7.0 + 0.8.0 Common message definitions for bebop_autonomy Mani Monajjemi diff --git a/bebop_tools/CHANGELOG.rst b/bebop_tools/CHANGELOG.rst index 4be5d1d..0324ee2 100644 --- a/bebop_tools/CHANGELOG.rst +++ b/bebop_tools/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bebop_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.0 (2018-07-03) +------------------ + 0.7.0 (2017-07-29) ------------------ diff --git a/bebop_tools/package.xml b/bebop_tools/package.xml index e9f595a..cedff1d 100644 --- a/bebop_tools/package.xml +++ b/bebop_tools/package.xml @@ -1,7 +1,7 @@ bebop_tools - 0.7.0 + 0.8.0 Miscellaneous tools for bebop_autonomy metapackage Mani Monajjemi diff --git a/docs/autogenerated/ardrone3_settings_param.rst b/docs/autogenerated/ardrone3_settings_param.rst index 84aca69..90c0b33 100644 --- a/docs/autogenerated/ardrone3_settings_param.rst +++ b/docs/autogenerated/ardrone3_settings_param.rst @@ -1,5 +1,5 @@ .. Ardrone3_settings_param.rst - .. auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + .. auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml .. Do not modify this file by hand. Check scripts/meta folder for generator files. ***************************************************************************************** diff --git a/docs/autogenerated/ardrone3_states_param_topic.rst b/docs/autogenerated/ardrone3_states_param_topic.rst index bfc8ca2..0885ed9 100644 --- a/docs/autogenerated/ardrone3_states_param_topic.rst +++ b/docs/autogenerated/ardrone3_states_param_topic.rst @@ -1,5 +1,5 @@ .. ardrone3_states_param_topic.rst - .. auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + .. auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml .. Do not modify this file by hand. Check scripts/meta folder for generator files. ***************************************************************************************** @@ -42,6 +42,12 @@ List of ardrone3 States and Corresponding ROS Parameters and Topics Drones air speed changed\n Expressed in the drones referential. `Ardrone3PilotingStatemoveToChanged`_ The drone moves or moved to a given location. +`Ardrone3PilotingStateMotionState`_ + Motion state.\n If [MotionDetection](#1-6-16) is disabled, motion is steady.\n This information is only valid when the drone is not flying. +`Ardrone3PilotingStatePilotedPOI`_ + Piloted POI state. +`Ardrone3PilotingStateReturnHomeBatteryCapacity`_ + Battery capacity status to return home. `Ardrone3NetworkStateWifiScanListChanged`_ Wifi scan results.\n Please note that the list is not complete until you receive the event [WifiScanEnded](#1-14-1). `Ardrone3NetworkStateAllWifiScanChanged`_ @@ -78,6 +84,10 @@ List of ardrone3 States and Corresponding ROS Parameters and Topics Pro features. `Ardrone3AccessoryStateConnectedAccessories`_ List of all connected accessories. This event presents the list of all connected accessories. To actually use the component, use the component dedicated feature. +`Ardrone3AccessoryStateBattery`_ + Connected accessories battery. +`Ardrone3SoundStateAlertSound`_ + Alert sound state. Ardrone3MediaRecordStatePictureStateChanged #################################################################################### @@ -349,6 +359,51 @@ The drone moves or moved to a given location. :caption: Ardrone3PilotingStatemoveToChanged.msg :name: Ardrone3PilotingStatemoveToChanged_msg +Ardrone3PilotingStateMotionState +#################################################################################### +Motion state.\n If [MotionDetection](#1-6-16) is disabled, motion is steady.\n This information is only valid when the drone is not flying. + +- Parameter: ``~states/enable_pilotingstate_motionstate`` +- Topic: ``states/ardrone3/PilotingState/MotionState`` +- Message type: ``bebop_msgs::Ardrone3PilotingStateMotionState`` + +.. literalinclude:: + ../../bebop_msgs/msg/autogenerated/Ardrone3PilotingStateMotionState.msg + :lines: 8- + :language: python + :caption: Ardrone3PilotingStateMotionState.msg + :name: Ardrone3PilotingStateMotionState_msg + +Ardrone3PilotingStatePilotedPOI +#################################################################################### +Piloted POI state. + +- Parameter: ``~states/enable_pilotingstate_pilotedpoi`` +- Topic: ``states/ardrone3/PilotingState/PilotedPOI`` +- Message type: ``bebop_msgs::Ardrone3PilotingStatePilotedPOI`` + +.. literalinclude:: + ../../bebop_msgs/msg/autogenerated/Ardrone3PilotingStatePilotedPOI.msg + :lines: 8- + :language: python + :caption: Ardrone3PilotingStatePilotedPOI.msg + :name: Ardrone3PilotingStatePilotedPOI_msg + +Ardrone3PilotingStateReturnHomeBatteryCapacity +#################################################################################### +Battery capacity status to return home. + +- Parameter: ``~states/enable_pilotingstate_returnhomebatterycapacity`` +- Topic: ``states/ardrone3/PilotingState/ReturnHomeBatteryCapacity`` +- Message type: ``bebop_msgs::Ardrone3PilotingStateReturnHomeBatteryCapacity`` + +.. literalinclude:: + ../../bebop_msgs/msg/autogenerated/Ardrone3PilotingStateReturnHomeBatteryCapacity.msg + :lines: 8- + :language: python + :caption: Ardrone3PilotingStateReturnHomeBatteryCapacity.msg + :name: Ardrone3PilotingStateReturnHomeBatteryCapacity_msg + Ardrone3NetworkStateWifiScanListChanged #################################################################################### Wifi scan results.\n Please note that the list is not complete until you receive the event [WifiScanEnded](#1-14-1). @@ -619,3 +674,33 @@ List of all connected accessories. This event presents the list of all connected :caption: Ardrone3AccessoryStateConnectedAccessories.msg :name: Ardrone3AccessoryStateConnectedAccessories_msg +Ardrone3AccessoryStateBattery +#################################################################################### +Connected accessories battery. + +- Parameter: ``~states/enable_accessorystate_battery`` +- Topic: ``states/ardrone3/AccessoryState/Battery`` +- Message type: ``bebop_msgs::Ardrone3AccessoryStateBattery`` + +.. literalinclude:: + ../../bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateBattery.msg + :lines: 8- + :language: python + :caption: Ardrone3AccessoryStateBattery.msg + :name: Ardrone3AccessoryStateBattery_msg + +Ardrone3SoundStateAlertSound +#################################################################################### +Alert sound state. + +- Parameter: ``~states/enable_soundstate_alertsound`` +- Topic: ``states/ardrone3/SoundState/AlertSound`` +- Message type: ``bebop_msgs::Ardrone3SoundStateAlertSound`` + +.. literalinclude:: + ../../bebop_msgs/msg/autogenerated/Ardrone3SoundStateAlertSound.msg + :lines: 8- + :language: python + :caption: Ardrone3SoundStateAlertSound.msg + :name: Ardrone3SoundStateAlertSound_msg + diff --git a/docs/autogenerated/common_states_param_topic.rst b/docs/autogenerated/common_states_param_topic.rst index 7ad70c4..1344d2c 100644 --- a/docs/autogenerated/common_states_param_topic.rst +++ b/docs/autogenerated/common_states_param_topic.rst @@ -1,5 +1,5 @@ .. common_states_param_topic.rst - .. auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/common.xml + .. auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/common.xml .. Do not modify this file by hand. Check scripts/meta folder for generator files. ***************************************************************************************** diff --git a/docs/dev.rst b/docs/dev.rst index 2df9d08..54af7c7 100644 --- a/docs/dev.rst +++ b/docs/dev.rst @@ -54,10 +54,10 @@ Running Bebop In The Loop Tests Upgrading the Bebop SDK ======================= -.. warning:: Since version 0.6, `Parrot ARSDK `_, the main underlying dependency of *bebop_autonomy* is not build inline anymore. Instead, the slightly patched and catkinized version of it, called `parrot_arsdk `_, is fetched as a dependency. **The following documentation needs to be updated**. +.. warning:: Since version 0.6, `Parrot ARSDK `_, the main underlying dependency of *bebop_autonomy* is not build inline anymore. Instead, the slightly patched and catkinized version of it, called `parrot_arsdk `_, is fetched as a dependency. -1. Update the ``GIT_TAG`` of ``ARDroneSDK3`` in ``bebop_driver/CMakeLists.txt::add_custom_target::./repo init ... -b GIT_TAG`` to your desired commit hash, branch or tag (release). The official upstream repository is hosted `here `_. -2. Checkout (or browse) the upstream repository at the same hash used in step (1) and open ``release.xml`` file. From this file, extract the commit hash of ``arsdk-xml`` from the ``revision`` property of this XML tag: ````. +1. Update the build_depend and run_depend version tag bebop_driver/package.xml to the updated parrot_arsdk package version. +2. Browse the upstream repository at and open ``release.xml`` file. From this file, extract the commit hash of ``arsdk-xml`` from the ``revision`` property of this XML tag: ````. 3. Open ``bebop_driver/scripts/meta/generate.py`` and update ``LIBARCOMMANDS_GIT_HASH`` variable to the hash obtained in step (2). 4. Change the working diretory to ``bebop_driver/scripts/meta``, then execute ``generate.py`` from the command line. This will regenerate all automatically generated message definitions, header files and documentations. 5. Copy the generated files to their target locations by calling ``./install.sh``. @@ -68,7 +68,7 @@ Upgrading the Bebop SDK - ``ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGRADIUSCHANGED_VALUE`` to ``ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGRADIUSCHANGED_CURRENT`` - ``ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGALTITUDECHANGED_VALUE`` to ``ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGALTITUDECHANGED_CURRENT`` -In ``bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h`` remove the following lines: +In ``bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h`` comment out the following lines: ``arg = NULL; HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_CONNECTEDACCESSORIES_LIST_FLAGS, arg); @@ -77,10 +77,27 @@ In ``bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h` msg_ptr->list_flags = arg->value.U8; }`` -In ``bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg`` remove the following lines: - ``# 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`` + ``arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_BATTERY_LIST_FLAGS, arg); + if (arg) + { + msg_ptr->list_flags = arg->value.U8; + }`` -These changes are required because the upstream XML file is inconsistent for a couple of variable names. +In ``bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg`` comment the following line: + ``uint8 list_flags`` + +In ``bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateBattery`` comment the following line: + ``uint8 list_flags`` + +These changes are required because the upstream XML file is inconsistent for a couple of variable names. All details on states and parameters supported by each of the devices see for reference. + +8. Update documentation. This includes: +* Updating all package.xml version numbers. +* Adding entries to all CHANGELOG.rst files. +* Update built against version in the main README.md file. +* Update table in docs/index.rst, specifically the SDK Version and device support information. 7. Remove ``build`` and ``devel`` space of your ``catkin`` workspace, then re-build it. + +8. Run in the loop tests to verify that the update works with the devices. diff --git a/docs/index.rst b/docs/index.rst index 2703ee8..79ec675 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -17,9 +17,9 @@ Features and Roadmap .. csv-table:: :header: "Feature", "Status", "Notes" - SDK Version,"3.12.6", "Since v0.7" + SDK Version,"3.14.0", "Since v0.8" Support for Parrot Bebop 1, Yes, "Tested up to Firmware 3.3" - Support for Parrot Bebop 2, Yes, "Tested up to Firmware 4.0.6" + Support for Parrot Bebop 2, Yes, "Tested up to Firmware 4.7.1" Support for Parrot Disco FPV, No, "Not tested (help wanted)" Core piloting, Yes, "" H264 video decoding, Yes, "Enhancement: `#1 `_" From 46acb87468146b161f6bce2824e2284e4a0ed674 Mon Sep 17 00:00:00 2001 From: Thomas Bamford Date: Tue, 3 Jul 2018 11:16:50 -0400 Subject: [PATCH 02/11] Add support for Disco --- bebop_driver/CHANGELOG.rst | 2 + bebop_driver/include/bebop_driver/bebop.h | 2 + .../bebop_driver/bebop_driver_nodelet.h | 2 + bebop_driver/src/bebop.cpp | 9 +++ bebop_driver/src/bebop_driver_nodelet.cpp | 14 ++++ bebop_tools/CHANGELOG.rst | 2 + bebop_tools/config/xbox360_disco.yaml | 81 +++++++++++++++++++ docs/index.rst | 2 +- docs/piloting.rst | 54 +++++++++---- 9 files changed, 152 insertions(+), 16 deletions(-) create mode 100644 bebop_tools/config/xbox360_disco.yaml diff --git a/bebop_driver/CHANGELOG.rst b/bebop_driver/CHANGELOG.rst index 50b4a46..2944bf9 100644 --- a/bebop_driver/CHANGELOG.rst +++ b/bebop_driver/CHANGELOG.rst @@ -5,6 +5,8 @@ Changelog for package bebop_driver 0.8.0 (2018-07-03) ------------------ * SDK 3.14.0 support +* Add support for Disco +* Add a new ROS topic for entering and exiting user takeoff mode: `user_takeoff` * Contributors: Thomas Bamford 0.7.0 (2017-07-29) diff --git a/bebop_driver/include/bebop_driver/bebop.h b/bebop_driver/include/bebop_driver/bebop.h index 2aa8fb9..7e223c0 100644 --- a/bebop_driver/include/bebop_driver/bebop.h +++ b/bebop_driver/include/bebop_driver/bebop.h @@ -163,6 +163,8 @@ class Bebop void FlatTrim(); // false: Stop, true: Start void NavigateHome(const bool& start_stop); + // false: Exit from user take off, true: Enter in user take off + void UserTakeoff(const bool& start_stop); void StartAutonomousFlight(const std::string &filepath); void PauseAutonomousFlight(); void StopAutonomousFlight(); diff --git a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h index 03cbebc..8cfcca0 100644 --- a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h +++ b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h @@ -117,6 +117,7 @@ class BebopDriverNodelet : public nodelet::Nodelet ros::Subscriber reset_sub_; ros::Subscriber flattrim_sub_; ros::Subscriber navigatehome_sub_; + ros::Subscriber usertakeoff_sub_; ros::Subscriber start_autoflight_sub_; ros::Subscriber pause_autoflight_sub_; ros::Subscriber stop_autoflight_sub_; @@ -157,6 +158,7 @@ class BebopDriverNodelet : public nodelet::Nodelet void EmergencyCallback(const std_msgs::EmptyConstPtr& empty_ptr); void FlatTrimCallback(const std_msgs::EmptyConstPtr& empty_ptr); void NavigateHomeCallback(const std_msgs::BoolConstPtr& start_stop_ptr); + void UserTakeoffCallback(const std_msgs::BoolConstPtr& start_stop_ptr); void StartAutonomousFlightCallback(const std_msgs::StringConstPtr& file_path_ptr); void PauseAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr); void StopAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr); diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp index d2664c1..368f51a 100644 --- a/bebop_driver/src/bebop.cpp +++ b/bebop_driver/src/bebop.cpp @@ -478,6 +478,15 @@ void Bebop::NavigateHome(const bool &start_stop) "Navigate home failed"); } +void Bebop::UserTakeoff(const bool &start_stop) +{ + ThrowOnInternalError("User takeoff failed"); + ThrowOnCtrlError( + device_controller_ptr_->aRDrone3->sendPilotingUserTakeOff( + device_controller_ptr_->aRDrone3, start_stop ? 1 : 0), + "User takeoff failed"); +} + void Bebop::StartAutonomousFlight(const std::string &filepath) { ThrowOnInternalError("Start autonomous flight failed"); diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index 1deca22..202e0e1 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/bebop_driver/src/bebop_driver_nodelet.cpp @@ -139,6 +139,7 @@ void BebopDriverNodelet::onInit() reset_sub_ = nh.subscribe("reset", 1, &BebopDriverNodelet::EmergencyCallback, this); flattrim_sub_ = nh.subscribe("flattrim", 1, &BebopDriverNodelet::FlatTrimCallback, this); navigatehome_sub_ = nh.subscribe("autoflight/navigate_home", 1, &BebopDriverNodelet::NavigateHomeCallback, this); + usertakeoff_sub_ = nh.subscribe("user_takeoff", 1, &BebopDriverNodelet::UserTakeoffCallback, this); start_autoflight_sub_ = nh.subscribe("autoflight/start", 1, &BebopDriverNodelet::StartAutonomousFlightCallback, this); pause_autoflight_sub_ = nh.subscribe("autoflight/pause", 1, &BebopDriverNodelet::PauseAutonomousFlightCallback, this); stop_autoflight_sub_ = nh.subscribe("autoflight/stop", 1, &BebopDriverNodelet::StopAutonomousFlightCallback, this); @@ -315,6 +316,19 @@ void BebopDriverNodelet::NavigateHomeCallback(const std_msgs::BoolConstPtr &star } } +void BebopDriverNodelet::UserTakeoffCallback(const std_msgs::BoolConstPtr &start_stop_ptr) +{ + try + { + ROS_INFO("%sing user takeoff behavior ...", start_stop_ptr->data ? "Enter" : "Exit"); + bebop_ptr_->UserTakeoff(start_stop_ptr->data); + } + catch (const std::runtime_error& e) + { + ROS_ERROR_STREAM(e.what()); + } +} + void BebopDriverNodelet::StartAutonomousFlightCallback(const std_msgs::StringConstPtr& filepath_ptr) { std::string filepath; diff --git a/bebop_tools/CHANGELOG.rst b/bebop_tools/CHANGELOG.rst index 0324ee2..2c08ae0 100644 --- a/bebop_tools/CHANGELOG.rst +++ b/bebop_tools/CHANGELOG.rst @@ -4,6 +4,8 @@ Changelog for package bebop_tools 0.8.0 (2018-07-03) ------------------ +* Added Xbox 360 config file for Disco +* Contributors: Thomas Bamford 0.7.0 (2017-07-29) ------------------ diff --git a/bebop_tools/config/xbox360_disco.yaml b/bebop_tools/config/xbox360_disco.yaml new file mode 100644 index 0000000..cc2a424 --- /dev/null +++ b/bebop_tools/config/xbox360_disco.yaml @@ -0,0 +1,81 @@ +# Xbox 360 wireless controller +# Deadman (enable) button: Right Bumper +teleop: + piloting: + type: topic + message_type: "geometry_msgs/Twist" + topic_name: cmd_vel + deadman_buttons: [5] + axis_mappings: + - + axis: 4 # Right thumb stick (pitch down/up) + target: linear.x + scale: 1.0 + offset: 0.0 + - + axis: 3 # Right thumb stick (roll left/right) + target: linear.y + scale: 1.0 + offset: 0.0 + - + axis: 1 # Left thumb stick (faster/slower) + target: linear.z + scale: 1.0 + offset: 0.0 + - + axis: 0 # Left thumb stick (circle left/right) + target: angular.z + scale: 1.0 + offset: 0.0 + camera: + type: topic + message_type: "geometry_msgs/Twist" + topic_name: camera_control + deadman_buttons: [7] # Start + axis_mappings: + - + axis: 1 # Left D-Pad (up/down) + target: angular.y + scale: 83.0 + offset: 0.0 + - + axis: 0 # Left D-Pad (left/right) + target: angular.z + scale: -35.0 + offset: 0.0 + takeoff: + type: topic + message_type: "std_msgs/Empty" + topic_name: takeoff + deadman_buttons: [5, 3] # RB + Y + axis_mappings: [] + land: + type: topic + message_type: "std_msgs/Empty" + topic_name: land + deadman_buttons: [5, 0] # RB + A + axis_mappings: [] + emergency: + type: topic + message_type: "std_msgs/Empty" + topic_name: reset + deadman_buttons: [5, 1] # RB + B + axis_mappings: [] + flattrim: + type: topic + message_type: "std_msgs/Empty" + topic_name: flattrim + deadman_buttons: [5, 2] # RB + X + axis_mappings: [] + flip: + type: topic + message_type: "std_msgs/UInt8" + topic_name: flip + deadman_buttons: [5, 4, 2] # RB + LB + X + axis_mappings: [] + snapshot: + type: topic + message_type: "std_msgs/Empty" + topic_name: snapshot + deadman_buttons: [5, 8] # RB + XBox + axis_mappings: [] diff --git a/docs/index.rst b/docs/index.rst index 79ec675..db7845b 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -20,7 +20,7 @@ Features and Roadmap SDK Version,"3.14.0", "Since v0.8" Support for Parrot Bebop 1, Yes, "Tested up to Firmware 3.3" Support for Parrot Bebop 2, Yes, "Tested up to Firmware 4.7.1" - Support for Parrot Disco FPV, No, "Not tested (help wanted)" + Support for Parrot Disco FPV, Yes, "Tested up to Firmware 1.7.1" Core piloting, Yes, "" H264 video decoding, Yes, "Enhancement: `#1 `_" ROS Camera Interface, Yes, "" diff --git a/docs/piloting.rst b/docs/piloting.rst index 9aed49b..ffd4ba3 100644 --- a/docs/piloting.rst +++ b/docs/piloting.rst @@ -9,14 +9,23 @@ Sending Commands to Bebop Takeoff ======= -Publish a message of type ``std_msgs/Empty`` to ``takeoff`` topic. +Ask the drone to take off. On fixed wings (such as Disco): not used except to cancel a land. Publish a message of type ``std_msgs/Empty`` to ``takeoff`` topic. .. code-block:: bash $ rostopic pub --once [namespace]/takeoff std_msgs/Empty +User Takeoff +======= + +On copters: initiates the thrown takeoff. Note that the drone will do the thrown take off even if it is steady. On fixed wings (such as Disco): initiates the take off process on the fixed wings. Publish a message of type `std_msgs/Bool `_ to `user_takeoff` topic with the ``data`` field set to ``true`` to enter user take off mode. To exit from user take off mode, publish another message with ``data`` set to ``false``. + +.. code-block:: bash + + $ rostopic pub --once [namespace]/user_takeoff std_msgs/Bool true + Land -==== +======= Publish a message of type ``std_msgs/Empty`` to ``land`` topic. @@ -36,7 +45,9 @@ Publish a message of type ``std_msgs/Empty`` to ``reset`` topic. Piloting ======== -To move Bebop around, publish messages of type `geometry_msgs/Twist `_ to `cmd_vel` topic while Bebop is flying. The effect of each field of the message on Bebop's movement is listed below: +To move the vehicle around, publish messages of type `geometry_msgs/Twist `_ to `cmd_vel` topic while the vehicle is flying. + +The effect of each field of the message on Bebop's movement is listed below: .. code-block:: text @@ -49,11 +60,24 @@ To move Bebop around, publish messages of type `geometry_msgs/Twist `_, which is specified in degrees and is dynamically reconfigurable (:ref:`sec-dyn-params`). +The ``linear.x`` and ``linear.y`` parts of this message set the **pitch** and **roll** angles of the vehicle, respectively, hence control its forward and lateral accelerations. The resulting pitch/roll angles depend on the value of ``~PilotingSettingsMaxTiltCurrent`` `parameter <./autogenerated/ardrone3_settings_param.html#pilotingsettingsmaxtiltcurrent>`_, which is specified in degrees and is dynamically reconfigurable (:ref:`sec-dyn-params`). -The ``linear.z`` part of this message controls the **vertical velocity** of the Bebop. The resulting velocity in m/s depends on the value of ``~SpeedSettingsMaxVerticalSpeedCurrent`` `parameter <./autogenerated/ardrone3_settings_param.html#speedsettingsmaxverticalspeedcurrent>`_, which is specified in meters per second and is also dynamically reconfigurable (:ref:`sec-dyn-params`). Similarly, the ``angular.z`` component of this message controls the rotational velocity of the Bebop (around the z-axis). The corresponding scaling `parameter <./autogenerated/ardrone3_settings_param.html#speedsettingsmaxrotationspeedcurrent>`_ is ``SpeedSettingsMaxRotationSpeedCurrent`` (in degrees per sec). +The ``linear.z`` part of this message controls the **vertical velocity** of the vehicle. The resulting velocity in m/s depends on the value of ``~SpeedSettingsMaxVerticalSpeedCurrent`` `parameter <./autogenerated/ardrone3_settings_param.html#speedsettingsmaxverticalspeedcurrent>`_, which is specified in meters per second and is also dynamically reconfigurable (:ref:`sec-dyn-params`). Similarly, the ``angular.z`` component of this message controls the rotational velocity of the Bebop (around the z-axis), or in the case of Disco, circling direction (around the z-axis). The corresponding scaling for Bebop `parameter <./autogenerated/ardrone3_settings_param.html#speedsettingsmaxrotationspeedcurrent>`_ is ``SpeedSettingsMaxRotationSpeedCurrent`` (in degrees per sec). .. code-block:: text @@ -86,19 +110,19 @@ An autonomous flight plan consists of a series of GPS waypoints along with Bebop Requirements that must be met before an autonomous flight can start: - * Bebop is calibrated - * Bebop is in outdoor mode - * Bebop has fixed its GPS + * Vehicle is calibrated + * Vehicle is in outdoor mode + * Vehicle has fixed its GPS -To start an autonomous flight plan, publish a message of type `std_msgs/String `_ to `autoflight/start` topic. The ``data`` field should contain the name of the flight plan to execute, which is already stored on-board Bebop. +To start an autonomous flight plan, publish a message of type `std_msgs/String `_ to `autoflight/start` topic. The ``data`` field should contain the name of the flight plan to execute, which is already stored on-board the vehicle. .. note:: If an empty string is published, then the default 'flightplan.mavlink' is used. .. warning:: If not already flying, Bebop will attempt to take off upon starting a flight plan. -The `Flight Plan App `_ allows easy construction of flight plans and saves them on-board Bebop. +The `Flight Plan App `_ allows easy construction of flight plans and saves them on-board the vehicle. -An FTP client can also be used to view and copy flight plans on-board Bebop. `FileZilla` is recommended: +An FTP client can also be used to view and copy flight plans on-board the vehicle. `FileZilla` is recommended: .. code-block:: bash @@ -116,20 +140,20 @@ Then open `Site Manager` (top left), click `New Site`: Pause Flight Plan ----------------- -To pause the execution of an autonomous flight plan, publish a message of type `std_msgs/Empty `_ to `autoflight/pause` topic. Bebop will then hover and await further commands. -To resume a paused flight plan, publish the same message that was used to start the autonomous flight (ie. to the topic `autoflight/start`). Bebop will fly to the lastest waypoint reached before continuing the flight plan. +To pause the execution of an autonomous flight plan, publish a message of type `std_msgs/Empty `_ to `autoflight/pause` topic. Bebop will then hover and await further commands. Disco will fly straight and level awaiting further commands. +To resume a paused flight plan, publish the same message that was used to start the autonomous flight (ie. to the topic `autoflight/start`). The vehicle will fly to the lastest waypoint reached before continuing the flight plan. .. note:: Any velocity commands sent to Bebop during an autonomous flight plan will pause the plan. Stop Flight Plan ---------------- -To stop the execution of an autonomous flight plan, publish a message of type `std_msgs/Empty `_ to `autoflight/stop` topic. Bebop will hover and await further commands. +To stop the execution of an autonomous flight plan, publish a message of type `std_msgs/Empty `_ to `autoflight/stop` topic. Bebop will hover and await further commands. Disco will fly straight and level awaiting further commands. Navigate Home ------------- -To ask Bebop to autonomously fly to it's home position, publish a message of type `std_msgs/Bool `_ to `autoflight/navigate_home` topic with the ``data`` field set to ``true``. To stop Bebop from navigating home, publish another message with ``data`` set to ``false``. +To ask the vehicle to autonomously fly to it's home position, publish a message of type `std_msgs/Bool `_ to `autoflight/navigate_home` topic with the ``data`` field set to ``true``. To stop Bebop from navigating home, publish another message with ``data`` set to ``false``. .. warning:: The topic has changed from `navigate_home` to `autoflight/navigate_home` after version 0.5.1. From 6c6d21c33643208315f4432a9e999901b1f55a67 Mon Sep 17 00:00:00 2001 From: Thomas Bamford Date: Tue, 3 Jul 2018 12:05:18 -0400 Subject: [PATCH 03/11] Add support for joy control of autoflight commands --- bebop_tools/CHANGELOG.rst | 1 + bebop_tools/CMakeLists.txt | 5 +- bebop_tools/config/xbox360.yaml | 32 +++++++- bebop_tools/config/xbox360_disco.yaml | 36 ++++++++- .../launch/joy_teleop_autoflight.launch | 25 +++++++ bebop_tools/package.xml | 2 + bebop_tools/scripts/__init__.py | 0 bebop_tools/scripts/autoflight_joy.py | 73 +++++++++++++++++++ bebop_tools/setup.py | 11 +++ docs/piloting.rst | 3 + 10 files changed, 182 insertions(+), 6 deletions(-) create mode 100644 bebop_tools/launch/joy_teleop_autoflight.launch create mode 100755 bebop_tools/scripts/__init__.py create mode 100755 bebop_tools/scripts/autoflight_joy.py create mode 100755 bebop_tools/setup.py diff --git a/bebop_tools/CHANGELOG.rst b/bebop_tools/CHANGELOG.rst index 2c08ae0..036a9f3 100644 --- a/bebop_tools/CHANGELOG.rst +++ b/bebop_tools/CHANGELOG.rst @@ -5,6 +5,7 @@ Changelog for package bebop_tools 0.8.0 (2018-07-03) ------------------ * Added Xbox 360 config file for Disco +* Added support for joy control of autoflight commands * Contributors: Thomas Bamford 0.7.0 (2017-07-29) diff --git a/bebop_tools/CMakeLists.txt b/bebop_tools/CMakeLists.txt index 6341b46..ef48fe9 100644 --- a/bebop_tools/CMakeLists.txt +++ b/bebop_tools/CMakeLists.txt @@ -3,13 +3,14 @@ project(bebop_tools) find_package(catkin REQUIRED COMPONENTS bebop_msgs + rospy ) -# catkin_python_setup() +catkin_python_setup() catkin_package( # INCLUDE_DIRS include - CATKIN_DEPENDS bebop_msgs + CATKIN_DEPENDS bebop_msgs rospy ) # TODO: Install Rules diff --git a/bebop_tools/config/xbox360.yaml b/bebop_tools/config/xbox360.yaml index 9b69862..c02fcf9 100644 --- a/bebop_tools/config/xbox360.yaml +++ b/bebop_tools/config/xbox360.yaml @@ -73,9 +73,39 @@ teleop: topic_name: flip deadman_buttons: [5, 4, 2] # RB + LB + X axis_mappings: [] + start: + type: topic + message_type: "std_msgs/Empty" + topic_name: autoflight/start/joy + deadman_buttons: [5, 7] # RB + start + axis_mappings: [] + stop: + type: topic + message_type: "std_msgs/Empty" + topic_name: autoflight/stop + deadman_buttons: [5, 6] # RB + back + axis_mappings: [] + navigatehome: + type: topic + message_type: "std_msgs/Empty" + topic_name: autoflight/navigate_home/joy + deadman_buttons: [5, 8] # RB + XBox + axis_mappings: [] snapshot: type: topic message_type: "std_msgs/Empty" topic_name: snapshot - deadman_buttons: [5, 8] # RB + XBox + deadman_buttons: [4, 8] # LB + XBox + axis_mappings: [] + recordstart: + type: topic + message_type: "std_msgs/Empty" + topic_name: record/joy/start + deadman_buttons: [4, 7] # LB + start + axis_mappings: [] + recordstop: + type: topic + message_type: "std_msgs/Empty" + topic_name: record/joy/stop + deadman_buttons: [4, 6] # LB + back axis_mappings: [] diff --git a/bebop_tools/config/xbox360_disco.yaml b/bebop_tools/config/xbox360_disco.yaml index cc2a424..a62573a 100644 --- a/bebop_tools/config/xbox360_disco.yaml +++ b/bebop_tools/config/xbox360_disco.yaml @@ -61,10 +61,10 @@ teleop: topic_name: reset deadman_buttons: [5, 1] # RB + B axis_mappings: [] - flattrim: + usertakeoff: type: topic message_type: "std_msgs/Empty" - topic_name: flattrim + topic_name: usertakeoff/joy deadman_buttons: [5, 2] # RB + X axis_mappings: [] flip: @@ -73,9 +73,39 @@ teleop: topic_name: flip deadman_buttons: [5, 4, 2] # RB + LB + X axis_mappings: [] + start: + type: topic + message_type: "std_msgs/Empty" + topic_name: autoflight/start/joy + deadman_buttons: [5, 7] # RB + start + axis_mappings: [] + stop: + type: topic + message_type: "std_msgs/Empty" + topic_name: autoflight/stop + deadman_buttons: [5, 6] # RB + back + axis_mappings: [] + navigatehome: + type: topic + message_type: "std_msgs/Empty" + topic_name: autoflight/navigate_home/joy + deadman_buttons: [5, 8] # RB + XBox + axis_mappings: [] snapshot: type: topic message_type: "std_msgs/Empty" topic_name: snapshot - deadman_buttons: [5, 8] # RB + XBox + deadman_buttons: [4, 8] # LB + XBox + axis_mappings: [] + recordstart: + type: topic + message_type: "std_msgs/Empty" + topic_name: record/joy/start + deadman_buttons: [4, 7] # LB + start + axis_mappings: [] + recordstop: + type: topic + message_type: "std_msgs/Empty" + topic_name: record/joy/stop + deadman_buttons: [4, 6] # LB + back axis_mappings: [] diff --git a/bebop_tools/launch/joy_teleop_autoflight.launch b/bebop_tools/launch/joy_teleop_autoflight.launch new file mode 100644 index 0000000..ac64ac9 --- /dev/null +++ b/bebop_tools/launch/joy_teleop_autoflight.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_tools/package.xml b/bebop_tools/package.xml index cedff1d..cd42633 100644 --- a/bebop_tools/package.xml +++ b/bebop_tools/package.xml @@ -17,11 +17,13 @@ catkin bebop_msgs + rospy bebop_driver bebop_msgs joy joy_teleop image_view + rospy diff --git a/bebop_tools/scripts/__init__.py b/bebop_tools/scripts/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/bebop_tools/scripts/autoflight_joy.py b/bebop_tools/scripts/autoflight_joy.py new file mode 100755 index 0000000..a1320fd --- /dev/null +++ b/bebop_tools/scripts/autoflight_joy.py @@ -0,0 +1,73 @@ +#!/usr/bin/python + +"""Listen to joy autoflight node and publish to complex autoflight topics.""" + +import rospy +from std_msgs.msg import Bool, Empty, String + + +class AutoFlightJoy: + """Subscribe to joy autoflight node and publish to bebop driver node.""" + + navigatehome = True + usertakeoff = True + + def __init__(self): + """Initialize publishers and subscribers.""" + self.pub_start = rospy.Publisher('autoflight/start', String, + queue_size=1) + self.pub_home = rospy.Publisher('autoflight/navigate_home', Bool, + queue_size=1) + self.pub_usertakeoff = rospy.Publisher('user_takeoff', Bool, + queue_size=1) + self.pub_record = rospy.Publisher('record', Bool, queue_size=1) + + rospy.Subscriber('autoflight/start/joy', Empty, self.start_callback) + rospy.Subscriber('autoflight/navigate_home/joy', Empty, + self.home_callback) + rospy.Subscriber('user_takeoff/joy', Empty, self.usertakeoff_callback) + rospy.Subscriber('record/joy/start', Empty, self.record_start_callback) + rospy.Subscriber('record/joy/stop', Empty, self.record_stop_callback) + + rospy.loginfo('autoflight: Ready to receive commands from joy.') + + def start_callback(self, empty): + """Retrieve flight plan name and publish to bebop autoflight/start.""" + string = rospy.get_param('~autoflight/start/fname', + 'flightPlan.mavlink') + self.pub_start.publish(string) + + def home_callback(self, empty): + """Publish alternating bool to bebop autoflight/navigate_home.""" + if self.navigatehome: + self.pub_home.publish(self.navigatehome) + self.navigatehome = False + else: + self.pub_home.publish(self.navigatehome) + self.navigatehome = True + + def usertakeoff_callback(self, empty): + """Publish alternating bool to bebop user_takeoff.""" + if self.usertakeoff: + self.pub_usertakeoff.publish(self.usertakeoff) + self.usertakeoff = False + else: + self.pub_usertakeoff.publish(self.usertakeoff) + self.usertakeoff = True + + def record_start_callback(self, empty): + """Publish true to bebop record.""" + self.pub_record.publish(True) + + def record_stop_callback(self, empty): + """Publish false to bebop record.""" + self.pub_record.publish(False) + + +if __name__ == '__main__': + rospy.init_node('autoflight_joy', anonymous=True) + AutoFlightJoy() + try: + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/bebop_tools/setup.py b/bebop_tools/setup.py new file mode 100755 index 0000000..726ca65 --- /dev/null +++ b/bebop_tools/setup.py @@ -0,0 +1,11 @@ +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['bebop_tools'], + package_dir={'': 'src'}) + +setup(**setup_args) diff --git a/docs/piloting.rst b/docs/piloting.rst index ffd4ba3..5077b54 100644 --- a/docs/piloting.rst +++ b/docs/piloting.rst @@ -6,6 +6,9 @@ Sending Commands to Bebop .. note:: ``bebop_tools`` package comes with a launch file for tele-operating Bebop with a joystick using ROS `joy_teleop `_ package. The configuration file (key-action map) is written for `Logitech F710 controller `_ and is located in ``bebop_tools/config`` folder. Adapting the file to your own controller is straightforward. To teleop Bebop while the driver is running execute ``roslaunch bebop_tools joy_teleop.launch``. +.. note:: To control the autoflight functions using the joystick while the driver is running execute ``roslaunch bebop_tools joy_teleop_autoflight.launch``. The autoflight configuration is written for the XBox 360 controller. Adapting the file to your own controller is straightforward. + + Takeoff ======= From b0cf0a75087b759628b9284692242bb16ddf356f Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Mon, 23 Jul 2018 05:04:39 +0200 Subject: [PATCH 04/11] Bug fix during catkin build (#164) * Bug fix during catkin build The bug during the catkin build process has been fixed in according to the issue #132 and explained in 78071a14 of the FFmpeg repository * Updated changelog and package files Updated changelog and package file into the bebop_driver folder. --- bebop_driver/CHANGELOG.rst | 5 +++++ bebop_driver/package.xml | 2 +- bebop_driver/src/bebop_video_decoder.cpp | 6 +++--- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/bebop_driver/CHANGELOG.rst b/bebop_driver/CHANGELOG.rst index 0dd8cca..de5aac7 100644 --- a/bebop_driver/CHANGELOG.rst +++ b/bebop_driver/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bebop_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.1 (2018-05-31) +------------------ +* Fixed the bug appearing when you run the catkin build command +* Contributors: Giuseppe Silano + 0.7.0 (2017-07-29) ------------------ * SDK 3.12.6 support (except 64bit Ubuntu Xenial, working on fix) diff --git a/bebop_driver/package.xml b/bebop_driver/package.xml index 1c0a3cb..63bf4dd 100644 --- a/bebop_driver/package.xml +++ b/bebop_driver/package.xml @@ -1,7 +1,7 @@ bebop_driver - 0.7.0 + 0.7.1 ROS driver for Parrot Bebop drone, based on Parrot’s official ARDroneSDK3 Mani Monajjemi 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 d11c918f31d1ba15365e1ea304cbdaf10d4c5db8 Mon Sep 17 00:00:00 2001 From: Thomas Bamford Date: Tue, 3 Jul 2018 08:04:50 -0400 Subject: [PATCH 05/11] Add support for SDK 3.14.0 --- README.md | 2 +- bebop_autonomy/CHANGELOG.rst | 3 + bebop_autonomy/package.xml | 2 +- bebop_description/CHANGELOG.rst | 3 + bebop_description/package.xml | 2 +- bebop_driver/CHANGELOG.rst | 5 + .../cfg/autogenerated/BebopArdrone3.cfg | 2 +- .../ardrone3_setting_callback_includes.h | 2 +- .../ardrone3_setting_callbacks.h | 2 +- .../ardrone3_state_callback_includes.h | 52 ++- .../autogenerated/ardrone3_state_callbacks.h | 304 +++++++++++++++++- .../autogenerated/callbacks_common.h | 2 +- .../common_state_callback_includes.h | 2 +- .../autogenerated/common_state_callbacks.h | 4 +- bebop_driver/package.xml | 6 +- bebop_driver/scripts/meta/generate.py | 4 +- bebop_driver/scripts/meta/last_build_info | 6 +- bebop_msgs/CHANGELOG.rst | 5 + .../Ardrone3AccessoryStateBattery.msg | 16 + ...one3AccessoryStateConnectedAccessories.msg | 4 +- ...rdrone3PilotingStateFlyingStateChanged.msg | 2 +- .../Ardrone3PilotingStateMotionState.msg | 14 + .../Ardrone3PilotingStatePilotedPOI.msg | 22 ++ ...PilotingStateReturnHomeBatteryCapacity.msg | 16 + .../Ardrone3PilotingStatemoveToChanged.msg | 2 +- .../Ardrone3SoundStateAlertSound.msg | 14 + bebop_msgs/msg/autogenerated/last_build_info | 6 +- bebop_msgs/package.xml | 2 +- bebop_tools/CHANGELOG.rst | 3 + bebop_tools/package.xml | 2 +- .../autogenerated/ardrone3_settings_param.rst | 2 +- .../ardrone3_states_param_topic.rst | 87 ++++- .../common_states_param_topic.rst | 2 +- docs/dev.rst | 33 +- docs/index.rst | 4 +- 35 files changed, 598 insertions(+), 41 deletions(-) create mode 100644 bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateBattery.msg create mode 100644 bebop_msgs/msg/autogenerated/Ardrone3PilotingStateMotionState.msg create mode 100644 bebop_msgs/msg/autogenerated/Ardrone3PilotingStatePilotedPOI.msg create mode 100644 bebop_msgs/msg/autogenerated/Ardrone3PilotingStateReturnHomeBatteryCapacity.msg create mode 100644 bebop_msgs/msg/autogenerated/Ardrone3SoundStateAlertSound.msg diff --git a/README.md b/README.md index 1c0d2d3..bc3a7e5 100644 --- a/README.md +++ b/README.md @@ -19,4 +19,4 @@ [![Build Status (ROS I,J,K) - TravisCI](https://travis-ci.org/AutonomyLab/bebop_autonomy.svg?branch=indigo-devel)](https://travis-ci.org/AutonomyLab/bebop_autonomy) [![Build Status (ROS I,J) - Semaphore](https://semaphoreci.com/api/v1/projects/11786233-d505-4d79-b27c-80c2742243a4/537552/badge.svg)](https://semaphoreci.com/mani_monaj/bebop_autonomy) -Built against [parrot_arsdk](https://github.com/AutonomyLab/parrot_arsdk) 3.12.6p1 +Built against [parrot_arsdk](https://github.com/AutonomyLab/parrot_arsdk) 3.14.0p0 diff --git a/bebop_autonomy/CHANGELOG.rst b/bebop_autonomy/CHANGELOG.rst index d5ffd1d..847985d 100644 --- a/bebop_autonomy/CHANGELOG.rst +++ b/bebop_autonomy/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bebop_autonomy ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.0 (2018-07-03) +------------------ + 0.7.0 (2017-07-29) ------------------ diff --git a/bebop_autonomy/package.xml b/bebop_autonomy/package.xml index 610dca4..ed5097c 100644 --- a/bebop_autonomy/package.xml +++ b/bebop_autonomy/package.xml @@ -1,7 +1,7 @@ bebop_autonomy - 0.7.0 + 0.8.0 bebop_autonomy is a ROS driver for Parrot Bebop drone, based on Parrot’s official ARDroneSDK3 Mani Monajjemi diff --git a/bebop_description/CHANGELOG.rst b/bebop_description/CHANGELOG.rst index e889325..c45e78b 100644 --- a/bebop_description/CHANGELOG.rst +++ b/bebop_description/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bebop_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.0 (2018-07-03) +------------------ + 0.7.0 (2017-07-29) ------------------ diff --git a/bebop_description/package.xml b/bebop_description/package.xml index 301fa3b..fff87ec 100644 --- a/bebop_description/package.xml +++ b/bebop_description/package.xml @@ -1,7 +1,7 @@ bebop_description - 0.7.0 + 0.8.0 URDF robot description for Parrot Bebop Drones Mani Monajjemi diff --git a/bebop_driver/CHANGELOG.rst b/bebop_driver/CHANGELOG.rst index de5aac7..0f29533 100644 --- a/bebop_driver/CHANGELOG.rst +++ b/bebop_driver/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bebop_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.0 (2018-07-03) +------------------ +* SDK 3.14.0 support +* Contributors: Thomas Bamford + 0.7.1 (2018-05-31) ------------------ * Fixed the bug appearing when you run the catkin build command diff --git a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg index f4b2763..a31702d 100755 --- a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg +++ b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg @@ -1,7 +1,7 @@ #!/usr/bin/env python # BebopArdrone3.cfg -# auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml +# auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml # Do not modify this file by hand. Check scripts/meta folder for generator files. PACKAGE = "bebop_driver" diff --git a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callback_includes.h b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callback_includes.h index cce9163..5084f93 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callback_includes.h +++ b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callback_includes.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * Ardrone3_setting_callback_includes.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ diff --git a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callbacks.h b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callbacks.h index f47b1ab..a064585 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callbacks.h +++ b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callbacks.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * Ardrone3_setting_callbacks.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ #ifndef BEBOP_AUTONOMY_AUTOGENERATED_Ardrone3_SETTING_CALLBACKS_H diff --git a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callback_includes.h b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callback_includes.h index 8d04c7e..5375fb4 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callback_includes.h +++ b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callback_includes.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * ardrone3_state_callback_includes.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ @@ -48,6 +48,9 @@ namespace cb class Ardrone3PilotingStateLandingStateChanged; class Ardrone3PilotingStateAirSpeedChanged; class Ardrone3PilotingStatemoveToChanged; + class Ardrone3PilotingStateMotionState; + class Ardrone3PilotingStatePilotedPOI; + class Ardrone3PilotingStateReturnHomeBatteryCapacity; class Ardrone3NetworkStateWifiScanListChanged; class Ardrone3NetworkStateAllWifiScanChanged; class Ardrone3NetworkStateWifiAuthChannelListChanged; @@ -66,6 +69,8 @@ namespace cb class Ardrone3GPSStateHomeTypeChosenChanged; class Ardrone3PROStateFeatures; class Ardrone3AccessoryStateConnectedAccessories; + class Ardrone3AccessoryStateBattery; + class Ardrone3SoundStateAlertSound; } // namespace cb #endif // FORWARD_DECLARATIONS @@ -107,6 +112,12 @@ boost::shared_ptr ardrone3_pilotingstate_airspeedchanged_ptr; boost::shared_ptr ardrone3_pilotingstate_movetochanged_ptr; +boost::shared_ptr + ardrone3_pilotingstate_motionstate_ptr; +boost::shared_ptr + ardrone3_pilotingstate_pilotedpoi_ptr; +boost::shared_ptr + ardrone3_pilotingstate_returnhomebatterycapacity_ptr; boost::shared_ptr ardrone3_networkstate_wifiscanlistchanged_ptr; boost::shared_ptr @@ -143,6 +154,10 @@ boost::shared_ptr ardrone3_prostate_features_ptr; boost::shared_ptr ardrone3_accessorystate_connectedaccessories_ptr; +boost::shared_ptr + ardrone3_accessorystate_battery_ptr; +boost::shared_ptr + ardrone3_soundstate_alertsound_ptr; #endif // DEFINE_SHARED_PTRS #ifdef UPDTAE_CALLBACK_MAP @@ -201,6 +216,15 @@ ardrone3_pilotingstate_airspeedchanged_ptr.reset( ardrone3_pilotingstate_movetochanged_ptr.reset( new cb::Ardrone3PilotingStatemoveToChanged( nh, priv_nh, "states/ardrone3/PilotingState/moveToChanged")); +ardrone3_pilotingstate_motionstate_ptr.reset( + new cb::Ardrone3PilotingStateMotionState( + nh, priv_nh, "states/ardrone3/PilotingState/MotionState")); +ardrone3_pilotingstate_pilotedpoi_ptr.reset( + new cb::Ardrone3PilotingStatePilotedPOI( + nh, priv_nh, "states/ardrone3/PilotingState/PilotedPOI")); +ardrone3_pilotingstate_returnhomebatterycapacity_ptr.reset( + new cb::Ardrone3PilotingStateReturnHomeBatteryCapacity( + nh, priv_nh, "states/ardrone3/PilotingState/ReturnHomeBatteryCapacity")); ardrone3_networkstate_wifiscanlistchanged_ptr.reset( new cb::Ardrone3NetworkStateWifiScanListChanged( nh, priv_nh, "states/ardrone3/NetworkState/WifiScanListChanged")); @@ -255,6 +279,12 @@ ardrone3_prostate_features_ptr.reset( ardrone3_accessorystate_connectedaccessories_ptr.reset( new cb::Ardrone3AccessoryStateConnectedAccessories( nh, priv_nh, "states/ardrone3/AccessoryState/ConnectedAccessories")); +ardrone3_accessorystate_battery_ptr.reset( + new cb::Ardrone3AccessoryStateBattery( + nh, priv_nh, "states/ardrone3/AccessoryState/Battery")); +ardrone3_soundstate_alertsound_ptr.reset( + new cb::Ardrone3SoundStateAlertSound( + nh, priv_nh, "states/ardrone3/SoundState/AlertSound")); // Add all wrappers to the callback map callback_map_.insert(std::pair >( @@ -329,6 +359,18 @@ callback_map_.insert(std::pairGetCommandKey(), ardrone3_pilotingstate_movetochanged_ptr)); // Add all wrappers to the callback map +callback_map_.insert(std::pair >( + ardrone3_pilotingstate_motionstate_ptr->GetCommandKey(), + ardrone3_pilotingstate_motionstate_ptr)); +// Add all wrappers to the callback map +callback_map_.insert(std::pair >( + ardrone3_pilotingstate_pilotedpoi_ptr->GetCommandKey(), + ardrone3_pilotingstate_pilotedpoi_ptr)); +// Add all wrappers to the callback map +callback_map_.insert(std::pair >( + ardrone3_pilotingstate_returnhomebatterycapacity_ptr->GetCommandKey(), + ardrone3_pilotingstate_returnhomebatterycapacity_ptr)); +// Add all wrappers to the callback map callback_map_.insert(std::pair >( ardrone3_networkstate_wifiscanlistchanged_ptr->GetCommandKey(), ardrone3_networkstate_wifiscanlistchanged_ptr)); @@ -400,4 +442,12 @@ callback_map_.insert(std::pair >( ardrone3_accessorystate_connectedaccessories_ptr->GetCommandKey(), ardrone3_accessorystate_connectedaccessories_ptr)); +// Add all wrappers to the callback map +callback_map_.insert(std::pair >( + ardrone3_accessorystate_battery_ptr->GetCommandKey(), + ardrone3_accessorystate_battery_ptr)); +// Add all wrappers to the callback map +callback_map_.insert(std::pair >( + ardrone3_soundstate_alertsound_ptr->GetCommandKey(), + ardrone3_soundstate_alertsound_ptr)); #endif // UPDTAE_CALLBACK_MAP 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 87c63cc..442ac91 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h +++ b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * ardrone3_state_callbacks.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ @@ -57,6 +57,9 @@ extern "C" #include "bebop_msgs/Ardrone3PilotingStateLandingStateChanged.h" #include "bebop_msgs/Ardrone3PilotingStateAirSpeedChanged.h" #include "bebop_msgs/Ardrone3PilotingStatemoveToChanged.h" +#include "bebop_msgs/Ardrone3PilotingStateMotionState.h" +#include "bebop_msgs/Ardrone3PilotingStatePilotedPOI.h" +#include "bebop_msgs/Ardrone3PilotingStateReturnHomeBatteryCapacity.h" #include "bebop_msgs/Ardrone3NetworkStateWifiScanListChanged.h" #include "bebop_msgs/Ardrone3NetworkStateAllWifiScanChanged.h" #include "bebop_msgs/Ardrone3NetworkStateWifiAuthChannelListChanged.h" @@ -75,6 +78,8 @@ extern "C" #include "bebop_msgs/Ardrone3GPSStateHomeTypeChosenChanged.h" #include "bebop_msgs/Ardrone3PROStateFeatures.h" #include "bebop_msgs/Ardrone3AccessoryStateConnectedAccessories.h" +#include "bebop_msgs/Ardrone3AccessoryStateBattery.h" +#include "bebop_msgs/Ardrone3SoundStateAlertSound.h" namespace bebop_driver { @@ -1147,6 +1152,180 @@ class Ardrone3PilotingStatemoveToChanged : public AbstractState }; // Ardrone3PilotingStatemoveToChanged +// Motion state.\n If [MotionDetection](#1-6-16) is disabled, motion is steady.\n This information is only valid when the drone is not flying. +class Ardrone3PilotingStateMotionState : public AbstractState +{ +private: + ::bebop_msgs::Ardrone3PilotingStateMotionState::Ptr msg_ptr; + +public: + + Ardrone3PilotingStateMotionState(::ros::NodeHandle& nh, ::ros::NodeHandle& priv_nh, const ::std::string& topic) + : AbstractState(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_MOTIONSTATE) + { + if (priv_nh.getParam("states/enable_pilotingstate_motionstate", pub_enabled_) && pub_enabled_) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB" , "[STATES] Enabling %s", topic.c_str()); + ros_pub_ = nh.advertise(topic, 10, true); + } // pub_enabled_ is false + } + + ::bebop_msgs::Ardrone3PilotingStateMotionState::ConstPtr GetDataCstPtr() const + { + ::boost::lock_guard lock(mutex_); + return msg_ptr; + } + + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", "Ardrone3PilotingStateMotionState::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + msg_ptr.reset(new ::bebop_msgs::Ardrone3PilotingStateMotionState()); + msg_ptr->header.stamp = t; + msg_ptr->header.frame_id = "base_link"; + + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_MOTIONSTATE_STATE, arg); + if (arg) + { + msg_ptr->state = arg->value.I32; + } + + if (pub_enabled_) ros_pub_.publish(msg_ptr); + } + +}; // Ardrone3PilotingStateMotionState + + +// Piloted POI state. +class Ardrone3PilotingStatePilotedPOI : public AbstractState +{ +private: + ::bebop_msgs::Ardrone3PilotingStatePilotedPOI::Ptr msg_ptr; + +public: + + Ardrone3PilotingStatePilotedPOI(::ros::NodeHandle& nh, ::ros::NodeHandle& priv_nh, const ::std::string& topic) + : AbstractState(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_PILOTEDPOI) + { + if (priv_nh.getParam("states/enable_pilotingstate_pilotedpoi", pub_enabled_) && pub_enabled_) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB" , "[STATES] Enabling %s", topic.c_str()); + ros_pub_ = nh.advertise(topic, 10, true); + } // pub_enabled_ is false + } + + ::bebop_msgs::Ardrone3PilotingStatePilotedPOI::ConstPtr GetDataCstPtr() const + { + ::boost::lock_guard lock(mutex_); + return msg_ptr; + } + + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", "Ardrone3PilotingStatePilotedPOI::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + msg_ptr.reset(new ::bebop_msgs::Ardrone3PilotingStatePilotedPOI()); + msg_ptr->header.stamp = t; + msg_ptr->header.frame_id = "base_link"; + + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_PILOTEDPOI_LATITUDE, arg); + if (arg) + { + msg_ptr->latitude = arg->value.Double; + } + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_PILOTEDPOI_LONGITUDE, arg); + if (arg) + { + msg_ptr->longitude = arg->value.Double; + } + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_PILOTEDPOI_ALTITUDE, arg); + if (arg) + { + msg_ptr->altitude = arg->value.Double; + } + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_PILOTEDPOI_STATUS, arg); + if (arg) + { + msg_ptr->status = arg->value.I32; + } + + if (pub_enabled_) ros_pub_.publish(msg_ptr); + } + +}; // Ardrone3PilotingStatePilotedPOI + + +// Battery capacity status to return home. +class Ardrone3PilotingStateReturnHomeBatteryCapacity : public AbstractState +{ +private: + ::bebop_msgs::Ardrone3PilotingStateReturnHomeBatteryCapacity::Ptr msg_ptr; + +public: + + Ardrone3PilotingStateReturnHomeBatteryCapacity(::ros::NodeHandle& nh, ::ros::NodeHandle& priv_nh, const ::std::string& topic) + : AbstractState(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_RETURNHOMEBATTERYCAPACITY) + { + if (priv_nh.getParam("states/enable_pilotingstate_returnhomebatterycapacity", pub_enabled_) && pub_enabled_) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB" , "[STATES] Enabling %s", topic.c_str()); + ros_pub_ = nh.advertise(topic, 10, true); + } // pub_enabled_ is false + } + + ::bebop_msgs::Ardrone3PilotingStateReturnHomeBatteryCapacity::ConstPtr GetDataCstPtr() const + { + ::boost::lock_guard lock(mutex_); + return msg_ptr; + } + + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", "Ardrone3PilotingStateReturnHomeBatteryCapacity::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + msg_ptr.reset(new ::bebop_msgs::Ardrone3PilotingStateReturnHomeBatteryCapacity()); + msg_ptr->header.stamp = t; + msg_ptr->header.frame_id = "base_link"; + + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSTATE_RETURNHOMEBATTERYCAPACITY_STATUS, arg); + if (arg) + { + msg_ptr->status = arg->value.I32; + } + + if (pub_enabled_) ros_pub_.publish(msg_ptr); + } + +}; // Ardrone3PilotingStateReturnHomeBatteryCapacity + + // Wifi scan results.\n Please note that the list is not complete until you receive the event [WifiScanEnded](#1-14-1). class Ardrone3NetworkStateWifiScanListChanged : public AbstractState { @@ -2143,12 +2322,135 @@ class Ardrone3AccessoryStateConnectedAccessories : public AbstractState msg_ptr->swVersion = arg->value.String; } +/** arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_CONNECTEDACCESSORIES_LIST_FLAGS, arg); + if (arg) + { + msg_ptr->list_flags = arg->value.U8; + } +**/ if (pub_enabled_) ros_pub_.publish(msg_ptr); } }; // Ardrone3AccessoryStateConnectedAccessories +// Connected accessories battery. +class Ardrone3AccessoryStateBattery : public AbstractState +{ +private: + ::bebop_msgs::Ardrone3AccessoryStateBattery::Ptr msg_ptr; + +public: + + Ardrone3AccessoryStateBattery(::ros::NodeHandle& nh, ::ros::NodeHandle& priv_nh, const ::std::string& topic) + : AbstractState(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_BATTERY) + { + if (priv_nh.getParam("states/enable_accessorystate_battery", pub_enabled_) && pub_enabled_) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB" , "[STATES] Enabling %s", topic.c_str()); + ros_pub_ = nh.advertise(topic, 10, true); + } // pub_enabled_ is false + } + + ::bebop_msgs::Ardrone3AccessoryStateBattery::ConstPtr GetDataCstPtr() const + { + ::boost::lock_guard lock(mutex_); + return msg_ptr; + } + + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", "Ardrone3AccessoryStateBattery::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + msg_ptr.reset(new ::bebop_msgs::Ardrone3AccessoryStateBattery()); + msg_ptr->header.stamp = t; + msg_ptr->header.frame_id = "base_link"; + + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_BATTERY_ID, arg); + if (arg) + { + msg_ptr->id = arg->value.U8; + } + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_BATTERY_BATTERYLEVEL, arg); + if (arg) + { + msg_ptr->batteryLevel = arg->value.U8; + } +/** + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_BATTERY_LIST_FLAGS, arg); + if (arg) + { + msg_ptr->list_flags = arg->value.U8; + } +**/ + if (pub_enabled_) ros_pub_.publish(msg_ptr); + } + +}; // Ardrone3AccessoryStateBattery + + +// Alert sound state. +class Ardrone3SoundStateAlertSound : public AbstractState +{ +private: + ::bebop_msgs::Ardrone3SoundStateAlertSound::Ptr msg_ptr; + +public: + + Ardrone3SoundStateAlertSound(::ros::NodeHandle& nh, ::ros::NodeHandle& priv_nh, const ::std::string& topic) + : AbstractState(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SOUNDSTATE_ALERTSOUND) + { + if (priv_nh.getParam("states/enable_soundstate_alertsound", pub_enabled_) && pub_enabled_) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB" , "[STATES] Enabling %s", topic.c_str()); + ros_pub_ = nh.advertise(topic, 10, true); + } // pub_enabled_ is false + } + + ::bebop_msgs::Ardrone3SoundStateAlertSound::ConstPtr GetDataCstPtr() const + { + ::boost::lock_guard lock(mutex_); + return msg_ptr; + } + + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", "Ardrone3SoundStateAlertSound::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + msg_ptr.reset(new ::bebop_msgs::Ardrone3SoundStateAlertSound()); + msg_ptr->header.stamp = t; + msg_ptr->header.frame_id = "base_link"; + + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SOUNDSTATE_ALERTSOUND_STATE, arg); + if (arg) + { + msg_ptr->state = arg->value.I32; + } + + if (pub_enabled_) ros_pub_.publish(msg_ptr); + } + +}; // Ardrone3SoundStateAlertSound + + } // namespace cb } // namespace bebop_driver #endif // BEBOP_AUTONOMY_AUTOGENERATED_ardrone3_STATE_CALLBACKS_H diff --git a/bebop_driver/include/bebop_driver/autogenerated/callbacks_common.h b/bebop_driver/include/bebop_driver/autogenerated/callbacks_common.h index e67f3b7..fa9e636 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/callbacks_common.h +++ b/bebop_driver/include/bebop_driver/autogenerated/callbacks_common.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * bebop_common_commands.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ #ifndef BEBOP_COMMON_COMMANDS_H diff --git a/bebop_driver/include/bebop_driver/autogenerated/common_state_callback_includes.h b/bebop_driver/include/bebop_driver/autogenerated/common_state_callback_includes.h index cba9ea9..a6b592b 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/common_state_callback_includes.h +++ b/bebop_driver/include/bebop_driver/autogenerated/common_state_callback_includes.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * common_state_callback_includes.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/common.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/common.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ 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 a5d7527..6e74f8c 100644 --- a/bebop_driver/include/bebop_driver/autogenerated/common_state_callbacks.h +++ b/bebop_driver/include/bebop_driver/autogenerated/common_state_callbacks.h @@ -23,7 +23,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * common_state_callbacks.h - * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/common.xml + * auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/common.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ @@ -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/package.xml b/bebop_driver/package.xml index 63bf4dd..379bb2e 100644 --- a/bebop_driver/package.xml +++ b/bebop_driver/package.xml @@ -1,7 +1,7 @@ bebop_driver - 0.7.1 + 0.8.0 ROS driver for Parrot Bebop drone, based on Parrot’s official ARDroneSDK3 Mani Monajjemi @@ -18,7 +18,7 @@ catkin - parrot_arsdk + parrot_arsdk bebop_msgs bebop_description pkg-config @@ -36,7 +36,7 @@ roslint rostest - parrot_arsdk + parrot_arsdk bebop_msgs bebop_description nodelet diff --git a/bebop_driver/scripts/meta/generate.py b/bebop_driver/scripts/meta/generate.py index bf4d647..b1ccd71 100755 --- a/bebop_driver/scripts/meta/generate.py +++ b/bebop_driver/scripts/meta/generate.py @@ -12,9 +12,9 @@ import subprocess import urllib2 -# SDK 3.12.6: https://github.com/Parrot-Developers/arsdk_manifests/blob/d7640c80ed7147971995222d9f4655932a904aa8/release.xml +# SDK 3.14.0: https://github.com/Parrot-Developers/arsdk_manifests/blob/1ff5bdc5458627c12eb22e1dd1814cff25778f31/release.xml LIBARCOMMANDS_GIT_OWNER = "Parrot-Developers" -LIBARCOMMANDS_GIT_HASH = "ab28dab91845cd36c4d7002b55f70805deaff3c8" +LIBARCOMMANDS_GIT_HASH = "6faa46898d94fa207fd1b039643d356d14b7db42" # From XML types to ROS primitive types ROS_TYPE_MAP = { diff --git a/bebop_driver/scripts/meta/last_build_info b/bebop_driver/scripts/meta/last_build_info index 17c91d4..733c8cc 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-06-27 13:53:57.790920 -- Generator generate.py @ 8801830 +- Source hash 6faa46898d94fa207fd1b039643d356d14b7db42 +- Date 2018-07-03 00:25:08.348220 +- Generator generate.py @ e00904c diff --git a/bebop_msgs/CHANGELOG.rst b/bebop_msgs/CHANGELOG.rst index 5cd6dc4..148f36d 100644 --- a/bebop_msgs/CHANGELOG.rst +++ b/bebop_msgs/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package bebop_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.0 (2018-07-03) +------------------ +* Update to Parrot SDK 3.14.0 (from 3.12.6) +* Contributors: Thomas Bamford + 0.7.0 (2017-07-29) ------------------ * Update to Parrot SDK 3.12.6 (from 3.10.1) diff --git a/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateBattery.msg b/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateBattery.msg new file mode 100644 index 0000000..e94e12a --- /dev/null +++ b/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateBattery.msg @@ -0,0 +1,16 @@ +# Ardrone3AccessoryStateBattery +# 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: Connected accessories battery. + +Header header + +# Id of the accessory for the session. +uint8 id +# Battery level in percentage. +uint8 batteryLevel +# 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/Ardrone3AccessoryStateConnectedAccessories.msg b/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg index 11d0602..00524a9 100644 --- a/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg +++ b/bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg @@ -12,9 +12,11 @@ Header header 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_flir=1 # FLIR 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/Ardrone3PilotingStateFlyingStateChanged.msg b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateFlyingStateChanged.msg index 384e9d3..d58288c 100644 --- a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateFlyingStateChanged.msg +++ b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateFlyingStateChanged.msg @@ -16,6 +16,6 @@ uint8 state_flying=3 # Flying state uint8 state_landing=4 # Landing state uint8 state_emergency=5 # Emergency state uint8 state_usertakeoff=6 # User take off state. Waiting for user action to take off. -uint8 state_motor_ramping=7 # Motor ramping state (for fixed wings). +uint8 state_motor_ramping=7 # Motor ramping state. uint8 state_emergency_landing=8 # Emergency landing state. Drone autopilot has detected defective sensor(s). Only Yaw argument in PCMD is taken into account. All others flying commands are ignored. uint8 state diff --git a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateMotionState.msg b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateMotionState.msg new file mode 100644 index 0000000..1b1e678 --- /dev/null +++ b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateMotionState.msg @@ -0,0 +1,14 @@ +# Ardrone3PilotingStateMotionState +# 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: Motion state.\n If [MotionDetection](#1-6-16) is disabled, motion is steady.\n This information is only valid when the drone is not flying. + +Header header + +# Motion state +uint8 state_steady=0 # Drone is steady +uint8 state_moving=1 # Drone is moving +uint8 state diff --git a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatePilotedPOI.msg b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatePilotedPOI.msg new file mode 100644 index 0000000..7f7076c --- /dev/null +++ b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatePilotedPOI.msg @@ -0,0 +1,22 @@ +# Ardrone3PilotingStatePilotedPOI +# 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: Piloted POI state. + +Header header + +# Latitude of the location (in degrees) to look at. This information is only valid when the state is pending or running. +float64 latitude +# Longitude of the location (in degrees) to look at. This information is only valid when the state is pending or running. +float64 longitude +# Altitude above sea level (in m) to look at. This information is only valid when the state is pending or running. +float64 altitude +# Status of the move to +uint8 status_UNAVAILABLE=0 # The piloted POI is not available +uint8 status_AVAILABLE=1 # The piloted POI is available +uint8 status_PENDING=2 # Piloted POI has been requested. Waiting to be in state that allow the piloted POI to start +uint8 status_RUNNING=3 # Piloted POI is running +uint8 status diff --git a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateReturnHomeBatteryCapacity.msg b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateReturnHomeBatteryCapacity.msg new file mode 100644 index 0000000..2a8bb61 --- /dev/null +++ b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStateReturnHomeBatteryCapacity.msg @@ -0,0 +1,16 @@ +# Ardrone3PilotingStateReturnHomeBatteryCapacity +# 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: Battery capacity status to return home. + +Header header + +# Status of battery to return home +uint8 status_OK=0 # The battery is full enough to do a return home +uint8 status_WARNING=1 # The battery is about to be too discharged to do a return home +uint8 status_CRITICAL=2 # The battery level is too low to return to the home position +uint8 status_UNKNOWN=3 # Battery capacity to do a return home is unknown. This can be either because the home is unknown or the position of the drone is unknown, or the drone has not enough information to determine how long it takes to fly home. +uint8 status diff --git a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg index 4ba4ccd..73d962d 100644 --- a/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg +++ b/bebop_msgs/msg/autogenerated/Ardrone3PilotingStatemoveToChanged.msg @@ -25,6 +25,6 @@ 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_CANCELED=2 # The move to has been canceled, either by a CancelMoveTo command or when a disconnection appears. 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/Ardrone3SoundStateAlertSound.msg b/bebop_msgs/msg/autogenerated/Ardrone3SoundStateAlertSound.msg new file mode 100644 index 0000000..3981bab --- /dev/null +++ b/bebop_msgs/msg/autogenerated/Ardrone3SoundStateAlertSound.msg @@ -0,0 +1,14 @@ +# Ardrone3SoundStateAlertSound +# 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: Alert sound state. + +Header header + +# State of the alert sound +uint8 state_stopped=0 # Alert sound is not playing +uint8 state_playing=1 # Alert sound is playing +uint8 state diff --git a/bebop_msgs/msg/autogenerated/last_build_info b/bebop_msgs/msg/autogenerated/last_build_info index 17c91d4..733c8cc 100644 --- a/bebop_msgs/msg/autogenerated/last_build_info +++ b/bebop_msgs/msg/autogenerated/last_build_info @@ -1,5 +1,5 @@ Last auto-generation build info: -- Source hash ab28dab91845cd36c4d7002b55f70805deaff3c8 -- Date 2017-06-27 13:53:57.790920 -- Generator generate.py @ 8801830 +- Source hash 6faa46898d94fa207fd1b039643d356d14b7db42 +- Date 2018-07-03 00:25:08.348220 +- Generator generate.py @ e00904c diff --git a/bebop_msgs/package.xml b/bebop_msgs/package.xml index 406daee..be1c58c 100644 --- a/bebop_msgs/package.xml +++ b/bebop_msgs/package.xml @@ -1,7 +1,7 @@ bebop_msgs - 0.7.0 + 0.8.0 Common message definitions for bebop_autonomy Mani Monajjemi diff --git a/bebop_tools/CHANGELOG.rst b/bebop_tools/CHANGELOG.rst index 4be5d1d..0324ee2 100644 --- a/bebop_tools/CHANGELOG.rst +++ b/bebop_tools/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package bebop_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.8.0 (2018-07-03) +------------------ + 0.7.0 (2017-07-29) ------------------ diff --git a/bebop_tools/package.xml b/bebop_tools/package.xml index e9f595a..cedff1d 100644 --- a/bebop_tools/package.xml +++ b/bebop_tools/package.xml @@ -1,7 +1,7 @@ bebop_tools - 0.7.0 + 0.8.0 Miscellaneous tools for bebop_autonomy metapackage Mani Monajjemi diff --git a/docs/autogenerated/ardrone3_settings_param.rst b/docs/autogenerated/ardrone3_settings_param.rst index 84aca69..90c0b33 100644 --- a/docs/autogenerated/ardrone3_settings_param.rst +++ b/docs/autogenerated/ardrone3_settings_param.rst @@ -1,5 +1,5 @@ .. Ardrone3_settings_param.rst - .. auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + .. auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml .. Do not modify this file by hand. Check scripts/meta folder for generator files. ***************************************************************************************** diff --git a/docs/autogenerated/ardrone3_states_param_topic.rst b/docs/autogenerated/ardrone3_states_param_topic.rst index bfc8ca2..0885ed9 100644 --- a/docs/autogenerated/ardrone3_states_param_topic.rst +++ b/docs/autogenerated/ardrone3_states_param_topic.rst @@ -1,5 +1,5 @@ .. ardrone3_states_param_topic.rst - .. auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/ardrone3.xml + .. auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/ardrone3.xml .. Do not modify this file by hand. Check scripts/meta folder for generator files. ***************************************************************************************** @@ -42,6 +42,12 @@ List of ardrone3 States and Corresponding ROS Parameters and Topics Drones air speed changed\n Expressed in the drones referential. `Ardrone3PilotingStatemoveToChanged`_ The drone moves or moved to a given location. +`Ardrone3PilotingStateMotionState`_ + Motion state.\n If [MotionDetection](#1-6-16) is disabled, motion is steady.\n This information is only valid when the drone is not flying. +`Ardrone3PilotingStatePilotedPOI`_ + Piloted POI state. +`Ardrone3PilotingStateReturnHomeBatteryCapacity`_ + Battery capacity status to return home. `Ardrone3NetworkStateWifiScanListChanged`_ Wifi scan results.\n Please note that the list is not complete until you receive the event [WifiScanEnded](#1-14-1). `Ardrone3NetworkStateAllWifiScanChanged`_ @@ -78,6 +84,10 @@ List of ardrone3 States and Corresponding ROS Parameters and Topics Pro features. `Ardrone3AccessoryStateConnectedAccessories`_ List of all connected accessories. This event presents the list of all connected accessories. To actually use the component, use the component dedicated feature. +`Ardrone3AccessoryStateBattery`_ + Connected accessories battery. +`Ardrone3SoundStateAlertSound`_ + Alert sound state. Ardrone3MediaRecordStatePictureStateChanged #################################################################################### @@ -349,6 +359,51 @@ The drone moves or moved to a given location. :caption: Ardrone3PilotingStatemoveToChanged.msg :name: Ardrone3PilotingStatemoveToChanged_msg +Ardrone3PilotingStateMotionState +#################################################################################### +Motion state.\n If [MotionDetection](#1-6-16) is disabled, motion is steady.\n This information is only valid when the drone is not flying. + +- Parameter: ``~states/enable_pilotingstate_motionstate`` +- Topic: ``states/ardrone3/PilotingState/MotionState`` +- Message type: ``bebop_msgs::Ardrone3PilotingStateMotionState`` + +.. literalinclude:: + ../../bebop_msgs/msg/autogenerated/Ardrone3PilotingStateMotionState.msg + :lines: 8- + :language: python + :caption: Ardrone3PilotingStateMotionState.msg + :name: Ardrone3PilotingStateMotionState_msg + +Ardrone3PilotingStatePilotedPOI +#################################################################################### +Piloted POI state. + +- Parameter: ``~states/enable_pilotingstate_pilotedpoi`` +- Topic: ``states/ardrone3/PilotingState/PilotedPOI`` +- Message type: ``bebop_msgs::Ardrone3PilotingStatePilotedPOI`` + +.. literalinclude:: + ../../bebop_msgs/msg/autogenerated/Ardrone3PilotingStatePilotedPOI.msg + :lines: 8- + :language: python + :caption: Ardrone3PilotingStatePilotedPOI.msg + :name: Ardrone3PilotingStatePilotedPOI_msg + +Ardrone3PilotingStateReturnHomeBatteryCapacity +#################################################################################### +Battery capacity status to return home. + +- Parameter: ``~states/enable_pilotingstate_returnhomebatterycapacity`` +- Topic: ``states/ardrone3/PilotingState/ReturnHomeBatteryCapacity`` +- Message type: ``bebop_msgs::Ardrone3PilotingStateReturnHomeBatteryCapacity`` + +.. literalinclude:: + ../../bebop_msgs/msg/autogenerated/Ardrone3PilotingStateReturnHomeBatteryCapacity.msg + :lines: 8- + :language: python + :caption: Ardrone3PilotingStateReturnHomeBatteryCapacity.msg + :name: Ardrone3PilotingStateReturnHomeBatteryCapacity_msg + Ardrone3NetworkStateWifiScanListChanged #################################################################################### Wifi scan results.\n Please note that the list is not complete until you receive the event [WifiScanEnded](#1-14-1). @@ -619,3 +674,33 @@ List of all connected accessories. This event presents the list of all connected :caption: Ardrone3AccessoryStateConnectedAccessories.msg :name: Ardrone3AccessoryStateConnectedAccessories_msg +Ardrone3AccessoryStateBattery +#################################################################################### +Connected accessories battery. + +- Parameter: ``~states/enable_accessorystate_battery`` +- Topic: ``states/ardrone3/AccessoryState/Battery`` +- Message type: ``bebop_msgs::Ardrone3AccessoryStateBattery`` + +.. literalinclude:: + ../../bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateBattery.msg + :lines: 8- + :language: python + :caption: Ardrone3AccessoryStateBattery.msg + :name: Ardrone3AccessoryStateBattery_msg + +Ardrone3SoundStateAlertSound +#################################################################################### +Alert sound state. + +- Parameter: ``~states/enable_soundstate_alertsound`` +- Topic: ``states/ardrone3/SoundState/AlertSound`` +- Message type: ``bebop_msgs::Ardrone3SoundStateAlertSound`` + +.. literalinclude:: + ../../bebop_msgs/msg/autogenerated/Ardrone3SoundStateAlertSound.msg + :lines: 8- + :language: python + :caption: Ardrone3SoundStateAlertSound.msg + :name: Ardrone3SoundStateAlertSound_msg + diff --git a/docs/autogenerated/common_states_param_topic.rst b/docs/autogenerated/common_states_param_topic.rst index 7ad70c4..1344d2c 100644 --- a/docs/autogenerated/common_states_param_topic.rst +++ b/docs/autogenerated/common_states_param_topic.rst @@ -1,5 +1,5 @@ .. common_states_param_topic.rst - .. auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/ab28dab91845cd36c4d7002b55f70805deaff3c8/xml/common.xml + .. auto-generated from https://raw.githubusercontent.com/Parrot-Developers/arsdk-xml/6faa46898d94fa207fd1b039643d356d14b7db42/xml/common.xml .. Do not modify this file by hand. Check scripts/meta folder for generator files. ***************************************************************************************** diff --git a/docs/dev.rst b/docs/dev.rst index 2df9d08..54af7c7 100644 --- a/docs/dev.rst +++ b/docs/dev.rst @@ -54,10 +54,10 @@ Running Bebop In The Loop Tests Upgrading the Bebop SDK ======================= -.. warning:: Since version 0.6, `Parrot ARSDK `_, the main underlying dependency of *bebop_autonomy* is not build inline anymore. Instead, the slightly patched and catkinized version of it, called `parrot_arsdk `_, is fetched as a dependency. **The following documentation needs to be updated**. +.. warning:: Since version 0.6, `Parrot ARSDK `_, the main underlying dependency of *bebop_autonomy* is not build inline anymore. Instead, the slightly patched and catkinized version of it, called `parrot_arsdk `_, is fetched as a dependency. -1. Update the ``GIT_TAG`` of ``ARDroneSDK3`` in ``bebop_driver/CMakeLists.txt::add_custom_target::./repo init ... -b GIT_TAG`` to your desired commit hash, branch or tag (release). The official upstream repository is hosted `here `_. -2. Checkout (or browse) the upstream repository at the same hash used in step (1) and open ``release.xml`` file. From this file, extract the commit hash of ``arsdk-xml`` from the ``revision`` property of this XML tag: ````. +1. Update the build_depend and run_depend version tag bebop_driver/package.xml to the updated parrot_arsdk package version. +2. Browse the upstream repository at and open ``release.xml`` file. From this file, extract the commit hash of ``arsdk-xml`` from the ``revision`` property of this XML tag: ````. 3. Open ``bebop_driver/scripts/meta/generate.py`` and update ``LIBARCOMMANDS_GIT_HASH`` variable to the hash obtained in step (2). 4. Change the working diretory to ``bebop_driver/scripts/meta``, then execute ``generate.py`` from the command line. This will regenerate all automatically generated message definitions, header files and documentations. 5. Copy the generated files to their target locations by calling ``./install.sh``. @@ -68,7 +68,7 @@ Upgrading the Bebop SDK - ``ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGRADIUSCHANGED_VALUE`` to ``ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGRADIUSCHANGED_CURRENT`` - ``ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGALTITUDECHANGED_VALUE`` to ``ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGALTITUDECHANGED_CURRENT`` -In ``bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h`` remove the following lines: +In ``bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h`` comment out the following lines: ``arg = NULL; HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_CONNECTEDACCESSORIES_LIST_FLAGS, arg); @@ -77,10 +77,27 @@ In ``bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h` msg_ptr->list_flags = arg->value.U8; }`` -In ``bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg`` remove the following lines: - ``# 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`` + ``arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_BATTERY_LIST_FLAGS, arg); + if (arg) + { + msg_ptr->list_flags = arg->value.U8; + }`` -These changes are required because the upstream XML file is inconsistent for a couple of variable names. +In ``bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateConnectedAccessories.msg`` comment the following line: + ``uint8 list_flags`` + +In ``bebop_msgs/msg/autogenerated/Ardrone3AccessoryStateBattery`` comment the following line: + ``uint8 list_flags`` + +These changes are required because the upstream XML file is inconsistent for a couple of variable names. All details on states and parameters supported by each of the devices see for reference. + +8. Update documentation. This includes: +* Updating all package.xml version numbers. +* Adding entries to all CHANGELOG.rst files. +* Update built against version in the main README.md file. +* Update table in docs/index.rst, specifically the SDK Version and device support information. 7. Remove ``build`` and ``devel`` space of your ``catkin`` workspace, then re-build it. + +8. Run in the loop tests to verify that the update works with the devices. diff --git a/docs/index.rst b/docs/index.rst index 2703ee8..79ec675 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -17,9 +17,9 @@ Features and Roadmap .. csv-table:: :header: "Feature", "Status", "Notes" - SDK Version,"3.12.6", "Since v0.7" + SDK Version,"3.14.0", "Since v0.8" Support for Parrot Bebop 1, Yes, "Tested up to Firmware 3.3" - Support for Parrot Bebop 2, Yes, "Tested up to Firmware 4.0.6" + Support for Parrot Bebop 2, Yes, "Tested up to Firmware 4.7.1" Support for Parrot Disco FPV, No, "Not tested (help wanted)" Core piloting, Yes, "" H264 video decoding, Yes, "Enhancement: `#1 `_" From 9d524a2ed6dc2ce19e6c2ab32a652f751721d649 Mon Sep 17 00:00:00 2001 From: Thomas Bamford Date: Tue, 3 Jul 2018 11:16:50 -0400 Subject: [PATCH 06/11] Add support for Disco --- bebop_driver/CHANGELOG.rst | 2 + bebop_driver/include/bebop_driver/bebop.h | 2 + .../bebop_driver/bebop_driver_nodelet.h | 2 + bebop_driver/src/bebop.cpp | 9 +++ bebop_driver/src/bebop_driver_nodelet.cpp | 14 ++++ bebop_tools/CHANGELOG.rst | 2 + bebop_tools/config/xbox360_disco.yaml | 81 +++++++++++++++++++ docs/index.rst | 2 +- docs/piloting.rst | 54 +++++++++---- 9 files changed, 152 insertions(+), 16 deletions(-) create mode 100644 bebop_tools/config/xbox360_disco.yaml diff --git a/bebop_driver/CHANGELOG.rst b/bebop_driver/CHANGELOG.rst index 0f29533..c29048f 100644 --- a/bebop_driver/CHANGELOG.rst +++ b/bebop_driver/CHANGELOG.rst @@ -5,6 +5,8 @@ Changelog for package bebop_driver 0.8.0 (2018-07-03) ------------------ * SDK 3.14.0 support +* Add support for Disco +* Add a new ROS topic for entering and exiting user takeoff mode: `user_takeoff` * Contributors: Thomas Bamford 0.7.1 (2018-05-31) diff --git a/bebop_driver/include/bebop_driver/bebop.h b/bebop_driver/include/bebop_driver/bebop.h index 2aa8fb9..7e223c0 100644 --- a/bebop_driver/include/bebop_driver/bebop.h +++ b/bebop_driver/include/bebop_driver/bebop.h @@ -163,6 +163,8 @@ class Bebop void FlatTrim(); // false: Stop, true: Start void NavigateHome(const bool& start_stop); + // false: Exit from user take off, true: Enter in user take off + void UserTakeoff(const bool& start_stop); void StartAutonomousFlight(const std::string &filepath); void PauseAutonomousFlight(); void StopAutonomousFlight(); diff --git a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h index 03cbebc..8cfcca0 100644 --- a/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h +++ b/bebop_driver/include/bebop_driver/bebop_driver_nodelet.h @@ -117,6 +117,7 @@ class BebopDriverNodelet : public nodelet::Nodelet ros::Subscriber reset_sub_; ros::Subscriber flattrim_sub_; ros::Subscriber navigatehome_sub_; + ros::Subscriber usertakeoff_sub_; ros::Subscriber start_autoflight_sub_; ros::Subscriber pause_autoflight_sub_; ros::Subscriber stop_autoflight_sub_; @@ -157,6 +158,7 @@ class BebopDriverNodelet : public nodelet::Nodelet void EmergencyCallback(const std_msgs::EmptyConstPtr& empty_ptr); void FlatTrimCallback(const std_msgs::EmptyConstPtr& empty_ptr); void NavigateHomeCallback(const std_msgs::BoolConstPtr& start_stop_ptr); + void UserTakeoffCallback(const std_msgs::BoolConstPtr& start_stop_ptr); void StartAutonomousFlightCallback(const std_msgs::StringConstPtr& file_path_ptr); void PauseAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr); void StopAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr); diff --git a/bebop_driver/src/bebop.cpp b/bebop_driver/src/bebop.cpp index d2664c1..368f51a 100644 --- a/bebop_driver/src/bebop.cpp +++ b/bebop_driver/src/bebop.cpp @@ -478,6 +478,15 @@ void Bebop::NavigateHome(const bool &start_stop) "Navigate home failed"); } +void Bebop::UserTakeoff(const bool &start_stop) +{ + ThrowOnInternalError("User takeoff failed"); + ThrowOnCtrlError( + device_controller_ptr_->aRDrone3->sendPilotingUserTakeOff( + device_controller_ptr_->aRDrone3, start_stop ? 1 : 0), + "User takeoff failed"); +} + void Bebop::StartAutonomousFlight(const std::string &filepath) { ThrowOnInternalError("Start autonomous flight failed"); diff --git a/bebop_driver/src/bebop_driver_nodelet.cpp b/bebop_driver/src/bebop_driver_nodelet.cpp index 1deca22..202e0e1 100644 --- a/bebop_driver/src/bebop_driver_nodelet.cpp +++ b/bebop_driver/src/bebop_driver_nodelet.cpp @@ -139,6 +139,7 @@ void BebopDriverNodelet::onInit() reset_sub_ = nh.subscribe("reset", 1, &BebopDriverNodelet::EmergencyCallback, this); flattrim_sub_ = nh.subscribe("flattrim", 1, &BebopDriverNodelet::FlatTrimCallback, this); navigatehome_sub_ = nh.subscribe("autoflight/navigate_home", 1, &BebopDriverNodelet::NavigateHomeCallback, this); + usertakeoff_sub_ = nh.subscribe("user_takeoff", 1, &BebopDriverNodelet::UserTakeoffCallback, this); start_autoflight_sub_ = nh.subscribe("autoflight/start", 1, &BebopDriverNodelet::StartAutonomousFlightCallback, this); pause_autoflight_sub_ = nh.subscribe("autoflight/pause", 1, &BebopDriverNodelet::PauseAutonomousFlightCallback, this); stop_autoflight_sub_ = nh.subscribe("autoflight/stop", 1, &BebopDriverNodelet::StopAutonomousFlightCallback, this); @@ -315,6 +316,19 @@ void BebopDriverNodelet::NavigateHomeCallback(const std_msgs::BoolConstPtr &star } } +void BebopDriverNodelet::UserTakeoffCallback(const std_msgs::BoolConstPtr &start_stop_ptr) +{ + try + { + ROS_INFO("%sing user takeoff behavior ...", start_stop_ptr->data ? "Enter" : "Exit"); + bebop_ptr_->UserTakeoff(start_stop_ptr->data); + } + catch (const std::runtime_error& e) + { + ROS_ERROR_STREAM(e.what()); + } +} + void BebopDriverNodelet::StartAutonomousFlightCallback(const std_msgs::StringConstPtr& filepath_ptr) { std::string filepath; diff --git a/bebop_tools/CHANGELOG.rst b/bebop_tools/CHANGELOG.rst index 0324ee2..2c08ae0 100644 --- a/bebop_tools/CHANGELOG.rst +++ b/bebop_tools/CHANGELOG.rst @@ -4,6 +4,8 @@ Changelog for package bebop_tools 0.8.0 (2018-07-03) ------------------ +* Added Xbox 360 config file for Disco +* Contributors: Thomas Bamford 0.7.0 (2017-07-29) ------------------ diff --git a/bebop_tools/config/xbox360_disco.yaml b/bebop_tools/config/xbox360_disco.yaml new file mode 100644 index 0000000..cc2a424 --- /dev/null +++ b/bebop_tools/config/xbox360_disco.yaml @@ -0,0 +1,81 @@ +# Xbox 360 wireless controller +# Deadman (enable) button: Right Bumper +teleop: + piloting: + type: topic + message_type: "geometry_msgs/Twist" + topic_name: cmd_vel + deadman_buttons: [5] + axis_mappings: + - + axis: 4 # Right thumb stick (pitch down/up) + target: linear.x + scale: 1.0 + offset: 0.0 + - + axis: 3 # Right thumb stick (roll left/right) + target: linear.y + scale: 1.0 + offset: 0.0 + - + axis: 1 # Left thumb stick (faster/slower) + target: linear.z + scale: 1.0 + offset: 0.0 + - + axis: 0 # Left thumb stick (circle left/right) + target: angular.z + scale: 1.0 + offset: 0.0 + camera: + type: topic + message_type: "geometry_msgs/Twist" + topic_name: camera_control + deadman_buttons: [7] # Start + axis_mappings: + - + axis: 1 # Left D-Pad (up/down) + target: angular.y + scale: 83.0 + offset: 0.0 + - + axis: 0 # Left D-Pad (left/right) + target: angular.z + scale: -35.0 + offset: 0.0 + takeoff: + type: topic + message_type: "std_msgs/Empty" + topic_name: takeoff + deadman_buttons: [5, 3] # RB + Y + axis_mappings: [] + land: + type: topic + message_type: "std_msgs/Empty" + topic_name: land + deadman_buttons: [5, 0] # RB + A + axis_mappings: [] + emergency: + type: topic + message_type: "std_msgs/Empty" + topic_name: reset + deadman_buttons: [5, 1] # RB + B + axis_mappings: [] + flattrim: + type: topic + message_type: "std_msgs/Empty" + topic_name: flattrim + deadman_buttons: [5, 2] # RB + X + axis_mappings: [] + flip: + type: topic + message_type: "std_msgs/UInt8" + topic_name: flip + deadman_buttons: [5, 4, 2] # RB + LB + X + axis_mappings: [] + snapshot: + type: topic + message_type: "std_msgs/Empty" + topic_name: snapshot + deadman_buttons: [5, 8] # RB + XBox + axis_mappings: [] diff --git a/docs/index.rst b/docs/index.rst index 79ec675..db7845b 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -20,7 +20,7 @@ Features and Roadmap SDK Version,"3.14.0", "Since v0.8" Support for Parrot Bebop 1, Yes, "Tested up to Firmware 3.3" Support for Parrot Bebop 2, Yes, "Tested up to Firmware 4.7.1" - Support for Parrot Disco FPV, No, "Not tested (help wanted)" + Support for Parrot Disco FPV, Yes, "Tested up to Firmware 1.7.1" Core piloting, Yes, "" H264 video decoding, Yes, "Enhancement: `#1 `_" ROS Camera Interface, Yes, "" diff --git a/docs/piloting.rst b/docs/piloting.rst index 9aed49b..ffd4ba3 100644 --- a/docs/piloting.rst +++ b/docs/piloting.rst @@ -9,14 +9,23 @@ Sending Commands to Bebop Takeoff ======= -Publish a message of type ``std_msgs/Empty`` to ``takeoff`` topic. +Ask the drone to take off. On fixed wings (such as Disco): not used except to cancel a land. Publish a message of type ``std_msgs/Empty`` to ``takeoff`` topic. .. code-block:: bash $ rostopic pub --once [namespace]/takeoff std_msgs/Empty +User Takeoff +======= + +On copters: initiates the thrown takeoff. Note that the drone will do the thrown take off even if it is steady. On fixed wings (such as Disco): initiates the take off process on the fixed wings. Publish a message of type `std_msgs/Bool `_ to `user_takeoff` topic with the ``data`` field set to ``true`` to enter user take off mode. To exit from user take off mode, publish another message with ``data`` set to ``false``. + +.. code-block:: bash + + $ rostopic pub --once [namespace]/user_takeoff std_msgs/Bool true + Land -==== +======= Publish a message of type ``std_msgs/Empty`` to ``land`` topic. @@ -36,7 +45,9 @@ Publish a message of type ``std_msgs/Empty`` to ``reset`` topic. Piloting ======== -To move Bebop around, publish messages of type `geometry_msgs/Twist `_ to `cmd_vel` topic while Bebop is flying. The effect of each field of the message on Bebop's movement is listed below: +To move the vehicle around, publish messages of type `geometry_msgs/Twist `_ to `cmd_vel` topic while the vehicle is flying. + +The effect of each field of the message on Bebop's movement is listed below: .. code-block:: text @@ -49,11 +60,24 @@ To move Bebop around, publish messages of type `geometry_msgs/Twist `_, which is specified in degrees and is dynamically reconfigurable (:ref:`sec-dyn-params`). +The ``linear.x`` and ``linear.y`` parts of this message set the **pitch** and **roll** angles of the vehicle, respectively, hence control its forward and lateral accelerations. The resulting pitch/roll angles depend on the value of ``~PilotingSettingsMaxTiltCurrent`` `parameter <./autogenerated/ardrone3_settings_param.html#pilotingsettingsmaxtiltcurrent>`_, which is specified in degrees and is dynamically reconfigurable (:ref:`sec-dyn-params`). -The ``linear.z`` part of this message controls the **vertical velocity** of the Bebop. The resulting velocity in m/s depends on the value of ``~SpeedSettingsMaxVerticalSpeedCurrent`` `parameter <./autogenerated/ardrone3_settings_param.html#speedsettingsmaxverticalspeedcurrent>`_, which is specified in meters per second and is also dynamically reconfigurable (:ref:`sec-dyn-params`). Similarly, the ``angular.z`` component of this message controls the rotational velocity of the Bebop (around the z-axis). The corresponding scaling `parameter <./autogenerated/ardrone3_settings_param.html#speedsettingsmaxrotationspeedcurrent>`_ is ``SpeedSettingsMaxRotationSpeedCurrent`` (in degrees per sec). +The ``linear.z`` part of this message controls the **vertical velocity** of the vehicle. The resulting velocity in m/s depends on the value of ``~SpeedSettingsMaxVerticalSpeedCurrent`` `parameter <./autogenerated/ardrone3_settings_param.html#speedsettingsmaxverticalspeedcurrent>`_, which is specified in meters per second and is also dynamically reconfigurable (:ref:`sec-dyn-params`). Similarly, the ``angular.z`` component of this message controls the rotational velocity of the Bebop (around the z-axis), or in the case of Disco, circling direction (around the z-axis). The corresponding scaling for Bebop `parameter <./autogenerated/ardrone3_settings_param.html#speedsettingsmaxrotationspeedcurrent>`_ is ``SpeedSettingsMaxRotationSpeedCurrent`` (in degrees per sec). .. code-block:: text @@ -86,19 +110,19 @@ An autonomous flight plan consists of a series of GPS waypoints along with Bebop Requirements that must be met before an autonomous flight can start: - * Bebop is calibrated - * Bebop is in outdoor mode - * Bebop has fixed its GPS + * Vehicle is calibrated + * Vehicle is in outdoor mode + * Vehicle has fixed its GPS -To start an autonomous flight plan, publish a message of type `std_msgs/String `_ to `autoflight/start` topic. The ``data`` field should contain the name of the flight plan to execute, which is already stored on-board Bebop. +To start an autonomous flight plan, publish a message of type `std_msgs/String `_ to `autoflight/start` topic. The ``data`` field should contain the name of the flight plan to execute, which is already stored on-board the vehicle. .. note:: If an empty string is published, then the default 'flightplan.mavlink' is used. .. warning:: If not already flying, Bebop will attempt to take off upon starting a flight plan. -The `Flight Plan App `_ allows easy construction of flight plans and saves them on-board Bebop. +The `Flight Plan App `_ allows easy construction of flight plans and saves them on-board the vehicle. -An FTP client can also be used to view and copy flight plans on-board Bebop. `FileZilla` is recommended: +An FTP client can also be used to view and copy flight plans on-board the vehicle. `FileZilla` is recommended: .. code-block:: bash @@ -116,20 +140,20 @@ Then open `Site Manager` (top left), click `New Site`: Pause Flight Plan ----------------- -To pause the execution of an autonomous flight plan, publish a message of type `std_msgs/Empty `_ to `autoflight/pause` topic. Bebop will then hover and await further commands. -To resume a paused flight plan, publish the same message that was used to start the autonomous flight (ie. to the topic `autoflight/start`). Bebop will fly to the lastest waypoint reached before continuing the flight plan. +To pause the execution of an autonomous flight plan, publish a message of type `std_msgs/Empty `_ to `autoflight/pause` topic. Bebop will then hover and await further commands. Disco will fly straight and level awaiting further commands. +To resume a paused flight plan, publish the same message that was used to start the autonomous flight (ie. to the topic `autoflight/start`). The vehicle will fly to the lastest waypoint reached before continuing the flight plan. .. note:: Any velocity commands sent to Bebop during an autonomous flight plan will pause the plan. Stop Flight Plan ---------------- -To stop the execution of an autonomous flight plan, publish a message of type `std_msgs/Empty `_ to `autoflight/stop` topic. Bebop will hover and await further commands. +To stop the execution of an autonomous flight plan, publish a message of type `std_msgs/Empty `_ to `autoflight/stop` topic. Bebop will hover and await further commands. Disco will fly straight and level awaiting further commands. Navigate Home ------------- -To ask Bebop to autonomously fly to it's home position, publish a message of type `std_msgs/Bool `_ to `autoflight/navigate_home` topic with the ``data`` field set to ``true``. To stop Bebop from navigating home, publish another message with ``data`` set to ``false``. +To ask the vehicle to autonomously fly to it's home position, publish a message of type `std_msgs/Bool `_ to `autoflight/navigate_home` topic with the ``data`` field set to ``true``. To stop Bebop from navigating home, publish another message with ``data`` set to ``false``. .. warning:: The topic has changed from `navigate_home` to `autoflight/navigate_home` after version 0.5.1. From 740b6cecd0abd8f51abbbfde3a8a889e67611d88 Mon Sep 17 00:00:00 2001 From: Thomas Bamford Date: Tue, 3 Jul 2018 12:05:18 -0400 Subject: [PATCH 07/11] Add support for joy control of autoflight commands --- bebop_tools/CHANGELOG.rst | 1 + bebop_tools/CMakeLists.txt | 5 +- bebop_tools/config/xbox360.yaml | 32 +++++++- bebop_tools/config/xbox360_disco.yaml | 36 ++++++++- .../launch/joy_teleop_autoflight.launch | 25 +++++++ bebop_tools/package.xml | 2 + bebop_tools/scripts/__init__.py | 0 bebop_tools/scripts/autoflight_joy.py | 73 +++++++++++++++++++ bebop_tools/setup.py | 11 +++ docs/piloting.rst | 3 + 10 files changed, 182 insertions(+), 6 deletions(-) create mode 100644 bebop_tools/launch/joy_teleop_autoflight.launch create mode 100755 bebop_tools/scripts/__init__.py create mode 100755 bebop_tools/scripts/autoflight_joy.py create mode 100755 bebop_tools/setup.py diff --git a/bebop_tools/CHANGELOG.rst b/bebop_tools/CHANGELOG.rst index 2c08ae0..036a9f3 100644 --- a/bebop_tools/CHANGELOG.rst +++ b/bebop_tools/CHANGELOG.rst @@ -5,6 +5,7 @@ Changelog for package bebop_tools 0.8.0 (2018-07-03) ------------------ * Added Xbox 360 config file for Disco +* Added support for joy control of autoflight commands * Contributors: Thomas Bamford 0.7.0 (2017-07-29) diff --git a/bebop_tools/CMakeLists.txt b/bebop_tools/CMakeLists.txt index 6341b46..ef48fe9 100644 --- a/bebop_tools/CMakeLists.txt +++ b/bebop_tools/CMakeLists.txt @@ -3,13 +3,14 @@ project(bebop_tools) find_package(catkin REQUIRED COMPONENTS bebop_msgs + rospy ) -# catkin_python_setup() +catkin_python_setup() catkin_package( # INCLUDE_DIRS include - CATKIN_DEPENDS bebop_msgs + CATKIN_DEPENDS bebop_msgs rospy ) # TODO: Install Rules diff --git a/bebop_tools/config/xbox360.yaml b/bebop_tools/config/xbox360.yaml index 9b69862..c02fcf9 100644 --- a/bebop_tools/config/xbox360.yaml +++ b/bebop_tools/config/xbox360.yaml @@ -73,9 +73,39 @@ teleop: topic_name: flip deadman_buttons: [5, 4, 2] # RB + LB + X axis_mappings: [] + start: + type: topic + message_type: "std_msgs/Empty" + topic_name: autoflight/start/joy + deadman_buttons: [5, 7] # RB + start + axis_mappings: [] + stop: + type: topic + message_type: "std_msgs/Empty" + topic_name: autoflight/stop + deadman_buttons: [5, 6] # RB + back + axis_mappings: [] + navigatehome: + type: topic + message_type: "std_msgs/Empty" + topic_name: autoflight/navigate_home/joy + deadman_buttons: [5, 8] # RB + XBox + axis_mappings: [] snapshot: type: topic message_type: "std_msgs/Empty" topic_name: snapshot - deadman_buttons: [5, 8] # RB + XBox + deadman_buttons: [4, 8] # LB + XBox + axis_mappings: [] + recordstart: + type: topic + message_type: "std_msgs/Empty" + topic_name: record/joy/start + deadman_buttons: [4, 7] # LB + start + axis_mappings: [] + recordstop: + type: topic + message_type: "std_msgs/Empty" + topic_name: record/joy/stop + deadman_buttons: [4, 6] # LB + back axis_mappings: [] diff --git a/bebop_tools/config/xbox360_disco.yaml b/bebop_tools/config/xbox360_disco.yaml index cc2a424..a62573a 100644 --- a/bebop_tools/config/xbox360_disco.yaml +++ b/bebop_tools/config/xbox360_disco.yaml @@ -61,10 +61,10 @@ teleop: topic_name: reset deadman_buttons: [5, 1] # RB + B axis_mappings: [] - flattrim: + usertakeoff: type: topic message_type: "std_msgs/Empty" - topic_name: flattrim + topic_name: usertakeoff/joy deadman_buttons: [5, 2] # RB + X axis_mappings: [] flip: @@ -73,9 +73,39 @@ teleop: topic_name: flip deadman_buttons: [5, 4, 2] # RB + LB + X axis_mappings: [] + start: + type: topic + message_type: "std_msgs/Empty" + topic_name: autoflight/start/joy + deadman_buttons: [5, 7] # RB + start + axis_mappings: [] + stop: + type: topic + message_type: "std_msgs/Empty" + topic_name: autoflight/stop + deadman_buttons: [5, 6] # RB + back + axis_mappings: [] + navigatehome: + type: topic + message_type: "std_msgs/Empty" + topic_name: autoflight/navigate_home/joy + deadman_buttons: [5, 8] # RB + XBox + axis_mappings: [] snapshot: type: topic message_type: "std_msgs/Empty" topic_name: snapshot - deadman_buttons: [5, 8] # RB + XBox + deadman_buttons: [4, 8] # LB + XBox + axis_mappings: [] + recordstart: + type: topic + message_type: "std_msgs/Empty" + topic_name: record/joy/start + deadman_buttons: [4, 7] # LB + start + axis_mappings: [] + recordstop: + type: topic + message_type: "std_msgs/Empty" + topic_name: record/joy/stop + deadman_buttons: [4, 6] # LB + back axis_mappings: [] diff --git a/bebop_tools/launch/joy_teleop_autoflight.launch b/bebop_tools/launch/joy_teleop_autoflight.launch new file mode 100644 index 0000000..ac64ac9 --- /dev/null +++ b/bebop_tools/launch/joy_teleop_autoflight.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bebop_tools/package.xml b/bebop_tools/package.xml index cedff1d..cd42633 100644 --- a/bebop_tools/package.xml +++ b/bebop_tools/package.xml @@ -17,11 +17,13 @@ catkin bebop_msgs + rospy bebop_driver bebop_msgs joy joy_teleop image_view + rospy diff --git a/bebop_tools/scripts/__init__.py b/bebop_tools/scripts/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/bebop_tools/scripts/autoflight_joy.py b/bebop_tools/scripts/autoflight_joy.py new file mode 100755 index 0000000..a1320fd --- /dev/null +++ b/bebop_tools/scripts/autoflight_joy.py @@ -0,0 +1,73 @@ +#!/usr/bin/python + +"""Listen to joy autoflight node and publish to complex autoflight topics.""" + +import rospy +from std_msgs.msg import Bool, Empty, String + + +class AutoFlightJoy: + """Subscribe to joy autoflight node and publish to bebop driver node.""" + + navigatehome = True + usertakeoff = True + + def __init__(self): + """Initialize publishers and subscribers.""" + self.pub_start = rospy.Publisher('autoflight/start', String, + queue_size=1) + self.pub_home = rospy.Publisher('autoflight/navigate_home', Bool, + queue_size=1) + self.pub_usertakeoff = rospy.Publisher('user_takeoff', Bool, + queue_size=1) + self.pub_record = rospy.Publisher('record', Bool, queue_size=1) + + rospy.Subscriber('autoflight/start/joy', Empty, self.start_callback) + rospy.Subscriber('autoflight/navigate_home/joy', Empty, + self.home_callback) + rospy.Subscriber('user_takeoff/joy', Empty, self.usertakeoff_callback) + rospy.Subscriber('record/joy/start', Empty, self.record_start_callback) + rospy.Subscriber('record/joy/stop', Empty, self.record_stop_callback) + + rospy.loginfo('autoflight: Ready to receive commands from joy.') + + def start_callback(self, empty): + """Retrieve flight plan name and publish to bebop autoflight/start.""" + string = rospy.get_param('~autoflight/start/fname', + 'flightPlan.mavlink') + self.pub_start.publish(string) + + def home_callback(self, empty): + """Publish alternating bool to bebop autoflight/navigate_home.""" + if self.navigatehome: + self.pub_home.publish(self.navigatehome) + self.navigatehome = False + else: + self.pub_home.publish(self.navigatehome) + self.navigatehome = True + + def usertakeoff_callback(self, empty): + """Publish alternating bool to bebop user_takeoff.""" + if self.usertakeoff: + self.pub_usertakeoff.publish(self.usertakeoff) + self.usertakeoff = False + else: + self.pub_usertakeoff.publish(self.usertakeoff) + self.usertakeoff = True + + def record_start_callback(self, empty): + """Publish true to bebop record.""" + self.pub_record.publish(True) + + def record_stop_callback(self, empty): + """Publish false to bebop record.""" + self.pub_record.publish(False) + + +if __name__ == '__main__': + rospy.init_node('autoflight_joy', anonymous=True) + AutoFlightJoy() + try: + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/bebop_tools/setup.py b/bebop_tools/setup.py new file mode 100755 index 0000000..726ca65 --- /dev/null +++ b/bebop_tools/setup.py @@ -0,0 +1,11 @@ +# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['bebop_tools'], + package_dir={'': 'src'}) + +setup(**setup_args) diff --git a/docs/piloting.rst b/docs/piloting.rst index ffd4ba3..5077b54 100644 --- a/docs/piloting.rst +++ b/docs/piloting.rst @@ -6,6 +6,9 @@ Sending Commands to Bebop .. note:: ``bebop_tools`` package comes with a launch file for tele-operating Bebop with a joystick using ROS `joy_teleop `_ package. The configuration file (key-action map) is written for `Logitech F710 controller `_ and is located in ``bebop_tools/config`` folder. Adapting the file to your own controller is straightforward. To teleop Bebop while the driver is running execute ``roslaunch bebop_tools joy_teleop.launch``. +.. note:: To control the autoflight functions using the joystick while the driver is running execute ``roslaunch bebop_tools joy_teleop_autoflight.launch``. The autoflight configuration is written for the XBox 360 controller. Adapting the file to your own controller is straightforward. + + Takeoff ======= From 1ef25662901414fbd36ca554a98bfe749629f255 Mon Sep 17 00:00:00 2001 From: Thomas Bamford Date: Tue, 3 Jul 2018 21:22:46 -0400 Subject: [PATCH 08/11] Add throttle to joy control of autoflight commands --- bebop_tools/scripts/autoflight_joy.py | 28 +++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/bebop_tools/scripts/autoflight_joy.py b/bebop_tools/scripts/autoflight_joy.py index a1320fd..0f79e76 100755 --- a/bebop_tools/scripts/autoflight_joy.py +++ b/bebop_tools/scripts/autoflight_joy.py @@ -29,16 +29,29 @@ def __init__(self): rospy.Subscriber('record/joy/start', Empty, self.record_start_callback) rospy.Subscriber('record/joy/stop', Empty, self.record_stop_callback) + self.allow_cmd = rospy.Time.now() + self.throttle = rospy.get_param('~throttle', 1.0) # default 1 s + rospy.loginfo('autoflight: Ready to receive commands from joy.') def start_callback(self, empty): """Retrieve flight plan name and publish to bebop autoflight/start.""" + if rospy.Time.now() <= self.allow_cmd: + return + self.allow_cmd = (rospy.Time.now() + + rospy.Duration.from_sec(self.throttle)) + string = rospy.get_param('~autoflight/start/fname', 'flightPlan.mavlink') self.pub_start.publish(string) def home_callback(self, empty): """Publish alternating bool to bebop autoflight/navigate_home.""" + if rospy.Time.now() <= self.allow_cmd: + return + self.allow_cmd = (rospy.Time.now() + + rospy.Duration.from_sec(self.throttle)) + if self.navigatehome: self.pub_home.publish(self.navigatehome) self.navigatehome = False @@ -48,6 +61,11 @@ def home_callback(self, empty): def usertakeoff_callback(self, empty): """Publish alternating bool to bebop user_takeoff.""" + if rospy.Time.now() <= self.allow_cmd: + return + self.allow_cmd = (rospy.Time.now() + + rospy.Duration.from_sec(self.throttle)) + if self.usertakeoff: self.pub_usertakeoff.publish(self.usertakeoff) self.usertakeoff = False @@ -57,10 +75,20 @@ def usertakeoff_callback(self, empty): def record_start_callback(self, empty): """Publish true to bebop record.""" + if rospy.Time.now() <= self.allow_cmd: + return + self.allow_cmd = (rospy.Time.now() + + rospy.Duration.from_sec(self.throttle)) + self.pub_record.publish(True) def record_stop_callback(self, empty): """Publish false to bebop record.""" + if rospy.Time.now() <= self.allow_cmd: + return + self.allow_cmd = (rospy.Time.now() + + rospy.Duration.from_sec(self.throttle)) + self.pub_record.publish(False) From 596c288f896d83b983d1511db80a5bbb23b16fea Mon Sep 17 00:00:00 2001 From: Thomas Bamford Date: Wed, 4 Jul 2018 19:23:37 -0400 Subject: [PATCH 09/11] Add documentation for joy autoflight and change xbox controller mapping for camera orientation control --- bebop_tools/config/xbox360.yaml | 2 +- bebop_tools/config/xbox360_disco.yaml | 2 +- docs/piloting.rst | 4 +++- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/bebop_tools/config/xbox360.yaml b/bebop_tools/config/xbox360.yaml index c02fcf9..a44f8cb 100644 --- a/bebop_tools/config/xbox360.yaml +++ b/bebop_tools/config/xbox360.yaml @@ -31,7 +31,7 @@ teleop: type: topic message_type: "geometry_msgs/Twist" topic_name: camera_control - deadman_buttons: [7] # Start + deadman_buttons: [4] # LB axis_mappings: - axis: 1 # Left D-Pad (up/down) diff --git a/bebop_tools/config/xbox360_disco.yaml b/bebop_tools/config/xbox360_disco.yaml index a62573a..a43bd34 100644 --- a/bebop_tools/config/xbox360_disco.yaml +++ b/bebop_tools/config/xbox360_disco.yaml @@ -31,7 +31,7 @@ teleop: type: topic message_type: "geometry_msgs/Twist" topic_name: camera_control - deadman_buttons: [7] # Start + deadman_buttons: [4] # LB axis_mappings: - axis: 1 # Left D-Pad (up/down) diff --git a/docs/piloting.rst b/docs/piloting.rst index 5077b54..03da3af 100644 --- a/docs/piloting.rst +++ b/docs/piloting.rst @@ -119,7 +119,9 @@ Requirements that must be met before an autonomous flight can start: To start an autonomous flight plan, publish a message of type `std_msgs/String `_ to `autoflight/start` topic. The ``data`` field should contain the name of the flight plan to execute, which is already stored on-board the vehicle. -.. note:: If an empty string is published, then the default 'flightplan.mavlink' is used. +.. note:: If an empty string is published, then the default 'flightPlan.mavlink' is used. + +.. note:: If using the autoflight functions while using the joystick (see above), the default string sent to start the flight plan is 'flightPlan.mavlink.' This string can be changed while the node is running through updating the [namespace]/autoflight/start/fname parameter. .. warning:: If not already flying, Bebop will attempt to take off upon starting a flight plan. From 82ae9e3f879ae2a412f57df73c16cbf585d00959 Mon Sep 17 00:00:00 2001 From: Thomas Bamford Date: Mon, 30 Jul 2018 00:55:58 -0400 Subject: [PATCH 10/11] Build against parrot_arsdk 3.14.0p1 --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index bc3a7e5..93d8d64 100644 --- a/README.md +++ b/README.md @@ -19,4 +19,4 @@ [![Build Status (ROS I,J,K) - TravisCI](https://travis-ci.org/AutonomyLab/bebop_autonomy.svg?branch=indigo-devel)](https://travis-ci.org/AutonomyLab/bebop_autonomy) [![Build Status (ROS I,J) - Semaphore](https://semaphoreci.com/api/v1/projects/11786233-d505-4d79-b27c-80c2742243a4/537552/badge.svg)](https://semaphoreci.com/mani_monaj/bebop_autonomy) -Built against [parrot_arsdk](https://github.com/AutonomyLab/parrot_arsdk) 3.14.0p0 +Built against [parrot_arsdk](https://github.com/AutonomyLab/parrot_arsdk) 3.14.0p1 From d023a63ea33f3fa4c3b8f7bafbe6170a82ed176f Mon Sep 17 00:00:00 2001 From: Thomas Bamford Date: Sun, 12 Aug 2018 23:13:24 -0400 Subject: [PATCH 11/11] Fix build error for bebop_tools autoflight_joy node by complying with ROS package format 2 --- bebop_tools/CMakeLists.txt | 7 +++++-- bebop_tools/{scripts => nodes}/__init__.py | 0 bebop_tools/{scripts => nodes}/autoflight_joy.py | 0 bebop_tools/setup.py | 11 ----------- 4 files changed, 5 insertions(+), 13 deletions(-) rename bebop_tools/{scripts => nodes}/__init__.py (100%) rename bebop_tools/{scripts => nodes}/autoflight_joy.py (100%) delete mode 100755 bebop_tools/setup.py diff --git a/bebop_tools/CMakeLists.txt b/bebop_tools/CMakeLists.txt index ef48fe9..0518762 100644 --- a/bebop_tools/CMakeLists.txt +++ b/bebop_tools/CMakeLists.txt @@ -6,11 +6,14 @@ find_package(catkin REQUIRED COMPONENTS rospy ) -catkin_python_setup() - catkin_package( # INCLUDE_DIRS include CATKIN_DEPENDS bebop_msgs rospy ) +catkin_install_python( + PROGRAMS nodes/autoflight_joy.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + # TODO: Install Rules diff --git a/bebop_tools/scripts/__init__.py b/bebop_tools/nodes/__init__.py similarity index 100% rename from bebop_tools/scripts/__init__.py rename to bebop_tools/nodes/__init__.py diff --git a/bebop_tools/scripts/autoflight_joy.py b/bebop_tools/nodes/autoflight_joy.py similarity index 100% rename from bebop_tools/scripts/autoflight_joy.py rename to bebop_tools/nodes/autoflight_joy.py diff --git a/bebop_tools/setup.py b/bebop_tools/setup.py deleted file mode 100755 index 726ca65..0000000 --- a/bebop_tools/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['bebop_tools'], - package_dir={'': 'src'}) - -setup(**setup_args)