00001
00029 #ifndef BEBOP_AUTONOMY_AUTOGENERATED_Ardrone3_SETTING_CALLBACKS_H
00030 #define BEBOP_AUTONOMY_AUTOGENERATED_Ardrone3_SETTING_CALLBACKS_H
00031
00032 extern "C"
00033 {
00034 #include "libARSAL/ARSAL.h"
00035 #include "libARController/ARController.h"
00036 }
00037
00038 #include "bebop_driver/autogenerated/callbacks_common.h"
00039 #include "bebop_driver/BebopArdrone3Config.h"
00040
00041 namespace bebop_driver
00042 {
00043 namespace cb
00044 {
00045
00046 class PilotingSettingsMaxAltitude : public AbstractSetting
00047 {
00048 private:
00049 double PilotingSettingsMaxAltitudeCurrent_bebop_value_;
00050 bool PilotingSettingsMaxAltitudeCurrent_bebop_sent_;
00051
00052 public:
00053 explicit PilotingSettingsMaxAltitude(ros::NodeHandle& priv_nh)
00054 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXALTITUDECHANGED, priv_nh)
00055 , PilotingSettingsMaxAltitudeCurrent_bebop_sent_(false)
00056 {}
00057
00058
00059 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
00060 {
00061 ::boost::lock_guard<boost::mutex> lock(mutex_);
00062 bool changed = false;
00063 bool all_inited = true;
00064
00065
00066 if ((config.PilotingSettingsMaxAltitudeCurrent != PilotingSettingsMaxAltitudeCurrent_bebop_value_))
00067 {
00068 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00069 "PilotingSettingsMaxAltitudeCurrent changed!");
00070 changed = true;
00071 }
00072 all_inited &= PilotingSettingsMaxAltitudeCurrent_bebop_sent_;
00073
00074 if (changed && !all_inited)
00075 {
00076 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
00077 "Value of PilotingSettingsMaxAltitude was not initialized either by Bebop or Params.");
00078 }
00079
00080 if (changed && all_inited)
00081 {
00082 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00083 "Sending PilotingSettingsMaxAltitude changes to bebop");
00084
00085 bebop_ctrl_ptr_->aRDrone3->sendPilotingSettingsMaxAltitude(bebop_ctrl_ptr_->aRDrone3
00086 , (config.PilotingSettingsMaxAltitudeCurrent)
00087 );
00088 }
00089 }
00090
00091
00092 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
00093 {
00094 if (arguments == NULL)
00095 {
00096 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
00097 "PilotingSettingsMaxAltitude::Update() arguments is NULL");
00098 return;
00099 }
00100
00101 ::boost::lock_guard<boost::mutex> lock(mutex_);
00102
00103 arg = NULL;
00104 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXALTITUDECHANGED_CURRENT, arg);
00105 if (arg)
00106 {
00107 PilotingSettingsMaxAltitudeCurrent_bebop_sent_ = true;
00108 ROS_INFO_STREAM("Value for PilotingSettingsMaxAltitudeCurrent recved: " << static_cast<double>(arg->value.Float));
00109 PilotingSettingsMaxAltitudeCurrent_bebop_value_ = static_cast<double>(arg->value.Float);
00110
00111 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PilotingSettingsMaxAltitudeCurrent exists in params ...");
00112 if (!priv_nh_.hasParam("PilotingSettingsMaxAltitudeCurrent"))
00113 {
00114 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
00115 priv_nh_.setParam("PilotingSettingsMaxAltitudeCurrent", static_cast<double>(arg->value.Float));
00116 }
00117 else
00118 {
00119 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
00120
00121 }
00122 }
00123 }
00124 };
00125
00126 class PilotingSettingsMaxTilt : public AbstractSetting
00127 {
00128 private:
00129 double PilotingSettingsMaxTiltCurrent_bebop_value_;
00130 bool PilotingSettingsMaxTiltCurrent_bebop_sent_;
00131
00132 public:
00133 explicit PilotingSettingsMaxTilt(ros::NodeHandle& priv_nh)
00134 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED, priv_nh)
00135 , PilotingSettingsMaxTiltCurrent_bebop_sent_(false)
00136 {}
00137
00138
00139 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
00140 {
00141 ::boost::lock_guard<boost::mutex> lock(mutex_);
00142 bool changed = false;
00143 bool all_inited = true;
00144
00145
00146 if ((config.PilotingSettingsMaxTiltCurrent != PilotingSettingsMaxTiltCurrent_bebop_value_))
00147 {
00148 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00149 "PilotingSettingsMaxTiltCurrent changed!");
00150 changed = true;
00151 }
00152 all_inited &= PilotingSettingsMaxTiltCurrent_bebop_sent_;
00153
00154 if (changed && !all_inited)
00155 {
00156 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
00157 "Value of PilotingSettingsMaxTilt was not initialized either by Bebop or Params.");
00158 }
00159
00160 if (changed && all_inited)
00161 {
00162 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00163 "Sending PilotingSettingsMaxTilt changes to bebop");
00164
00165 bebop_ctrl_ptr_->aRDrone3->sendPilotingSettingsMaxTilt(bebop_ctrl_ptr_->aRDrone3
00166 , (config.PilotingSettingsMaxTiltCurrent)
00167 );
00168 }
00169 }
00170
00171
00172 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
00173 {
00174 if (arguments == NULL)
00175 {
00176 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
00177 "PilotingSettingsMaxTilt::Update() arguments is NULL");
00178 return;
00179 }
00180
00181 ::boost::lock_guard<boost::mutex> lock(mutex_);
00182
00183 arg = NULL;
00184 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXTILTCHANGED_CURRENT, arg);
00185 if (arg)
00186 {
00187 PilotingSettingsMaxTiltCurrent_bebop_sent_ = true;
00188 ROS_INFO_STREAM("Value for PilotingSettingsMaxTiltCurrent recved: " << static_cast<double>(arg->value.Float));
00189 PilotingSettingsMaxTiltCurrent_bebop_value_ = static_cast<double>(arg->value.Float);
00190
00191 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PilotingSettingsMaxTiltCurrent exists in params ...");
00192 if (!priv_nh_.hasParam("PilotingSettingsMaxTiltCurrent"))
00193 {
00194 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
00195 priv_nh_.setParam("PilotingSettingsMaxTiltCurrent", static_cast<double>(arg->value.Float));
00196 }
00197 else
00198 {
00199 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
00200
00201 }
00202 }
00203 }
00204 };
00205
00206 class PilotingSettingsAbsolutControl : public AbstractSetting
00207 {
00208 private:
00209 int32_t PilotingSettingsAbsolutControlOn_bebop_value_;
00210 bool PilotingSettingsAbsolutControlOn_bebop_sent_;
00211
00212 public:
00213 explicit PilotingSettingsAbsolutControl(ros::NodeHandle& priv_nh)
00214 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_ABSOLUTCONTROLCHANGED, priv_nh)
00215 , PilotingSettingsAbsolutControlOn_bebop_sent_(false)
00216 {}
00217
00218
00219 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
00220 {
00221 ::boost::lock_guard<boost::mutex> lock(mutex_);
00222 bool changed = false;
00223 bool all_inited = true;
00224
00225
00226 if ((config.PilotingSettingsAbsolutControlOn != PilotingSettingsAbsolutControlOn_bebop_value_))
00227 {
00228 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00229 "PilotingSettingsAbsolutControlOn changed!");
00230 changed = true;
00231 }
00232 all_inited &= PilotingSettingsAbsolutControlOn_bebop_sent_;
00233
00234 if (changed && !all_inited)
00235 {
00236 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
00237 "Value of PilotingSettingsAbsolutControl was not initialized either by Bebop or Params.");
00238 }
00239
00240 if (changed && all_inited)
00241 {
00242 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00243 "Sending PilotingSettingsAbsolutControl changes to bebop");
00244
00245 bebop_ctrl_ptr_->aRDrone3->sendPilotingSettingsAbsolutControl(bebop_ctrl_ptr_->aRDrone3
00246 , (config.PilotingSettingsAbsolutControlOn)
00247 );
00248 }
00249 }
00250
00251
00252 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
00253 {
00254 if (arguments == NULL)
00255 {
00256 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
00257 "PilotingSettingsAbsolutControl::Update() arguments is NULL");
00258 return;
00259 }
00260
00261 ::boost::lock_guard<boost::mutex> lock(mutex_);
00262
00263 arg = NULL;
00264 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_ABSOLUTCONTROLCHANGED_ON, arg);
00265 if (arg)
00266 {
00267 PilotingSettingsAbsolutControlOn_bebop_sent_ = true;
00268 ROS_INFO_STREAM("Value for PilotingSettingsAbsolutControlOn recved: " << static_cast<int32_t>(arg->value.U8));
00269 PilotingSettingsAbsolutControlOn_bebop_value_ = static_cast<int32_t>(arg->value.U8);
00270
00271 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PilotingSettingsAbsolutControlOn exists in params ...");
00272 if (!priv_nh_.hasParam("PilotingSettingsAbsolutControlOn"))
00273 {
00274 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
00275 priv_nh_.setParam("PilotingSettingsAbsolutControlOn", static_cast<int32_t>(arg->value.U8));
00276 }
00277 else
00278 {
00279 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
00280
00281 }
00282 }
00283 }
00284 };
00285
00286 class PilotingSettingsMaxDistance : public AbstractSetting
00287 {
00288 private:
00289 double PilotingSettingsMaxDistanceValue_bebop_value_;
00290 bool PilotingSettingsMaxDistanceValue_bebop_sent_;
00291
00292 public:
00293 explicit PilotingSettingsMaxDistance(ros::NodeHandle& priv_nh)
00294 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXDISTANCECHANGED, priv_nh)
00295 , PilotingSettingsMaxDistanceValue_bebop_sent_(false)
00296 {}
00297
00298
00299 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
00300 {
00301 ::boost::lock_guard<boost::mutex> lock(mutex_);
00302 bool changed = false;
00303 bool all_inited = true;
00304
00305
00306 if ((config.PilotingSettingsMaxDistanceValue != PilotingSettingsMaxDistanceValue_bebop_value_))
00307 {
00308 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00309 "PilotingSettingsMaxDistanceValue changed!");
00310 changed = true;
00311 }
00312 all_inited &= PilotingSettingsMaxDistanceValue_bebop_sent_;
00313
00314 if (changed && !all_inited)
00315 {
00316 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
00317 "Value of PilotingSettingsMaxDistance was not initialized either by Bebop or Params.");
00318 }
00319
00320 if (changed && all_inited)
00321 {
00322 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00323 "Sending PilotingSettingsMaxDistance changes to bebop");
00324
00325 bebop_ctrl_ptr_->aRDrone3->sendPilotingSettingsMaxDistance(bebop_ctrl_ptr_->aRDrone3
00326 , (config.PilotingSettingsMaxDistanceValue)
00327 );
00328 }
00329 }
00330
00331
00332 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
00333 {
00334 if (arguments == NULL)
00335 {
00336 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
00337 "PilotingSettingsMaxDistance::Update() arguments is NULL");
00338 return;
00339 }
00340
00341 ::boost::lock_guard<boost::mutex> lock(mutex_);
00342
00343 arg = NULL;
00344 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MAXDISTANCECHANGED_CURRENT, arg);
00345 if (arg)
00346 {
00347 PilotingSettingsMaxDistanceValue_bebop_sent_ = true;
00348 ROS_INFO_STREAM("Value for PilotingSettingsMaxDistanceValue recved: " << static_cast<double>(arg->value.Float));
00349 PilotingSettingsMaxDistanceValue_bebop_value_ = static_cast<double>(arg->value.Float);
00350
00351 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PilotingSettingsMaxDistanceValue exists in params ...");
00352 if (!priv_nh_.hasParam("PilotingSettingsMaxDistanceValue"))
00353 {
00354 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
00355 priv_nh_.setParam("PilotingSettingsMaxDistanceValue", static_cast<double>(arg->value.Float));
00356 }
00357 else
00358 {
00359 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
00360
00361 }
00362 }
00363 }
00364 };
00365
00366 class PilotingSettingsNoFlyOverMaxDistance : public AbstractSetting
00367 {
00368 private:
00369 int32_t PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover_bebop_value_;
00370 bool PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover_bebop_sent_;
00371
00372 public:
00373 explicit PilotingSettingsNoFlyOverMaxDistance(ros::NodeHandle& priv_nh)
00374 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_NOFLYOVERMAXDISTANCECHANGED, priv_nh)
00375 , PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover_bebop_sent_(false)
00376 {}
00377
00378
00379 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
00380 {
00381 ::boost::lock_guard<boost::mutex> lock(mutex_);
00382 bool changed = false;
00383 bool all_inited = true;
00384
00385
00386 if ((config.PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover != PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover_bebop_value_))
00387 {
00388 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00389 "PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover changed!");
00390 changed = true;
00391 }
00392 all_inited &= PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover_bebop_sent_;
00393
00394 if (changed && !all_inited)
00395 {
00396 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
00397 "Value of PilotingSettingsNoFlyOverMaxDistance was not initialized either by Bebop or Params.");
00398 }
00399
00400 if (changed && all_inited)
00401 {
00402 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00403 "Sending PilotingSettingsNoFlyOverMaxDistance changes to bebop");
00404
00405 bebop_ctrl_ptr_->aRDrone3->sendPilotingSettingsNoFlyOverMaxDistance(bebop_ctrl_ptr_->aRDrone3
00406 , (config.PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover)
00407 );
00408 }
00409 }
00410
00411
00412 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
00413 {
00414 if (arguments == NULL)
00415 {
00416 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
00417 "PilotingSettingsNoFlyOverMaxDistance::Update() arguments is NULL");
00418 return;
00419 }
00420
00421 ::boost::lock_guard<boost::mutex> lock(mutex_);
00422
00423 arg = NULL;
00424 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_NOFLYOVERMAXDISTANCECHANGED_SHOULDNOTFLYOVER, arg);
00425 if (arg)
00426 {
00427 PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover_bebop_sent_ = true;
00428 ROS_INFO_STREAM("Value for PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover recved: " << static_cast<int32_t>(arg->value.U8));
00429 PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover_bebop_value_ = static_cast<int32_t>(arg->value.U8);
00430
00431 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover exists in params ...");
00432 if (!priv_nh_.hasParam("PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover"))
00433 {
00434 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
00435 priv_nh_.setParam("PilotingSettingsNoFlyOverMaxDistanceShouldnotflyover", static_cast<int32_t>(arg->value.U8));
00436 }
00437 else
00438 {
00439 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
00440
00441 }
00442 }
00443 }
00444 };
00445
00446 class PilotingSettingsBankedTurn : public AbstractSetting
00447 {
00448 private:
00449 int32_t PilotingSettingsBankedTurnValue_bebop_value_;
00450 bool PilotingSettingsBankedTurnValue_bebop_sent_;
00451
00452 public:
00453 explicit PilotingSettingsBankedTurn(ros::NodeHandle& priv_nh)
00454 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_BANKEDTURNCHANGED, priv_nh)
00455 , PilotingSettingsBankedTurnValue_bebop_sent_(false)
00456 {}
00457
00458
00459 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
00460 {
00461 ::boost::lock_guard<boost::mutex> lock(mutex_);
00462 bool changed = false;
00463 bool all_inited = true;
00464
00465
00466 if ((config.PilotingSettingsBankedTurnValue != PilotingSettingsBankedTurnValue_bebop_value_))
00467 {
00468 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00469 "PilotingSettingsBankedTurnValue changed!");
00470 changed = true;
00471 }
00472 all_inited &= PilotingSettingsBankedTurnValue_bebop_sent_;
00473
00474 if (changed && !all_inited)
00475 {
00476 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
00477 "Value of PilotingSettingsBankedTurn was not initialized either by Bebop or Params.");
00478 }
00479
00480 if (changed && all_inited)
00481 {
00482 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00483 "Sending PilotingSettingsBankedTurn changes to bebop");
00484
00485 bebop_ctrl_ptr_->aRDrone3->sendPilotingSettingsBankedTurn(bebop_ctrl_ptr_->aRDrone3
00486 , (config.PilotingSettingsBankedTurnValue)
00487 );
00488 }
00489 }
00490
00491
00492 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
00493 {
00494 if (arguments == NULL)
00495 {
00496 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
00497 "PilotingSettingsBankedTurn::Update() arguments is NULL");
00498 return;
00499 }
00500
00501 ::boost::lock_guard<boost::mutex> lock(mutex_);
00502
00503 arg = NULL;
00504 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_BANKEDTURNCHANGED_STATE, arg);
00505 if (arg)
00506 {
00507 PilotingSettingsBankedTurnValue_bebop_sent_ = true;
00508 ROS_INFO_STREAM("Value for PilotingSettingsBankedTurnValue recved: " << static_cast<int32_t>(arg->value.U8));
00509 PilotingSettingsBankedTurnValue_bebop_value_ = static_cast<int32_t>(arg->value.U8);
00510
00511 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PilotingSettingsBankedTurnValue exists in params ...");
00512 if (!priv_nh_.hasParam("PilotingSettingsBankedTurnValue"))
00513 {
00514 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
00515 priv_nh_.setParam("PilotingSettingsBankedTurnValue", static_cast<int32_t>(arg->value.U8));
00516 }
00517 else
00518 {
00519 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
00520
00521 }
00522 }
00523 }
00524 };
00525
00526 class PilotingSettingsMinAltitude : public AbstractSetting
00527 {
00528 private:
00529 double PilotingSettingsMinAltitudeCurrent_bebop_value_;
00530 bool PilotingSettingsMinAltitudeCurrent_bebop_sent_;
00531
00532 public:
00533 explicit PilotingSettingsMinAltitude(ros::NodeHandle& priv_nh)
00534 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MINALTITUDECHANGED, priv_nh)
00535 , PilotingSettingsMinAltitudeCurrent_bebop_sent_(false)
00536 {}
00537
00538
00539 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
00540 {
00541 ::boost::lock_guard<boost::mutex> lock(mutex_);
00542 bool changed = false;
00543 bool all_inited = true;
00544
00545
00546 if ((config.PilotingSettingsMinAltitudeCurrent != PilotingSettingsMinAltitudeCurrent_bebop_value_))
00547 {
00548 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00549 "PilotingSettingsMinAltitudeCurrent changed!");
00550 changed = true;
00551 }
00552 all_inited &= PilotingSettingsMinAltitudeCurrent_bebop_sent_;
00553
00554 if (changed && !all_inited)
00555 {
00556 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
00557 "Value of PilotingSettingsMinAltitude was not initialized either by Bebop or Params.");
00558 }
00559
00560 if (changed && all_inited)
00561 {
00562 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00563 "Sending PilotingSettingsMinAltitude changes to bebop");
00564
00565 bebop_ctrl_ptr_->aRDrone3->sendPilotingSettingsMinAltitude(bebop_ctrl_ptr_->aRDrone3
00566 , (config.PilotingSettingsMinAltitudeCurrent)
00567 );
00568 }
00569 }
00570
00571
00572 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
00573 {
00574 if (arguments == NULL)
00575 {
00576 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
00577 "PilotingSettingsMinAltitude::Update() arguments is NULL");
00578 return;
00579 }
00580
00581 ::boost::lock_guard<boost::mutex> lock(mutex_);
00582
00583 arg = NULL;
00584 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_MINALTITUDECHANGED_CURRENT, arg);
00585 if (arg)
00586 {
00587 PilotingSettingsMinAltitudeCurrent_bebop_sent_ = true;
00588 ROS_INFO_STREAM("Value for PilotingSettingsMinAltitudeCurrent recved: " << static_cast<double>(arg->value.Float));
00589 PilotingSettingsMinAltitudeCurrent_bebop_value_ = static_cast<double>(arg->value.Float);
00590
00591 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PilotingSettingsMinAltitudeCurrent exists in params ...");
00592 if (!priv_nh_.hasParam("PilotingSettingsMinAltitudeCurrent"))
00593 {
00594 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
00595 priv_nh_.setParam("PilotingSettingsMinAltitudeCurrent", static_cast<double>(arg->value.Float));
00596 }
00597 else
00598 {
00599 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
00600
00601 }
00602 }
00603 }
00604 };
00605
00606 class PilotingSettingsCirclingDirection : public AbstractSetting
00607 {
00608 private:
00609 int32_t PilotingSettingsCirclingDirectionValue_bebop_value_;
00610 bool PilotingSettingsCirclingDirectionValue_bebop_sent_;
00611
00612 public:
00613 explicit PilotingSettingsCirclingDirection(ros::NodeHandle& priv_nh)
00614 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGDIRECTIONCHANGED, priv_nh)
00615 , PilotingSettingsCirclingDirectionValue_bebop_sent_(false)
00616 {}
00617
00618
00619 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
00620 {
00621 ::boost::lock_guard<boost::mutex> lock(mutex_);
00622 bool changed = false;
00623 bool all_inited = true;
00624
00625
00626 if ((config.PilotingSettingsCirclingDirectionValue != PilotingSettingsCirclingDirectionValue_bebop_value_))
00627 {
00628 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00629 "PilotingSettingsCirclingDirectionValue changed!");
00630 changed = true;
00631 }
00632 all_inited &= PilotingSettingsCirclingDirectionValue_bebop_sent_;
00633
00634 if (changed && !all_inited)
00635 {
00636 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
00637 "Value of PilotingSettingsCirclingDirection was not initialized either by Bebop or Params.");
00638 }
00639
00640 if (changed && all_inited)
00641 {
00642 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00643 "Sending PilotingSettingsCirclingDirection changes to bebop");
00644
00645 bebop_ctrl_ptr_->aRDrone3->sendPilotingSettingsCirclingDirection(bebop_ctrl_ptr_->aRDrone3
00646 , static_cast<eARCOMMANDS_ARDRONE3_PILOTINGSETTINGS_CIRCLINGDIRECTION_VALUE>(config.PilotingSettingsCirclingDirectionValue)
00647 );
00648 }
00649 }
00650
00651
00652 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
00653 {
00654 if (arguments == NULL)
00655 {
00656 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
00657 "PilotingSettingsCirclingDirection::Update() arguments is NULL");
00658 return;
00659 }
00660
00661 ::boost::lock_guard<boost::mutex> lock(mutex_);
00662
00663 arg = NULL;
00664 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGDIRECTIONCHANGED_VALUE, arg);
00665 if (arg)
00666 {
00667 PilotingSettingsCirclingDirectionValue_bebop_sent_ = true;
00668 ROS_INFO_STREAM("Value for PilotingSettingsCirclingDirectionValue recved: " << static_cast<int32_t>(arg->value.I32));
00669 PilotingSettingsCirclingDirectionValue_bebop_value_ = static_cast<int32_t>(arg->value.I32);
00670
00671 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PilotingSettingsCirclingDirectionValue exists in params ...");
00672 if (!priv_nh_.hasParam("PilotingSettingsCirclingDirectionValue"))
00673 {
00674 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
00675 priv_nh_.setParam("PilotingSettingsCirclingDirectionValue", static_cast<int32_t>(arg->value.I32));
00676 }
00677 else
00678 {
00679 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
00680
00681 }
00682 }
00683 }
00684 };
00685
00686 class PilotingSettingsCirclingRadius : public AbstractSetting
00687 {
00688 private:
00689 int32_t PilotingSettingsCirclingRadiusValue_bebop_value_;
00690 bool PilotingSettingsCirclingRadiusValue_bebop_sent_;
00691
00692 public:
00693 explicit PilotingSettingsCirclingRadius(ros::NodeHandle& priv_nh)
00694 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGRADIUSCHANGED, priv_nh)
00695 , PilotingSettingsCirclingRadiusValue_bebop_sent_(false)
00696 {}
00697
00698
00699 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
00700 {
00701 ::boost::lock_guard<boost::mutex> lock(mutex_);
00702 bool changed = false;
00703 bool all_inited = true;
00704
00705
00706 if ((config.PilotingSettingsCirclingRadiusValue != PilotingSettingsCirclingRadiusValue_bebop_value_))
00707 {
00708 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00709 "PilotingSettingsCirclingRadiusValue changed!");
00710 changed = true;
00711 }
00712 all_inited &= PilotingSettingsCirclingRadiusValue_bebop_sent_;
00713
00714 if (changed && !all_inited)
00715 {
00716 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
00717 "Value of PilotingSettingsCirclingRadius was not initialized either by Bebop or Params.");
00718 }
00719
00720 if (changed && all_inited)
00721 {
00722 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00723 "Sending PilotingSettingsCirclingRadius changes to bebop");
00724
00725 bebop_ctrl_ptr_->aRDrone3->sendPilotingSettingsCirclingRadius(bebop_ctrl_ptr_->aRDrone3
00726 , (config.PilotingSettingsCirclingRadiusValue)
00727 );
00728 }
00729 }
00730
00731
00732 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
00733 {
00734 if (arguments == NULL)
00735 {
00736 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
00737 "PilotingSettingsCirclingRadius::Update() arguments is NULL");
00738 return;
00739 }
00740
00741 ::boost::lock_guard<boost::mutex> lock(mutex_);
00742
00743 arg = NULL;
00744 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGRADIUSCHANGED_CURRENT, arg);
00745 if (arg)
00746 {
00747 PilotingSettingsCirclingRadiusValue_bebop_sent_ = true;
00748 ROS_INFO_STREAM("Value for PilotingSettingsCirclingRadiusValue recved: " << static_cast<int32_t>(arg->value.U16));
00749 PilotingSettingsCirclingRadiusValue_bebop_value_ = static_cast<int32_t>(arg->value.U16);
00750
00751 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PilotingSettingsCirclingRadiusValue exists in params ...");
00752 if (!priv_nh_.hasParam("PilotingSettingsCirclingRadiusValue"))
00753 {
00754 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
00755 priv_nh_.setParam("PilotingSettingsCirclingRadiusValue", static_cast<int32_t>(arg->value.U16));
00756 }
00757 else
00758 {
00759 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
00760
00761 }
00762 }
00763 }
00764 };
00765
00766 class PilotingSettingsCirclingAltitude : public AbstractSetting
00767 {
00768 private:
00769 int32_t PilotingSettingsCirclingAltitudeValue_bebop_value_;
00770 bool PilotingSettingsCirclingAltitudeValue_bebop_sent_;
00771
00772 public:
00773 explicit PilotingSettingsCirclingAltitude(ros::NodeHandle& priv_nh)
00774 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGALTITUDECHANGED, priv_nh)
00775 , PilotingSettingsCirclingAltitudeValue_bebop_sent_(false)
00776 {}
00777
00778
00779 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
00780 {
00781 ::boost::lock_guard<boost::mutex> lock(mutex_);
00782 bool changed = false;
00783 bool all_inited = true;
00784
00785
00786 if ((config.PilotingSettingsCirclingAltitudeValue != PilotingSettingsCirclingAltitudeValue_bebop_value_))
00787 {
00788 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00789 "PilotingSettingsCirclingAltitudeValue changed!");
00790 changed = true;
00791 }
00792 all_inited &= PilotingSettingsCirclingAltitudeValue_bebop_sent_;
00793
00794 if (changed && !all_inited)
00795 {
00796 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
00797 "Value of PilotingSettingsCirclingAltitude was not initialized either by Bebop or Params.");
00798 }
00799
00800 if (changed && all_inited)
00801 {
00802 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00803 "Sending PilotingSettingsCirclingAltitude changes to bebop");
00804
00805 bebop_ctrl_ptr_->aRDrone3->sendPilotingSettingsCirclingAltitude(bebop_ctrl_ptr_->aRDrone3
00806 , (config.PilotingSettingsCirclingAltitudeValue)
00807 );
00808 }
00809 }
00810
00811
00812 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
00813 {
00814 if (arguments == NULL)
00815 {
00816 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
00817 "PilotingSettingsCirclingAltitude::Update() arguments is NULL");
00818 return;
00819 }
00820
00821 ::boost::lock_guard<boost::mutex> lock(mutex_);
00822
00823 arg = NULL;
00824 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_CIRCLINGALTITUDECHANGED_CURRENT, arg);
00825 if (arg)
00826 {
00827 PilotingSettingsCirclingAltitudeValue_bebop_sent_ = true;
00828 ROS_INFO_STREAM("Value for PilotingSettingsCirclingAltitudeValue recved: " << static_cast<int32_t>(arg->value.U16));
00829 PilotingSettingsCirclingAltitudeValue_bebop_value_ = static_cast<int32_t>(arg->value.U16);
00830
00831 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PilotingSettingsCirclingAltitudeValue exists in params ...");
00832 if (!priv_nh_.hasParam("PilotingSettingsCirclingAltitudeValue"))
00833 {
00834 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
00835 priv_nh_.setParam("PilotingSettingsCirclingAltitudeValue", static_cast<int32_t>(arg->value.U16));
00836 }
00837 else
00838 {
00839 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
00840
00841 }
00842 }
00843 }
00844 };
00845
00846 class PilotingSettingsPitchMode : public AbstractSetting
00847 {
00848 private:
00849 int32_t PilotingSettingsPitchModeValue_bebop_value_;
00850 bool PilotingSettingsPitchModeValue_bebop_sent_;
00851
00852 public:
00853 explicit PilotingSettingsPitchMode(ros::NodeHandle& priv_nh)
00854 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_PITCHMODECHANGED, priv_nh)
00855 , PilotingSettingsPitchModeValue_bebop_sent_(false)
00856 {}
00857
00858
00859 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
00860 {
00861 ::boost::lock_guard<boost::mutex> lock(mutex_);
00862 bool changed = false;
00863 bool all_inited = true;
00864
00865
00866 if ((config.PilotingSettingsPitchModeValue != PilotingSettingsPitchModeValue_bebop_value_))
00867 {
00868 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00869 "PilotingSettingsPitchModeValue changed!");
00870 changed = true;
00871 }
00872 all_inited &= PilotingSettingsPitchModeValue_bebop_sent_;
00873
00874 if (changed && !all_inited)
00875 {
00876 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
00877 "Value of PilotingSettingsPitchMode was not initialized either by Bebop or Params.");
00878 }
00879
00880 if (changed && all_inited)
00881 {
00882 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00883 "Sending PilotingSettingsPitchMode changes to bebop");
00884
00885 bebop_ctrl_ptr_->aRDrone3->sendPilotingSettingsPitchMode(bebop_ctrl_ptr_->aRDrone3
00886 , static_cast<eARCOMMANDS_ARDRONE3_PILOTINGSETTINGS_PITCHMODE_VALUE>(config.PilotingSettingsPitchModeValue)
00887 );
00888 }
00889 }
00890
00891
00892 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
00893 {
00894 if (arguments == NULL)
00895 {
00896 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
00897 "PilotingSettingsPitchMode::Update() arguments is NULL");
00898 return;
00899 }
00900
00901 ::boost::lock_guard<boost::mutex> lock(mutex_);
00902
00903 arg = NULL;
00904 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PILOTINGSETTINGSSTATE_PITCHMODECHANGED_VALUE, arg);
00905 if (arg)
00906 {
00907 PilotingSettingsPitchModeValue_bebop_sent_ = true;
00908 ROS_INFO_STREAM("Value for PilotingSettingsPitchModeValue recved: " << static_cast<int32_t>(arg->value.I32));
00909 PilotingSettingsPitchModeValue_bebop_value_ = static_cast<int32_t>(arg->value.I32);
00910
00911 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PilotingSettingsPitchModeValue exists in params ...");
00912 if (!priv_nh_.hasParam("PilotingSettingsPitchModeValue"))
00913 {
00914 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
00915 priv_nh_.setParam("PilotingSettingsPitchModeValue", static_cast<int32_t>(arg->value.I32));
00916 }
00917 else
00918 {
00919 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
00920
00921 }
00922 }
00923 }
00924 };
00925
00926 class SpeedSettingsMaxVerticalSpeed : public AbstractSetting
00927 {
00928 private:
00929 double SpeedSettingsMaxVerticalSpeedCurrent_bebop_value_;
00930 bool SpeedSettingsMaxVerticalSpeedCurrent_bebop_sent_;
00931
00932 public:
00933 explicit SpeedSettingsMaxVerticalSpeed(ros::NodeHandle& priv_nh)
00934 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SPEEDSETTINGSSTATE_MAXVERTICALSPEEDCHANGED, priv_nh)
00935 , SpeedSettingsMaxVerticalSpeedCurrent_bebop_sent_(false)
00936 {}
00937
00938
00939 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
00940 {
00941 ::boost::lock_guard<boost::mutex> lock(mutex_);
00942 bool changed = false;
00943 bool all_inited = true;
00944
00945
00946 if ((config.SpeedSettingsMaxVerticalSpeedCurrent != SpeedSettingsMaxVerticalSpeedCurrent_bebop_value_))
00947 {
00948 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00949 "SpeedSettingsMaxVerticalSpeedCurrent changed!");
00950 changed = true;
00951 }
00952 all_inited &= SpeedSettingsMaxVerticalSpeedCurrent_bebop_sent_;
00953
00954 if (changed && !all_inited)
00955 {
00956 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
00957 "Value of SpeedSettingsMaxVerticalSpeed was not initialized either by Bebop or Params.");
00958 }
00959
00960 if (changed && all_inited)
00961 {
00962 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
00963 "Sending SpeedSettingsMaxVerticalSpeed changes to bebop");
00964
00965 bebop_ctrl_ptr_->aRDrone3->sendSpeedSettingsMaxVerticalSpeed(bebop_ctrl_ptr_->aRDrone3
00966 , (config.SpeedSettingsMaxVerticalSpeedCurrent)
00967 );
00968 }
00969 }
00970
00971
00972 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
00973 {
00974 if (arguments == NULL)
00975 {
00976 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
00977 "SpeedSettingsMaxVerticalSpeed::Update() arguments is NULL");
00978 return;
00979 }
00980
00981 ::boost::lock_guard<boost::mutex> lock(mutex_);
00982
00983 arg = NULL;
00984 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SPEEDSETTINGSSTATE_MAXVERTICALSPEEDCHANGED_CURRENT, arg);
00985 if (arg)
00986 {
00987 SpeedSettingsMaxVerticalSpeedCurrent_bebop_sent_ = true;
00988 ROS_INFO_STREAM("Value for SpeedSettingsMaxVerticalSpeedCurrent recved: " << static_cast<double>(arg->value.Float));
00989 SpeedSettingsMaxVerticalSpeedCurrent_bebop_value_ = static_cast<double>(arg->value.Float);
00990
00991 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if SpeedSettingsMaxVerticalSpeedCurrent exists in params ...");
00992 if (!priv_nh_.hasParam("SpeedSettingsMaxVerticalSpeedCurrent"))
00993 {
00994 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
00995 priv_nh_.setParam("SpeedSettingsMaxVerticalSpeedCurrent", static_cast<double>(arg->value.Float));
00996 }
00997 else
00998 {
00999 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01000
01001 }
01002 }
01003 }
01004 };
01005
01006 class SpeedSettingsMaxRotationSpeed : public AbstractSetting
01007 {
01008 private:
01009 double SpeedSettingsMaxRotationSpeedCurrent_bebop_value_;
01010 bool SpeedSettingsMaxRotationSpeedCurrent_bebop_sent_;
01011
01012 public:
01013 explicit SpeedSettingsMaxRotationSpeed(ros::NodeHandle& priv_nh)
01014 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SPEEDSETTINGSSTATE_MAXROTATIONSPEEDCHANGED, priv_nh)
01015 , SpeedSettingsMaxRotationSpeedCurrent_bebop_sent_(false)
01016 {}
01017
01018
01019 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
01020 {
01021 ::boost::lock_guard<boost::mutex> lock(mutex_);
01022 bool changed = false;
01023 bool all_inited = true;
01024
01025
01026 if ((config.SpeedSettingsMaxRotationSpeedCurrent != SpeedSettingsMaxRotationSpeedCurrent_bebop_value_))
01027 {
01028 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01029 "SpeedSettingsMaxRotationSpeedCurrent changed!");
01030 changed = true;
01031 }
01032 all_inited &= SpeedSettingsMaxRotationSpeedCurrent_bebop_sent_;
01033
01034 if (changed && !all_inited)
01035 {
01036 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
01037 "Value of SpeedSettingsMaxRotationSpeed was not initialized either by Bebop or Params.");
01038 }
01039
01040 if (changed && all_inited)
01041 {
01042 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01043 "Sending SpeedSettingsMaxRotationSpeed changes to bebop");
01044
01045 bebop_ctrl_ptr_->aRDrone3->sendSpeedSettingsMaxRotationSpeed(bebop_ctrl_ptr_->aRDrone3
01046 , (config.SpeedSettingsMaxRotationSpeedCurrent)
01047 );
01048 }
01049 }
01050
01051
01052 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
01053 {
01054 if (arguments == NULL)
01055 {
01056 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
01057 "SpeedSettingsMaxRotationSpeed::Update() arguments is NULL");
01058 return;
01059 }
01060
01061 ::boost::lock_guard<boost::mutex> lock(mutex_);
01062
01063 arg = NULL;
01064 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SPEEDSETTINGSSTATE_MAXROTATIONSPEEDCHANGED_CURRENT, arg);
01065 if (arg)
01066 {
01067 SpeedSettingsMaxRotationSpeedCurrent_bebop_sent_ = true;
01068 ROS_INFO_STREAM("Value for SpeedSettingsMaxRotationSpeedCurrent recved: " << static_cast<double>(arg->value.Float));
01069 SpeedSettingsMaxRotationSpeedCurrent_bebop_value_ = static_cast<double>(arg->value.Float);
01070
01071 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if SpeedSettingsMaxRotationSpeedCurrent exists in params ...");
01072 if (!priv_nh_.hasParam("SpeedSettingsMaxRotationSpeedCurrent"))
01073 {
01074 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01075 priv_nh_.setParam("SpeedSettingsMaxRotationSpeedCurrent", static_cast<double>(arg->value.Float));
01076 }
01077 else
01078 {
01079 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01080
01081 }
01082 }
01083 }
01084 };
01085
01086 class SpeedSettingsHullProtection : public AbstractSetting
01087 {
01088 private:
01089 int32_t SpeedSettingsHullProtectionPresent_bebop_value_;
01090 bool SpeedSettingsHullProtectionPresent_bebop_sent_;
01091
01092 public:
01093 explicit SpeedSettingsHullProtection(ros::NodeHandle& priv_nh)
01094 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SPEEDSETTINGSSTATE_HULLPROTECTIONCHANGED, priv_nh)
01095 , SpeedSettingsHullProtectionPresent_bebop_sent_(false)
01096 {}
01097
01098
01099 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
01100 {
01101 ::boost::lock_guard<boost::mutex> lock(mutex_);
01102 bool changed = false;
01103 bool all_inited = true;
01104
01105
01106 if ((config.SpeedSettingsHullProtectionPresent != SpeedSettingsHullProtectionPresent_bebop_value_))
01107 {
01108 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01109 "SpeedSettingsHullProtectionPresent changed!");
01110 changed = true;
01111 }
01112 all_inited &= SpeedSettingsHullProtectionPresent_bebop_sent_;
01113
01114 if (changed && !all_inited)
01115 {
01116 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
01117 "Value of SpeedSettingsHullProtection was not initialized either by Bebop or Params.");
01118 }
01119
01120 if (changed && all_inited)
01121 {
01122 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01123 "Sending SpeedSettingsHullProtection changes to bebop");
01124
01125 bebop_ctrl_ptr_->aRDrone3->sendSpeedSettingsHullProtection(bebop_ctrl_ptr_->aRDrone3
01126 , (config.SpeedSettingsHullProtectionPresent)
01127 );
01128 }
01129 }
01130
01131
01132 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
01133 {
01134 if (arguments == NULL)
01135 {
01136 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
01137 "SpeedSettingsHullProtection::Update() arguments is NULL");
01138 return;
01139 }
01140
01141 ::boost::lock_guard<boost::mutex> lock(mutex_);
01142
01143 arg = NULL;
01144 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SPEEDSETTINGSSTATE_HULLPROTECTIONCHANGED_PRESENT, arg);
01145 if (arg)
01146 {
01147 SpeedSettingsHullProtectionPresent_bebop_sent_ = true;
01148 ROS_INFO_STREAM("Value for SpeedSettingsHullProtectionPresent recved: " << static_cast<int32_t>(arg->value.U8));
01149 SpeedSettingsHullProtectionPresent_bebop_value_ = static_cast<int32_t>(arg->value.U8);
01150
01151 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if SpeedSettingsHullProtectionPresent exists in params ...");
01152 if (!priv_nh_.hasParam("SpeedSettingsHullProtectionPresent"))
01153 {
01154 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01155 priv_nh_.setParam("SpeedSettingsHullProtectionPresent", static_cast<int32_t>(arg->value.U8));
01156 }
01157 else
01158 {
01159 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01160
01161 }
01162 }
01163 }
01164 };
01165
01166 class SpeedSettingsOutdoor : public AbstractSetting
01167 {
01168 private:
01169 int32_t SpeedSettingsOutdoorOutdoor_bebop_value_;
01170 bool SpeedSettingsOutdoorOutdoor_bebop_sent_;
01171
01172 public:
01173 explicit SpeedSettingsOutdoor(ros::NodeHandle& priv_nh)
01174 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SPEEDSETTINGSSTATE_OUTDOORCHANGED, priv_nh)
01175 , SpeedSettingsOutdoorOutdoor_bebop_sent_(false)
01176 {}
01177
01178
01179 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
01180 {
01181 ::boost::lock_guard<boost::mutex> lock(mutex_);
01182 bool changed = false;
01183 bool all_inited = true;
01184
01185
01186 if ((config.SpeedSettingsOutdoorOutdoor != SpeedSettingsOutdoorOutdoor_bebop_value_))
01187 {
01188 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01189 "SpeedSettingsOutdoorOutdoor changed!");
01190 changed = true;
01191 }
01192 all_inited &= SpeedSettingsOutdoorOutdoor_bebop_sent_;
01193
01194 if (changed && !all_inited)
01195 {
01196 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
01197 "Value of SpeedSettingsOutdoor was not initialized either by Bebop or Params.");
01198 }
01199
01200 if (changed && all_inited)
01201 {
01202 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01203 "Sending SpeedSettingsOutdoor changes to bebop");
01204
01205 bebop_ctrl_ptr_->aRDrone3->sendSpeedSettingsOutdoor(bebop_ctrl_ptr_->aRDrone3
01206 , (config.SpeedSettingsOutdoorOutdoor)
01207 );
01208 }
01209 }
01210
01211
01212 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
01213 {
01214 if (arguments == NULL)
01215 {
01216 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
01217 "SpeedSettingsOutdoor::Update() arguments is NULL");
01218 return;
01219 }
01220
01221 ::boost::lock_guard<boost::mutex> lock(mutex_);
01222
01223 arg = NULL;
01224 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SPEEDSETTINGSSTATE_OUTDOORCHANGED_OUTDOOR, arg);
01225 if (arg)
01226 {
01227 SpeedSettingsOutdoorOutdoor_bebop_sent_ = true;
01228 ROS_INFO_STREAM("Value for SpeedSettingsOutdoorOutdoor recved: " << static_cast<int32_t>(arg->value.U8));
01229 SpeedSettingsOutdoorOutdoor_bebop_value_ = static_cast<int32_t>(arg->value.U8);
01230
01231 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if SpeedSettingsOutdoorOutdoor exists in params ...");
01232 if (!priv_nh_.hasParam("SpeedSettingsOutdoorOutdoor"))
01233 {
01234 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01235 priv_nh_.setParam("SpeedSettingsOutdoorOutdoor", static_cast<int32_t>(arg->value.U8));
01236 }
01237 else
01238 {
01239 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01240
01241 }
01242 }
01243 }
01244 };
01245
01246 class SpeedSettingsMaxPitchRollRotationSpeed : public AbstractSetting
01247 {
01248 private:
01249 double SpeedSettingsMaxPitchRollRotationSpeedCurrent_bebop_value_;
01250 bool SpeedSettingsMaxPitchRollRotationSpeedCurrent_bebop_sent_;
01251
01252 public:
01253 explicit SpeedSettingsMaxPitchRollRotationSpeed(ros::NodeHandle& priv_nh)
01254 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SPEEDSETTINGSSTATE_MAXPITCHROLLROTATIONSPEEDCHANGED, priv_nh)
01255 , SpeedSettingsMaxPitchRollRotationSpeedCurrent_bebop_sent_(false)
01256 {}
01257
01258
01259 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
01260 {
01261 ::boost::lock_guard<boost::mutex> lock(mutex_);
01262 bool changed = false;
01263 bool all_inited = true;
01264
01265
01266 if ((config.SpeedSettingsMaxPitchRollRotationSpeedCurrent != SpeedSettingsMaxPitchRollRotationSpeedCurrent_bebop_value_))
01267 {
01268 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01269 "SpeedSettingsMaxPitchRollRotationSpeedCurrent changed!");
01270 changed = true;
01271 }
01272 all_inited &= SpeedSettingsMaxPitchRollRotationSpeedCurrent_bebop_sent_;
01273
01274 if (changed && !all_inited)
01275 {
01276 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
01277 "Value of SpeedSettingsMaxPitchRollRotationSpeed was not initialized either by Bebop or Params.");
01278 }
01279
01280 if (changed && all_inited)
01281 {
01282 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01283 "Sending SpeedSettingsMaxPitchRollRotationSpeed changes to bebop");
01284
01285 bebop_ctrl_ptr_->aRDrone3->sendSpeedSettingsMaxPitchRollRotationSpeed(bebop_ctrl_ptr_->aRDrone3
01286 , (config.SpeedSettingsMaxPitchRollRotationSpeedCurrent)
01287 );
01288 }
01289 }
01290
01291
01292 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
01293 {
01294 if (arguments == NULL)
01295 {
01296 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
01297 "SpeedSettingsMaxPitchRollRotationSpeed::Update() arguments is NULL");
01298 return;
01299 }
01300
01301 ::boost::lock_guard<boost::mutex> lock(mutex_);
01302
01303 arg = NULL;
01304 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_SPEEDSETTINGSSTATE_MAXPITCHROLLROTATIONSPEEDCHANGED_CURRENT, arg);
01305 if (arg)
01306 {
01307 SpeedSettingsMaxPitchRollRotationSpeedCurrent_bebop_sent_ = true;
01308 ROS_INFO_STREAM("Value for SpeedSettingsMaxPitchRollRotationSpeedCurrent recved: " << static_cast<double>(arg->value.Float));
01309 SpeedSettingsMaxPitchRollRotationSpeedCurrent_bebop_value_ = static_cast<double>(arg->value.Float);
01310
01311 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if SpeedSettingsMaxPitchRollRotationSpeedCurrent exists in params ...");
01312 if (!priv_nh_.hasParam("SpeedSettingsMaxPitchRollRotationSpeedCurrent"))
01313 {
01314 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01315 priv_nh_.setParam("SpeedSettingsMaxPitchRollRotationSpeedCurrent", static_cast<double>(arg->value.Float));
01316 }
01317 else
01318 {
01319 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01320
01321 }
01322 }
01323 }
01324 };
01325
01326 class NetworkSettingsWifiSelection : public AbstractSetting
01327 {
01328 private:
01329 int32_t NetworkSettingsWifiSelectionType_bebop_value_;
01330 bool NetworkSettingsWifiSelectionType_bebop_sent_;
01331 int32_t NetworkSettingsWifiSelectionBand_bebop_value_;
01332 bool NetworkSettingsWifiSelectionBand_bebop_sent_;
01333 int32_t NetworkSettingsWifiSelectionChannel_bebop_value_;
01334 bool NetworkSettingsWifiSelectionChannel_bebop_sent_;
01335
01336 public:
01337 explicit NetworkSettingsWifiSelection(ros::NodeHandle& priv_nh)
01338 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_NETWORKSETTINGSSTATE_WIFISELECTIONCHANGED, priv_nh)
01339 , NetworkSettingsWifiSelectionType_bebop_sent_(false)
01340 , NetworkSettingsWifiSelectionBand_bebop_sent_(false)
01341 , NetworkSettingsWifiSelectionChannel_bebop_sent_(false)
01342 {}
01343
01344
01345 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
01346 {
01347 ::boost::lock_guard<boost::mutex> lock(mutex_);
01348 bool changed = false;
01349 bool all_inited = true;
01350
01351
01352 if ((config.NetworkSettingsWifiSelectionType != NetworkSettingsWifiSelectionType_bebop_value_))
01353 {
01354 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01355 "NetworkSettingsWifiSelectionType changed!");
01356 changed = true;
01357 }
01358 all_inited &= NetworkSettingsWifiSelectionType_bebop_sent_;
01359
01360
01361 if ((config.NetworkSettingsWifiSelectionBand != NetworkSettingsWifiSelectionBand_bebop_value_))
01362 {
01363 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01364 "NetworkSettingsWifiSelectionBand changed!");
01365 changed = true;
01366 }
01367 all_inited &= NetworkSettingsWifiSelectionBand_bebop_sent_;
01368
01369
01370 if ((config.NetworkSettingsWifiSelectionChannel != NetworkSettingsWifiSelectionChannel_bebop_value_))
01371 {
01372 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01373 "NetworkSettingsWifiSelectionChannel changed!");
01374 changed = true;
01375 }
01376 all_inited &= NetworkSettingsWifiSelectionChannel_bebop_sent_;
01377
01378 if (changed && !all_inited)
01379 {
01380 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
01381 "Value of NetworkSettingsWifiSelection was not initialized either by Bebop or Params.");
01382 }
01383
01384 if (changed && all_inited)
01385 {
01386 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01387 "Sending NetworkSettingsWifiSelection changes to bebop");
01388
01389 bebop_ctrl_ptr_->aRDrone3->sendNetworkSettingsWifiSelection(bebop_ctrl_ptr_->aRDrone3
01390 , static_cast<eARCOMMANDS_ARDRONE3_NETWORKSETTINGS_WIFISELECTION_TYPE>(config.NetworkSettingsWifiSelectionType)
01391 , static_cast<eARCOMMANDS_ARDRONE3_NETWORKSETTINGS_WIFISELECTION_BAND>(config.NetworkSettingsWifiSelectionBand)
01392 , (config.NetworkSettingsWifiSelectionChannel)
01393 );
01394 }
01395 }
01396
01397
01398 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
01399 {
01400 if (arguments == NULL)
01401 {
01402 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
01403 "NetworkSettingsWifiSelection::Update() arguments is NULL");
01404 return;
01405 }
01406
01407 ::boost::lock_guard<boost::mutex> lock(mutex_);
01408
01409 arg = NULL;
01410 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_NETWORKSETTINGSSTATE_WIFISELECTIONCHANGED_TYPE, arg);
01411 if (arg)
01412 {
01413 NetworkSettingsWifiSelectionType_bebop_sent_ = true;
01414 ROS_INFO_STREAM("Value for NetworkSettingsWifiSelectionType recved: " << static_cast<int32_t>(arg->value.I32));
01415 NetworkSettingsWifiSelectionType_bebop_value_ = static_cast<int32_t>(arg->value.I32);
01416
01417 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if NetworkSettingsWifiSelectionType exists in params ...");
01418 if (!priv_nh_.hasParam("NetworkSettingsWifiSelectionType"))
01419 {
01420 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01421 priv_nh_.setParam("NetworkSettingsWifiSelectionType", static_cast<int32_t>(arg->value.I32));
01422 }
01423 else
01424 {
01425 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01426
01427 }
01428 }
01429
01430 arg = NULL;
01431 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_NETWORKSETTINGSSTATE_WIFISELECTIONCHANGED_BAND, arg);
01432 if (arg)
01433 {
01434 NetworkSettingsWifiSelectionBand_bebop_sent_ = true;
01435 ROS_INFO_STREAM("Value for NetworkSettingsWifiSelectionBand recved: " << static_cast<int32_t>(arg->value.I32));
01436 NetworkSettingsWifiSelectionBand_bebop_value_ = static_cast<int32_t>(arg->value.I32);
01437
01438 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if NetworkSettingsWifiSelectionBand exists in params ...");
01439 if (!priv_nh_.hasParam("NetworkSettingsWifiSelectionBand"))
01440 {
01441 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01442 priv_nh_.setParam("NetworkSettingsWifiSelectionBand", static_cast<int32_t>(arg->value.I32));
01443 }
01444 else
01445 {
01446 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01447
01448 }
01449 }
01450
01451 arg = NULL;
01452 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_NETWORKSETTINGSSTATE_WIFISELECTIONCHANGED_CHANNEL, arg);
01453 if (arg)
01454 {
01455 NetworkSettingsWifiSelectionChannel_bebop_sent_ = true;
01456 ROS_INFO_STREAM("Value for NetworkSettingsWifiSelectionChannel recved: " << static_cast<int32_t>(arg->value.U8));
01457 NetworkSettingsWifiSelectionChannel_bebop_value_ = static_cast<int32_t>(arg->value.U8);
01458
01459 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if NetworkSettingsWifiSelectionChannel exists in params ...");
01460 if (!priv_nh_.hasParam("NetworkSettingsWifiSelectionChannel"))
01461 {
01462 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01463 priv_nh_.setParam("NetworkSettingsWifiSelectionChannel", static_cast<int32_t>(arg->value.U8));
01464 }
01465 else
01466 {
01467 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01468
01469 }
01470 }
01471 }
01472 };
01473
01474 class PictureSettingsVideoStabilizationMode : public AbstractSetting
01475 {
01476 private:
01477 int32_t PictureSettingsVideoStabilizationModeMode_bebop_value_;
01478 bool PictureSettingsVideoStabilizationModeMode_bebop_sent_;
01479
01480 public:
01481 explicit PictureSettingsVideoStabilizationMode(ros::NodeHandle& priv_nh)
01482 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEOSTABILIZATIONMODECHANGED, priv_nh)
01483 , PictureSettingsVideoStabilizationModeMode_bebop_sent_(false)
01484 {}
01485
01486
01487 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
01488 {
01489 ::boost::lock_guard<boost::mutex> lock(mutex_);
01490 bool changed = false;
01491 bool all_inited = true;
01492
01493
01494 if ((config.PictureSettingsVideoStabilizationModeMode != PictureSettingsVideoStabilizationModeMode_bebop_value_))
01495 {
01496 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01497 "PictureSettingsVideoStabilizationModeMode changed!");
01498 changed = true;
01499 }
01500 all_inited &= PictureSettingsVideoStabilizationModeMode_bebop_sent_;
01501
01502 if (changed && !all_inited)
01503 {
01504 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
01505 "Value of PictureSettingsVideoStabilizationMode was not initialized either by Bebop or Params.");
01506 }
01507
01508 if (changed && all_inited)
01509 {
01510 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01511 "Sending PictureSettingsVideoStabilizationMode changes to bebop");
01512
01513 bebop_ctrl_ptr_->aRDrone3->sendPictureSettingsVideoStabilizationMode(bebop_ctrl_ptr_->aRDrone3
01514 , static_cast<eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOSTABILIZATIONMODE_MODE>(config.PictureSettingsVideoStabilizationModeMode)
01515 );
01516 }
01517 }
01518
01519
01520 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
01521 {
01522 if (arguments == NULL)
01523 {
01524 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
01525 "PictureSettingsVideoStabilizationMode::Update() arguments is NULL");
01526 return;
01527 }
01528
01529 ::boost::lock_guard<boost::mutex> lock(mutex_);
01530
01531 arg = NULL;
01532 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEOSTABILIZATIONMODECHANGED_MODE, arg);
01533 if (arg)
01534 {
01535 PictureSettingsVideoStabilizationModeMode_bebop_sent_ = true;
01536 ROS_INFO_STREAM("Value for PictureSettingsVideoStabilizationModeMode recved: " << static_cast<int32_t>(arg->value.I32));
01537 PictureSettingsVideoStabilizationModeMode_bebop_value_ = static_cast<int32_t>(arg->value.I32);
01538
01539 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PictureSettingsVideoStabilizationModeMode exists in params ...");
01540 if (!priv_nh_.hasParam("PictureSettingsVideoStabilizationModeMode"))
01541 {
01542 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01543 priv_nh_.setParam("PictureSettingsVideoStabilizationModeMode", static_cast<int32_t>(arg->value.I32));
01544 }
01545 else
01546 {
01547 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01548
01549 }
01550 }
01551 }
01552 };
01553
01554 class PictureSettingsVideoRecordingMode : public AbstractSetting
01555 {
01556 private:
01557 int32_t PictureSettingsVideoRecordingModeMode_bebop_value_;
01558 bool PictureSettingsVideoRecordingModeMode_bebop_sent_;
01559
01560 public:
01561 explicit PictureSettingsVideoRecordingMode(ros::NodeHandle& priv_nh)
01562 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEORECORDINGMODECHANGED, priv_nh)
01563 , PictureSettingsVideoRecordingModeMode_bebop_sent_(false)
01564 {}
01565
01566
01567 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
01568 {
01569 ::boost::lock_guard<boost::mutex> lock(mutex_);
01570 bool changed = false;
01571 bool all_inited = true;
01572
01573
01574 if ((config.PictureSettingsVideoRecordingModeMode != PictureSettingsVideoRecordingModeMode_bebop_value_))
01575 {
01576 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01577 "PictureSettingsVideoRecordingModeMode changed!");
01578 changed = true;
01579 }
01580 all_inited &= PictureSettingsVideoRecordingModeMode_bebop_sent_;
01581
01582 if (changed && !all_inited)
01583 {
01584 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
01585 "Value of PictureSettingsVideoRecordingMode was not initialized either by Bebop or Params.");
01586 }
01587
01588 if (changed && all_inited)
01589 {
01590 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01591 "Sending PictureSettingsVideoRecordingMode changes to bebop");
01592
01593 bebop_ctrl_ptr_->aRDrone3->sendPictureSettingsVideoRecordingMode(bebop_ctrl_ptr_->aRDrone3
01594 , static_cast<eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORECORDINGMODE_MODE>(config.PictureSettingsVideoRecordingModeMode)
01595 );
01596 }
01597 }
01598
01599
01600 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
01601 {
01602 if (arguments == NULL)
01603 {
01604 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
01605 "PictureSettingsVideoRecordingMode::Update() arguments is NULL");
01606 return;
01607 }
01608
01609 ::boost::lock_guard<boost::mutex> lock(mutex_);
01610
01611 arg = NULL;
01612 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEORECORDINGMODECHANGED_MODE, arg);
01613 if (arg)
01614 {
01615 PictureSettingsVideoRecordingModeMode_bebop_sent_ = true;
01616 ROS_INFO_STREAM("Value for PictureSettingsVideoRecordingModeMode recved: " << static_cast<int32_t>(arg->value.I32));
01617 PictureSettingsVideoRecordingModeMode_bebop_value_ = static_cast<int32_t>(arg->value.I32);
01618
01619 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PictureSettingsVideoRecordingModeMode exists in params ...");
01620 if (!priv_nh_.hasParam("PictureSettingsVideoRecordingModeMode"))
01621 {
01622 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01623 priv_nh_.setParam("PictureSettingsVideoRecordingModeMode", static_cast<int32_t>(arg->value.I32));
01624 }
01625 else
01626 {
01627 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01628
01629 }
01630 }
01631 }
01632 };
01633
01634 class PictureSettingsVideoFramerate : public AbstractSetting
01635 {
01636 private:
01637 int32_t PictureSettingsVideoFramerateFramerate_bebop_value_;
01638 bool PictureSettingsVideoFramerateFramerate_bebop_sent_;
01639
01640 public:
01641 explicit PictureSettingsVideoFramerate(ros::NodeHandle& priv_nh)
01642 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEOFRAMERATECHANGED, priv_nh)
01643 , PictureSettingsVideoFramerateFramerate_bebop_sent_(false)
01644 {}
01645
01646
01647 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
01648 {
01649 ::boost::lock_guard<boost::mutex> lock(mutex_);
01650 bool changed = false;
01651 bool all_inited = true;
01652
01653
01654 if ((config.PictureSettingsVideoFramerateFramerate != PictureSettingsVideoFramerateFramerate_bebop_value_))
01655 {
01656 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01657 "PictureSettingsVideoFramerateFramerate changed!");
01658 changed = true;
01659 }
01660 all_inited &= PictureSettingsVideoFramerateFramerate_bebop_sent_;
01661
01662 if (changed && !all_inited)
01663 {
01664 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
01665 "Value of PictureSettingsVideoFramerate was not initialized either by Bebop or Params.");
01666 }
01667
01668 if (changed && all_inited)
01669 {
01670 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01671 "Sending PictureSettingsVideoFramerate changes to bebop");
01672
01673 bebop_ctrl_ptr_->aRDrone3->sendPictureSettingsVideoFramerate(bebop_ctrl_ptr_->aRDrone3
01674 , static_cast<eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEOFRAMERATE_FRAMERATE>(config.PictureSettingsVideoFramerateFramerate)
01675 );
01676 }
01677 }
01678
01679
01680 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
01681 {
01682 if (arguments == NULL)
01683 {
01684 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
01685 "PictureSettingsVideoFramerate::Update() arguments is NULL");
01686 return;
01687 }
01688
01689 ::boost::lock_guard<boost::mutex> lock(mutex_);
01690
01691 arg = NULL;
01692 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEOFRAMERATECHANGED_FRAMERATE, arg);
01693 if (arg)
01694 {
01695 PictureSettingsVideoFramerateFramerate_bebop_sent_ = true;
01696 ROS_INFO_STREAM("Value for PictureSettingsVideoFramerateFramerate recved: " << static_cast<int32_t>(arg->value.I32));
01697 PictureSettingsVideoFramerateFramerate_bebop_value_ = static_cast<int32_t>(arg->value.I32);
01698
01699 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PictureSettingsVideoFramerateFramerate exists in params ...");
01700 if (!priv_nh_.hasParam("PictureSettingsVideoFramerateFramerate"))
01701 {
01702 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01703 priv_nh_.setParam("PictureSettingsVideoFramerateFramerate", static_cast<int32_t>(arg->value.I32));
01704 }
01705 else
01706 {
01707 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01708
01709 }
01710 }
01711 }
01712 };
01713
01714 class PictureSettingsVideoResolutions : public AbstractSetting
01715 {
01716 private:
01717 int32_t PictureSettingsVideoResolutionsType_bebop_value_;
01718 bool PictureSettingsVideoResolutionsType_bebop_sent_;
01719
01720 public:
01721 explicit PictureSettingsVideoResolutions(ros::NodeHandle& priv_nh)
01722 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEORESOLUTIONSCHANGED, priv_nh)
01723 , PictureSettingsVideoResolutionsType_bebop_sent_(false)
01724 {}
01725
01726
01727 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
01728 {
01729 ::boost::lock_guard<boost::mutex> lock(mutex_);
01730 bool changed = false;
01731 bool all_inited = true;
01732
01733
01734 if ((config.PictureSettingsVideoResolutionsType != PictureSettingsVideoResolutionsType_bebop_value_))
01735 {
01736 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01737 "PictureSettingsVideoResolutionsType changed!");
01738 changed = true;
01739 }
01740 all_inited &= PictureSettingsVideoResolutionsType_bebop_sent_;
01741
01742 if (changed && !all_inited)
01743 {
01744 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
01745 "Value of PictureSettingsVideoResolutions was not initialized either by Bebop or Params.");
01746 }
01747
01748 if (changed && all_inited)
01749 {
01750 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01751 "Sending PictureSettingsVideoResolutions changes to bebop");
01752
01753 bebop_ctrl_ptr_->aRDrone3->sendPictureSettingsVideoResolutions(bebop_ctrl_ptr_->aRDrone3
01754 , static_cast<eARCOMMANDS_ARDRONE3_PICTURESETTINGS_VIDEORESOLUTIONS_TYPE>(config.PictureSettingsVideoResolutionsType)
01755 );
01756 }
01757 }
01758
01759
01760 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
01761 {
01762 if (arguments == NULL)
01763 {
01764 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
01765 "PictureSettingsVideoResolutions::Update() arguments is NULL");
01766 return;
01767 }
01768
01769 ::boost::lock_guard<boost::mutex> lock(mutex_);
01770
01771 arg = NULL;
01772 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_PICTURESETTINGSSTATE_VIDEORESOLUTIONSCHANGED_TYPE, arg);
01773 if (arg)
01774 {
01775 PictureSettingsVideoResolutionsType_bebop_sent_ = true;
01776 ROS_INFO_STREAM("Value for PictureSettingsVideoResolutionsType recved: " << static_cast<int32_t>(arg->value.I32));
01777 PictureSettingsVideoResolutionsType_bebop_value_ = static_cast<int32_t>(arg->value.I32);
01778
01779 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if PictureSettingsVideoResolutionsType exists in params ...");
01780 if (!priv_nh_.hasParam("PictureSettingsVideoResolutionsType"))
01781 {
01782 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01783 priv_nh_.setParam("PictureSettingsVideoResolutionsType", static_cast<int32_t>(arg->value.I32));
01784 }
01785 else
01786 {
01787 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01788
01789 }
01790 }
01791 }
01792 };
01793
01794 class GPSSettingsHomeType : public AbstractSetting
01795 {
01796 private:
01797 int32_t GPSSettingsHomeTypeType_bebop_value_;
01798 bool GPSSettingsHomeTypeType_bebop_sent_;
01799
01800 public:
01801 explicit GPSSettingsHomeType(ros::NodeHandle& priv_nh)
01802 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_HOMETYPECHANGED, priv_nh)
01803 , GPSSettingsHomeTypeType_bebop_sent_(false)
01804 {}
01805
01806
01807 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
01808 {
01809 ::boost::lock_guard<boost::mutex> lock(mutex_);
01810 bool changed = false;
01811 bool all_inited = true;
01812
01813
01814 if ((config.GPSSettingsHomeTypeType != GPSSettingsHomeTypeType_bebop_value_))
01815 {
01816 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01817 "GPSSettingsHomeTypeType changed!");
01818 changed = true;
01819 }
01820 all_inited &= GPSSettingsHomeTypeType_bebop_sent_;
01821
01822 if (changed && !all_inited)
01823 {
01824 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
01825 "Value of GPSSettingsHomeType was not initialized either by Bebop or Params.");
01826 }
01827
01828 if (changed && all_inited)
01829 {
01830 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01831 "Sending GPSSettingsHomeType changes to bebop");
01832
01833 bebop_ctrl_ptr_->aRDrone3->sendGPSSettingsHomeType(bebop_ctrl_ptr_->aRDrone3
01834 , static_cast<eARCOMMANDS_ARDRONE3_GPSSETTINGS_HOMETYPE_TYPE>(config.GPSSettingsHomeTypeType)
01835 );
01836 }
01837 }
01838
01839
01840 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
01841 {
01842 if (arguments == NULL)
01843 {
01844 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
01845 "GPSSettingsHomeType::Update() arguments is NULL");
01846 return;
01847 }
01848
01849 ::boost::lock_guard<boost::mutex> lock(mutex_);
01850
01851 arg = NULL;
01852 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_HOMETYPECHANGED_TYPE, arg);
01853 if (arg)
01854 {
01855 GPSSettingsHomeTypeType_bebop_sent_ = true;
01856 ROS_INFO_STREAM("Value for GPSSettingsHomeTypeType recved: " << static_cast<int32_t>(arg->value.I32));
01857 GPSSettingsHomeTypeType_bebop_value_ = static_cast<int32_t>(arg->value.I32);
01858
01859 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if GPSSettingsHomeTypeType exists in params ...");
01860 if (!priv_nh_.hasParam("GPSSettingsHomeTypeType"))
01861 {
01862 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01863 priv_nh_.setParam("GPSSettingsHomeTypeType", static_cast<int32_t>(arg->value.I32));
01864 }
01865 else
01866 {
01867 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01868
01869 }
01870 }
01871 }
01872 };
01873
01874 class GPSSettingsReturnHomeDelay : public AbstractSetting
01875 {
01876 private:
01877 int32_t GPSSettingsReturnHomeDelayDelay_bebop_value_;
01878 bool GPSSettingsReturnHomeDelayDelay_bebop_sent_;
01879
01880 public:
01881 explicit GPSSettingsReturnHomeDelay(ros::NodeHandle& priv_nh)
01882 : AbstractSetting(ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_RETURNHOMEDELAYCHANGED, priv_nh)
01883 , GPSSettingsReturnHomeDelayDelay_bebop_sent_(false)
01884 {}
01885
01886
01887 void UpdateBebopFromROS(const BebopArdrone3Config &config, const ARCONTROLLER_Device_t* bebop_ctrl_ptr_)
01888 {
01889 ::boost::lock_guard<boost::mutex> lock(mutex_);
01890 bool changed = false;
01891 bool all_inited = true;
01892
01893
01894 if ((config.GPSSettingsReturnHomeDelayDelay != GPSSettingsReturnHomeDelayDelay_bebop_value_))
01895 {
01896 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01897 "GPSSettingsReturnHomeDelayDelay changed!");
01898 changed = true;
01899 }
01900 all_inited &= GPSSettingsReturnHomeDelayDelay_bebop_sent_;
01901
01902 if (changed && !all_inited)
01903 {
01904 ARSAL_PRINT(ARSAL_PRINT_ERROR, "CB",
01905 "Value of GPSSettingsReturnHomeDelay was not initialized either by Bebop or Params.");
01906 }
01907
01908 if (changed && all_inited)
01909 {
01910 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB",
01911 "Sending GPSSettingsReturnHomeDelay changes to bebop");
01912
01913 bebop_ctrl_ptr_->aRDrone3->sendGPSSettingsReturnHomeDelay(bebop_ctrl_ptr_->aRDrone3
01914 , (config.GPSSettingsReturnHomeDelayDelay)
01915 );
01916 }
01917 }
01918
01919
01920 void Update(const ARCONTROLLER_DICTIONARY_ARG_t *arguments, const ::ros::Time& t)
01921 {
01922 if (arguments == NULL)
01923 {
01924 ARSAL_PRINT(ARSAL_PRINT_WARNING, "CB",
01925 "GPSSettingsReturnHomeDelay::Update() arguments is NULL");
01926 return;
01927 }
01928
01929 ::boost::lock_guard<boost::mutex> lock(mutex_);
01930
01931 arg = NULL;
01932 HASH_FIND_STR (arguments, ARCONTROLLER_DICTIONARY_KEY_ARDRONE3_GPSSETTINGSSTATE_RETURNHOMEDELAYCHANGED_DELAY, arg);
01933 if (arg)
01934 {
01935 GPSSettingsReturnHomeDelayDelay_bebop_sent_ = true;
01936 ROS_INFO_STREAM("Value for GPSSettingsReturnHomeDelayDelay recved: " << static_cast<int32_t>(arg->value.U16));
01937 GPSSettingsReturnHomeDelayDelay_bebop_value_ = static_cast<int32_t>(arg->value.U16);
01938
01939 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", "Checking if GPSSettingsReturnHomeDelayDelay exists in params ...");
01940 if (!priv_nh_.hasParam("GPSSettingsReturnHomeDelayDelay"))
01941 {
01942 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " No");
01943 priv_nh_.setParam("GPSSettingsReturnHomeDelayDelay", static_cast<int32_t>(arg->value.U16));
01944 }
01945 else
01946 {
01947 ARSAL_PRINT(ARSAL_PRINT_INFO, "CB", " Yes");
01948
01949 }
01950 }
01951 }
01952 };
01953
01954 }
01955 }
01956 #endif // BEBOP_AUTONOMY_AUTOGENERATED_Ardrone3_SETTING_CALLBACKS_H