diff --git a/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg b/bebop_driver/cfg/autogenerated/BebopArdrone3.cfg index f4b2763..4821203 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/bluecamel/arsdk-xml/53ed05daac60e1ff8391f09c8b6eb06c18a992b6/xml/ardrone3.xml # Do not modify this file by hand. Check scripts/meta folder for generator files. PACKAGE = "bebop_driver" @@ -23,7 +23,7 @@ PilotingSettingsAbsolutControlOn_enum = gen.enum([ ], "1 to enable, 0 to disable") pilotingsettings_group_gen.add("PilotingSettingsAbsolutControlOn", int_t, 0, "1 to enable, 0 to disable", 0, 0, 1, edit_method=PilotingSettingsAbsolutControlOn_enum) # Set max distance.\n You can get the bounds from the event [MaxDistance](#1-6-3).\n\n If [Geofence](#1-6-4) is activated, the drone wont fly over the given max distance. -pilotingsettings_group_gen.add("PilotingSettingsMaxDistanceValue", double_t, 0, "Current max distance in meter", 0 , 0.0, 2000.0) +pilotingsettings_group_gen.add("PilotingSettingsMaxDistanceValue", double_t, 0, "Current max distance in meter", 0 , 0, 160) # Enable geofence.\n If geofence is enabled, the drone wont fly over the given max distance.\n You can get the max distance from the event [MaxDistance](#1-6-3). \n For copters: the distance is computed from the controller position, if this position is not known, it will use the take off.\n For fixed wings: the distance is computed from the take off position. PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover_enum = gen.enum([ gen.const("PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover_OFF", int_t, 0, "Disabled"), @@ -94,6 +94,34 @@ networksettings_group_gen.add("NetworkSettingsWifiSelectionChannel", int_t, 0, " # Photo settings chosen by the user picturesettings_group_gen = gen.add_group("picturesettings") +# Set picture format.\n Please note that the time required to take the picture is highly related to this format.\n Also, please note that if your picture format is different from snapshot, picture taking will stop video recording (it will restart after the picture has been taken). +PictureSettingsPictureFormatSelectionType_enum = gen.enum([ + gen.const("PictureSettingsPictureFormatSelectionType_raw", int_t, 0, "Take raw image"), + gen.const("PictureSettingsPictureFormatSelectionType_jpeg", int_t, 1, "Take a 4:3 jpeg photo"), + gen.const("PictureSettingsPictureFormatSelectionType_snapshot", int_t, 2, "Take a 16:9 snapshot from camera"), + gen.const("PictureSettingsPictureFormatSelectionType_jpeg_fisheye", int_t, 3, "Take jpeg fisheye image only"), +], "The type of photo format") +picturesettings_group_gen.add("PictureSettingsPictureFormatSelectionType", int_t, 0, "The type of photo format", 0, 0, 3, edit_method=PictureSettingsPictureFormatSelectionType_enum) +# Set White Balance mode. +PictureSettingsAutoWhiteBalanceSelectionType_enum = gen.enum([ + gen.const("PictureSettingsAutoWhiteBalanceSelectionType_auto", int_t, 0, "Auto guess of best white balance params"), + gen.const("PictureSettingsAutoWhiteBalanceSelectionType_tungsten", int_t, 1, "Tungsten white balance"), + gen.const("PictureSettingsAutoWhiteBalanceSelectionType_daylight", int_t, 2, "Daylight white balance"), + gen.const("PictureSettingsAutoWhiteBalanceSelectionType_cloudy", int_t, 3, "Cloudy white balance"), + gen.const("PictureSettingsAutoWhiteBalanceSelectionType_cool_white", int_t, 4, "White balance for a flash"), +], "The type auto white balance") +picturesettings_group_gen.add("PictureSettingsAutoWhiteBalanceSelectionType", int_t, 0, "The type auto white balance", 0, 0, 4, edit_method=PictureSettingsAutoWhiteBalanceSelectionType_enum) +# Set image exposure. +picturesettings_group_gen.add("PictureSettingsExpositionSelectionValue", double_t, 0, "Exposition value (bounds given by ExpositionChanged arg min and max, by default [-3:3])", 0 , -3.0, 3.0) +# Set image saturation. +picturesettings_group_gen.add("PictureSettingsSaturationSelectionValue", double_t, 0, "Saturation value (bounds given by SaturationChanged arg min and max, by default [-100:100])", 0 , -100.0, 100.0) +# Set timelapse mode.\n If timelapse mode is set, instead of taking a video, the drone will take picture regularly.\n Watch out, this command only configure the timelapse mode. Once it is configured, you can start/stop the timelapse with the [RecordVideo](#1-7-3) command. +PictureSettingsTimelapseSelectionEnabled_enum = gen.enum([ + gen.const("PictureSettingsTimelapseSelectionEnabled_OFF", int_t, 0, "Disabled"), + gen.const("PictureSettingsTimelapseSelectionEnabled_ON", int_t, 1, "Enabled"), +], "1 if timelapse is enabled, 0 otherwise") +picturesettings_group_gen.add("PictureSettingsTimelapseSelectionEnabled", int_t, 0, "1 if timelapse is enabled, 0 otherwise", 0, 0, 1, edit_method=PictureSettingsTimelapseSelectionEnabled_enum) +picturesettings_group_gen.add("PictureSettingsTimelapseSelectionInterval", double_t, 0, "interval in seconds for taking pictures", 0 , 0, 120) # Set video stabilization mode. PictureSettingsVideoStabilizationModeMode_enum = gen.enum([ gen.const("PictureSettingsVideoStabilizationModeMode_roll_pitch", int_t, 0, "Video flat on roll and pitch"), @@ -124,6 +152,10 @@ picturesettings_group_gen.add("PictureSettingsVideoResolutionsType", int_t, 0, " # GPS settings gpssettings_group_gen = gen.add_group("gpssettings") +# Set home position. +gpssettings_group_gen.add("GPSSettingsSetHomeLatitude", double_t, 0, "Home latitude in decimal degrees", 0 , -90.0, 90.0) +gpssettings_group_gen.add("GPSSettingsSetHomeLongitude", double_t, 0, "Home longitude in decimal degrees", 0 , -180.0, 180.0) +gpssettings_group_gen.add("GPSSettingsSetHomeAltitude", double_t, 0, "Home altitude in meters", 0 , 0, 160) # Set the preferred home type.\n Please note that this is only a preference. The actual type chosen is given by the event [HomeType](#1-31-2).\n You can get the currently available types with the event [HomeTypeAvailability](#1-31-1). GPSSettingsHomeTypeType_enum = gen.enum([ gen.const("GPSSettingsHomeTypeType_TAKEOFF", int_t, 0, "The drone will try to return to the take off position"), @@ -135,4 +167,4 @@ gpssettings_group_gen.add("GPSSettingsHomeTypeType", int_t, 0, "The type of the gpssettings_group_gen.add("GPSSettingsReturnHomeDelayDelay", int_t, 0, "Delay in second", 0 , 0, 120) -exit(gen.generate(PACKAGE, "bebop_driver_nodelet", "BebopArdrone3")) +exit(gen.generate(PACKAGE, "bebop_driver_nodelet", "BebopArdrone3")) \ No newline at end of file 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..d633347 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/bluecamel/arsdk-xml/53ed05daac60e1ff8391f09c8b6eb06c18a992b6/xml/ardrone3.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ @@ -47,10 +47,16 @@ namespace cb class SpeedSettingsOutdoor; class SpeedSettingsMaxPitchRollRotationSpeed; class NetworkSettingsWifiSelection; + class PictureSettingsPictureFormatSelection; + class PictureSettingsAutoWhiteBalanceSelection; + class PictureSettingsExpositionSelection; + class PictureSettingsSaturationSelection; + class PictureSettingsTimelapseSelection; class PictureSettingsVideoStabilizationMode; class PictureSettingsVideoRecordingMode; class PictureSettingsVideoFramerate; class PictureSettingsVideoResolutions; + class GPSSettingsSetHome; class GPSSettingsHomeType; class GPSSettingsReturnHomeDelay; } // namespace cb @@ -75,10 +81,16 @@ boost::shared_ptr ardrone3_speedsettings_hullpr boost::shared_ptr ardrone3_speedsettings_outdoor_ptr; boost::shared_ptr ardrone3_speedsettings_maxpitchrollrotationspeed_ptr; boost::shared_ptr ardrone3_networksettings_wifiselection_ptr; +boost::shared_ptr ardrone3_picturesettings_pictureformatselection_ptr; +boost::shared_ptr ardrone3_picturesettings_autowhitebalanceselection_ptr; +boost::shared_ptr ardrone3_picturesettings_expositionselection_ptr; +boost::shared_ptr ardrone3_picturesettings_saturationselection_ptr; +boost::shared_ptr ardrone3_picturesettings_timelapseselection_ptr; boost::shared_ptr ardrone3_picturesettings_videostabilizationmode_ptr; boost::shared_ptr ardrone3_picturesettings_videorecordingmode_ptr; boost::shared_ptr ardrone3_picturesettings_videoframerate_ptr; boost::shared_ptr ardrone3_picturesettings_videoresolutions_ptr; +boost::shared_ptr ardrone3_gpssettings_sethome_ptr; boost::shared_ptr ardrone3_gpssettings_hometype_ptr; boost::shared_ptr ardrone3_gpssettings_returnhomedelay_ptr; #endif // DEFINE_SHARED_PTRS @@ -102,10 +114,16 @@ ardrone3_speedsettings_hullprotection_ptr.reset(new cb::SpeedSettingsHullProtect ardrone3_speedsettings_outdoor_ptr.reset(new cb::SpeedSettingsOutdoor(priv_nh)); ardrone3_speedsettings_maxpitchrollrotationspeed_ptr.reset(new cb::SpeedSettingsMaxPitchRollRotationSpeed(priv_nh)); ardrone3_networksettings_wifiselection_ptr.reset(new cb::NetworkSettingsWifiSelection(priv_nh)); +ardrone3_picturesettings_pictureformatselection_ptr.reset(new cb::PictureSettingsPictureFormatSelection(priv_nh)); +ardrone3_picturesettings_autowhitebalanceselection_ptr.reset(new cb::PictureSettingsAutoWhiteBalanceSelection(priv_nh)); +ardrone3_picturesettings_expositionselection_ptr.reset(new cb::PictureSettingsExpositionSelection(priv_nh)); +ardrone3_picturesettings_saturationselection_ptr.reset(new cb::PictureSettingsSaturationSelection(priv_nh)); +ardrone3_picturesettings_timelapseselection_ptr.reset(new cb::PictureSettingsTimelapseSelection(priv_nh)); ardrone3_picturesettings_videostabilizationmode_ptr.reset(new cb::PictureSettingsVideoStabilizationMode(priv_nh)); ardrone3_picturesettings_videorecordingmode_ptr.reset(new cb::PictureSettingsVideoRecordingMode(priv_nh)); ardrone3_picturesettings_videoframerate_ptr.reset(new cb::PictureSettingsVideoFramerate(priv_nh)); ardrone3_picturesettings_videoresolutions_ptr.reset(new cb::PictureSettingsVideoResolutions(priv_nh)); +ardrone3_gpssettings_sethome_ptr.reset(new cb::GPSSettingsSetHome(priv_nh)); ardrone3_gpssettings_hometype_ptr.reset(new cb::GPSSettingsHomeType(priv_nh)); ardrone3_gpssettings_returnhomedelay_ptr.reset(new cb::GPSSettingsReturnHomeDelay(priv_nh)); @@ -161,6 +179,21 @@ callback_map_.insert(std::pair >( ardrone3_networksettings_wifiselection_ptr->GetCommandKey(), ardrone3_networksettings_wifiselection_ptr)); +callback_map_.insert(std::pair >( + ardrone3_picturesettings_pictureformatselection_ptr->GetCommandKey(), + ardrone3_picturesettings_pictureformatselection_ptr)); +callback_map_.insert(std::pair >( + ardrone3_picturesettings_autowhitebalanceselection_ptr->GetCommandKey(), + ardrone3_picturesettings_autowhitebalanceselection_ptr)); +callback_map_.insert(std::pair >( + ardrone3_picturesettings_expositionselection_ptr->GetCommandKey(), + ardrone3_picturesettings_expositionselection_ptr)); +callback_map_.insert(std::pair >( + ardrone3_picturesettings_saturationselection_ptr->GetCommandKey(), + ardrone3_picturesettings_saturationselection_ptr)); +callback_map_.insert(std::pair >( + ardrone3_picturesettings_timelapseselection_ptr->GetCommandKey(), + ardrone3_picturesettings_timelapseselection_ptr)); callback_map_.insert(std::pair >( ardrone3_picturesettings_videostabilizationmode_ptr->GetCommandKey(), ardrone3_picturesettings_videostabilizationmode_ptr)); @@ -173,6 +206,9 @@ callback_map_.insert(std::pair >( ardrone3_picturesettings_videoresolutions_ptr->GetCommandKey(), ardrone3_picturesettings_videoresolutions_ptr)); +callback_map_.insert(std::pair >( + ardrone3_gpssettings_sethome_ptr->GetCommandKey(), + ardrone3_gpssettings_sethome_ptr)); callback_map_.insert(std::pair >( ardrone3_gpssettings_hometype_ptr->GetCommandKey(), ardrone3_gpssettings_hometype_ptr)); 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..0248e9f 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/bluecamel/arsdk-xml/53ed05daac60e1ff8391f09c8b6eb06c18a992b6/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 @@ -1471,6 +1471,440 @@ class NetworkSettingsWifiSelection : public AbstractSetting } }; // NetworkSettingsWifiSelection +class PictureSettingsPictureFormatSelection : public AbstractSetting +{ +private: + int32_t PictureSettingsPictureFormatSelectionType_bebop_value_; + bool PictureSettingsPictureFormatSelectionType_bebop_sent_; + +public: + explicit PictureSettingsPictureFormatSelection(ros::NodeHandle& priv_nh) + : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_PICTUREFORMATCHANGED, priv_nh) + , PictureSettingsPictureFormatSelectionType_bebop_sent_(false) + {} + + // Runs in Nodelet's main thread's context (Dynamic Reconfigure Update Callback) + void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_) + { + ::boost::lock_guard lock(mutex_); + bool changed = false; + bool all_inited = true; + + // This will likely fail for float or double values + if ((config.PictureSettingsPictureFormatSelectionType != PictureSettingsPictureFormatSelectionType_bebop_value_)) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "PictureSettingsPictureFormatSelectionType changed!"); + changed = true; + } + all_inited &= PictureSettingsPictureFormatSelectionType_bebop_sent_; + + if (changed && !all_inited) + { + ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB", + "Value of PictureSettingsPictureFormatSelection was not initialized either by Bebop or Params."); + } + + if (changed && all_inited) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "Sending PictureSettingsPictureFormatSelection changes to bebop"); + + bebop_ctrl_ptr_->aRDrone3->sendPictureSettingsPictureFormatSelection(bebop_ctrl_ptr_->aRDrone3 + , static_cast(config.PictureSettingsPictureFormatSelectionType) + ); + } + } + + // Runs in SDK's CommandReceivedCallback's context + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", + "PictureSettingsPictureFormatSelection::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_PICTUREFORMATCHANGED_TYPE, arg); + if (arg) + { + PictureSettingsPictureFormatSelectionType_bebop_sent_ = true; + ROS_INFO_STREAM("Value for PictureSettingsPictureFormatSelectionType recved: " << static_cast(arg->value.I32)); + PictureSettingsPictureFormatSelectionType_bebop_value_ = static_cast(arg->value.I32); + + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PictureSettingsPictureFormatSelectionType exists in params ..."); + if (!priv_nh_.hasParam("PictureSettingsPictureFormatSelectionType")) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No"); + priv_nh_.setParam("PictureSettingsPictureFormatSelectionType", static_cast(arg->value.I32)); + } + else + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes"); + //ROS_INFO_STREAM("New value for ros interanal variable: " << PictureSettingsPictureFormatSelectionType_rosparam_value_); + } + } + } +}; // PictureSettingsPictureFormatSelection + +class PictureSettingsAutoWhiteBalanceSelection : public AbstractSetting +{ +private: + int32_t PictureSettingsAutoWhiteBalanceSelectionType_bebop_value_; + bool PictureSettingsAutoWhiteBalanceSelectionType_bebop_sent_; + +public: + explicit PictureSettingsAutoWhiteBalanceSelection(ros::NodeHandle& priv_nh) + : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_AUTOWHITEBALANCECHANGED, priv_nh) + , PictureSettingsAutoWhiteBalanceSelectionType_bebop_sent_(false) + {} + + // Runs in Nodelet's main thread's context (Dynamic Reconfigure Update Callback) + void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_) + { + ::boost::lock_guard lock(mutex_); + bool changed = false; + bool all_inited = true; + + // This will likely fail for float or double values + if ((config.PictureSettingsAutoWhiteBalanceSelectionType != PictureSettingsAutoWhiteBalanceSelectionType_bebop_value_)) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "PictureSettingsAutoWhiteBalanceSelectionType changed!"); + changed = true; + } + all_inited &= PictureSettingsAutoWhiteBalanceSelectionType_bebop_sent_; + + if (changed && !all_inited) + { + ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB", + "Value of PictureSettingsAutoWhiteBalanceSelection was not initialized either by Bebop or Params."); + } + + if (changed && all_inited) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "Sending PictureSettingsAutoWhiteBalanceSelection changes to bebop"); + + bebop_ctrl_ptr_->aRDrone3->sendPictureSettingsAutoWhiteBalanceSelection(bebop_ctrl_ptr_->aRDrone3 + , static_cast(config.PictureSettingsAutoWhiteBalanceSelectionType) + ); + } + } + + // Runs in SDK's CommandReceivedCallback's context + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", + "PictureSettingsAutoWhiteBalanceSelection::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_AUTOWHITEBALANCECHANGED_TYPE, arg); + if (arg) + { + PictureSettingsAutoWhiteBalanceSelectionType_bebop_sent_ = true; + ROS_INFO_STREAM("Value for PictureSettingsAutoWhiteBalanceSelectionType recved: " << static_cast(arg->value.I32)); + PictureSettingsAutoWhiteBalanceSelectionType_bebop_value_ = static_cast(arg->value.I32); + + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PictureSettingsAutoWhiteBalanceSelectionType exists in params ..."); + if (!priv_nh_.hasParam("PictureSettingsAutoWhiteBalanceSelectionType")) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No"); + priv_nh_.setParam("PictureSettingsAutoWhiteBalanceSelectionType", static_cast(arg->value.I32)); + } + else + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes"); + //ROS_INFO_STREAM("New value for ros interanal variable: " << PictureSettingsAutoWhiteBalanceSelectionType_rosparam_value_); + } + } + } +}; // PictureSettingsAutoWhiteBalanceSelection + +class PictureSettingsExpositionSelection : public AbstractSetting +{ +private: + double PictureSettingsExpositionSelectionValue_bebop_value_; + bool PictureSettingsExpositionSelectionValue_bebop_sent_; + +public: + explicit PictureSettingsExpositionSelection(ros::NodeHandle& priv_nh) + : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED, priv_nh) + , PictureSettingsExpositionSelectionValue_bebop_sent_(false) + {} + + // Runs in Nodelet's main thread's context (Dynamic Reconfigure Update Callback) + void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_) + { + ::boost::lock_guard lock(mutex_); + bool changed = false; + bool all_inited = true; + + // This will likely fail for float or double values + if ((config.PictureSettingsExpositionSelectionValue != PictureSettingsExpositionSelectionValue_bebop_value_)) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "PictureSettingsExpositionSelectionValue changed!"); + changed = true; + } + all_inited &= PictureSettingsExpositionSelectionValue_bebop_sent_; + + if (changed && !all_inited) + { + ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB", + "Value of PictureSettingsExpositionSelection was not initialized either by Bebop or Params."); + } + + if (changed && all_inited) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "Sending PictureSettingsExpositionSelection changes to bebop"); + + bebop_ctrl_ptr_->aRDrone3->sendPictureSettingsExpositionSelection(bebop_ctrl_ptr_->aRDrone3 + , (config.PictureSettingsExpositionSelectionValue) + ); + } + } + + // Runs in SDK's CommandReceivedCallback's context + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", + "PictureSettingsExpositionSelection::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED_VALUE, arg); + if (arg) + { + PictureSettingsExpositionSelectionValue_bebop_sent_ = true; + ROS_INFO_STREAM("Value for PictureSettingsExpositionSelectionValue recved: " << static_cast(arg->value.Float)); + PictureSettingsExpositionSelectionValue_bebop_value_ = static_cast(arg->value.Float); + + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PictureSettingsExpositionSelectionValue exists in params ..."); + if (!priv_nh_.hasParam("PictureSettingsExpositionSelectionValue")) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No"); + priv_nh_.setParam("PictureSettingsExpositionSelectionValue", static_cast(arg->value.Float)); + } + else + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes"); + //ROS_INFO_STREAM("New value for ros interanal variable: " << PictureSettingsExpositionSelectionValue_rosparam_value_); + } + } + } +}; // PictureSettingsExpositionSelection + +class PictureSettingsSaturationSelection : public AbstractSetting +{ +private: + double PictureSettingsSaturationSelectionValue_bebop_value_; + bool PictureSettingsSaturationSelectionValue_bebop_sent_; + +public: + explicit PictureSettingsSaturationSelection(ros::NodeHandle& priv_nh) + : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_SATURATIONCHANGED, priv_nh) + , PictureSettingsSaturationSelectionValue_bebop_sent_(false) + {} + + // Runs in Nodelet's main thread's context (Dynamic Reconfigure Update Callback) + void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_) + { + ::boost::lock_guard lock(mutex_); + bool changed = false; + bool all_inited = true; + + // This will likely fail for float or double values + if ((config.PictureSettingsSaturationSelectionValue != PictureSettingsSaturationSelectionValue_bebop_value_)) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "PictureSettingsSaturationSelectionValue changed!"); + changed = true; + } + all_inited &= PictureSettingsSaturationSelectionValue_bebop_sent_; + + if (changed && !all_inited) + { + ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB", + "Value of PictureSettingsSaturationSelection was not initialized either by Bebop or Params."); + } + + if (changed && all_inited) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "Sending PictureSettingsSaturationSelection changes to bebop"); + + bebop_ctrl_ptr_->aRDrone3->sendPictureSettingsSaturationSelection(bebop_ctrl_ptr_->aRDrone3 + , (config.PictureSettingsSaturationSelectionValue) + ); + } + } + + // Runs in SDK's CommandReceivedCallback's context + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", + "PictureSettingsSaturationSelection::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_SATURATIONCHANGED_VALUE, arg); + if (arg) + { + PictureSettingsSaturationSelectionValue_bebop_sent_ = true; + ROS_INFO_STREAM("Value for PictureSettingsSaturationSelectionValue recved: " << static_cast(arg->value.Float)); + PictureSettingsSaturationSelectionValue_bebop_value_ = static_cast(arg->value.Float); + + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PictureSettingsSaturationSelectionValue exists in params ..."); + if (!priv_nh_.hasParam("PictureSettingsSaturationSelectionValue")) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No"); + priv_nh_.setParam("PictureSettingsSaturationSelectionValue", static_cast(arg->value.Float)); + } + else + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes"); + //ROS_INFO_STREAM("New value for ros interanal variable: " << PictureSettingsSaturationSelectionValue_rosparam_value_); + } + } + } +}; // PictureSettingsSaturationSelection + +class PictureSettingsTimelapseSelection : public AbstractSetting +{ +private: + int32_t PictureSettingsTimelapseSelectionEnabled_bebop_value_; + bool PictureSettingsTimelapseSelectionEnabled_bebop_sent_; + double PictureSettingsTimelapseSelectionInterval_bebop_value_; + bool PictureSettingsTimelapseSelectionInterval_bebop_sent_; + +public: + explicit PictureSettingsTimelapseSelection(ros::NodeHandle& priv_nh) + : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_TIMELAPSECHANGED, priv_nh) + , PictureSettingsTimelapseSelectionEnabled_bebop_sent_(false) + , PictureSettingsTimelapseSelectionInterval_bebop_sent_(false) + {} + + // Runs in Nodelet's main thread's context (Dynamic Reconfigure Update Callback) + void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_) + { + ::boost::lock_guard lock(mutex_); + bool changed = false; + bool all_inited = true; + + // This will likely fail for float or double values + if ((config.PictureSettingsTimelapseSelectionEnabled != PictureSettingsTimelapseSelectionEnabled_bebop_value_)) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "PictureSettingsTimelapseSelectionEnabled changed!"); + changed = true; + } + all_inited &= PictureSettingsTimelapseSelectionEnabled_bebop_sent_; + + // This will likely fail for float or double values + if ((config.PictureSettingsTimelapseSelectionInterval != PictureSettingsTimelapseSelectionInterval_bebop_value_)) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "PictureSettingsTimelapseSelectionInterval changed!"); + changed = true; + } + all_inited &= PictureSettingsTimelapseSelectionInterval_bebop_sent_; + + if (changed && !all_inited) + { + ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB", + "Value of PictureSettingsTimelapseSelection was not initialized either by Bebop or Params."); + } + + if (changed && all_inited) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "Sending PictureSettingsTimelapseSelection changes to bebop"); + + bebop_ctrl_ptr_->aRDrone3->sendPictureSettingsTimelapseSelection(bebop_ctrl_ptr_->aRDrone3 + , (config.PictureSettingsTimelapseSelectionEnabled) + , (config.PictureSettingsTimelapseSelectionInterval) + ); + } + } + + // Runs in SDK's CommandReceivedCallback's context + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", + "PictureSettingsTimelapseSelection::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_TIMELAPSECHANGED_ENABLED, arg); + if (arg) + { + PictureSettingsTimelapseSelectionEnabled_bebop_sent_ = true; + ROS_INFO_STREAM("Value for PictureSettingsTimelapseSelectionEnabled recved: " << static_cast(arg->value.U8)); + PictureSettingsTimelapseSelectionEnabled_bebop_value_ = static_cast(arg->value.U8); + + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PictureSettingsTimelapseSelectionEnabled exists in params ..."); + if (!priv_nh_.hasParam("PictureSettingsTimelapseSelectionEnabled")) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No"); + priv_nh_.setParam("PictureSettingsTimelapseSelectionEnabled", static_cast(arg->value.U8)); + } + else + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes"); + //ROS_INFO_STREAM("New value for ros interanal variable: " << PictureSettingsTimelapseSelectionEnabled_rosparam_value_); + } + } + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_TIMELAPSECHANGED_INTERVAL, arg); + if (arg) + { + PictureSettingsTimelapseSelectionInterval_bebop_sent_ = true; + ROS_INFO_STREAM("Value for PictureSettingsTimelapseSelectionInterval recved: " << static_cast(arg->value.Float)); + PictureSettingsTimelapseSelectionInterval_bebop_value_ = static_cast(arg->value.Float); + + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PictureSettingsTimelapseSelectionInterval exists in params ..."); + if (!priv_nh_.hasParam("PictureSettingsTimelapseSelectionInterval")) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No"); + priv_nh_.setParam("PictureSettingsTimelapseSelectionInterval", static_cast(arg->value.Float)); + } + else + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes"); + //ROS_INFO_STREAM("New value for ros interanal variable: " << PictureSettingsTimelapseSelectionInterval_rosparam_value_); + } + } + } +}; // PictureSettingsTimelapseSelection + class PictureSettingsVideoStabilizationMode : public AbstractSetting { private: @@ -1791,6 +2225,154 @@ class PictureSettingsVideoResolutions : public AbstractSetting } }; // PictureSettingsVideoResolutions +class GPSSettingsSetHome : public AbstractSetting +{ +private: + double GPSSettingsSetHomeLatitude_bebop_value_; + bool GPSSettingsSetHomeLatitude_bebop_sent_; + double GPSSettingsSetHomeLongitude_bebop_value_; + bool GPSSettingsSetHomeLongitude_bebop_sent_; + double GPSSettingsSetHomeAltitude_bebop_value_; + bool GPSSettingsSetHomeAltitude_bebop_sent_; + +public: + explicit GPSSettingsSetHome(ros::NodeHandle& priv_nh) + : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_HOMECHANGED, priv_nh) + , GPSSettingsSetHomeLatitude_bebop_sent_(false) + , GPSSettingsSetHomeLongitude_bebop_sent_(false) + , GPSSettingsSetHomeAltitude_bebop_sent_(false) + {} + + // Runs in Nodelet's main thread's context (Dynamic Reconfigure Update Callback) + void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_) + { + ::boost::lock_guard lock(mutex_); + bool changed = false; + bool all_inited = true; + + // This will likely fail for float or double values + if ((config.GPSSettingsSetHomeLatitude != GPSSettingsSetHomeLatitude_bebop_value_)) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "GPSSettingsSetHomeLatitude changed!"); + changed = true; + } + all_inited &= GPSSettingsSetHomeLatitude_bebop_sent_; + + // This will likely fail for float or double values + if ((config.GPSSettingsSetHomeLongitude != GPSSettingsSetHomeLongitude_bebop_value_)) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "GPSSettingsSetHomeLongitude changed!"); + changed = true; + } + all_inited &= GPSSettingsSetHomeLongitude_bebop_sent_; + + // This will likely fail for float or double values + if ((config.GPSSettingsSetHomeAltitude != GPSSettingsSetHomeAltitude_bebop_value_)) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "GPSSettingsSetHomeAltitude changed!"); + changed = true; + } + all_inited &= GPSSettingsSetHomeAltitude_bebop_sent_; + + if (changed && !all_inited) + { + ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB", + "Value of GPSSettingsSetHome was not initialized either by Bebop or Params."); + } + + if (changed && all_inited) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", + "Sending GPSSettingsSetHome changes to bebop"); + + bebop_ctrl_ptr_->aRDrone3->sendGPSSettingsSetHome(bebop_ctrl_ptr_->aRDrone3 + , (config.GPSSettingsSetHomeLatitude) + , (config.GPSSettingsSetHomeLongitude) + , (config.GPSSettingsSetHomeAltitude) + ); + } + } + + // Runs in SDK's CommandReceivedCallback's context + void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t) + { + if (arguments == NULL) + { + ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB", + "GPSSettingsSetHome::Update() arguments is NULL"); + return; + } + + ::boost::lock_guard lock(mutex_); + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_HOMECHANGED_LATITUDE, arg); + if (arg) + { + GPSSettingsSetHomeLatitude_bebop_sent_ = true; + ROS_INFO_STREAM("Value for GPSSettingsSetHomeLatitude recved: " << static_cast(arg->value.Double)); + GPSSettingsSetHomeLatitude_bebop_value_ = static_cast(arg->value.Double); + + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if GPSSettingsSetHomeLatitude exists in params ..."); + if (!priv_nh_.hasParam("GPSSettingsSetHomeLatitude")) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No"); + priv_nh_.setParam("GPSSettingsSetHomeLatitude", static_cast(arg->value.Double)); + } + else + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes"); + //ROS_INFO_STREAM("New value for ros interanal variable: " << GPSSettingsSetHomeLatitude_rosparam_value_); + } + } + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_HOMECHANGED_LONGITUDE, arg); + if (arg) + { + GPSSettingsSetHomeLongitude_bebop_sent_ = true; + ROS_INFO_STREAM("Value for GPSSettingsSetHomeLongitude recved: " << static_cast(arg->value.Double)); + GPSSettingsSetHomeLongitude_bebop_value_ = static_cast(arg->value.Double); + + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if GPSSettingsSetHomeLongitude exists in params ..."); + if (!priv_nh_.hasParam("GPSSettingsSetHomeLongitude")) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No"); + priv_nh_.setParam("GPSSettingsSetHomeLongitude", static_cast(arg->value.Double)); + } + else + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes"); + //ROS_INFO_STREAM("New value for ros interanal variable: " << GPSSettingsSetHomeLongitude_rosparam_value_); + } + } + + arg = NULL; + HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_HOMECHANGED_ALTITUDE, arg); + if (arg) + { + GPSSettingsSetHomeAltitude_bebop_sent_ = true; + ROS_INFO_STREAM("Value for GPSSettingsSetHomeAltitude recved: " << static_cast(arg->value.Double)); + GPSSettingsSetHomeAltitude_bebop_value_ = static_cast(arg->value.Double); + + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if GPSSettingsSetHomeAltitude exists in params ..."); + if (!priv_nh_.hasParam("GPSSettingsSetHomeAltitude")) + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No"); + priv_nh_.setParam("GPSSettingsSetHomeAltitude", static_cast(arg->value.Double)); + } + else + { + ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes"); + //ROS_INFO_STREAM("New value for ros interanal variable: " << GPSSettingsSetHomeAltitude_rosparam_value_); + } + } + } +}; // GPSSettingsSetHome + class GPSSettingsHomeType : public AbstractSetting { private: @@ -1953,4 +2535,4 @@ class GPSSettingsReturnHomeDelay : public AbstractSetting } // namespace cb } // namespace bebop_driver -#endif // BEBOP_AUTONOMY_AUTOGENERATED_Ardrone3_SETTING_CALLBACKS_H +#endif // BEBOP_AUTONOMY_AUTOGENERATED_Ardrone3_SETTING_CALLBACKS_H \ No newline at end of file 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..feb9823 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/bluecamel/arsdk-xml/53ed05daac60e1ff8391f09c8b6eb06c18a992b6/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_state_callbacks.h b/bebop_driver/include/bebop_driver/autogenerated/ardrone3_state_callbacks.h index 87c63cc..6f0c801 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/bluecamel/arsdk-xml/53ed05daac60e1ff8391f09c8b6eb06c18a992b6/xml/ardrone3.xml * Do not modify this file by hand. Check scripts/meta folder for generator files. */ @@ -2143,7 +2143,6 @@ class Ardrone3AccessoryStateConnectedAccessories : public AbstractState msg_ptr->swVersion = arg->value.String; } - if (pub_enabled_) ros_pub_.publish(msg_ptr); } }; // Ardrone3AccessoryStateConnectedAccessories diff --git a/bebop_driver/include/bebop_driver/autogenerated/ardrone_state_callbacks.h b/bebop_driver/include/bebop_driver/autogenerated/ardrone_state_callbacks.h new file mode 100644 index 0000000..e69de29 diff --git a/bebop_driver/include/bebop_driver/autogenerated/callbacks_common.h b/bebop_driver/include/bebop_driver/autogenerated/callbacks_common.h index e67f3b7..e1f6a24 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/bluecamel/arsdk-xml/53ed05daac60e1ff8391f09c8b6eb06c18a992b6/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..021bd96 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/bluecamel/arsdk-xml/53ed05daac60e1ff8391f09c8b6eb06c18a992b6/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..a349139 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/bluecamel/arsdk-xml/53ed05daac60e1ff8391f09c8b6eb06c18a992b6/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/scripts/meta/fix_references.sh b/bebop_driver/scripts/meta/fix_references.sh new file mode 100755 index 0000000..429d657 --- /dev/null +++ b/bebop_driver/scripts/meta/fix_references.sh @@ -0,0 +1,29 @@ +#!/usr/bin/env bash + +# fix ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_SETHOMECHANGED_ALTITUDE/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_HOMECHANGED_ALTITUDE/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_SETHOMECHANGED_LONGITUDE/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_HOMECHANGED_LONGITUDE/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_SETHOMECHANGED_LATITUDE/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_HOMECHANGED_LATITUDE/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_SETHOMECHANGED/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_HOMECHANGED/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_SETHOMECHANGED/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_HOMECHANGED/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_TIMELAPSESELECTIONCHANGED_INTERVAL/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_TIMELAPSECHANGED_INTERVAL/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_TIMELAPSESELECTIONCHANGED_ENABLED/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_TIMELAPSECHANGED_ENABLED/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_TIMELAPSESELECTIONCHANGED/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_TIMELAPSECHANGED/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_SATURATIONSELECTIONCHANGED_VALUE/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_SATURATIONCHANGED_VALUE/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_SATURATIONSELECTIONCHANGED/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_SATURATIONCHANGED/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONSELECTIONCHANGED_VALUE/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED_VALUE/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONSELECTIONCHANGED/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_EXPOSITIONCHANGED/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_AUTOWHITEBALANCESELECTIONCHANGED_TYPE/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_AUTOWHITEBALANCECHANGED_TYPE/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_AUTOWHITEBALANCESELECTIONCHANGED/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_AUTOWHITEBALANCECHANGED/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_PICTUREFORMATSELECTIONCHANGED_TYPE/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_PICTUREFORMATCHANGED_TYPE/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_PICTUREFORMATSELECTIONCHANGED/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_PICTUREFORMATCHANGED/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGALTITUDECHANGED_VALUE/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGALTITUDECHANGED_CURRENT/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGRADIUSCHANGED_VALUE/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGRADIUSCHANGED_CURRENT/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_BANKEDTURNCHANGED_VALUE/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_BANKEDTURNCHANGED_STATE/g' ardrone3_setting_callbacks.h +sed -i 's/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXDISTANCECHANGED_VALUE/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXDISTANCECHANGED_CURRENT/g' ardrone3_setting_callbacks.h + +# fix ardrone3_state_callbacks.h +ex -c 'g/ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_CONNECTEDACCESSORIES_LIST_FLAGS/?NULL?,/publish/d' -c 'wq' ardrone3_state_callbacks.h + +# fix Ardrone3AccessoryStateConnectedAccessories.msg +ex -c 'g/list_flags/?List?,/list_flags/d' -c 'wq' Ardrone3AccessoryStateConnectedAccessories.msg diff --git a/bebop_driver/scripts/meta/generate.py b/bebop_driver/scripts/meta/generate.py index bf4d647..2d66da8 100755 --- a/bebop_driver/scripts/meta/generate.py +++ b/bebop_driver/scripts/meta/generate.py @@ -13,8 +13,8 @@ import urllib2 # SDK 3.12.6: https://github.com/Parrot-Developers/arsdk_manifests/blob/d7640c80ed7147971995222d9f4655932a904aa8/release.xml -LIBARCOMMANDS_GIT_OWNER = "Parrot-Developers" -LIBARCOMMANDS_GIT_HASH = "ab28dab91845cd36c4d7002b55f70805deaff3c8" +LIBARCOMMANDS_GIT_OWNER = "bluecamel" +LIBARCOMMANDS_GIT_HASH = "53ed05daac60e1ff8391f09c8b6eb06c18a992b6" # From XML types to ROS primitive types ROS_TYPE_MAP = { diff --git a/bebop_driver/scripts/meta/install.sh b/bebop_driver/scripts/meta/install.sh index 90b4bad..5a3c11e 100755 --- a/bebop_driver/scripts/meta/install.sh +++ b/bebop_driver/scripts/meta/install.sh @@ -4,6 +4,8 @@ echo "Removing old .msg files ..." rm -f ../../../bebop_msgs/msg/autogenerated/*.msg echo "Installing .msg files ..." cp *.msg ../../../bebop_msgs/msg/autogenerated +echo "Fixing references ..." +./fix_references.sh echo "Removing old .h files ..." rm -f ../../../bebop_driver/include/bebop_driver/autogenerated/*.h echo "Installing .h files .." diff --git a/bebop_driver/scripts/meta/last_build_info b/bebop_driver/scripts/meta/last_build_info index 17c91d4..844b688 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 53ed05daac60e1ff8391f09c8b6eb06c18a992b6 +- Date 2020-05-17 00:27:33.210386 +- Generator generate.py @ b0cf0a7 diff --git a/bebop_msgs/msg/autogenerated/last_build_info b/bebop_msgs/msg/autogenerated/last_build_info index 17c91d4..844b688 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 53ed05daac60e1ff8391f09c8b6eb06c18a992b6 +- Date 2020-05-17 00:27:33.210386 +- Generator generate.py @ b0cf0a7 diff --git a/docs/autogenerated/ardrone3_settings_param.rst b/docs/autogenerated/ardrone3_settings_param.rst index 84aca69..e79791a 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/bluecamel/arsdk-xml/53ed05daac60e1ff8391f09c8b6eb06c18a992b6/xml/ardrone3.xml .. Do not modify this file by hand. Check scripts/meta folder for generator files. ***************************************************************************************** @@ -59,6 +59,18 @@ List of Ardrone3 Settings and Corresponding ROS Parameter `picturesettings`_ Photo settings chosen by the user + `PictureSettingsPictureFormatSelectionType`_ + The type of photo format + `PictureSettingsAutoWhiteBalanceSelectionType`_ + The type auto white balance + `PictureSettingsExpositionSelectionValue`_ + Exposition value (bounds given by ExpositionChanged arg min and max, by default [-3:3]) + `PictureSettingsSaturationSelectionValue`_ + Saturation value (bounds given by SaturationChanged arg min and max, by default [-100:100]) + `PictureSettingsTimelapseSelectionEnabled`_ + 1 if timelapse is enabled, 0 otherwise + `PictureSettingsTimelapseSelectionInterval`_ + interval in seconds for taking pictures `PictureSettingsVideoStabilizationModeMode`_ Video stabilization mode `PictureSettingsVideoRecordingModeMode`_ @@ -71,6 +83,12 @@ List of Ardrone3 Settings and Corresponding ROS Parameter `gpssettings`_ GPS settings + `GPSSettingsSetHomeLatitude`_ + Home latitude in decimal degrees + `GPSSettingsSetHomeLongitude`_ + Home longitude in decimal degrees + `GPSSettingsSetHomeAltitude`_ + Home altitude in meters `GPSSettingsHomeTypeType`_ The type of the home position `GPSSettingsReturnHomeDelayDelay`_ @@ -217,6 +235,53 @@ NetworkSettingsWifiSelectionChannel * Type: ``int_t`` picturesettings =========================================================== +PictureSettingsPictureFormatSelectionType +----------------------------------------------------------- +* Parameter: ``~PictureSettingsPictureFormatSelectionType`` +* Details: The type of photo format +* Type: ``int_t`` + + * 0: Take raw image + * 1: Take a 4:3 jpeg photo + * 2: Take a 16:9 snapshot from camera + * 3: Take jpeg fisheye image only + +PictureSettingsAutoWhiteBalanceSelectionType +----------------------------------------------------------- +* Parameter: ``~PictureSettingsAutoWhiteBalanceSelectionType`` +* Details: The type auto white balance +* Type: ``int_t`` + + * 0: Auto guess of best white balance params + * 1: Tungsten white balance + * 2: Daylight white balance + * 3: Cloudy white balance + * 4: White balance for a flash + +PictureSettingsExpositionSelectionValue +----------------------------------------------------------- +* Parameter: ``~PictureSettingsExpositionSelectionValue`` +* Details: Exposition value (bounds given by ExpositionChanged arg min and max, by default [-3:3]) +* Type: ``double_t`` +PictureSettingsSaturationSelectionValue +----------------------------------------------------------- +* Parameter: ``~PictureSettingsSaturationSelectionValue`` +* Details: Saturation value (bounds given by SaturationChanged arg min and max, by default [-100:100]) +* Type: ``double_t`` +PictureSettingsTimelapseSelectionEnabled +----------------------------------------------------------- +* Parameter: ``~PictureSettingsTimelapseSelectionEnabled`` +* Details: 1 if timelapse is enabled, 0 otherwise +* Type: ``int_t`` + + * 0: Disabled + * 1: Enabled + +PictureSettingsTimelapseSelectionInterval +----------------------------------------------------------- +* Parameter: ``~PictureSettingsTimelapseSelectionInterval`` +* Details: interval in seconds for taking pictures +* Type: ``double_t`` PictureSettingsVideoStabilizationModeMode ----------------------------------------------------------- * Parameter: ``~PictureSettingsVideoStabilizationModeMode`` @@ -258,6 +323,21 @@ PictureSettingsVideoResolutionsType gpssettings =========================================================== +GPSSettingsSetHomeLatitude +----------------------------------------------------------- +* Parameter: ``~GPSSettingsSetHomeLatitude`` +* Details: Home latitude in decimal degrees +* Type: ``double_t`` +GPSSettingsSetHomeLongitude +----------------------------------------------------------- +* Parameter: ``~GPSSettingsSetHomeLongitude`` +* Details: Home longitude in decimal degrees +* Type: ``double_t`` +GPSSettingsSetHomeAltitude +----------------------------------------------------------- +* Parameter: ``~GPSSettingsSetHomeAltitude`` +* Details: Home altitude in meters +* Type: ``double_t`` GPSSettingsHomeTypeType ----------------------------------------------------------- * Parameter: ``~GPSSettingsHomeTypeType`` diff --git a/docs/autogenerated/ardrone3_states_param_topic.rst b/docs/autogenerated/ardrone3_states_param_topic.rst index bfc8ca2..e37dda5 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/bluecamel/arsdk-xml/53ed05daac60e1ff8391f09c8b6eb06c18a992b6/xml/ardrone3.xml .. Do not modify this file by hand. Check scripts/meta folder for generator files. ***************************************************************************************** diff --git a/docs/autogenerated/common_states_param_topic.rst b/docs/autogenerated/common_states_param_topic.rst index 7ad70c4..dd9a955 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/bluecamel/arsdk-xml/53ed05daac60e1ff8391f09c8b6eb06c18a992b6/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..dbd9d60 100644 --- a/docs/dev.rst +++ b/docs/dev.rst @@ -61,26 +61,4 @@ Upgrading the Bebop SDK 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``. -6. In ``bebop_driver/include/bebop_driver/autogenerated/ardrone3_setting_callbacks.h`` change the following varialbles: - - - ``ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXDISTANCECHANGED_VALUE`` to ``ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXDISTANCECHANGED_CURRENT``. - - ``ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_BANKEDTURNCHANGED_VALUE`` to ``ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_BANKEDTURNCHANGED_STATE`` - - ``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: - - ``arg = NULL; - HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_ACCESSORYSTATE_CONNECTEDACCESSORIES_LIST_FLAGS, arg); - if (arg) - { - 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`` - -These changes are required because the upstream XML file is inconsistent for a couple of variable names. - -7. Remove ``build`` and ``devel`` space of your ``catkin`` workspace, then re-build it. +6. Remove ``build`` and ``devel`` space of your ``catkin`` workspace, then re-build it.