+
+ Line data Source code
+
+ 1 : /* includes //{ */
+ 2 :
+ 3 : #include <ros/ros.h>
+ 4 : #include <ros/package.h>
+ 5 : #include <nodelet/nodelet.h>
+ 6 :
+ 7 : #include <mrs_uav_managers/control_manager/common.h>
+ 8 : #include <control_manager/output_publisher.h>
+ 9 :
+ 10 : #include <mrs_uav_managers/controller.h>
+ 11 : #include <mrs_uav_managers/tracker.h>
+ 12 :
+ 13 : #include <mrs_msgs/String.h>
+ 14 : #include <mrs_msgs/Float64Stamped.h>
+ 15 : #include <mrs_msgs/Float64StampedSrv.h>
+ 16 : #include <mrs_msgs/ObstacleSectors.h>
+ 17 : #include <mrs_msgs/BoolStamped.h>
+ 18 : #include <mrs_msgs/ControlManagerDiagnostics.h>
+ 19 : #include <mrs_msgs/DynamicsConstraints.h>
+ 20 : #include <mrs_msgs/ControlError.h>
+ 21 : #include <mrs_msgs/GetFloat64.h>
+ 22 : #include <mrs_msgs/ValidateReference.h>
+ 23 : #include <mrs_msgs/ValidateReferenceList.h>
+ 24 : #include <mrs_msgs/TrackerCommand.h>
+ 25 : #include <mrs_msgs/EstimatorInput.h>
+ 26 :
+ 27 : #include <geometry_msgs/Point32.h>
+ 28 : #include <geometry_msgs/TwistStamped.h>
+ 29 : #include <geometry_msgs/PoseArray.h>
+ 30 : #include <geometry_msgs/Vector3Stamped.h>
+ 31 :
+ 32 : #include <nav_msgs/Odometry.h>
+ 33 :
+ 34 : #include <sensor_msgs/Joy.h>
+ 35 : #include <sensor_msgs/NavSatFix.h>
+ 36 :
+ 37 : #include <mrs_lib/safety_zone/safety_zone.h>
+ 38 : #include <mrs_lib/profiler.h>
+ 39 : #include <mrs_lib/param_loader.h>
+ 40 : #include <mrs_lib/utils.h>
+ 41 : #include <mrs_lib/mutex.h>
+ 42 : #include <mrs_lib/transformer.h>
+ 43 : #include <mrs_lib/geometry/misc.h>
+ 44 : #include <mrs_lib/geometry/cyclic.h>
+ 45 : #include <mrs_lib/attitude_converter.h>
+ 46 : #include <mrs_lib/subscribe_handler.h>
+ 47 : #include <mrs_lib/msg_extractor.h>
+ 48 : #include <mrs_lib/quadratic_throttle_model.h>
+ 49 : #include <mrs_lib/publisher_handler.h>
+ 50 : #include <mrs_lib/service_client_handler.h>
+ 51 :
+ 52 : #include <mrs_msgs/HwApiCapabilities.h>
+ 53 : #include <mrs_msgs/HwApiStatus.h>
+ 54 : #include <mrs_msgs/HwApiRcChannels.h>
+ 55 :
+ 56 : #include <mrs_msgs/HwApiActuatorCmd.h>
+ 57 : #include <mrs_msgs/HwApiControlGroupCmd.h>
+ 58 : #include <mrs_msgs/HwApiAttitudeRateCmd.h>
+ 59 : #include <mrs_msgs/HwApiAttitudeCmd.h>
+ 60 : #include <mrs_msgs/HwApiAccelerationHdgRateCmd.h>
+ 61 : #include <mrs_msgs/HwApiAccelerationHdgCmd.h>
+ 62 : #include <mrs_msgs/HwApiVelocityHdgRateCmd.h>
+ 63 : #include <mrs_msgs/HwApiVelocityHdgCmd.h>
+ 64 : #include <mrs_msgs/HwApiPositionCmd.h>
+ 65 :
+ 66 : #include <std_msgs/Float64.h>
+ 67 :
+ 68 : #include <future>
+ 69 :
+ 70 : #include <pluginlib/class_loader.h>
+ 71 :
+ 72 : #include <nodelet/loader.h>
+ 73 :
+ 74 : #include <eigen3/Eigen/Eigen>
+ 75 :
+ 76 : #include <visualization_msgs/Marker.h>
+ 77 : #include <visualization_msgs/MarkerArray.h>
+ 78 :
+ 79 : #include <mrs_msgs/Reference.h>
+ 80 : #include <mrs_msgs/ReferenceStamped.h>
+ 81 : #include <mrs_msgs/ReferenceList.h>
+ 82 : #include <mrs_msgs/TrajectoryReference.h>
+ 83 :
+ 84 : #include <mrs_msgs/ReferenceStampedSrv.h>
+ 85 : #include <mrs_msgs/ReferenceStampedSrvRequest.h>
+ 86 : #include <mrs_msgs/ReferenceStampedSrvResponse.h>
+ 87 :
+ 88 : #include <mrs_msgs/VelocityReferenceStampedSrv.h>
+ 89 : #include <mrs_msgs/VelocityReferenceStampedSrvRequest.h>
+ 90 : #include <mrs_msgs/VelocityReferenceStampedSrvResponse.h>
+ 91 :
+ 92 : #include <mrs_msgs/TransformReferenceSrv.h>
+ 93 : #include <mrs_msgs/TransformReferenceSrvRequest.h>
+ 94 : #include <mrs_msgs/TransformReferenceSrvResponse.h>
+ 95 :
+ 96 : #include <mrs_msgs/TransformPoseSrv.h>
+ 97 : #include <mrs_msgs/TransformPoseSrvRequest.h>
+ 98 : #include <mrs_msgs/TransformPoseSrvResponse.h>
+ 99 :
+ 100 : #include <mrs_msgs/TransformVector3Srv.h>
+ 101 : #include <mrs_msgs/TransformVector3SrvRequest.h>
+ 102 : #include <mrs_msgs/TransformVector3SrvResponse.h>
+ 103 :
+ 104 : #include <mrs_msgs/Float64StampedSrv.h>
+ 105 : #include <mrs_msgs/Float64StampedSrvRequest.h>
+ 106 : #include <mrs_msgs/Float64StampedSrvResponse.h>
+ 107 :
+ 108 : #include <mrs_msgs/Vec4.h>
+ 109 : #include <mrs_msgs/Vec4Request.h>
+ 110 : #include <mrs_msgs/Vec4Response.h>
+ 111 :
+ 112 : #include <mrs_msgs/Vec1.h>
+ 113 : #include <mrs_msgs/Vec1Request.h>
+ 114 : #include <mrs_msgs/Vec1Response.h>
+ 115 :
+ 116 : //}
+ 117 :
+ 118 : /* defines //{ */
+ 119 :
+ 120 : #define TAU 2 * M_PI
+ 121 : #define REF_X 0
+ 122 : #define REF_Y 1
+ 123 : #define REF_Z 2
+ 124 : #define REF_HEADING 3
+ 125 : #define ELAND_STR "eland"
+ 126 : #define EHOVER_STR "ehover"
+ 127 : #define ESCALATING_FAILSAFE_STR "escalating_failsafe"
+ 128 : #define FAILSAFE_STR "failsafe"
+ 129 : #define INPUT_UAV_STATE 0
+ 130 : #define INPUT_ODOMETRY 1
+ 131 : #define RC_DEADBAND 0.2
+ 132 :
+ 133 : //}
+ 134 :
+ 135 : /* using //{ */
+ 136 :
+ 137 : using vec2_t = mrs_lib::geometry::vec_t<2>;
+ 138 : using vec3_t = mrs_lib::geometry::vec_t<3>;
+ 139 :
+ 140 : using radians = mrs_lib::geometry::radians;
+ 141 : using sradians = mrs_lib::geometry::sradians;
+ 142 :
+ 143 : //}
+ 144 :
+ 145 : namespace mrs_uav_managers
+ 146 : {
+ 147 :
+ 148 : namespace control_manager
+ 149 : {
+ 150 :
+ 151 : /* //{ class ControlManager */
+ 152 :
+ 153 : // state machine
+ 154 : typedef enum
+ 155 : {
+ 156 :
+ 157 : IDLE_STATE,
+ 158 : LANDING_STATE,
+ 159 :
+ 160 : } LandingStates_t;
+ 161 :
+ 162 : const char* state_names[2] = {"IDLING", "LANDING"};
+ 163 :
+ 164 : // state machine
+ 165 : typedef enum
+ 166 : {
+ 167 :
+ 168 : FCU_FRAME,
+ 169 : RELATIVE_FRAME,
+ 170 : ABSOLUTE_FRAME
+ 171 :
+ 172 : } ReferenceFrameType_t;
+ 173 :
+ 174 : // state machine
+ 175 : typedef enum
+ 176 : {
+ 177 :
+ 178 : ESC_NONE_STATE = 0,
+ 179 : ESC_EHOVER_STATE = 1,
+ 180 : ESC_ELAND_STATE = 2,
+ 181 : ESC_FAILSAFE_STATE = 3,
+ 182 : ESC_FINISHED_STATE = 4,
+ 183 :
+ 184 : } EscalatingFailsafeStates_t;
+ 185 :
+ 186 : /* class ControllerParams() //{ */
+ 187 :
+ 188 : class ControllerParams {
+ 189 :
+ 190 : public:
+ 191 : ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold, double odometry_innovation_threshold,
+ 192 : bool human_switchable);
+ 193 :
+ 194 : public:
+ 195 : double failsafe_threshold;
+ 196 : double eland_threshold;
+ 197 : double odometry_innovation_threshold;
+ 198 : std::string address;
+ 199 : std::string name_space;
+ 200 : bool human_switchable;
+ 201 : };
+ 202 :
+ 203 455 : ControllerParams::ControllerParams(std::string address, std::string name_space, double eland_threshold, double failsafe_threshold,
+ 204 455 : double odometry_innovation_threshold, bool human_switchable) {
+ 205 :
+ 206 455 : this->eland_threshold = eland_threshold;
+ 207 455 : this->odometry_innovation_threshold = odometry_innovation_threshold;
+ 208 455 : this->failsafe_threshold = failsafe_threshold;
+ 209 455 : this->address = address;
+ 210 455 : this->name_space = name_space;
+ 211 455 : this->human_switchable = human_switchable;
+ 212 455 : }
+ 213 :
+ 214 : //}
+ 215 :
+ 216 : /* class TrackerParams() //{ */
+ 217 :
+ 218 : class TrackerParams {
+ 219 :
+ 220 : public:
+ 221 : TrackerParams(std::string address, std::string name_space, bool human_switchable);
+ 222 :
+ 223 : public:
+ 224 : std::string address;
+ 225 : std::string name_space;
+ 226 : bool human_switchable;
+ 227 : };
+ 228 :
+ 229 547 : TrackerParams::TrackerParams(std::string address, std::string name_space, bool human_switchable) {
+ 230 :
+ 231 547 : this->address = address;
+ 232 547 : this->name_space = name_space;
+ 233 547 : this->human_switchable = human_switchable;
+ 234 547 : }
+ 235 :
+ 236 : //}
+ 237 :
+ 238 : class ControlManager : public nodelet::Nodelet {
+ 239 :
+ 240 : public:
+ 241 : virtual void onInit();
+ 242 :
+ 243 : private:
+ 244 : ros::NodeHandle nh_;
+ 245 : std::atomic<bool> is_initialized_ = false;
+ 246 : std::string _uav_name_;
+ 247 : std::string _body_frame_;
+ 248 :
+ 249 : std::string _custom_config_;
+ 250 : std::string _platform_config_;
+ 251 : std::string _world_config_;
+ 252 : std::string _network_config_;
+ 253 :
+ 254 : // | --------------- dynamic loading of trackers -------------- |
+ 255 :
+ 256 : std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Tracker>> tracker_loader_; // pluginlib loader of dynamically loaded trackers
+ 257 : std::vector<std::string> _tracker_names_; // list of tracker names
+ 258 : std::map<std::string, TrackerParams> trackers_; // map between tracker names and tracker param
+ 259 : std::vector<boost::shared_ptr<mrs_uav_managers::Tracker>> tracker_list_; // list of trackers, routines are callable from this
+ 260 : std::mutex mutex_tracker_list_;
+ 261 :
+ 262 : // | ------------- dynamic loading of controllers ------------- |
+ 263 :
+ 264 : std::unique_ptr<pluginlib::ClassLoader<mrs_uav_managers::Controller>> controller_loader_; // pluginlib loader of dynamically loaded controllers
+ 265 : std::vector<std::string> _controller_names_; // list of controller names
+ 266 : std::map<std::string, ControllerParams> controllers_; // map between controller names and controller params
+ 267 : std::vector<boost::shared_ptr<mrs_uav_managers::Controller>> controller_list_; // list of controllers, routines are callable from this
+ 268 : std::mutex mutex_controller_list_;
+ 269 :
+ 270 : // | ------------------------- HW API ------------------------- |
+ 271 :
+ 272 : mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities> sh_hw_api_capabilities_;
+ 273 :
+ 274 : OutputPublisher control_output_publisher_;
+ 275 :
+ 276 : ControlOutputModalities_t _hw_api_inputs_;
+ 277 :
+ 278 : double desired_uav_state_rate_ = 100.0;
+ 279 :
+ 280 : // this timer will check till we already got the hardware api diagnostics
+ 281 : // then it will trigger the initialization of the controllers and finish
+ 282 : // the initialization of the ControlManager
+ 283 : ros::Timer timer_hw_api_capabilities_;
+ 284 : void timerHwApiCapabilities(const ros::TimerEvent& event);
+ 285 :
+ 286 : void preinitialize(void);
+ 287 : void initialize(void);
+ 288 :
+ 289 : // | ------------ tracker and controller switching ------------ |
+ 290 :
+ 291 : std::tuple<bool, std::string> switchController(const std::string& controller_name);
+ 292 : std::tuple<bool, std::string> switchTracker(const std::string& tracker_name);
+ 293 :
+ 294 : // the time of last switching of a tracker or a controller
+ 295 : ros::Time controller_tracker_switch_time_;
+ 296 : std::mutex mutex_controller_tracker_switch_time_;
+ 297 :
+ 298 : // | -------------------- the transformer -------------------- |
+ 299 :
+ 300 : std::shared_ptr<mrs_lib::Transformer> transformer_;
+ 301 :
+ 302 : // | ------------------- scope timer logger ------------------- |
+ 303 :
+ 304 : bool scope_timer_enabled_ = false;
+ 305 : std::shared_ptr<mrs_lib::ScopeTimerLogger> scope_timer_logger_;
+ 306 :
+ 307 : // | --------------------- general params --------------------- |
+ 308 :
+ 309 : // defines the type of state input: odometry or uav_state mesasge types
+ 310 : int _state_input_;
+ 311 :
+ 312 : // names of important trackers
+ 313 : std::string _null_tracker_name_; // null tracker is active when UAV is not in the air
+ 314 : std::string _ehover_tracker_name_; // ehover tracker is used for emergency hovering
+ 315 : std::string _landoff_tracker_name_; // landoff is used for landing and takeoff
+ 316 :
+ 317 : // names of important controllers
+ 318 : std::string _failsafe_controller_name_; // controller used for feed-forward failsafe
+ 319 : std::string _eland_controller_name_; // controller used for emergancy landing
+ 320 :
+ 321 : // joystick control
+ 322 : bool _joystick_enabled_ = false;
+ 323 : int _joystick_mode_;
+ 324 : std::string _joystick_tracker_name_;
+ 325 : std::string _joystick_controller_name_;
+ 326 : std::string _joystick_fallback_tracker_name_;
+ 327 : std::string _joystick_fallback_controller_name_;
+ 328 :
+ 329 : // should disarm after emergancy landing?
+ 330 : bool _eland_disarm_enabled_ = false;
+ 331 :
+ 332 : // enabling the emergency handoff -> will disable eland and failsafe
+ 333 : bool _rc_emergency_handoff_ = false;
+ 334 :
+ 335 : // what throttle should be output when null tracker is active?
+ 336 : double _min_throttle_null_tracker_ = 0.0;
+ 337 :
+ 338 : // rates of all the timers
+ 339 : double _status_timer_rate_ = 0;
+ 340 : double _safety_timer_rate_ = 0;
+ 341 : double _elanding_timer_rate_ = 0;
+ 342 : double _failsafe_timer_rate_ = 0;
+ 343 : double _bumper_timer_rate_ = 0;
+ 344 :
+ 345 : bool _snap_trajectory_to_safety_area_ = false;
+ 346 :
+ 347 : // | -------------- uav_state/odometry subscriber ------------- |
+ 348 :
+ 349 : mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_;
+ 350 : mrs_lib::SubscribeHandler<mrs_msgs::UavState> sh_uav_state_;
+ 351 :
+ 352 : mrs_msgs::UavState uav_state_;
+ 353 : mrs_msgs::UavState previous_uav_state_;
+ 354 : bool got_uav_state_ = false;
+ 355 : double _uav_state_max_missing_time_ = 0; // how long should we tolerate missing state estimate?
+ 356 : double uav_roll_ = 0;
+ 357 : double uav_pitch_ = 0;
+ 358 : double uav_yaw_ = 0;
+ 359 : double uav_heading_ = 0;
+ 360 : std::mutex mutex_uav_state_;
+ 361 :
+ 362 : // odometry hiccup detection
+ 363 : double uav_state_avg_dt_ = 1;
+ 364 : double uav_state_hiccup_factor_ = 1;
+ 365 : int uav_state_count_ = 0;
+ 366 :
+ 367 : mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix> sh_gnss_;
+ 368 :
+ 369 : // | -------------- safety area max z subscriber -------------- |
+ 370 :
+ 371 : mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped> sh_max_z_;
+ 372 :
+ 373 : // | ------------- odometry innovation subscriber ------------- |
+ 374 :
+ 375 : // odometry innovation is published by the odometry node
+ 376 : // it is used to issue eland if the estimator's input is too wonky
+ 377 : mrs_lib::SubscribeHandler<nav_msgs::Odometry> sh_odometry_innovation_;
+ 378 :
+ 379 : // | --------------------- common handlers -------------------- |
+ 380 :
+ 381 : // contains handlers that are shared with trackers and controllers
+ 382 : std::shared_ptr<mrs_uav_managers::control_manager::CommonHandlers_t> common_handlers_;
+ 383 :
+ 384 : // | --------------- tracker and controller IDs --------------- |
+ 385 :
+ 386 : // keeping track of currently active controllers and trackers
+ 387 : unsigned int active_tracker_idx_ = 0;
+ 388 : unsigned int active_controller_idx_ = 0;
+ 389 :
+ 390 : // indeces of some notable trackers
+ 391 : unsigned int _ehover_tracker_idx_ = 0;
+ 392 : unsigned int _landoff_tracker_idx_ = 0;
+ 393 : unsigned int _joystick_tracker_idx_ = 0;
+ 394 : unsigned int _joystick_controller_idx_ = 0;
+ 395 : unsigned int _failsafe_controller_idx_ = 0;
+ 396 : unsigned int _joystick_fallback_controller_idx_ = 0;
+ 397 : unsigned int _joystick_fallback_tracker_idx_ = 0;
+ 398 : unsigned int _null_tracker_idx_ = 0;
+ 399 : unsigned int _eland_controller_idx_ = 0;
+ 400 :
+ 401 : // | -------------- enabling the output publisher ------------- |
+ 402 :
+ 403 : void toggleOutput(const bool& input);
+ 404 : std::atomic<bool> output_enabled_ = false;
+ 405 :
+ 406 : // | ----------------------- publishers ----------------------- |
+ 407 :
+ 408 : mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics> ph_controller_diagnostics_;
+ 409 : mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand> ph_tracker_cmd_;
+ 410 : mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput> ph_mrs_odom_input_;
+ 411 : mrs_lib::PublisherHandler<nav_msgs::Odometry> ph_control_reference_odom_;
+ 412 : mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics> ph_diagnostics_;
+ 413 : mrs_lib::PublisherHandler<std_msgs::Empty> ph_offboard_on_;
+ 414 : mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_tilt_error_;
+ 415 : mrs_lib::PublisherHandler<std_msgs::Float64> ph_mass_estimate_;
+ 416 : mrs_lib::PublisherHandler<std_msgs::Float64> ph_mass_nominal_;
+ 417 : mrs_lib::PublisherHandler<std_msgs::Float64> ph_throttle_;
+ 418 : mrs_lib::PublisherHandler<std_msgs::Float64> ph_thrust_;
+ 419 : mrs_lib::PublisherHandler<mrs_msgs::ControlError> ph_control_error_;
+ 420 : mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> ph_safety_area_markers_;
+ 421 : mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> ph_safety_area_coordinates_markers_;
+ 422 : mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> ph_disturbances_markers_;
+ 423 : mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints> ph_current_constraints_;
+ 424 : mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_heading_;
+ 425 : mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_speed_;
+ 426 :
+ 427 : // | --------------------- service servers -------------------- |
+ 428 :
+ 429 : ros::ServiceServer service_server_switch_tracker_;
+ 430 : ros::ServiceServer service_server_switch_controller_;
+ 431 : ros::ServiceServer service_server_reset_tracker_;
+ 432 : ros::ServiceServer service_server_hover_;
+ 433 : ros::ServiceServer service_server_ehover_;
+ 434 : ros::ServiceServer service_server_failsafe_;
+ 435 : ros::ServiceServer service_server_failsafe_escalating_;
+ 436 : ros::ServiceServer service_server_toggle_output_;
+ 437 : ros::ServiceServer service_server_arm_;
+ 438 : ros::ServiceServer service_server_enable_callbacks_;
+ 439 : ros::ServiceServer service_server_set_constraints_;
+ 440 : ros::ServiceServer service_server_use_joystick_;
+ 441 : ros::ServiceServer service_server_use_safety_area_;
+ 442 : ros::ServiceServer service_server_emergency_reference_;
+ 443 : ros::ServiceServer service_server_pirouette_;
+ 444 : ros::ServiceServer service_server_eland_;
+ 445 : ros::ServiceServer service_server_parachute_;
+ 446 : ros::ServiceServer service_server_set_min_z_;
+ 447 :
+ 448 : // human callbable services for references
+ 449 : ros::ServiceServer service_server_goto_;
+ 450 : ros::ServiceServer service_server_goto_fcu_;
+ 451 : ros::ServiceServer service_server_goto_relative_;
+ 452 : ros::ServiceServer service_server_goto_altitude_;
+ 453 : ros::ServiceServer service_server_set_heading_;
+ 454 : ros::ServiceServer service_server_set_heading_relative_;
+ 455 :
+ 456 : // the reference service and subscriber
+ 457 : ros::ServiceServer service_server_reference_;
+ 458 : mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped> sh_reference_;
+ 459 :
+ 460 : // the velocity reference service and subscriber
+ 461 : ros::ServiceServer service_server_velocity_reference_;
+ 462 : mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped> sh_velocity_reference_;
+ 463 :
+ 464 : // trajectory tracking
+ 465 : ros::ServiceServer service_server_trajectory_reference_;
+ 466 : mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference> sh_trajectory_reference_;
+ 467 : ros::ServiceServer service_server_start_trajectory_tracking_;
+ 468 : ros::ServiceServer service_server_stop_trajectory_tracking_;
+ 469 : ros::ServiceServer service_server_resume_trajectory_tracking_;
+ 470 : ros::ServiceServer service_server_goto_trajectory_start_;
+ 471 :
+ 472 : // transform service servers
+ 473 : ros::ServiceServer service_server_transform_reference_;
+ 474 : ros::ServiceServer service_server_transform_pose_;
+ 475 : ros::ServiceServer service_server_transform_vector3_;
+ 476 :
+ 477 : // safety area services
+ 478 : ros::ServiceServer service_server_validate_reference_;
+ 479 : ros::ServiceServer service_server_validate_reference_2d_;
+ 480 : ros::ServiceServer service_server_validate_reference_list_;
+ 481 :
+ 482 : // bumper service servers
+ 483 : ros::ServiceServer service_server_bumper_enabler_;
+ 484 : ros::ServiceServer service_server_bumper_repulsion_enabler_;
+ 485 :
+ 486 : // service clients
+ 487 : mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_arming_;
+ 488 : mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_eland_;
+ 489 : mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_shutdown_;
+ 490 : mrs_lib::ServiceClientHandler<std_srvs::SetBool> sch_set_odometry_callbacks_;
+ 491 : mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_parachute_;
+ 492 :
+ 493 : // safety area min z servers
+ 494 : ros::ServiceServer service_server_get_min_z_;
+ 495 :
+ 496 : // | --------- trackers' and controllers' last results -------- |
+ 497 :
+ 498 : // the last result of an active tracker
+ 499 : std::optional<mrs_msgs::TrackerCommand> last_tracker_cmd_;
+ 500 : std::mutex mutex_last_tracker_cmd_;
+ 501 :
+ 502 : // the last result of an active controller
+ 503 : Controller::ControlOutput last_control_output_;
+ 504 : std::mutex mutex_last_control_output_;
+ 505 :
+ 506 : // | -------------- HW API diagnostics subscriber ------------- |
+ 507 :
+ 508 : mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus> sh_hw_api_status_;
+ 509 :
+ 510 : bool offboard_mode_ = false;
+ 511 : bool offboard_mode_was_true_ = false; // if it was ever true
+ 512 : bool armed_ = false;
+ 513 :
+ 514 : // | -------------------- throttle and mass ------------------- |
+ 515 :
+ 516 : // throttle mass estimation during eland
+ 517 : double throttle_mass_estimate_ = 0;
+ 518 : bool throttle_under_threshold_ = false;
+ 519 : ros::Time throttle_mass_estimate_first_time_;
+ 520 :
+ 521 : // | ---------------------- safety params --------------------- |
+ 522 :
+ 523 : // failsafe when tilt error is too large
+ 524 : bool _tilt_error_disarm_enabled_;
+ 525 : double _tilt_error_disarm_timeout_;
+ 526 : double _tilt_error_disarm_threshold_;
+ 527 :
+ 528 : ros::Time tilt_error_disarm_time_;
+ 529 : bool tilt_error_disarm_over_thr_ = false;
+ 530 :
+ 531 : // elanding when tilt error is too large
+ 532 : bool _tilt_limit_eland_enabled_;
+ 533 : double _tilt_limit_eland_ = 0; // [rad]
+ 534 :
+ 535 : // disarming when tilt error is too large
+ 536 : bool _tilt_limit_disarm_enabled_;
+ 537 : double _tilt_limit_disarm_ = 0; // [rad]
+ 538 :
+ 539 : // elanding when yaw error is too large
+ 540 : bool _yaw_error_eland_enabled_;
+ 541 : double _yaw_error_eland_ = 0; // [rad]
+ 542 :
+ 543 : // keeping track of control errors
+ 544 : std::optional<double> tilt_error_ = 0;
+ 545 : std::optional<double> yaw_error_ = 0;
+ 546 : std::mutex mutex_attitude_error_;
+ 547 :
+ 548 : std::optional<Eigen::Vector3d> position_error_;
+ 549 : std::mutex mutex_position_error_;
+ 550 :
+ 551 : // control error for triggering failsafe, eland, etc.
+ 552 : // this filled with the current controllers failsafe threshold
+ 553 : double _failsafe_threshold_ = 0; // control error for triggering failsafe
+ 554 : double _eland_threshold_ = 0; // control error for triggering eland
+ 555 : bool _odometry_innovation_check_enabled_ = false;
+ 556 : double _odometry_innovation_threshold_ = 0; // innovation size for triggering eland
+ 557 :
+ 558 : bool callbacks_enabled_ = true;
+ 559 :
+ 560 : // | ------------------------ parachute ----------------------- |
+ 561 :
+ 562 : bool _parachute_enabled_ = false;
+ 563 :
+ 564 : std::tuple<bool, std::string> deployParachute(void);
+ 565 : bool parachuteSrv(void);
+ 566 :
+ 567 : // | ----------------------- safety area ---------------------- |
+ 568 :
+ 569 : // safety area
+ 570 : std::unique_ptr<mrs_lib::SafetyZone> safety_zone_;
+ 571 :
+ 572 : std::atomic<bool> use_safety_area_ = false;
+ 573 :
+ 574 : std::string _safety_area_horizontal_frame_;
+ 575 : std::string _safety_area_vertical_frame_;
+ 576 :
+ 577 : std::atomic<double> _safety_area_min_z_ = 0;
+ 578 :
+ 579 : double _safety_area_max_z_ = 0;
+ 580 :
+ 581 : // safety area routines
+ 582 : // those are passed to trackers using the common_handlers object
+ 583 : bool isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point);
+ 584 : bool isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point);
+ 585 : bool isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+ 586 : bool isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& from, const mrs_msgs::ReferenceStamped& to);
+ 587 : double getMinZ(const std::string& frame_id);
+ 588 : double getMaxZ(const std::string& frame_id);
+ 589 :
+ 590 : // | ------------------------ callbacks ----------------------- |
+ 591 :
+ 592 : // topic callbacks
+ 593 : void callbackOdometry(const nav_msgs::Odometry::ConstPtr msg);
+ 594 : void callbackUavState(const mrs_msgs::UavState::ConstPtr msg);
+ 595 : void callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg);
+ 596 : void callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg);
+ 597 : void callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg);
+ 598 :
+ 599 : // topic timeouts
+ 600 : void timeoutUavState(const double& missing_for);
+ 601 :
+ 602 : // switching controller and tracker services
+ 603 : bool callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+ 604 : bool callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res);
+ 605 : bool callbackTrackerResetStatic(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 606 :
+ 607 : // reference callbacks
+ 608 : void callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg);
+ 609 : void callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg);
+ 610 : void callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg);
+ 611 : bool callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+ 612 : bool callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+ 613 : bool callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res);
+ 614 : bool callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+ 615 : bool callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+ 616 : bool callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res);
+ 617 : bool callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+ 618 : bool callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request& req, mrs_msgs::VelocityReferenceStampedSrv::Response& res);
+ 619 : bool callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res);
+ 620 : bool callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res);
+ 621 :
+ 622 : // safety callbacks
+ 623 : bool callbackHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 624 : bool callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 625 : bool callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 626 : bool callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 627 : bool callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 628 : bool callbackEHover(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 629 : bool callbackFailsafe(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 630 : bool callbackFailsafeEscalating(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 631 : bool callbackEland(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 632 : bool callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 633 : bool callbackSetMinZ(mrs_msgs::Float64StampedSrv::Request& req, mrs_msgs::Float64StampedSrv::Response& res);
+ 634 : bool callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+ 635 : bool callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+ 636 : bool callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+ 637 : bool callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+ 638 : bool callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res);
+ 639 :
+ 640 : bool callbackGetMinZ(mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res);
+ 641 :
+ 642 : bool callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+ 643 : bool callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res);
+ 644 : bool callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res);
+ 645 :
+ 646 : // transformation callbacks
+ 647 : bool callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res);
+ 648 : bool callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res);
+ 649 : bool callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res);
+ 650 :
+ 651 : // | ----------------------- constraints ---------------------- |
+ 652 :
+ 653 : // sets constraints to all trackers
+ 654 : bool callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res);
+ 655 :
+ 656 : // constraints management
+ 657 : bool got_constraints_ = false;
+ 658 : std::mutex mutex_constraints_;
+ 659 : void setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+ 660 : void setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+ 661 : void setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+ 662 : std::atomic<bool> constraints_being_enforced_ = false;
+ 663 :
+ 664 : std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> enforceControllersConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints);
+ 665 :
+ 666 : mrs_msgs::DynamicsConstraintsSrvRequest current_constraints_;
+ 667 : mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints_;
+ 668 :
+ 669 : // | ------------------ emergency triggered? ------------------ |
+ 670 :
+ 671 : std::atomic<bool> failsafe_triggered_ = false;
+ 672 : std::atomic<bool> eland_triggered_ = false;
+ 673 :
+ 674 : // | ------------------------- timers ------------------------- |
+ 675 :
+ 676 : // timer for regular status publishing
+ 677 : ros::Timer timer_status_;
+ 678 : void timerStatus(const ros::TimerEvent& event);
+ 679 :
+ 680 : // timer for issuing the failsafe landing
+ 681 : ros::Timer timer_failsafe_;
+ 682 : void timerFailsafe(const ros::TimerEvent& event);
+ 683 :
+ 684 : // oneshot timer for running controllers and trackers
+ 685 : void asyncControl(void);
+ 686 : std::atomic<bool> running_async_control_ = false;
+ 687 : std::future<void> async_control_result_;
+ 688 :
+ 689 : // timer for issuing emergancy landing
+ 690 : ros::Timer timer_eland_;
+ 691 : void timerEland(const ros::TimerEvent& event);
+ 692 :
+ 693 : // timer for regular checking of controller errors
+ 694 : ros::Timer timer_safety_;
+ 695 : void timerSafety(const ros::TimerEvent& event);
+ 696 : std::atomic<bool> running_safety_timer_ = false;
+ 697 : std::atomic<bool> odometry_switch_in_progress_ = false;
+ 698 :
+ 699 : // timer for issuing the pirouette
+ 700 : ros::Timer timer_pirouette_;
+ 701 : void timerPirouette(const ros::TimerEvent& event);
+ 702 :
+ 703 : // | --------------------- obstacle bumper -------------------- |
+ 704 :
+ 705 : // bumper timer
+ 706 : ros::Timer timer_bumper_;
+ 707 : void timerBumper(const ros::TimerEvent& event);
+ 708 :
+ 709 : // bumper subscriber
+ 710 : mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors> sh_bumper_;
+ 711 :
+ 712 : bool _bumper_switch_tracker_ = false;
+ 713 : bool _bumper_switch_controller_ = false;
+ 714 : std::string _bumper_tracker_name_;
+ 715 : std::string _bumper_controller_name_;
+ 716 : std::string bumper_previous_tracker_;
+ 717 : std::string bumper_previous_controller_;
+ 718 :
+ 719 : std::atomic<bool> bumper_enabled_ = false;
+ 720 : std::atomic<bool> bumper_repulsing_ = false;
+ 721 :
+ 722 : bool _bumper_horizontal_derive_from_dynamics_;
+ 723 : bool _bumper_vertical_derive_from_dynamics_;
+ 724 :
+ 725 : double _bumper_horizontal_distance_ = 0;
+ 726 : double _bumper_vertical_distance_ = 0;
+ 727 :
+ 728 : double _bumper_horizontal_overshoot_ = 0;
+ 729 : double _bumper_vertical_overshoot_ = 0;
+ 730 :
+ 731 : int bumperGetSectorId(const double& x, const double& y, const double& z);
+ 732 : void bumperPushFromObstacle(void);
+ 733 :
+ 734 : // | --------------- safety checks and failsafes -------------- |
+ 735 :
+ 736 : // escalating failsafe (eland -> failsafe -> disarm)
+ 737 : bool _service_escalating_failsafe_enabled_ = false;
+ 738 : bool _rc_escalating_failsafe_enabled_ = false;
+ 739 : double _escalating_failsafe_timeout_ = 0;
+ 740 : ros::Time escalating_failsafe_time_;
+ 741 : bool _escalating_failsafe_ehover_ = false;
+ 742 : bool _escalating_failsafe_eland_ = false;
+ 743 : bool _escalating_failsafe_failsafe_ = false;
+ 744 : double _rc_escalating_failsafe_threshold_;
+ 745 : int _rc_escalating_failsafe_channel_ = 0;
+ 746 : bool rc_escalating_failsafe_triggered_ = false;
+ 747 : EscalatingFailsafeStates_t state_escalating_failsafe_ = ESC_NONE_STATE;
+ 748 :
+ 749 : std::string _tracker_error_action_;
+ 750 :
+ 751 : // emergancy landing state machine
+ 752 : LandingStates_t current_state_landing_ = IDLE_STATE;
+ 753 : LandingStates_t previous_state_landing_ = IDLE_STATE;
+ 754 : std::mutex mutex_landing_state_machine_;
+ 755 : void changeLandingState(LandingStates_t new_state);
+ 756 : double _uav_mass_ = 0;
+ 757 : double _elanding_cutoff_mass_factor_;
+ 758 : double _elanding_cutoff_timeout_;
+ 759 : double landing_uav_mass_ = 0;
+ 760 :
+ 761 : // initial body disturbance loaded from params
+ 762 : double _initial_body_disturbance_x_ = 0;
+ 763 : double _initial_body_disturbance_y_ = 0;
+ 764 :
+ 765 : // profiling
+ 766 : mrs_lib::Profiler profiler_;
+ 767 : bool _profiler_enabled_ = false;
+ 768 :
+ 769 : // diagnostics publishing
+ 770 : void publishDiagnostics(void);
+ 771 : std::mutex mutex_diagnostics_;
+ 772 :
+ 773 : void ungripSrv(void);
+ 774 : mrs_lib::ServiceClientHandler<std_srvs::Trigger> sch_ungrip_;
+ 775 :
+ 776 : bool isFlyingNormally(void);
+ 777 :
+ 778 : // | ------------------------ pirouette ----------------------- |
+ 779 :
+ 780 : bool _pirouette_enabled_ = false;
+ 781 : double _pirouette_speed_;
+ 782 : double _pirouette_timer_rate_;
+ 783 : std::mutex mutex_pirouette_;
+ 784 : double pirouette_initial_heading_;
+ 785 : double pirouette_iterator_;
+ 786 : bool callbackPirouette(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 787 :
+ 788 : // | -------------------- joystick control -------------------- |
+ 789 :
+ 790 : mrs_lib::SubscribeHandler<sensor_msgs::Joy> sh_joystick_;
+ 791 :
+ 792 : void callbackJoystick(const sensor_msgs::Joy::ConstPtr msg);
+ 793 : bool callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
+ 794 :
+ 795 : // joystick buttons mappings
+ 796 : int _channel_A_, _channel_B_, _channel_X_, _channel_Y_, _channel_start_, _channel_back_, _channel_LT_, _channel_RT_, _channel_L_joy_, _channel_R_joy_;
+ 797 :
+ 798 : // channel numbers and channel multipliers
+ 799 : int _channel_pitch_, _channel_roll_, _channel_heading_, _channel_throttle_;
+ 800 : double _channel_mult_pitch_, _channel_mult_roll_, _channel_mult_heading_, _channel_mult_throttle_;
+ 801 :
+ 802 : ros::Timer timer_joystick_;
+ 803 : void timerJoystick(const ros::TimerEvent& event);
+ 804 : double _joystick_timer_rate_ = 0;
+ 805 :
+ 806 : double _joystick_carrot_distance_ = 0;
+ 807 :
+ 808 : ros::Time joystick_start_press_time_;
+ 809 : bool joystick_start_pressed_ = false;
+ 810 :
+ 811 : ros::Time joystick_back_press_time_;
+ 812 : bool joystick_back_pressed_ = false;
+ 813 : bool joystick_goto_enabled_ = false;
+ 814 :
+ 815 : bool joystick_failsafe_pressed_ = false;
+ 816 : ros::Time joystick_failsafe_press_time_;
+ 817 :
+ 818 : bool joystick_eland_pressed_ = false;
+ 819 : ros::Time joystick_eland_press_time_;
+ 820 :
+ 821 : // | ------------------- RC joystick control ------------------ |
+ 822 :
+ 823 : // listening to the RC channels as told by pixhawk
+ 824 : mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels> sh_hw_api_rc_;
+ 825 :
+ 826 : // the RC channel mapping of the main 4 control signals
+ 827 : double _rc_channel_pitch_, _rc_channel_roll_, _rc_channel_heading_, _rc_channel_throttle_;
+ 828 :
+ 829 : bool _rc_goto_enabled_ = false;
+ 830 : std::atomic<bool> rc_goto_active_ = false;
+ 831 : double rc_joystick_channel_last_value_ = 0.5;
+ 832 : bool rc_joystick_channel_was_low_ = false;
+ 833 : int _rc_joystick_channel_ = 0;
+ 834 :
+ 835 : double _rc_horizontal_speed_ = 0;
+ 836 : double _rc_vertical_speed_ = 0;
+ 837 : double _rc_heading_rate_ = 0;
+ 838 :
+ 839 : // | ------------------- trajectory loading ------------------- |
+ 840 :
+ 841 : mrs_lib::PublisherHandler<geometry_msgs::PoseArray> pub_debug_original_trajectory_poses_;
+ 842 : mrs_lib::PublisherHandler<visualization_msgs::MarkerArray> pub_debug_original_trajectory_markers_;
+ 843 :
+ 844 : // | --------------------- other routines --------------------- |
+ 845 :
+ 846 : // this is called to update the trackers and to receive position control command from the active one
+ 847 : void updateTrackers(void);
+ 848 :
+ 849 : // this is called to update the controllers and to receive attitude control command from the active one
+ 850 : void updateControllers(const mrs_msgs::UavState& uav_state);
+ 851 :
+ 852 : // sets the reference to the active tracker
+ 853 : std::tuple<bool, std::string> setReference(const mrs_msgs::ReferenceStamped reference_in);
+ 854 :
+ 855 : // sets the velocity reference to the active tracker
+ 856 : std::tuple<bool, std::string> setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in);
+ 857 :
+ 858 : // sets the reference trajectory to the active tracker
+ 859 : std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> setTrajectoryReference(
+ 860 : const mrs_msgs::TrajectoryReference trajectory_in);
+ 861 :
+ 862 : // publishes
+ 863 : void publish(void);
+ 864 :
+ 865 : bool loadConfigFile(const std::string& file_path, const std::string ns);
+ 866 :
+ 867 : double getMass(void);
+ 868 :
+ 869 : // publishes rviz-visualizable control reference
+ 870 : void publishControlReferenceOdom(const std::optional<mrs_msgs::TrackerCommand>& tracker_command, const Controller::ControlOutput& control_output);
+ 871 :
+ 872 : void initializeControlOutput(void);
+ 873 :
+ 874 : // tell the mrs_odometry to disable its callbacks
+ 875 : void odometryCallbacksSrv(const bool input);
+ 876 :
+ 877 : mrs_msgs::ReferenceStamped velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference);
+ 878 :
+ 879 : void setCallbacks(bool in);
+ 880 : bool isOffboard(void);
+ 881 : bool elandSrv(void);
+ 882 : std::tuple<bool, std::string> arming(const bool input);
+ 883 :
+ 884 : // safety functions impl
+ 885 : std::tuple<bool, std::string> ehover(void);
+ 886 : std::tuple<bool, std::string> hover(void);
+ 887 : std::tuple<bool, std::string> startTrajectoryTracking(void);
+ 888 : std::tuple<bool, std::string> stopTrajectoryTracking(void);
+ 889 : std::tuple<bool, std::string> resumeTrajectoryTracking(void);
+ 890 : std::tuple<bool, std::string> gotoTrajectoryStart(void);
+ 891 : std::tuple<bool, std::string> eland(void);
+ 892 : std::tuple<bool, std::string> failsafe(void);
+ 893 : std::tuple<bool, std::string> escalatingFailsafe(void);
+ 894 :
+ 895 : EscalatingFailsafeStates_t getNextEscFailsafeState(void);
+ 896 : };
+ 897 :
+ 898 : //}
+ 899 :
+ 900 : /* //{ onInit() */
+ 901 :
+ 902 91 : void ControlManager::onInit() {
+ 903 91 : preinitialize();
+ 904 91 : }
+ 905 :
+ 906 : //}
+ 907 :
+ 908 : /* preinitialize() //{ */
+ 909 :
+ 910 91 : void ControlManager::preinitialize(void) {
+ 911 :
+ 912 91 : nh_ = nodelet::Nodelet::getMTPrivateNodeHandle();
+ 913 :
+ 914 91 : ros::Time::waitForValid();
+ 915 :
+ 916 91 : mrs_lib::SubscribeHandlerOptions shopts;
+ 917 91 : shopts.nh = nh_;
+ 918 91 : shopts.node_name = "ControlManager";
+ 919 91 : shopts.no_message_timeout = mrs_lib::no_timeout;
+ 920 91 : shopts.threadsafe = true;
+ 921 91 : shopts.autostart = true;
+ 922 91 : shopts.queue_size = 10;
+ 923 91 : shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+ 924 :
+ 925 91 : sh_hw_api_capabilities_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiCapabilities>(shopts, "hw_api_capabilities_in");
+ 926 :
+ 927 91 : timer_hw_api_capabilities_ = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerHwApiCapabilities, this);
+ 928 91 : }
+ 929 :
+ 930 : //}
+ 931 :
+ 932 : /* initialize() //{ */
+ 933 :
+ 934 91 : void ControlManager::initialize(void) {
+ 935 :
+ 936 91 : joystick_start_press_time_ = ros::Time(0);
+ 937 91 : joystick_failsafe_press_time_ = ros::Time(0);
+ 938 91 : joystick_eland_press_time_ = ros::Time(0);
+ 939 91 : escalating_failsafe_time_ = ros::Time(0);
+ 940 91 : controller_tracker_switch_time_ = ros::Time(0);
+ 941 :
+ 942 91 : ROS_INFO("[ControlManager]: initializing");
+ 943 :
+ 944 : // --------------------------------------------------------------
+ 945 : // | common handler for trackers and controllers |
+ 946 : // --------------------------------------------------------------
+ 947 :
+ 948 91 : common_handlers_ = std::make_shared<mrs_uav_managers::control_manager::CommonHandlers_t>();
+ 949 :
+ 950 : // --------------------------------------------------------------
+ 951 : // | params |
+ 952 : // --------------------------------------------------------------
+ 953 :
+ 954 182 : mrs_lib::ParamLoader param_loader(nh_, "ControlManager");
+ 955 :
+ 956 91 : param_loader.loadParam("custom_config", _custom_config_);
+ 957 91 : param_loader.loadParam("platform_config", _platform_config_);
+ 958 91 : param_loader.loadParam("world_config", _world_config_);
+ 959 91 : param_loader.loadParam("network_config", _network_config_);
+ 960 :
+ 961 91 : if (_custom_config_ != "") {
+ 962 91 : param_loader.addYamlFile(_custom_config_);
+ 963 : }
+ 964 :
+ 965 91 : if (_platform_config_ != "") {
+ 966 91 : param_loader.addYamlFile(_platform_config_);
+ 967 : }
+ 968 :
+ 969 91 : if (_world_config_ != "") {
+ 970 91 : param_loader.addYamlFile(_world_config_);
+ 971 : }
+ 972 :
+ 973 91 : if (_network_config_ != "") {
+ 974 91 : param_loader.addYamlFile(_network_config_);
+ 975 : }
+ 976 :
+ 977 91 : param_loader.addYamlFileFromParam("private_config");
+ 978 91 : param_loader.addYamlFileFromParam("public_config");
+ 979 91 : param_loader.addYamlFileFromParam("private_trackers");
+ 980 91 : param_loader.addYamlFileFromParam("private_controllers");
+ 981 91 : param_loader.addYamlFileFromParam("public_controllers");
+ 982 :
+ 983 : // params passed from the launch file are not prefixed
+ 984 91 : param_loader.loadParam("uav_name", _uav_name_);
+ 985 91 : param_loader.loadParam("body_frame", _body_frame_);
+ 986 91 : param_loader.loadParam("enable_profiler", _profiler_enabled_);
+ 987 91 : param_loader.loadParam("uav_mass", _uav_mass_);
+ 988 91 : param_loader.loadParam("body_disturbance_x", _initial_body_disturbance_x_);
+ 989 91 : param_loader.loadParam("body_disturbance_y", _initial_body_disturbance_y_);
+ 990 91 : param_loader.loadParam("g", common_handlers_->g);
+ 991 :
+ 992 : // motor params are also not prefixed, since they are common to more nodes
+ 993 91 : param_loader.loadParam("motor_params/a", common_handlers_->throttle_model.A);
+ 994 91 : param_loader.loadParam("motor_params/b", common_handlers_->throttle_model.B);
+ 995 91 : param_loader.loadParam("motor_params/n_motors", common_handlers_->throttle_model.n_motors);
+ 996 :
+ 997 : // | ----------------------- safety area ---------------------- |
+ 998 :
+ 999 : bool use_safety_area;
+ 1000 91 : param_loader.loadParam("safety_area/enabled", use_safety_area);
+ 1001 91 : use_safety_area_ = use_safety_area;
+ 1002 :
+ 1003 91 : param_loader.loadParam("safety_area/horizontal/frame_name", _safety_area_horizontal_frame_);
+ 1004 :
+ 1005 91 : param_loader.loadParam("safety_area/vertical/frame_name", _safety_area_vertical_frame_);
+ 1006 91 : param_loader.loadParam("safety_area/vertical/max_z", _safety_area_max_z_);
+ 1007 :
+ 1008 : {
+ 1009 : double temp;
+ 1010 91 : param_loader.loadParam("safety_area/vertical/min_z", temp);
+ 1011 :
+ 1012 91 : _safety_area_min_z_ = temp;
+ 1013 : }
+ 1014 :
+ 1015 91 : if (use_safety_area_) {
+ 1016 :
+ 1017 213 : Eigen::MatrixXd border_points = param_loader.loadMatrixDynamic2("safety_area/horizontal/points", -1, 2);
+ 1018 :
+ 1019 : try {
+ 1020 :
+ 1021 142 : std::vector<Eigen::MatrixXd> polygon_obstacle_points;
+ 1022 71 : std::vector<Eigen::MatrixXd> point_obstacle_points;
+ 1023 :
+ 1024 71 : safety_zone_ = std::make_unique<mrs_lib::SafetyZone>(border_points);
+ 1025 : }
+ 1026 :
+ 1027 0 : catch (mrs_lib::SafetyZone::BorderError& e) {
+ 1028 0 : ROS_ERROR("[ControlManager]: SafetyArea: wrong configruation for the safety zone border polygon");
+ 1029 0 : ros::shutdown();
+ 1030 : }
+ 1031 0 : catch (...) {
+ 1032 0 : ROS_ERROR("[ControlManager]: SafetyArea: unhandled exception!");
+ 1033 0 : ros::shutdown();
+ 1034 : }
+ 1035 :
+ 1036 71 : ROS_INFO("[ControlManager]: safety area initialized");
+ 1037 : }
+ 1038 :
+ 1039 91 : param_loader.setPrefix("mrs_uav_managers/control_manager/");
+ 1040 :
+ 1041 91 : param_loader.loadParam("state_input", _state_input_);
+ 1042 :
+ 1043 91 : if (!(_state_input_ == INPUT_UAV_STATE || _state_input_ == INPUT_ODOMETRY)) {
+ 1044 0 : ROS_ERROR("[ControlManager]: the state_input parameter has to be in {0, 1}");
+ 1045 0 : ros::shutdown();
+ 1046 : }
+ 1047 :
+ 1048 91 : param_loader.loadParam("safety/min_throttle_null_tracker", _min_throttle_null_tracker_);
+ 1049 91 : param_loader.loadParam("safety/ehover_tracker", _ehover_tracker_name_);
+ 1050 91 : param_loader.loadParam("safety/failsafe_controller", _failsafe_controller_name_);
+ 1051 :
+ 1052 91 : param_loader.loadParam("safety/eland/controller", _eland_controller_name_);
+ 1053 91 : param_loader.loadParam("safety/eland/cutoff_mass_factor", _elanding_cutoff_mass_factor_);
+ 1054 91 : param_loader.loadParam("safety/eland/cutoff_timeout", _elanding_cutoff_timeout_);
+ 1055 91 : param_loader.loadParam("safety/eland/timer_rate", _elanding_timer_rate_);
+ 1056 91 : param_loader.loadParam("safety/eland/disarm", _eland_disarm_enabled_);
+ 1057 :
+ 1058 91 : param_loader.loadParam("safety/escalating_failsafe/service/enabled", _service_escalating_failsafe_enabled_);
+ 1059 91 : param_loader.loadParam("safety/escalating_failsafe/rc/enabled", _rc_escalating_failsafe_enabled_);
+ 1060 91 : param_loader.loadParam("safety/escalating_failsafe/rc/channel_number", _rc_escalating_failsafe_channel_);
+ 1061 91 : param_loader.loadParam("safety/escalating_failsafe/rc/threshold", _rc_escalating_failsafe_threshold_);
+ 1062 91 : param_loader.loadParam("safety/escalating_failsafe/timeout", _escalating_failsafe_timeout_);
+ 1063 91 : param_loader.loadParam("safety/escalating_failsafe/ehover", _escalating_failsafe_ehover_);
+ 1064 91 : param_loader.loadParam("safety/escalating_failsafe/eland", _escalating_failsafe_eland_);
+ 1065 91 : param_loader.loadParam("safety/escalating_failsafe/failsafe", _escalating_failsafe_failsafe_);
+ 1066 :
+ 1067 91 : param_loader.loadParam("safety/tilt_limit/eland/enabled", _tilt_limit_eland_enabled_);
+ 1068 91 : param_loader.loadParam("safety/tilt_limit/eland/limit", _tilt_limit_eland_);
+ 1069 :
+ 1070 91 : _tilt_limit_eland_ = M_PI * (_tilt_limit_eland_ / 180.0);
+ 1071 :
+ 1072 91 : if (_tilt_limit_eland_enabled_ && fabs(_tilt_limit_eland_) < 1e-3) {
+ 1073 0 : ROS_ERROR("[ControlManager]: safety/tilt_limit/eland/enabled = 'TRUE' but the limit is too low");
+ 1074 0 : ros::shutdown();
+ 1075 : }
+ 1076 :
+ 1077 91 : param_loader.loadParam("safety/tilt_limit/disarm/enabled", _tilt_limit_disarm_enabled_);
+ 1078 91 : param_loader.loadParam("safety/tilt_limit/disarm/limit", _tilt_limit_disarm_);
+ 1079 :
+ 1080 91 : _tilt_limit_disarm_ = M_PI * (_tilt_limit_disarm_ / 180.0);
+ 1081 :
+ 1082 91 : if (_tilt_limit_disarm_enabled_ && fabs(_tilt_limit_disarm_) < 1e-3) {
+ 1083 0 : ROS_ERROR("[ControlManager]: safety/tilt_limit/disarm/enabled = 'TRUE' but the limit is too low");
+ 1084 0 : ros::shutdown();
+ 1085 : }
+ 1086 :
+ 1087 91 : param_loader.loadParam("safety/yaw_error_eland/enabled", _yaw_error_eland_enabled_);
+ 1088 91 : param_loader.loadParam("safety/yaw_error_eland/limit", _yaw_error_eland_);
+ 1089 :
+ 1090 91 : _yaw_error_eland_ = M_PI * (_yaw_error_eland_ / 180.0);
+ 1091 :
+ 1092 91 : if (_yaw_error_eland_enabled_ && fabs(_yaw_error_eland_) < 1e-3) {
+ 1093 0 : ROS_ERROR("[ControlManager]: safety/yaw_error_eland/enabled = 'TRUE' but the limit is too low");
+ 1094 0 : ros::shutdown();
+ 1095 : }
+ 1096 :
+ 1097 91 : param_loader.loadParam("status_timer_rate", _status_timer_rate_);
+ 1098 91 : param_loader.loadParam("safety/safety_timer_rate", _safety_timer_rate_);
+ 1099 91 : param_loader.loadParam("safety/failsafe_timer_rate", _failsafe_timer_rate_);
+ 1100 91 : param_loader.loadParam("safety/rc_emergency_handoff/enabled", _rc_emergency_handoff_);
+ 1101 :
+ 1102 91 : param_loader.loadParam("safety/odometry_max_missing_time", _uav_state_max_missing_time_);
+ 1103 91 : param_loader.loadParam("safety/odometry_innovation_eland/enabled", _odometry_innovation_check_enabled_);
+ 1104 :
+ 1105 91 : param_loader.loadParam("safety/tilt_error_disarm/enabled", _tilt_error_disarm_enabled_);
+ 1106 91 : param_loader.loadParam("safety/tilt_error_disarm/timeout", _tilt_error_disarm_timeout_);
+ 1107 91 : param_loader.loadParam("safety/tilt_error_disarm/error_threshold", _tilt_error_disarm_threshold_);
+ 1108 :
+ 1109 91 : _tilt_error_disarm_threshold_ = M_PI * (_tilt_error_disarm_threshold_ / 180.0);
+ 1110 :
+ 1111 91 : if (_tilt_error_disarm_enabled_ && fabs(_tilt_error_disarm_threshold_) < 1e-3) {
+ 1112 0 : ROS_ERROR("[ControlManager]: safety/tilt_error_disarm/enabled = 'TRUE' but the limit is too low");
+ 1113 0 : ros::shutdown();
+ 1114 : }
+ 1115 :
+ 1116 : // default constraints
+ 1117 :
+ 1118 91 : param_loader.loadParam("default_constraints/horizontal/speed", current_constraints_.constraints.horizontal_speed);
+ 1119 91 : param_loader.loadParam("default_constraints/horizontal/acceleration", current_constraints_.constraints.horizontal_acceleration);
+ 1120 91 : param_loader.loadParam("default_constraints/horizontal/jerk", current_constraints_.constraints.horizontal_jerk);
+ 1121 91 : param_loader.loadParam("default_constraints/horizontal/snap", current_constraints_.constraints.horizontal_snap);
+ 1122 :
+ 1123 91 : param_loader.loadParam("default_constraints/vertical/ascending/speed", current_constraints_.constraints.vertical_ascending_speed);
+ 1124 91 : param_loader.loadParam("default_constraints/vertical/ascending/acceleration", current_constraints_.constraints.vertical_ascending_acceleration);
+ 1125 91 : param_loader.loadParam("default_constraints/vertical/ascending/jerk", current_constraints_.constraints.vertical_ascending_jerk);
+ 1126 91 : param_loader.loadParam("default_constraints/vertical/ascending/snap", current_constraints_.constraints.vertical_ascending_snap);
+ 1127 :
+ 1128 91 : param_loader.loadParam("default_constraints/vertical/descending/speed", current_constraints_.constraints.vertical_descending_speed);
+ 1129 91 : param_loader.loadParam("default_constraints/vertical/descending/acceleration", current_constraints_.constraints.vertical_descending_acceleration);
+ 1130 91 : param_loader.loadParam("default_constraints/vertical/descending/jerk", current_constraints_.constraints.vertical_descending_jerk);
+ 1131 91 : param_loader.loadParam("default_constraints/vertical/descending/snap", current_constraints_.constraints.vertical_descending_snap);
+ 1132 :
+ 1133 91 : param_loader.loadParam("default_constraints/heading/speed", current_constraints_.constraints.heading_speed);
+ 1134 91 : param_loader.loadParam("default_constraints/heading/acceleration", current_constraints_.constraints.heading_acceleration);
+ 1135 91 : param_loader.loadParam("default_constraints/heading/jerk", current_constraints_.constraints.heading_jerk);
+ 1136 91 : param_loader.loadParam("default_constraints/heading/snap", current_constraints_.constraints.heading_snap);
+ 1137 :
+ 1138 91 : param_loader.loadParam("default_constraints/angular_speed/roll", current_constraints_.constraints.roll_rate);
+ 1139 91 : param_loader.loadParam("default_constraints/angular_speed/pitch", current_constraints_.constraints.pitch_rate);
+ 1140 91 : param_loader.loadParam("default_constraints/angular_speed/yaw", current_constraints_.constraints.yaw_rate);
+ 1141 :
+ 1142 91 : param_loader.loadParam("default_constraints/tilt", current_constraints_.constraints.tilt);
+ 1143 :
+ 1144 91 : current_constraints_.constraints.tilt = M_PI * (current_constraints_.constraints.tilt / 180.0);
+ 1145 :
+ 1146 : // joystick
+ 1147 :
+ 1148 91 : param_loader.loadParam("joystick/enabled", _joystick_enabled_);
+ 1149 91 : param_loader.loadParam("joystick/mode", _joystick_mode_);
+ 1150 91 : param_loader.loadParam("joystick/carrot_distance", _joystick_carrot_distance_);
+ 1151 91 : param_loader.loadParam("joystick/joystick_timer_rate", _joystick_timer_rate_);
+ 1152 91 : param_loader.loadParam("joystick/attitude_control/tracker", _joystick_tracker_name_);
+ 1153 91 : param_loader.loadParam("joystick/attitude_control/controller", _joystick_controller_name_);
+ 1154 91 : param_loader.loadParam("joystick/attitude_control/fallback/tracker", _joystick_fallback_tracker_name_);
+ 1155 91 : param_loader.loadParam("joystick/attitude_control/fallback/controller", _joystick_fallback_controller_name_);
+ 1156 :
+ 1157 91 : param_loader.loadParam("joystick/channels/A", _channel_A_);
+ 1158 91 : param_loader.loadParam("joystick/channels/B", _channel_B_);
+ 1159 91 : param_loader.loadParam("joystick/channels/X", _channel_X_);
+ 1160 91 : param_loader.loadParam("joystick/channels/Y", _channel_Y_);
+ 1161 91 : param_loader.loadParam("joystick/channels/start", _channel_start_);
+ 1162 91 : param_loader.loadParam("joystick/channels/back", _channel_back_);
+ 1163 91 : param_loader.loadParam("joystick/channels/LT", _channel_LT_);
+ 1164 91 : param_loader.loadParam("joystick/channels/RT", _channel_RT_);
+ 1165 91 : param_loader.loadParam("joystick/channels/L_joy", _channel_L_joy_);
+ 1166 91 : param_loader.loadParam("joystick/channels/R_joy", _channel_R_joy_);
+ 1167 :
+ 1168 : // load channels
+ 1169 91 : param_loader.loadParam("joystick/channels/pitch", _channel_pitch_);
+ 1170 91 : param_loader.loadParam("joystick/channels/roll", _channel_roll_);
+ 1171 91 : param_loader.loadParam("joystick/channels/heading", _channel_heading_);
+ 1172 91 : param_loader.loadParam("joystick/channels/throttle", _channel_throttle_);
+ 1173 :
+ 1174 : // load channel multipliers
+ 1175 91 : param_loader.loadParam("joystick/channel_multipliers/pitch", _channel_mult_pitch_);
+ 1176 91 : param_loader.loadParam("joystick/channel_multipliers/roll", _channel_mult_roll_);
+ 1177 91 : param_loader.loadParam("joystick/channel_multipliers/heading", _channel_mult_heading_);
+ 1178 91 : param_loader.loadParam("joystick/channel_multipliers/throttle", _channel_mult_throttle_);
+ 1179 :
+ 1180 : bool bumper_enabled;
+ 1181 91 : param_loader.loadParam("obstacle_bumper/enabled", bumper_enabled);
+ 1182 91 : bumper_enabled_ = bumper_enabled;
+ 1183 :
+ 1184 91 : param_loader.loadParam("obstacle_bumper/switch_tracker", _bumper_switch_tracker_);
+ 1185 91 : param_loader.loadParam("obstacle_bumper/switch_controller", _bumper_switch_controller_);
+ 1186 91 : param_loader.loadParam("obstacle_bumper/tracker", _bumper_tracker_name_);
+ 1187 91 : param_loader.loadParam("obstacle_bumper/controller", _bumper_controller_name_);
+ 1188 91 : param_loader.loadParam("obstacle_bumper/timer_rate", _bumper_timer_rate_);
+ 1189 :
+ 1190 91 : param_loader.loadParam("obstacle_bumper/horizontal/min_distance_to_obstacle", _bumper_horizontal_distance_);
+ 1191 91 : param_loader.loadParam("obstacle_bumper/horizontal/derived_from_dynamics", _bumper_horizontal_derive_from_dynamics_);
+ 1192 :
+ 1193 91 : param_loader.loadParam("obstacle_bumper/vertical/min_distance_to_obstacle", _bumper_vertical_distance_);
+ 1194 91 : param_loader.loadParam("obstacle_bumper/vertical/derived_from_dynamics", _bumper_vertical_derive_from_dynamics_);
+ 1195 :
+ 1196 91 : param_loader.loadParam("obstacle_bumper/horizontal/overshoot", _bumper_horizontal_overshoot_);
+ 1197 91 : param_loader.loadParam("obstacle_bumper/vertical/overshoot", _bumper_vertical_overshoot_);
+ 1198 :
+ 1199 91 : param_loader.loadParam("safety/tracker_error_action", _tracker_error_action_);
+ 1200 :
+ 1201 91 : param_loader.loadParam("trajectory_tracking/snap_to_safety_area", _snap_trajectory_to_safety_area_);
+ 1202 :
+ 1203 : // check the values of tracker error action
+ 1204 91 : if (_tracker_error_action_ != ELAND_STR && _tracker_error_action_ != EHOVER_STR) {
+ 1205 0 : ROS_ERROR("[ControlManager]: the tracker_error_action parameter (%s) is not correct, requires {%s, %s}", _tracker_error_action_.c_str(), ELAND_STR,
+ 1206 : EHOVER_STR);
+ 1207 0 : ros::shutdown();
+ 1208 : }
+ 1209 :
+ 1210 91 : param_loader.loadParam("rc_joystick/enabled", _rc_goto_enabled_);
+ 1211 91 : param_loader.loadParam("rc_joystick/channel_number", _rc_joystick_channel_);
+ 1212 91 : param_loader.loadParam("rc_joystick/horizontal_speed", _rc_horizontal_speed_);
+ 1213 91 : param_loader.loadParam("rc_joystick/vertical_speed", _rc_vertical_speed_);
+ 1214 91 : param_loader.loadParam("rc_joystick/heading_rate", _rc_heading_rate_);
+ 1215 :
+ 1216 91 : param_loader.loadParam("rc_joystick/channels/pitch", _rc_channel_pitch_);
+ 1217 91 : param_loader.loadParam("rc_joystick/channels/roll", _rc_channel_roll_);
+ 1218 91 : param_loader.loadParam("rc_joystick/channels/heading", _rc_channel_heading_);
+ 1219 91 : param_loader.loadParam("rc_joystick/channels/throttle", _rc_channel_throttle_);
+ 1220 :
+ 1221 91 : param_loader.loadParam("pirouette/speed", _pirouette_speed_);
+ 1222 91 : param_loader.loadParam("pirouette/timer_rate", _pirouette_timer_rate_);
+ 1223 :
+ 1224 91 : param_loader.loadParam("safety/parachute/enabled", _parachute_enabled_);
+ 1225 :
+ 1226 : // --------------------------------------------------------------
+ 1227 : // | initialize the last control output |
+ 1228 : // --------------------------------------------------------------
+ 1229 :
+ 1230 91 : initializeControlOutput();
+ 1231 :
+ 1232 : // | --------------------- tf transformer --------------------- |
+ 1233 :
+ 1234 91 : transformer_ = std::make_shared<mrs_lib::Transformer>(nh_, "ControlManager");
+ 1235 91 : transformer_->setDefaultPrefix(_uav_name_);
+ 1236 91 : transformer_->retryLookupNewest(true);
+ 1237 :
+ 1238 : // | ------------------- scope timer logger ------------------- |
+ 1239 :
+ 1240 91 : param_loader.loadParam("scope_timer/enabled", scope_timer_enabled_);
+ 1241 273 : const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string(""));
+ 1242 91 : scope_timer_logger_ = std::make_shared<mrs_lib::ScopeTimerLogger>(scope_timer_log_filename, scope_timer_enabled_);
+ 1243 :
+ 1244 : // bind transformer to trackers and controllers for use
+ 1245 91 : common_handlers_->transformer = transformer_;
+ 1246 :
+ 1247 : // bind scope timer to trackers and controllers for use
+ 1248 91 : common_handlers_->scope_timer.enabled = scope_timer_enabled_;
+ 1249 91 : common_handlers_->scope_timer.logger = scope_timer_logger_;
+ 1250 :
+ 1251 91 : common_handlers_->safety_area.use_safety_area = use_safety_area_;
+ 1252 91 : common_handlers_->safety_area.isPointInSafetyArea2d = boost::bind(&ControlManager::isPointInSafetyArea2d, this, _1);
+ 1253 91 : common_handlers_->safety_area.isPointInSafetyArea3d = boost::bind(&ControlManager::isPointInSafetyArea3d, this, _1);
+ 1254 91 : common_handlers_->safety_area.getMinZ = boost::bind(&ControlManager::getMinZ, this, _1);
+ 1255 91 : common_handlers_->safety_area.getMaxZ = boost::bind(&ControlManager::getMaxZ, this, _1);
+ 1256 :
+ 1257 91 : common_handlers_->getMass = boost::bind(&ControlManager::getMass, this);
+ 1258 :
+ 1259 91 : common_handlers_->detailed_model_params = loadDetailedUavModelParams(nh_, "ControlManager", _platform_config_, _custom_config_);
+ 1260 :
+ 1261 91 : common_handlers_->control_output_modalities = _hw_api_inputs_;
+ 1262 :
+ 1263 91 : common_handlers_->uav_name = _uav_name_;
+ 1264 :
+ 1265 91 : common_handlers_->parent_nh = nh_;
+ 1266 :
+ 1267 : // --------------------------------------------------------------
+ 1268 : // | load trackers |
+ 1269 : // --------------------------------------------------------------
+ 1270 :
+ 1271 182 : std::vector<std::string> custom_trackers;
+ 1272 :
+ 1273 91 : param_loader.loadParam("mrs_trackers", _tracker_names_);
+ 1274 91 : param_loader.loadParam("trackers", custom_trackers);
+ 1275 :
+ 1276 91 : if (!custom_trackers.empty()) {
+ 1277 1 : _tracker_names_.insert(_tracker_names_.end(), custom_trackers.begin(), custom_trackers.end());
+ 1278 : }
+ 1279 :
+ 1280 91 : param_loader.loadParam("null_tracker", _null_tracker_name_);
+ 1281 91 : param_loader.loadParam("landing_takeoff_tracker", _landoff_tracker_name_);
+ 1282 :
+ 1283 91 : tracker_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Tracker>>("mrs_uav_managers", "mrs_uav_managers::Tracker");
+ 1284 :
+ 1285 638 : for (int i = 0; i < int(_tracker_names_.size()); i++) {
+ 1286 :
+ 1287 1094 : std::string tracker_name = _tracker_names_.at(i);
+ 1288 :
+ 1289 : // load the controller parameters
+ 1290 1094 : std::string address;
+ 1291 1094 : std::string name_space;
+ 1292 : bool human_switchable;
+ 1293 :
+ 1294 547 : param_loader.loadParam(tracker_name + "/address", address);
+ 1295 547 : param_loader.loadParam(tracker_name + "/namespace", name_space);
+ 1296 547 : param_loader.loadParam(tracker_name + "/human_switchable", human_switchable, false);
+ 1297 :
+ 1298 1641 : TrackerParams new_tracker(address, name_space, human_switchable);
+ 1299 547 : trackers_.insert(std::pair<std::string, TrackerParams>(tracker_name, new_tracker));
+ 1300 :
+ 1301 : try {
+ 1302 547 : ROS_INFO("[ControlManager]: loading the tracker '%s'", new_tracker.address.c_str());
+ 1303 547 : tracker_list_.push_back(tracker_loader_->createInstance(new_tracker.address.c_str()));
+ 1304 : }
+ 1305 0 : catch (pluginlib::CreateClassException& ex1) {
+ 1306 0 : ROS_ERROR("[ControlManager]: CreateClassException for the tracker '%s'", new_tracker.address.c_str());
+ 1307 0 : ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+ 1308 0 : ros::shutdown();
+ 1309 : }
+ 1310 0 : catch (pluginlib::PluginlibException& ex) {
+ 1311 0 : ROS_ERROR("[ControlManager]: PluginlibException for the tracker '%s'", new_tracker.address.c_str());
+ 1312 0 : ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+ 1313 0 : ros::shutdown();
+ 1314 : }
+ 1315 : }
+ 1316 :
+ 1317 91 : ROS_INFO("[ControlManager]: trackers were loaded");
+ 1318 :
+ 1319 638 : for (int i = 0; i < int(tracker_list_.size()); i++) {
+ 1320 :
+ 1321 547 : std::map<std::string, TrackerParams>::iterator it;
+ 1322 547 : it = trackers_.find(_tracker_names_.at(i));
+ 1323 :
+ 1324 : // create private handlers
+ 1325 : std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+ 1326 1094 : std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+ 1327 :
+ 1328 547 : private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+ 1329 547 : private_handlers->name_space = it->second.name_space;
+ 1330 547 : private_handlers->runtime_name = _tracker_names_.at(i);
+ 1331 547 : private_handlers->param_loader = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _tracker_names_.at(i));
+ 1332 :
+ 1333 547 : if (_custom_config_ != "") {
+ 1334 547 : private_handlers->param_loader->addYamlFile(_custom_config_);
+ 1335 : }
+ 1336 :
+ 1337 547 : if (_platform_config_ != "") {
+ 1338 547 : private_handlers->param_loader->addYamlFile(_platform_config_);
+ 1339 : }
+ 1340 :
+ 1341 547 : if (_world_config_ != "") {
+ 1342 547 : private_handlers->param_loader->addYamlFile(_world_config_);
+ 1343 : }
+ 1344 :
+ 1345 547 : if (_network_config_ != "") {
+ 1346 547 : private_handlers->param_loader->addYamlFile(_network_config_);
+ 1347 : }
+ 1348 :
+ 1349 547 : bool success = false;
+ 1350 :
+ 1351 : try {
+ 1352 547 : ROS_INFO("[ControlManager]: initializing the tracker '%s'", it->second.address.c_str());
+ 1353 547 : success = tracker_list_.at(i)->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+ 1354 : }
+ 1355 0 : catch (std::runtime_error& ex) {
+ 1356 0 : ROS_ERROR("[ControlManager]: exception caught during tracker initialization: '%s'", ex.what());
+ 1357 : }
+ 1358 :
+ 1359 547 : if (!success) {
+ 1360 0 : ROS_ERROR("[ControlManager]: failed to initialize the tracker '%s'", it->second.address.c_str());
+ 1361 0 : ros::shutdown();
+ 1362 : }
+ 1363 : }
+ 1364 :
+ 1365 91 : ROS_INFO("[ControlManager]: trackers were initialized");
+ 1366 :
+ 1367 : // --------------------------------------------------------------
+ 1368 : // | check the existance of selected trackers |
+ 1369 : // --------------------------------------------------------------
+ 1370 :
+ 1371 : // | ------ check for the existance of the hover tracker ------ |
+ 1372 :
+ 1373 : // check if the hover_tracker is within the loaded trackers
+ 1374 : {
+ 1375 91 : auto idx = idxInVector(_ehover_tracker_name_, _tracker_names_);
+ 1376 :
+ 1377 91 : if (idx) {
+ 1378 91 : _ehover_tracker_idx_ = idx.value();
+ 1379 : } else {
+ 1380 0 : ROS_ERROR("[ControlManager]: the safety/hover_tracker (%s) is not within the loaded trackers", _ehover_tracker_name_.c_str());
+ 1381 0 : ros::shutdown();
+ 1382 : }
+ 1383 : }
+ 1384 :
+ 1385 : // | ----- check for the existence of the landoff tracker ----- |
+ 1386 :
+ 1387 : {
+ 1388 91 : auto idx = idxInVector(_landoff_tracker_name_, _tracker_names_);
+ 1389 :
+ 1390 91 : if (idx) {
+ 1391 91 : _landoff_tracker_idx_ = idx.value();
+ 1392 : } else {
+ 1393 0 : ROS_ERROR("[ControlManager]: the landoff tracker (%s) is not within the loaded trackers", _landoff_tracker_name_.c_str());
+ 1394 0 : ros::shutdown();
+ 1395 : }
+ 1396 : }
+ 1397 :
+ 1398 : // | ------- check for the existence of the null tracker ------ |
+ 1399 :
+ 1400 : {
+ 1401 91 : auto idx = idxInVector(_null_tracker_name_, _tracker_names_);
+ 1402 :
+ 1403 91 : if (idx) {
+ 1404 91 : _null_tracker_idx_ = idx.value();
+ 1405 : } else {
+ 1406 0 : ROS_ERROR("[ControlManager]: the null tracker (%s) is not within the loaded trackers", _null_tracker_name_.c_str());
+ 1407 0 : ros::shutdown();
+ 1408 : }
+ 1409 : }
+ 1410 :
+ 1411 : // --------------------------------------------------------------
+ 1412 : // | check existance of trackers for joystick |
+ 1413 : // --------------------------------------------------------------
+ 1414 :
+ 1415 91 : if (_joystick_enabled_) {
+ 1416 :
+ 1417 91 : auto idx = idxInVector(_joystick_tracker_name_, _tracker_names_);
+ 1418 :
+ 1419 91 : if (idx) {
+ 1420 91 : _joystick_tracker_idx_ = idx.value();
+ 1421 : } else {
+ 1422 0 : ROS_ERROR("[ControlManager]: the joystick tracker (%s) is not within the loaded trackers", _joystick_tracker_name_.c_str());
+ 1423 0 : ros::shutdown();
+ 1424 : }
+ 1425 : }
+ 1426 :
+ 1427 91 : if (_bumper_switch_tracker_) {
+ 1428 :
+ 1429 91 : auto idx = idxInVector(_bumper_tracker_name_, _tracker_names_);
+ 1430 :
+ 1431 91 : if (!idx) {
+ 1432 0 : ROS_ERROR("[ControlManager]: the bumper tracker (%s) is not within the loaded trackers", _bumper_tracker_name_.c_str());
+ 1433 0 : ros::shutdown();
+ 1434 : }
+ 1435 : }
+ 1436 :
+ 1437 : {
+ 1438 91 : auto idx = idxInVector(_joystick_fallback_tracker_name_, _tracker_names_);
+ 1439 :
+ 1440 91 : if (idx) {
+ 1441 91 : _joystick_fallback_tracker_idx_ = idx.value();
+ 1442 : } else {
+ 1443 0 : ROS_ERROR("[ControlManager]: the joystick fallback tracker (%s) is not within the loaded trackers", _joystick_fallback_tracker_name_.c_str());
+ 1444 0 : ros::shutdown();
+ 1445 : }
+ 1446 : }
+ 1447 :
+ 1448 : // --------------------------------------------------------------
+ 1449 : // | load the controllers |
+ 1450 : // --------------------------------------------------------------
+ 1451 :
+ 1452 182 : std::vector<std::string> custom_controllers;
+ 1453 :
+ 1454 91 : param_loader.loadParam("mrs_controllers", _controller_names_);
+ 1455 91 : param_loader.loadParam("controllers", custom_controllers);
+ 1456 :
+ 1457 91 : if (!custom_controllers.empty()) {
+ 1458 0 : _controller_names_.insert(_controller_names_.end(), custom_controllers.begin(), custom_controllers.end());
+ 1459 : }
+ 1460 :
+ 1461 91 : controller_loader_ = std::make_unique<pluginlib::ClassLoader<mrs_uav_managers::Controller>>("mrs_uav_managers", "mrs_uav_managers::Controller");
+ 1462 :
+ 1463 : // for each controller in the list
+ 1464 546 : for (int i = 0; i < int(_controller_names_.size()); i++) {
+ 1465 :
+ 1466 910 : std::string controller_name = _controller_names_.at(i);
+ 1467 :
+ 1468 : // load the controller parameters
+ 1469 910 : std::string address;
+ 1470 910 : std::string name_space;
+ 1471 : double eland_threshold, failsafe_threshold, odometry_innovation_threshold;
+ 1472 : bool human_switchable;
+ 1473 455 : param_loader.loadParam(controller_name + "/address", address);
+ 1474 455 : param_loader.loadParam(controller_name + "/namespace", name_space);
+ 1475 455 : param_loader.loadParam(controller_name + "/eland_threshold", eland_threshold);
+ 1476 455 : param_loader.loadParam(controller_name + "/failsafe_threshold", failsafe_threshold);
+ 1477 455 : param_loader.loadParam(controller_name + "/odometry_innovation_threshold", odometry_innovation_threshold);
+ 1478 455 : param_loader.loadParam(controller_name + "/human_switchable", human_switchable, false);
+ 1479 :
+ 1480 : // check if the controller can output some of the required outputs
+ 1481 : {
+ 1482 :
+ 1483 455 : ControlOutputModalities_t outputs;
+ 1484 455 : param_loader.loadParam(controller_name + "/outputs/actuators", outputs.actuators, false);
+ 1485 455 : param_loader.loadParam(controller_name + "/outputs/control_group", outputs.control_group, false);
+ 1486 455 : param_loader.loadParam(controller_name + "/outputs/attitude_rate", outputs.attitude_rate, false);
+ 1487 455 : param_loader.loadParam(controller_name + "/outputs/attitude", outputs.attitude, false);
+ 1488 455 : param_loader.loadParam(controller_name + "/outputs/acceleration_hdg_rate", outputs.acceleration_hdg_rate, false);
+ 1489 455 : param_loader.loadParam(controller_name + "/outputs/acceleration_hdg", outputs.acceleration_hdg, false);
+ 1490 455 : param_loader.loadParam(controller_name + "/outputs/velocity_hdg_rate", outputs.velocity_hdg_rate, false);
+ 1491 455 : param_loader.loadParam(controller_name + "/outputs/velocity_hdg", outputs.velocity_hdg, false);
+ 1492 455 : param_loader.loadParam(controller_name + "/outputs/position", outputs.position, false);
+ 1493 :
+ 1494 455 : bool meets_actuators = (_hw_api_inputs_.actuators && outputs.actuators);
+ 1495 455 : bool meets_control_group = (_hw_api_inputs_.control_group && outputs.control_group);
+ 1496 455 : bool meets_attitude_rate = (_hw_api_inputs_.attitude_rate && outputs.attitude_rate);
+ 1497 455 : bool meets_attitude = (_hw_api_inputs_.attitude && outputs.attitude);
+ 1498 455 : bool meets_acceleration_hdg_rate = (_hw_api_inputs_.acceleration_hdg_rate && outputs.acceleration_hdg_rate);
+ 1499 455 : bool meets_acceleration_hdg = (_hw_api_inputs_.acceleration_hdg && outputs.acceleration_hdg);
+ 1500 455 : bool meets_velocity_hdg_rate = (_hw_api_inputs_.velocity_hdg_rate && outputs.velocity_hdg_rate);
+ 1501 455 : bool meets_velocity_hdg = (_hw_api_inputs_.velocity_hdg && outputs.velocity_hdg);
+ 1502 455 : bool meets_position = (_hw_api_inputs_.position && outputs.position);
+ 1503 :
+ 1504 440 : bool meets_requirements = meets_actuators || meets_control_group || meets_attitude_rate || meets_attitude || meets_acceleration_hdg_rate ||
+ 1505 895 : meets_acceleration_hdg || meets_velocity_hdg_rate || meets_velocity_hdg || meets_position;
+ 1506 :
+ 1507 455 : if (!meets_requirements) {
+ 1508 :
+ 1509 0 : ROS_ERROR("[ControlManager]: the controller '%s' does not meet the control output requirements, which are some of the following",
+ 1510 : controller_name.c_str());
+ 1511 :
+ 1512 0 : if (_hw_api_inputs_.actuators) {
+ 1513 0 : ROS_ERROR("[ControlManager]: - actuators");
+ 1514 : }
+ 1515 :
+ 1516 0 : if (_hw_api_inputs_.control_group) {
+ 1517 0 : ROS_ERROR("[ControlManager]: - control group");
+ 1518 : }
+ 1519 :
+ 1520 0 : if (_hw_api_inputs_.attitude_rate) {
+ 1521 0 : ROS_ERROR("[ControlManager]: - attitude rate");
+ 1522 : }
+ 1523 :
+ 1524 0 : if (_hw_api_inputs_.attitude) {
+ 1525 0 : ROS_ERROR("[ControlManager]: - attitude");
+ 1526 : }
+ 1527 :
+ 1528 0 : if (_hw_api_inputs_.acceleration_hdg_rate) {
+ 1529 0 : ROS_ERROR("[ControlManager]: - acceleration+hdg rate");
+ 1530 : }
+ 1531 :
+ 1532 0 : if (_hw_api_inputs_.acceleration_hdg) {
+ 1533 0 : ROS_ERROR("[ControlManager]: - acceleration+hdg");
+ 1534 : }
+ 1535 :
+ 1536 0 : if (_hw_api_inputs_.velocity_hdg_rate) {
+ 1537 0 : ROS_ERROR("[ControlManager]: - velocity+hdg rate");
+ 1538 : }
+ 1539 :
+ 1540 0 : if (_hw_api_inputs_.velocity_hdg) {
+ 1541 0 : ROS_ERROR("[ControlManager]: - velocity+hdg");
+ 1542 : }
+ 1543 :
+ 1544 0 : if (_hw_api_inputs_.position) {
+ 1545 0 : ROS_ERROR("[ControlManager]: - position");
+ 1546 : }
+ 1547 :
+ 1548 0 : ros::shutdown();
+ 1549 : }
+ 1550 :
+ 1551 455 : if ((_hw_api_inputs_.actuators || _hw_api_inputs_.control_group) && !common_handlers_->detailed_model_params) {
+ 1552 0 : ROS_ERROR(
+ 1553 : "[ControlManager]: the HW API supports 'actuators' or 'control_group' input, but the 'detailed uav model params' were not loaded sucessfully");
+ 1554 0 : ros::shutdown();
+ 1555 : }
+ 1556 : }
+ 1557 :
+ 1558 : // | --- alter the timer rates based on the hw capabilities --- |
+ 1559 :
+ 1560 455 : CONTROL_OUTPUT lowest_output = getLowestOuput(_hw_api_inputs_);
+ 1561 :
+ 1562 455 : if (lowest_output == ACTUATORS_CMD || lowest_output == CONTROL_GROUP) {
+ 1563 30 : _safety_timer_rate_ = 200.0;
+ 1564 30 : desired_uav_state_rate_ = 250.0;
+ 1565 425 : } else if (lowest_output == ATTITUDE_RATE || lowest_output == ATTITUDE) {
+ 1566 335 : _safety_timer_rate_ = 100.0;
+ 1567 335 : desired_uav_state_rate_ = 100.0;
+ 1568 90 : } else if (lowest_output == ACCELERATION_HDG_RATE || lowest_output == ACCELERATION_HDG) {
+ 1569 20 : _safety_timer_rate_ = 30.0;
+ 1570 20 : _status_timer_rate_ = 1.0;
+ 1571 20 : desired_uav_state_rate_ = 40.0;
+ 1572 :
+ 1573 20 : if (_uav_state_max_missing_time_ < 0.2) {
+ 1574 4 : _uav_state_max_missing_time_ = 0.2;
+ 1575 : }
+ 1576 70 : } else if (lowest_output >= VELOCITY_HDG_RATE) {
+ 1577 70 : _safety_timer_rate_ = 20.0;
+ 1578 70 : _status_timer_rate_ = 1.0;
+ 1579 70 : desired_uav_state_rate_ = 20.0;
+ 1580 :
+ 1581 70 : if (_uav_state_max_missing_time_ < 1.0) {
+ 1582 14 : _uav_state_max_missing_time_ = 1.0;
+ 1583 : }
+ 1584 : }
+ 1585 :
+ 1586 455 : if (eland_threshold == 0) {
+ 1587 92 : eland_threshold = 1e6;
+ 1588 : }
+ 1589 :
+ 1590 455 : if (failsafe_threshold == 0) {
+ 1591 92 : failsafe_threshold = 1e6;
+ 1592 : }
+ 1593 :
+ 1594 455 : if (odometry_innovation_threshold == 0) {
+ 1595 93 : odometry_innovation_threshold = 1e6;
+ 1596 : }
+ 1597 :
+ 1598 1365 : ControllerParams new_controller(address, name_space, eland_threshold, failsafe_threshold, odometry_innovation_threshold, human_switchable);
+ 1599 455 : controllers_.insert(std::pair<std::string, ControllerParams>(controller_name, new_controller));
+ 1600 :
+ 1601 : try {
+ 1602 455 : ROS_INFO("[ControlManager]: loading the controller '%s'", new_controller.address.c_str());
+ 1603 455 : controller_list_.push_back(controller_loader_->createInstance(new_controller.address.c_str()));
+ 1604 : }
+ 1605 0 : catch (pluginlib::CreateClassException& ex1) {
+ 1606 0 : ROS_ERROR("[ControlManager]: CreateClassException for the controller '%s'", new_controller.address.c_str());
+ 1607 0 : ROS_ERROR("[ControlManager]: Error: %s", ex1.what());
+ 1608 0 : ros::shutdown();
+ 1609 : }
+ 1610 0 : catch (pluginlib::PluginlibException& ex) {
+ 1611 0 : ROS_ERROR("[ControlManager]: PluginlibException for the controller '%s'", new_controller.address.c_str());
+ 1612 0 : ROS_ERROR("[ControlManager]: Error: %s", ex.what());
+ 1613 0 : ros::shutdown();
+ 1614 : }
+ 1615 : }
+ 1616 :
+ 1617 91 : ROS_INFO("[ControlManager]: controllers were loaded");
+ 1618 :
+ 1619 546 : for (int i = 0; i < int(controller_list_.size()); i++) {
+ 1620 :
+ 1621 455 : std::map<std::string, ControllerParams>::iterator it;
+ 1622 455 : it = controllers_.find(_controller_names_.at(i));
+ 1623 :
+ 1624 : // create private handlers
+ 1625 : std::shared_ptr<mrs_uav_managers::control_manager::PrivateHandlers_t> private_handlers =
+ 1626 910 : std::make_shared<mrs_uav_managers::control_manager::PrivateHandlers_t>();
+ 1627 :
+ 1628 455 : private_handlers->loadConfigFile = boost::bind(&ControlManager::loadConfigFile, this, _1, it->second.name_space);
+ 1629 455 : private_handlers->name_space = it->second.name_space;
+ 1630 455 : private_handlers->runtime_name = _controller_names_.at(i);
+ 1631 455 : private_handlers->param_loader = std::make_unique<mrs_lib::ParamLoader>(ros::NodeHandle(nh_, it->second.name_space), _controller_names_.at(i));
+ 1632 :
+ 1633 455 : if (_custom_config_ != "") {
+ 1634 455 : private_handlers->param_loader->addYamlFile(_custom_config_);
+ 1635 : }
+ 1636 :
+ 1637 455 : if (_platform_config_ != "") {
+ 1638 455 : private_handlers->param_loader->addYamlFile(_platform_config_);
+ 1639 : }
+ 1640 :
+ 1641 455 : if (_world_config_ != "") {
+ 1642 455 : private_handlers->param_loader->addYamlFile(_world_config_);
+ 1643 : }
+ 1644 :
+ 1645 455 : if (_network_config_ != "") {
+ 1646 455 : private_handlers->param_loader->addYamlFile(_network_config_);
+ 1647 : }
+ 1648 :
+ 1649 455 : bool success = false;
+ 1650 :
+ 1651 : try {
+ 1652 :
+ 1653 455 : ROS_INFO("[ControlManager]: initializing the controller '%s'", it->second.address.c_str());
+ 1654 455 : success = controller_list_.at(i)->initialize(ros::NodeHandle(nh_, it->second.name_space), common_handlers_, private_handlers);
+ 1655 : }
+ 1656 0 : catch (std::runtime_error& ex) {
+ 1657 0 : ROS_ERROR("[ControlManager]: exception caught during controller initialization: '%s'", ex.what());
+ 1658 : }
+ 1659 :
+ 1660 455 : if (!success) {
+ 1661 0 : ROS_ERROR("[ControlManager]: failed to initialize the controller '%s'", it->second.address.c_str());
+ 1662 0 : ros::shutdown();
+ 1663 : }
+ 1664 : }
+ 1665 :
+ 1666 91 : ROS_INFO("[ControlManager]: controllers were initialized");
+ 1667 :
+ 1668 : {
+ 1669 91 : auto idx = idxInVector(_failsafe_controller_name_, _controller_names_);
+ 1670 :
+ 1671 91 : if (idx) {
+ 1672 91 : _failsafe_controller_idx_ = idx.value();
+ 1673 : } else {
+ 1674 0 : ROS_ERROR("[ControlManager]: the failsafe controller (%s) is not within the loaded controllers", _failsafe_controller_name_.c_str());
+ 1675 0 : ros::shutdown();
+ 1676 : }
+ 1677 : }
+ 1678 :
+ 1679 : {
+ 1680 91 : auto idx = idxInVector(_eland_controller_name_, _controller_names_);
+ 1681 :
+ 1682 91 : if (idx) {
+ 1683 91 : _eland_controller_idx_ = idx.value();
+ 1684 : } else {
+ 1685 0 : ROS_ERROR("[ControlManager]: the eland controller (%s) is not within the loaded controllers", _eland_controller_name_.c_str());
+ 1686 0 : ros::shutdown();
+ 1687 : }
+ 1688 : }
+ 1689 :
+ 1690 : {
+ 1691 91 : auto idx = idxInVector(_joystick_controller_name_, _controller_names_);
+ 1692 :
+ 1693 91 : if (idx) {
+ 1694 91 : _joystick_controller_idx_ = idx.value();
+ 1695 : } else {
+ 1696 0 : ROS_ERROR("[ControlManager]: the joystick controller (%s) is not within the loaded controllers", _joystick_controller_name_.c_str());
+ 1697 0 : ros::shutdown();
+ 1698 : }
+ 1699 : }
+ 1700 :
+ 1701 91 : if (_bumper_switch_controller_) {
+ 1702 :
+ 1703 91 : auto idx = idxInVector(_bumper_controller_name_, _controller_names_);
+ 1704 :
+ 1705 91 : if (!idx) {
+ 1706 0 : ROS_ERROR("[ControlManager]: the bumper controller (%s) is not within the loaded controllers", _bumper_controller_name_.c_str());
+ 1707 0 : ros::shutdown();
+ 1708 : }
+ 1709 : }
+ 1710 :
+ 1711 : {
+ 1712 91 : auto idx = idxInVector(_joystick_fallback_controller_name_, _controller_names_);
+ 1713 :
+ 1714 91 : if (idx) {
+ 1715 91 : _joystick_fallback_controller_idx_ = idx.value();
+ 1716 : } else {
+ 1717 0 : ROS_ERROR("[ControlManager]: the joystick fallback controller (%s) is not within the loaded controllers", _joystick_fallback_controller_name_.c_str());
+ 1718 0 : ros::shutdown();
+ 1719 : }
+ 1720 : }
+ 1721 :
+ 1722 : // --------------------------------------------------------------
+ 1723 : // | activate the NullTracker |
+ 1724 : // --------------------------------------------------------------
+ 1725 :
+ 1726 91 : ROS_INFO("[ControlManager]: activating the null tracker");
+ 1727 :
+ 1728 91 : tracker_list_.at(_null_tracker_idx_)->activate(last_tracker_cmd_);
+ 1729 91 : active_tracker_idx_ = _null_tracker_idx_;
+ 1730 :
+ 1731 : // --------------------------------------------------------------
+ 1732 : // | activate the eland controller as the first controller |
+ 1733 : // --------------------------------------------------------------
+ 1734 :
+ 1735 91 : ROS_INFO("[ControlManager]: activating the the eland controller (%s) as the first controller", _controller_names_.at(_eland_controller_idx_).c_str());
+ 1736 :
+ 1737 91 : controller_list_.at(_eland_controller_idx_)->activate(last_control_output_);
+ 1738 91 : active_controller_idx_ = _eland_controller_idx_;
+ 1739 :
+ 1740 : // update the time
+ 1741 : {
+ 1742 182 : std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+ 1743 :
+ 1744 91 : controller_tracker_switch_time_ = ros::Time::now();
+ 1745 : }
+ 1746 :
+ 1747 91 : output_enabled_ = false;
+ 1748 :
+ 1749 : // | --------------- set the default constraints -------------- |
+ 1750 :
+ 1751 91 : sanitized_constraints_ = current_constraints_;
+ 1752 91 : setConstraints(current_constraints_);
+ 1753 :
+ 1754 : // | ------------------------ profiler ------------------------ |
+ 1755 :
+ 1756 91 : profiler_ = mrs_lib::Profiler(nh_, "ControlManager", _profiler_enabled_);
+ 1757 :
+ 1758 : // | ----------------------- publishers ----------------------- |
+ 1759 :
+ 1760 91 : control_output_publisher_ = OutputPublisher(nh_);
+ 1761 :
+ 1762 91 : ph_controller_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::ControllerDiagnostics>(nh_, "controller_diagnostics_out", 1);
+ 1763 91 : ph_tracker_cmd_ = mrs_lib::PublisherHandler<mrs_msgs::TrackerCommand>(nh_, "tracker_cmd_out", 1);
+ 1764 91 : ph_mrs_odom_input_ = mrs_lib::PublisherHandler<mrs_msgs::EstimatorInput>(nh_, "estimator_input_out", 1);
+ 1765 91 : ph_control_reference_odom_ = mrs_lib::PublisherHandler<nav_msgs::Odometry>(nh_, "control_reference_out", 1);
+ 1766 91 : ph_diagnostics_ = mrs_lib::PublisherHandler<mrs_msgs::ControlManagerDiagnostics>(nh_, "diagnostics_out", 1);
+ 1767 91 : ph_offboard_on_ = mrs_lib::PublisherHandler<std_msgs::Empty>(nh_, "offboard_on_out", 1);
+ 1768 91 : ph_tilt_error_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "tilt_error_out", 1);
+ 1769 91 : ph_mass_estimate_ = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_estimate_out", 1, false, 10.0);
+ 1770 91 : ph_mass_nominal_ = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "mass_nominal_out", 1, true);
+ 1771 91 : ph_throttle_ = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "throttle_out", 1, false, 10.0);
+ 1772 91 : ph_thrust_ = mrs_lib::PublisherHandler<std_msgs::Float64>(nh_, "thrust_out", 1, false, 100.0);
+ 1773 91 : ph_control_error_ = mrs_lib::PublisherHandler<mrs_msgs::ControlError>(nh_, "control_error_out", 1);
+ 1774 91 : ph_safety_area_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_markers_out", 1, true, 1.0);
+ 1775 91 : ph_safety_area_coordinates_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "safety_area_coordinates_markers_out", 1, true, 1.0);
+ 1776 91 : ph_disturbances_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "disturbances_markers_out", 1, false, 10.0);
+ 1777 91 : ph_current_constraints_ = mrs_lib::PublisherHandler<mrs_msgs::DynamicsConstraints>(nh_, "current_constraints_out", 1);
+ 1778 91 : ph_heading_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "heading_out", 1);
+ 1779 91 : ph_speed_ = mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped>(nh_, "speed_out", 1, false, 10.0);
+ 1780 91 : pub_debug_original_trajectory_poses_ = mrs_lib::PublisherHandler<geometry_msgs::PoseArray>(nh_, "trajectory_original/poses_out", 1, true);
+ 1781 91 : pub_debug_original_trajectory_markers_ = mrs_lib::PublisherHandler<visualization_msgs::MarkerArray>(nh_, "trajectory_original/markers_out", 1, true);
+ 1782 :
+ 1783 : // | ------------------ publish nominal mass ------------------ |
+ 1784 :
+ 1785 : {
+ 1786 91 : std_msgs::Float64 nominal_mass;
+ 1787 :
+ 1788 91 : nominal_mass.data = _uav_mass_;
+ 1789 :
+ 1790 91 : ph_mass_nominal_.publish(nominal_mass);
+ 1791 : }
+ 1792 :
+ 1793 : // | ----------------------- subscribers ---------------------- |
+ 1794 :
+ 1795 182 : mrs_lib::SubscribeHandlerOptions shopts;
+ 1796 91 : shopts.nh = nh_;
+ 1797 91 : shopts.node_name = "ControlManager";
+ 1798 91 : shopts.no_message_timeout = mrs_lib::no_timeout;
+ 1799 91 : shopts.threadsafe = true;
+ 1800 91 : shopts.autostart = true;
+ 1801 91 : shopts.queue_size = 10;
+ 1802 91 : shopts.transport_hints = ros::TransportHints().tcpNoDelay();
+ 1803 :
+ 1804 91 : if (_state_input_ == INPUT_UAV_STATE) {
+ 1805 91 : sh_uav_state_ = mrs_lib::SubscribeHandler<mrs_msgs::UavState>(shopts, "uav_state_in", &ControlManager::callbackUavState, this);
+ 1806 0 : } else if (_state_input_ == INPUT_ODOMETRY) {
+ 1807 0 : sh_odometry_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_in", &ControlManager::callbackOdometry, this);
+ 1808 : }
+ 1809 :
+ 1810 91 : if (_odometry_innovation_check_enabled_) {
+ 1811 91 : sh_odometry_innovation_ = mrs_lib::SubscribeHandler<nav_msgs::Odometry>(shopts, "odometry_innovation_in");
+ 1812 : }
+ 1813 :
+ 1814 91 : sh_bumper_ = mrs_lib::SubscribeHandler<mrs_msgs::ObstacleSectors>(shopts, "bumper_sectors_in");
+ 1815 91 : sh_max_z_ = mrs_lib::SubscribeHandler<mrs_msgs::Float64Stamped>(shopts, "max_z_in");
+ 1816 91 : sh_joystick_ = mrs_lib::SubscribeHandler<sensor_msgs::Joy>(shopts, "joystick_in", &ControlManager::callbackJoystick, this);
+ 1817 91 : sh_gnss_ = mrs_lib::SubscribeHandler<sensor_msgs::NavSatFix>(shopts, "gnss_in", &ControlManager::callbackGNSS, this);
+ 1818 91 : sh_hw_api_rc_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiRcChannels>(shopts, "hw_api_rc_in", &ControlManager::callbackRC, this);
+ 1819 :
+ 1820 91 : sh_hw_api_status_ = mrs_lib::SubscribeHandler<mrs_msgs::HwApiStatus>(shopts, "hw_api_status_in", &ControlManager::callbackHwApiStatus, this);
+ 1821 :
+ 1822 : // | -------------------- general services -------------------- |
+ 1823 :
+ 1824 91 : service_server_switch_tracker_ = nh_.advertiseService("switch_tracker_in", &ControlManager::callbackSwitchTracker, this);
+ 1825 91 : service_server_switch_controller_ = nh_.advertiseService("switch_controller_in", &ControlManager::callbackSwitchController, this);
+ 1826 91 : service_server_reset_tracker_ = nh_.advertiseService("tracker_reset_static_in", &ControlManager::callbackTrackerResetStatic, this);
+ 1827 91 : service_server_hover_ = nh_.advertiseService("hover_in", &ControlManager::callbackHover, this);
+ 1828 91 : service_server_ehover_ = nh_.advertiseService("ehover_in", &ControlManager::callbackEHover, this);
+ 1829 91 : service_server_failsafe_ = nh_.advertiseService("failsafe_in", &ControlManager::callbackFailsafe, this);
+ 1830 91 : service_server_failsafe_escalating_ = nh_.advertiseService("failsafe_escalating_in", &ControlManager::callbackFailsafeEscalating, this);
+ 1831 91 : service_server_toggle_output_ = nh_.advertiseService("toggle_output_in", &ControlManager::callbackToggleOutput, this);
+ 1832 91 : service_server_arm_ = nh_.advertiseService("arm_in", &ControlManager::callbackArm, this);
+ 1833 91 : service_server_enable_callbacks_ = nh_.advertiseService("enable_callbacks_in", &ControlManager::callbackEnableCallbacks, this);
+ 1834 91 : service_server_set_constraints_ = nh_.advertiseService("set_constraints_in", &ControlManager::callbackSetConstraints, this);
+ 1835 91 : service_server_use_joystick_ = nh_.advertiseService("use_joystick_in", &ControlManager::callbackUseJoystick, this);
+ 1836 91 : service_server_use_safety_area_ = nh_.advertiseService("use_safety_area_in", &ControlManager::callbackUseSafetyArea, this);
+ 1837 91 : service_server_eland_ = nh_.advertiseService("eland_in", &ControlManager::callbackEland, this);
+ 1838 91 : service_server_parachute_ = nh_.advertiseService("parachute_in", &ControlManager::callbackParachute, this);
+ 1839 91 : service_server_set_min_z_ = nh_.advertiseService("set_min_z_in", &ControlManager::callbackSetMinZ, this);
+ 1840 91 : service_server_transform_reference_ = nh_.advertiseService("transform_reference_in", &ControlManager::callbackTransformReference, this);
+ 1841 91 : service_server_transform_pose_ = nh_.advertiseService("transform_pose_in", &ControlManager::callbackTransformPose, this);
+ 1842 91 : service_server_transform_vector3_ = nh_.advertiseService("transform_vector3_in", &ControlManager::callbackTransformVector3, this);
+ 1843 91 : service_server_bumper_enabler_ = nh_.advertiseService("bumper_in", &ControlManager::callbackEnableBumper, this);
+ 1844 91 : service_server_get_min_z_ = nh_.advertiseService("get_min_z_in", &ControlManager::callbackGetMinZ, this);
+ 1845 91 : service_server_validate_reference_ = nh_.advertiseService("validate_reference_in", &ControlManager::callbackValidateReference, this);
+ 1846 91 : service_server_validate_reference_2d_ = nh_.advertiseService("validate_reference_2d_in", &ControlManager::callbackValidateReference2d, this);
+ 1847 91 : service_server_validate_reference_list_ = nh_.advertiseService("validate_reference_list_in", &ControlManager::callbackValidateReferenceList, this);
+ 1848 91 : service_server_start_trajectory_tracking_ = nh_.advertiseService("start_trajectory_tracking_in", &ControlManager::callbackStartTrajectoryTracking, this);
+ 1849 91 : service_server_stop_trajectory_tracking_ = nh_.advertiseService("stop_trajectory_tracking_in", &ControlManager::callbackStopTrajectoryTracking, this);
+ 1850 91 : service_server_resume_trajectory_tracking_ = nh_.advertiseService("resume_trajectory_tracking_in", &ControlManager::callbackResumeTrajectoryTracking, this);
+ 1851 91 : service_server_goto_trajectory_start_ = nh_.advertiseService("goto_trajectory_start_in", &ControlManager::callbackGotoTrajectoryStart, this);
+ 1852 :
+ 1853 91 : sch_arming_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "hw_api_arming_out");
+ 1854 91 : sch_eland_ = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "eland_out");
+ 1855 91 : sch_shutdown_ = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "shutdown_out");
+ 1856 91 : sch_set_odometry_callbacks_ = mrs_lib::ServiceClientHandler<std_srvs::SetBool>(nh_, "set_odometry_callbacks_out");
+ 1857 91 : sch_ungrip_ = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "ungrip_out");
+ 1858 91 : sch_parachute_ = mrs_lib::ServiceClientHandler<std_srvs::Trigger>(nh_, "parachute_out");
+ 1859 :
+ 1860 : // | ---------------- setpoint command services --------------- |
+ 1861 :
+ 1862 : // human callable
+ 1863 91 : service_server_goto_ = nh_.advertiseService("goto_in", &ControlManager::callbackGoto, this);
+ 1864 91 : service_server_goto_fcu_ = nh_.advertiseService("goto_fcu_in", &ControlManager::callbackGotoFcu, this);
+ 1865 91 : service_server_goto_relative_ = nh_.advertiseService("goto_relative_in", &ControlManager::callbackGotoRelative, this);
+ 1866 91 : service_server_goto_altitude_ = nh_.advertiseService("goto_altitude_in", &ControlManager::callbackGotoAltitude, this);
+ 1867 91 : service_server_set_heading_ = nh_.advertiseService("set_heading_in", &ControlManager::callbackSetHeading, this);
+ 1868 91 : service_server_set_heading_relative_ = nh_.advertiseService("set_heading_relative_in", &ControlManager::callbackSetHeadingRelative, this);
+ 1869 :
+ 1870 91 : service_server_reference_ = nh_.advertiseService("reference_in", &ControlManager::callbackReferenceService, this);
+ 1871 91 : sh_reference_ = mrs_lib::SubscribeHandler<mrs_msgs::ReferenceStamped>(shopts, "reference_in", &ControlManager::callbackReferenceTopic, this);
+ 1872 :
+ 1873 91 : service_server_velocity_reference_ = nh_.advertiseService("velocity_reference_in", &ControlManager::callbackVelocityReferenceService, this);
+ 1874 : sh_velocity_reference_ =
+ 1875 91 : mrs_lib::SubscribeHandler<mrs_msgs::VelocityReferenceStamped>(shopts, "velocity_reference_in", &ControlManager::callbackVelocityReferenceTopic, this);
+ 1876 :
+ 1877 91 : service_server_trajectory_reference_ = nh_.advertiseService("trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceService, this);
+ 1878 : sh_trajectory_reference_ =
+ 1879 91 : mrs_lib::SubscribeHandler<mrs_msgs::TrajectoryReference>(shopts, "trajectory_reference_in", &ControlManager::callbackTrajectoryReferenceTopic, this);
+ 1880 :
+ 1881 : // | --------------------- other services --------------------- |
+ 1882 :
+ 1883 91 : service_server_emergency_reference_ = nh_.advertiseService("emergency_reference_in", &ControlManager::callbackEmergencyReference, this);
+ 1884 91 : service_server_pirouette_ = nh_.advertiseService("pirouette_in", &ControlManager::callbackPirouette, this);
+ 1885 :
+ 1886 : // | ------------------------- timers ------------------------- |
+ 1887 :
+ 1888 91 : timer_status_ = nh_.createTimer(ros::Rate(_status_timer_rate_), &ControlManager::timerStatus, this);
+ 1889 91 : timer_safety_ = nh_.createTimer(ros::Rate(_safety_timer_rate_), &ControlManager::timerSafety, this);
+ 1890 91 : timer_bumper_ = nh_.createTimer(ros::Rate(1.0), &ControlManager::timerBumper, this);
+ 1891 91 : timer_eland_ = nh_.createTimer(ros::Rate(_elanding_timer_rate_), &ControlManager::timerEland, this, false, false);
+ 1892 91 : timer_failsafe_ = nh_.createTimer(ros::Rate(_failsafe_timer_rate_), &ControlManager::timerFailsafe, this, false, false);
+ 1893 91 : timer_pirouette_ = nh_.createTimer(ros::Rate(_pirouette_timer_rate_), &ControlManager::timerPirouette, this, false, false);
+ 1894 91 : timer_joystick_ = nh_.createTimer(ros::Rate(_joystick_timer_rate_), &ControlManager::timerJoystick, this);
+ 1895 :
+ 1896 : // | ----------------------- finish init ---------------------- |
+ 1897 :
+ 1898 91 : if (!param_loader.loadedSuccessfully()) {
+ 1899 0 : ROS_ERROR("[ControlManager]: could not load all parameters!");
+ 1900 0 : ros::shutdown();
+ 1901 : }
+ 1902 :
+ 1903 91 : is_initialized_ = true;
+ 1904 :
+ 1905 91 : ROS_INFO("[ControlManager]: initialized");
+ 1906 91 : }
+ 1907 :
+ 1908 : //}
+ 1909 :
+ 1910 : // --------------------------------------------------------------
+ 1911 : // | timers |
+ 1912 : // --------------------------------------------------------------
+ 1913 :
+ 1914 : /* timerHwApiCapabilities() //{ */
+ 1915 :
+ 1916 160 : void ControlManager::timerHwApiCapabilities(const ros::TimerEvent& event) {
+ 1917 :
+ 1918 320 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("timerHwApiCapabilities", _status_timer_rate_, 1.0, event);
+ 1919 320 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::timerHwApiCapabilities", scope_timer_logger_, scope_timer_enabled_);
+ 1920 :
+ 1921 160 : if (!sh_hw_api_capabilities_.hasMsg()) {
+ 1922 69 : ROS_INFO_THROTTLE(1.0, "[ControlManager]: waiting for HW API capabilities");
+ 1923 69 : return;
+ 1924 : }
+ 1925 :
+ 1926 182 : auto hw_ap_capabilities = sh_hw_api_capabilities_.getMsg();
+ 1927 :
+ 1928 91 : ROS_INFO("[ControlManager]: got HW API capabilities, the possible control modes are:");
+ 1929 :
+ 1930 91 : if (hw_ap_capabilities->accepts_actuator_cmd) {
+ 1931 3 : ROS_INFO("[ControlManager]: - actuator command");
+ 1932 3 : _hw_api_inputs_.actuators = true;
+ 1933 : }
+ 1934 :
+ 1935 91 : if (hw_ap_capabilities->accepts_control_group_cmd) {
+ 1936 3 : ROS_INFO("[ControlManager]: - control group command");
+ 1937 3 : _hw_api_inputs_.control_group = true;
+ 1938 : }
+ 1939 :
+ 1940 91 : if (hw_ap_capabilities->accepts_attitude_rate_cmd) {
+ 1941 64 : ROS_INFO("[ControlManager]: - attitude rate command");
+ 1942 64 : _hw_api_inputs_.attitude_rate = true;
+ 1943 : }
+ 1944 :
+ 1945 91 : if (hw_ap_capabilities->accepts_attitude_cmd) {
+ 1946 62 : ROS_INFO("[ControlManager]: - attitude command");
+ 1947 62 : _hw_api_inputs_.attitude = true;
+ 1948 : }
+ 1949 :
+ 1950 91 : if (hw_ap_capabilities->accepts_acceleration_hdg_rate_cmd) {
+ 1951 2 : ROS_INFO("[ControlManager]: - acceleration+hdg rate command");
+ 1952 2 : _hw_api_inputs_.acceleration_hdg_rate = true;
+ 1953 : }
+ 1954 :
+ 1955 91 : if (hw_ap_capabilities->accepts_acceleration_hdg_cmd) {
+ 1956 2 : ROS_INFO("[ControlManager]: - acceleration+hdg command");
+ 1957 2 : _hw_api_inputs_.acceleration_hdg = true;
+ 1958 : }
+ 1959 :
+ 1960 91 : if (hw_ap_capabilities->accepts_velocity_hdg_rate_cmd) {
+ 1961 8 : ROS_INFO("[ControlManager]: - velocityhdg rate command");
+ 1962 8 : _hw_api_inputs_.velocity_hdg_rate = true;
+ 1963 : }
+ 1964 :
+ 1965 91 : if (hw_ap_capabilities->accepts_velocity_hdg_cmd) {
+ 1966 4 : ROS_INFO("[ControlManager]: - velocityhdg command");
+ 1967 4 : _hw_api_inputs_.velocity_hdg = true;
+ 1968 : }
+ 1969 :
+ 1970 91 : if (hw_ap_capabilities->accepts_position_cmd) {
+ 1971 2 : ROS_INFO("[ControlManager]: - position command");
+ 1972 2 : _hw_api_inputs_.position = true;
+ 1973 : }
+ 1974 :
+ 1975 91 : initialize();
+ 1976 :
+ 1977 91 : timer_hw_api_capabilities_.stop();
+ 1978 : }
+ 1979 :
+ 1980 : //}
+ 1981 :
+ 1982 : /* //{ timerStatus() */
+ 1983 :
+ 1984 16379 : void ControlManager::timerStatus(const ros::TimerEvent& event) {
+ 1985 :
+ 1986 16379 : if (!is_initialized_)
+ 1987 0 : return;
+ 1988 :
+ 1989 49137 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("timerStatus", _status_timer_rate_, 0.1, event);
+ 1990 49137 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::timerStatus", scope_timer_logger_, scope_timer_enabled_);
+ 1991 :
+ 1992 : // copy member variables
+ 1993 32758 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 1994 32758 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 1995 16379 : auto yaw_error = mrs_lib::get_mutexed(mutex_attitude_error_, yaw_error_);
+ 1996 16379 : auto position_error = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+ 1997 16379 : auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+ 1998 16379 : auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+ 1999 :
+ 2000 : double uav_x, uav_y, uav_z;
+ 2001 16379 : uav_x = uav_state.pose.position.x;
+ 2002 16379 : uav_y = uav_state.pose.position.y;
+ 2003 16379 : uav_z = uav_state.pose.position.z;
+ 2004 :
+ 2005 : // --------------------------------------------------------------
+ 2006 : // | print the status |
+ 2007 : // --------------------------------------------------------------
+ 2008 :
+ 2009 : {
+ 2010 32758 : std::string controller = _controller_names_.at(active_controller_idx);
+ 2011 32758 : std::string tracker = _tracker_names_.at(active_tracker_idx);
+ 2012 16379 : double mass = last_control_output.diagnostics.total_mass;
+ 2013 16379 : double bx_b = last_control_output.diagnostics.disturbance_bx_b;
+ 2014 16379 : double by_b = last_control_output.diagnostics.disturbance_by_b;
+ 2015 16379 : double wx_w = last_control_output.diagnostics.disturbance_wx_w;
+ 2016 16379 : double wy_w = last_control_output.diagnostics.disturbance_wy_w;
+ 2017 :
+ 2018 16379 : ROS_INFO_THROTTLE(5.0, "[ControlManager]: tracker: '%s', controller: '%s', mass: '%.2f kg', disturbances: body [%.2f, %.2f] N, world [%.2f, %.2f] N",
+ 2019 : tracker.c_str(), controller.c_str(), mass, bx_b, by_b, wx_w, wy_w);
+ 2020 : }
+ 2021 :
+ 2022 : // --------------------------------------------------------------
+ 2023 : // | publish the diagnostics |
+ 2024 : // --------------------------------------------------------------
+ 2025 :
+ 2026 16379 : publishDiagnostics();
+ 2027 :
+ 2028 : // --------------------------------------------------------------
+ 2029 : // | publish if the offboard is on |
+ 2030 : // --------------------------------------------------------------
+ 2031 :
+ 2032 16379 : if (offboard_mode_) {
+ 2033 :
+ 2034 11163 : std_msgs::Empty offboard_on_out;
+ 2035 :
+ 2036 11163 : ph_offboard_on_.publish(offboard_on_out);
+ 2037 : }
+ 2038 :
+ 2039 : // --------------------------------------------------------------
+ 2040 : // | publish the tilt error |
+ 2041 : // --------------------------------------------------------------
+ 2042 : {
+ 2043 32758 : std::scoped_lock lock(mutex_attitude_error_);
+ 2044 :
+ 2045 16379 : if (tilt_error_) {
+ 2046 :
+ 2047 32758 : mrs_msgs::Float64Stamped tilt_error_out;
+ 2048 16379 : tilt_error_out.header.stamp = ros::Time::now();
+ 2049 16379 : tilt_error_out.header.frame_id = uav_state.header.frame_id;
+ 2050 16379 : tilt_error_out.value = (180.0 / M_PI) * tilt_error_.value();
+ 2051 :
+ 2052 16379 : ph_tilt_error_.publish(tilt_error_out);
+ 2053 : }
+ 2054 : }
+ 2055 :
+ 2056 : // --------------------------------------------------------------
+ 2057 : // | publish the control error |
+ 2058 : // --------------------------------------------------------------
+ 2059 :
+ 2060 16379 : if (position_error) {
+ 2061 :
+ 2062 10059 : Eigen::Vector3d pos_error_value = position_error.value();
+ 2063 :
+ 2064 20118 : mrs_msgs::ControlError msg_out;
+ 2065 :
+ 2066 10059 : msg_out.header.stamp = ros::Time::now();
+ 2067 10059 : msg_out.header.frame_id = uav_state.header.frame_id;
+ 2068 :
+ 2069 10059 : msg_out.position_errors.x = pos_error_value(0);
+ 2070 10059 : msg_out.position_errors.y = pos_error_value(1);
+ 2071 10059 : msg_out.position_errors.z = pos_error_value(2);
+ 2072 10059 : msg_out.total_position_error = pos_error_value.norm();
+ 2073 :
+ 2074 10059 : if (yaw_error_) {
+ 2075 10059 : msg_out.yaw_error = yaw_error.value();
+ 2076 : }
+ 2077 :
+ 2078 10059 : std::map<std::string, ControllerParams>::iterator it;
+ 2079 :
+ 2080 10059 : it = controllers_.find(_controller_names_.at(active_controller_idx));
+ 2081 :
+ 2082 10059 : msg_out.position_eland_threshold = it->second.eland_threshold;
+ 2083 10059 : msg_out.position_failsafe_threshold = it->second.failsafe_threshold;
+ 2084 :
+ 2085 10059 : ph_control_error_.publish(msg_out);
+ 2086 : }
+ 2087 :
+ 2088 : // --------------------------------------------------------------
+ 2089 : // | publish the mass estimate |
+ 2090 : // --------------------------------------------------------------
+ 2091 :
+ 2092 16379 : if (last_control_output.diagnostics.mass_estimator) {
+ 2093 :
+ 2094 8750 : std_msgs::Float64 mass_estimate_out;
+ 2095 8750 : mass_estimate_out.data = _uav_mass_ + last_control_output.diagnostics.mass_difference;
+ 2096 :
+ 2097 8750 : ph_mass_estimate_.publish(mass_estimate_out);
+ 2098 : }
+ 2099 :
+ 2100 : // --------------------------------------------------------------
+ 2101 : // | publish the current heading |
+ 2102 : // --------------------------------------------------------------
+ 2103 :
+ 2104 16379 : if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+ 2105 :
+ 2106 : try {
+ 2107 :
+ 2108 : double heading;
+ 2109 :
+ 2110 13964 : heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+ 2111 :
+ 2112 27928 : mrs_msgs::Float64Stamped heading_out;
+ 2113 13964 : heading_out.header = uav_state.header;
+ 2114 13964 : heading_out.value = heading;
+ 2115 :
+ 2116 13964 : ph_heading_.publish(heading_out);
+ 2117 : }
+ 2118 0 : catch (...) {
+ 2119 0 : ROS_ERROR_THROTTLE(1.0, "exception caught, could not transform heading");
+ 2120 : }
+ 2121 : }
+ 2122 :
+ 2123 : // --------------------------------------------------------------
+ 2124 : // | publish the current speed |
+ 2125 : // --------------------------------------------------------------
+ 2126 :
+ 2127 16379 : if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+ 2128 :
+ 2129 13964 : double speed = sqrt(pow(uav_state.velocity.linear.x, 2) + pow(uav_state.velocity.linear.y, 2) + pow(uav_state.velocity.linear.z, 2));
+ 2130 :
+ 2131 27928 : mrs_msgs::Float64Stamped speed_out;
+ 2132 13964 : speed_out.header = uav_state.header;
+ 2133 13964 : speed_out.value = speed;
+ 2134 :
+ 2135 13964 : ph_speed_.publish(speed_out);
+ 2136 : }
+ 2137 :
+ 2138 : // --------------------------------------------------------------
+ 2139 : // | publish the safety area markers |
+ 2140 : // --------------------------------------------------------------
+ 2141 :
+ 2142 16379 : if (use_safety_area_) {
+ 2143 :
+ 2144 26242 : mrs_msgs::ReferenceStamped temp_ref;
+ 2145 13121 : temp_ref.header.frame_id = _safety_area_horizontal_frame_;
+ 2146 :
+ 2147 26242 : geometry_msgs::TransformStamped tf;
+ 2148 :
+ 2149 39363 : auto ret = transformer_->getTransform(_safety_area_horizontal_frame_, "local_origin", ros::Time(0));
+ 2150 :
+ 2151 13121 : if (ret) {
+ 2152 :
+ 2153 10981 : ROS_INFO_ONCE("[ControlManager]: got TFs, publishing safety area markers");
+ 2154 :
+ 2155 21962 : visualization_msgs::MarkerArray safety_area_marker_array;
+ 2156 21962 : visualization_msgs::MarkerArray safety_area_coordinates_marker_array;
+ 2157 :
+ 2158 21962 : mrs_lib::Polygon border = safety_zone_->getBorder();
+ 2159 :
+ 2160 21962 : std::vector<geometry_msgs::Point> border_points_bot_original = border.getPointMessageVector(getMinZ(_safety_area_horizontal_frame_));
+ 2161 21962 : std::vector<geometry_msgs::Point> border_points_top_original = border.getPointMessageVector(getMaxZ(_safety_area_horizontal_frame_));
+ 2162 :
+ 2163 21962 : std::vector<geometry_msgs::Point> border_points_bot_transformed = border_points_bot_original;
+ 2164 21962 : std::vector<geometry_msgs::Point> border_points_top_transformed = border_points_bot_original;
+ 2165 :
+ 2166 : // if we fail in transforming the area at some point
+ 2167 : // do not publish it at all
+ 2168 10981 : bool tf_success = true;
+ 2169 :
+ 2170 21962 : geometry_msgs::TransformStamped tf = ret.value();
+ 2171 :
+ 2172 : /* transform area points to local origin //{ */
+ 2173 :
+ 2174 : // transform border bottom points to local origin
+ 2175 54905 : for (size_t i = 0; i < border_points_bot_original.size(); i++) {
+ 2176 :
+ 2177 43924 : temp_ref.header.frame_id = _safety_area_horizontal_frame_;
+ 2178 43924 : temp_ref.header.stamp = ros::Time(0);
+ 2179 43924 : temp_ref.reference.position.x = border_points_bot_original.at(i).x;
+ 2180 43924 : temp_ref.reference.position.y = border_points_bot_original.at(i).y;
+ 2181 43924 : temp_ref.reference.position.z = border_points_bot_original.at(i).z;
+ 2182 :
+ 2183 87848 : if (auto ret = transformer_->transform(temp_ref, tf)) {
+ 2184 :
+ 2185 43924 : temp_ref = ret.value();
+ 2186 :
+ 2187 43924 : border_points_bot_transformed.at(i).x = temp_ref.reference.position.x;
+ 2188 43924 : border_points_bot_transformed.at(i).y = temp_ref.reference.position.y;
+ 2189 43924 : border_points_bot_transformed.at(i).z = temp_ref.reference.position.z;
+ 2190 :
+ 2191 : } else {
+ 2192 0 : tf_success = false;
+ 2193 : }
+ 2194 : }
+ 2195 :
+ 2196 : // transform border top points to local origin
+ 2197 54905 : for (size_t i = 0; i < border_points_top_original.size(); i++) {
+ 2198 :
+ 2199 43924 : temp_ref.header.frame_id = _safety_area_horizontal_frame_;
+ 2200 43924 : temp_ref.header.stamp = ros::Time(0);
+ 2201 43924 : temp_ref.reference.position.x = border_points_top_original.at(i).x;
+ 2202 43924 : temp_ref.reference.position.y = border_points_top_original.at(i).y;
+ 2203 43924 : temp_ref.reference.position.z = border_points_top_original.at(i).z;
+ 2204 :
+ 2205 87848 : if (auto ret = transformer_->transform(temp_ref, tf)) {
+ 2206 :
+ 2207 43924 : temp_ref = ret.value();
+ 2208 :
+ 2209 43924 : border_points_top_transformed.at(i).x = temp_ref.reference.position.x;
+ 2210 43924 : border_points_top_transformed.at(i).y = temp_ref.reference.position.y;
+ 2211 43924 : border_points_top_transformed.at(i).z = temp_ref.reference.position.z;
+ 2212 :
+ 2213 : } else {
+ 2214 0 : tf_success = false;
+ 2215 : }
+ 2216 : }
+ 2217 :
+ 2218 : //}
+ 2219 :
+ 2220 21962 : visualization_msgs::Marker safety_area_marker;
+ 2221 :
+ 2222 10981 : safety_area_marker.header.frame_id = _uav_name_ + "/local_origin";
+ 2223 10981 : safety_area_marker.type = visualization_msgs::Marker::LINE_LIST;
+ 2224 10981 : safety_area_marker.color.a = 0.15;
+ 2225 10981 : safety_area_marker.scale.x = 0.2;
+ 2226 10981 : safety_area_marker.color.r = 1;
+ 2227 10981 : safety_area_marker.color.g = 0;
+ 2228 10981 : safety_area_marker.color.b = 0;
+ 2229 :
+ 2230 10981 : safety_area_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+ 2231 :
+ 2232 21962 : visualization_msgs::Marker safety_area_coordinates_marker;
+ 2233 :
+ 2234 10981 : safety_area_coordinates_marker.header.frame_id = _uav_name_ + "/local_origin";
+ 2235 10981 : safety_area_coordinates_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+ 2236 10981 : safety_area_coordinates_marker.color.a = 1;
+ 2237 10981 : safety_area_coordinates_marker.scale.z = 1.0;
+ 2238 10981 : safety_area_coordinates_marker.color.r = 0;
+ 2239 10981 : safety_area_coordinates_marker.color.g = 0;
+ 2240 10981 : safety_area_coordinates_marker.color.b = 0;
+ 2241 :
+ 2242 10981 : safety_area_coordinates_marker.id = 0;
+ 2243 :
+ 2244 10981 : safety_area_coordinates_marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+ 2245 :
+ 2246 : /* adding safety area points //{ */
+ 2247 :
+ 2248 : // bottom border
+ 2249 54905 : for (size_t i = 0; i < border_points_bot_transformed.size(); i++) {
+ 2250 :
+ 2251 43924 : safety_area_marker.points.push_back(border_points_bot_transformed.at(i));
+ 2252 43924 : safety_area_marker.points.push_back(border_points_bot_transformed.at((i + 1) % border_points_bot_transformed.size()));
+ 2253 :
+ 2254 87848 : std::stringstream ss;
+ 2255 :
+ 2256 43924 : if (_safety_area_horizontal_frame_ == "latlon_origin") {
+ 2257 0 : ss << "idx: " << i << std::endl
+ 2258 0 : << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original.at(i).x << std::endl
+ 2259 0 : << "lon: " << border_points_bot_original.at(i).y;
+ 2260 : } else {
+ 2261 43924 : ss << "idx: " << i << std::endl
+ 2262 43924 : << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original.at(i).x << std::endl
+ 2263 43924 : << "y: " << border_points_bot_original.at(i).y;
+ 2264 : }
+ 2265 :
+ 2266 43924 : safety_area_coordinates_marker.color.r = 0;
+ 2267 43924 : safety_area_coordinates_marker.color.g = 0;
+ 2268 43924 : safety_area_coordinates_marker.color.b = 0;
+ 2269 :
+ 2270 43924 : safety_area_coordinates_marker.pose.position = border_points_bot_transformed.at(i);
+ 2271 43924 : safety_area_coordinates_marker.text = ss.str();
+ 2272 43924 : safety_area_coordinates_marker.id++;
+ 2273 :
+ 2274 43924 : safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+ 2275 : }
+ 2276 :
+ 2277 : // top border + top/bot edges
+ 2278 54905 : for (size_t i = 0; i < border_points_top_transformed.size(); i++) {
+ 2279 :
+ 2280 43924 : safety_area_marker.points.push_back(border_points_top_transformed.at(i));
+ 2281 43924 : safety_area_marker.points.push_back(border_points_top_transformed.at((i + 1) % border_points_top_transformed.size()));
+ 2282 :
+ 2283 43924 : safety_area_marker.points.push_back(border_points_bot_transformed.at(i));
+ 2284 43924 : safety_area_marker.points.push_back(border_points_top_transformed.at(i));
+ 2285 :
+ 2286 87848 : std::stringstream ss;
+ 2287 :
+ 2288 43924 : if (_safety_area_horizontal_frame_ == "latlon_origin") {
+ 2289 0 : ss << "idx: " << i << std::endl
+ 2290 0 : << std::setprecision(6) << std::fixed << "lat: " << border_points_bot_original.at(i).x << std::endl
+ 2291 0 : << "lon: " << border_points_bot_original.at(i).y;
+ 2292 : } else {
+ 2293 43924 : ss << "idx: " << i << std::endl
+ 2294 43924 : << std::setprecision(1) << std::fixed << "x: " << border_points_bot_original.at(i).x << std::endl
+ 2295 43924 : << "y: " << border_points_bot_original.at(i).y;
+ 2296 : }
+ 2297 :
+ 2298 43924 : safety_area_coordinates_marker.color.r = 1;
+ 2299 43924 : safety_area_coordinates_marker.color.g = 1;
+ 2300 43924 : safety_area_coordinates_marker.color.b = 1;
+ 2301 :
+ 2302 43924 : safety_area_coordinates_marker.pose.position = border_points_top_transformed.at(i);
+ 2303 43924 : safety_area_coordinates_marker.text = ss.str();
+ 2304 43924 : safety_area_coordinates_marker.id++;
+ 2305 :
+ 2306 43924 : safety_area_coordinates_marker_array.markers.push_back(safety_area_coordinates_marker);
+ 2307 : }
+ 2308 :
+ 2309 : //}
+ 2310 :
+ 2311 10981 : if (tf_success) {
+ 2312 :
+ 2313 10981 : safety_area_marker_array.markers.push_back(safety_area_marker);
+ 2314 :
+ 2315 10981 : ph_safety_area_markers_.publish(safety_area_marker_array);
+ 2316 :
+ 2317 10981 : ph_safety_area_coordinates_markers_.publish(safety_area_coordinates_marker_array);
+ 2318 : }
+ 2319 :
+ 2320 : } else {
+ 2321 2140 : ROS_WARN_ONCE("[ControlManager]: missing TFs, can not publish safety area markers");
+ 2322 : }
+ 2323 : }
+ 2324 :
+ 2325 : // --------------------------------------------------------------
+ 2326 : // | publish the disturbances markers |
+ 2327 : // --------------------------------------------------------------
+ 2328 :
+ 2329 16379 : if (last_control_output.diagnostics.disturbance_estimator && got_uav_state_) {
+ 2330 :
+ 2331 17604 : visualization_msgs::MarkerArray msg_out;
+ 2332 :
+ 2333 8802 : double id = 0;
+ 2334 :
+ 2335 8802 : double multiplier = 1.0;
+ 2336 :
+ 2337 8802 : Eigen::Quaterniond quat_eigen = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+ 2338 :
+ 2339 8802 : Eigen::Vector3d vec3d;
+ 2340 8802 : geometry_msgs::Point point;
+ 2341 :
+ 2342 : /* world disturbance //{ */
+ 2343 : {
+ 2344 :
+ 2345 17604 : visualization_msgs::Marker marker;
+ 2346 :
+ 2347 8802 : marker.header.frame_id = uav_state.header.frame_id;
+ 2348 8802 : marker.header.stamp = ros::Time::now();
+ 2349 8802 : marker.ns = "control_manager";
+ 2350 8802 : marker.id = id++;
+ 2351 8802 : marker.type = visualization_msgs::Marker::ARROW;
+ 2352 8802 : marker.action = visualization_msgs::Marker::ADD;
+ 2353 :
+ 2354 : /* position //{ */
+ 2355 :
+ 2356 8802 : marker.pose.position.x = 0.0;
+ 2357 8802 : marker.pose.position.y = 0.0;
+ 2358 8802 : marker.pose.position.z = 0.0;
+ 2359 :
+ 2360 : //}
+ 2361 :
+ 2362 : /* orientation //{ */
+ 2363 :
+ 2364 8802 : marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+ 2365 :
+ 2366 : //}
+ 2367 :
+ 2368 : /* origin //{ */
+ 2369 8802 : point.x = uav_x;
+ 2370 8802 : point.y = uav_y;
+ 2371 8802 : point.z = uav_z;
+ 2372 :
+ 2373 8802 : marker.points.push_back(point);
+ 2374 :
+ 2375 : //}
+ 2376 :
+ 2377 : /* tip //{ */
+ 2378 :
+ 2379 8802 : point.x = uav_x + multiplier * last_control_output.diagnostics.disturbance_wx_w;
+ 2380 8802 : point.y = uav_y + multiplier * last_control_output.diagnostics.disturbance_wy_w;
+ 2381 8802 : point.z = uav_z;
+ 2382 :
+ 2383 8802 : marker.points.push_back(point);
+ 2384 :
+ 2385 : //}
+ 2386 :
+ 2387 8802 : marker.scale.x = 0.05;
+ 2388 8802 : marker.scale.y = 0.05;
+ 2389 8802 : marker.scale.z = 0.05;
+ 2390 :
+ 2391 8802 : marker.color.a = 0.5;
+ 2392 8802 : marker.color.r = 1.0;
+ 2393 8802 : marker.color.g = 0.0;
+ 2394 8802 : marker.color.b = 0.0;
+ 2395 :
+ 2396 8802 : marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+ 2397 :
+ 2398 8802 : msg_out.markers.push_back(marker);
+ 2399 : }
+ 2400 :
+ 2401 : //}
+ 2402 :
+ 2403 : /* body disturbance //{ */
+ 2404 : {
+ 2405 :
+ 2406 17604 : visualization_msgs::Marker marker;
+ 2407 :
+ 2408 8802 : marker.header.frame_id = uav_state.header.frame_id;
+ 2409 8802 : marker.header.stamp = ros::Time::now();
+ 2410 8802 : marker.ns = "control_manager";
+ 2411 8802 : marker.id = id++;
+ 2412 8802 : marker.type = visualization_msgs::Marker::ARROW;
+ 2413 8802 : marker.action = visualization_msgs::Marker::ADD;
+ 2414 :
+ 2415 : /* position //{ */
+ 2416 :
+ 2417 8802 : marker.pose.position.x = 0.0;
+ 2418 8802 : marker.pose.position.y = 0.0;
+ 2419 8802 : marker.pose.position.z = 0.0;
+ 2420 :
+ 2421 : //}
+ 2422 :
+ 2423 : /* orientation //{ */
+ 2424 :
+ 2425 8802 : marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+ 2426 :
+ 2427 : //}
+ 2428 :
+ 2429 : /* origin //{ */
+ 2430 :
+ 2431 8802 : point.x = uav_x;
+ 2432 8802 : point.y = uav_y;
+ 2433 8802 : point.z = uav_z;
+ 2434 :
+ 2435 8802 : marker.points.push_back(point);
+ 2436 :
+ 2437 : //}
+ 2438 :
+ 2439 : /* tip //{ */
+ 2440 :
+ 2441 8802 : vec3d << multiplier * last_control_output.diagnostics.disturbance_bx_b, multiplier * last_control_output.diagnostics.disturbance_by_b, 0;
+ 2442 8802 : vec3d = quat_eigen * vec3d;
+ 2443 :
+ 2444 8802 : point.x = uav_x + vec3d(0);
+ 2445 8802 : point.y = uav_y + vec3d(1);
+ 2446 8802 : point.z = uav_z + vec3d(2);
+ 2447 :
+ 2448 8802 : marker.points.push_back(point);
+ 2449 :
+ 2450 : //}
+ 2451 :
+ 2452 8802 : marker.scale.x = 0.05;
+ 2453 8802 : marker.scale.y = 0.05;
+ 2454 8802 : marker.scale.z = 0.05;
+ 2455 :
+ 2456 8802 : marker.color.a = 0.5;
+ 2457 8802 : marker.color.r = 0.0;
+ 2458 8802 : marker.color.g = 1.0;
+ 2459 8802 : marker.color.b = 0.0;
+ 2460 :
+ 2461 8802 : marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
+ 2462 :
+ 2463 8802 : msg_out.markers.push_back(marker);
+ 2464 : }
+ 2465 :
+ 2466 : //}
+ 2467 :
+ 2468 8802 : ph_disturbances_markers_.publish(msg_out);
+ 2469 : }
+ 2470 :
+ 2471 : // --------------------------------------------------------------
+ 2472 : // | publish the current constraints |
+ 2473 : // --------------------------------------------------------------
+ 2474 :
+ 2475 16379 : if (got_constraints_) {
+ 2476 :
+ 2477 13905 : auto sanitized_constraints = mrs_lib::get_mutexed(mutex_constraints_, sanitized_constraints_);
+ 2478 :
+ 2479 13905 : mrs_msgs::DynamicsConstraints constraints = sanitized_constraints.constraints;
+ 2480 :
+ 2481 13905 : ph_current_constraints_.publish(constraints);
+ 2482 : }
+ 2483 : }
+ 2484 :
+ 2485 : //}
+ 2486 :
+ 2487 : /* //{ timerSafety() */
+ 2488 :
+ 2489 299586 : void ControlManager::timerSafety(const ros::TimerEvent& event) {
+ 2490 :
+ 2491 299586 : mrs_lib::AtomicScopeFlag unset_running(running_safety_timer_);
+ 2492 :
+ 2493 299586 : if (!is_initialized_)
+ 2494 0 : return;
+ 2495 :
+ 2496 599171 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("timerSafety", _safety_timer_rate_, 0.05, event);
+ 2497 599172 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::timerSafety", scope_timer_logger_, scope_timer_enabled_);
+ 2498 :
+ 2499 : // copy member variables
+ 2500 299586 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 2501 299586 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 2502 299585 : auto [uav_state, uav_yaw] = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_, uav_yaw_);
+ 2503 299586 : auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+ 2504 299586 : auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+ 2505 :
+ 2506 572044 : if (!got_uav_state_ || (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) ||
+ 2507 272458 : active_tracker_idx == _null_tracker_idx_) {
+ 2508 103014 : return;
+ 2509 : }
+ 2510 :
+ 2511 196572 : if (odometry_switch_in_progress_) {
+ 2512 5 : ROS_WARN("[ControlManager]: timerSafety tried to run while odometry switch in progress");
+ 2513 5 : return;
+ 2514 : }
+ 2515 :
+ 2516 : // | ------------------------ timeouts ------------------------ |
+ 2517 :
+ 2518 196567 : if (_state_input_ == INPUT_UAV_STATE && sh_uav_state_.hasMsg()) {
+ 2519 196567 : double missing_for = (ros::Time::now() - sh_uav_state_.lastMsgTime()).toSec();
+ 2520 :
+ 2521 196567 : if (missing_for > _uav_state_max_missing_time_) {
+ 2522 0 : timeoutUavState(missing_for);
+ 2523 : }
+ 2524 : }
+ 2525 :
+ 2526 196567 : if (_state_input_ == INPUT_ODOMETRY && sh_odometry_.hasMsg()) {
+ 2527 0 : double missing_for = (ros::Time::now() - sh_odometry_.lastMsgTime()).toSec();
+ 2528 :
+ 2529 0 : if (missing_for > _uav_state_max_missing_time_) {
+ 2530 0 : timeoutUavState(missing_for);
+ 2531 : }
+ 2532 : }
+ 2533 :
+ 2534 : // | -------------- eland and failsafe thresholds ------------- |
+ 2535 :
+ 2536 196567 : std::map<std::string, ControllerParams>::iterator it;
+ 2537 196567 : it = controllers_.find(_controller_names_.at(active_controller_idx));
+ 2538 :
+ 2539 196567 : _eland_threshold_ = it->second.eland_threshold;
+ 2540 196567 : _failsafe_threshold_ = it->second.failsafe_threshold;
+ 2541 196567 : _odometry_innovation_threshold_ = it->second.odometry_innovation_threshold;
+ 2542 :
+ 2543 : // | --------- calculate control errors and tilt angle -------- |
+ 2544 :
+ 2545 : // This means that the timerFailsafe only does its work when Controllers and Trackers produce valid output.
+ 2546 : // Cases when the commands are not valid should be handle in updateControllers() and updateTrackers() methods.
+ 2547 196567 : if (!last_tracker_cmd || !last_control_output.control_output) {
+ 2548 171 : return;
+ 2549 : }
+ 2550 :
+ 2551 : {
+ 2552 196396 : std::scoped_lock lock(mutex_attitude_error_);
+ 2553 :
+ 2554 196396 : tilt_error_ = 0;
+ 2555 196396 : yaw_error_ = 0;
+ 2556 : }
+ 2557 :
+ 2558 : {
+ 2559 196396 : Eigen::Vector3d position_error = Eigen::Vector3d::Zero();
+ 2560 :
+ 2561 196396 : bool position_error_set = false;
+ 2562 :
+ 2563 196396 : if (last_tracker_cmd->use_position_horizontal && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+ 2564 :
+ 2565 195383 : position_error(0) = last_tracker_cmd->position.x - uav_state.pose.position.x;
+ 2566 195383 : position_error(1) = last_tracker_cmd->position.y - uav_state.pose.position.y;
+ 2567 :
+ 2568 195383 : position_error_set = true;
+ 2569 : }
+ 2570 :
+ 2571 196396 : if (last_tracker_cmd->use_position_vertical && !std::holds_alternative<mrs_msgs::HwApiPositionCmd>(last_control_output.control_output.value())) {
+ 2572 :
+ 2573 195383 : position_error(2) = last_tracker_cmd->position.z - uav_state.pose.position.z;
+ 2574 :
+ 2575 195383 : position_error_set = true;
+ 2576 : }
+ 2577 :
+ 2578 196396 : if (position_error_set) {
+ 2579 :
+ 2580 195383 : mrs_lib::set_mutexed(mutex_position_error_, {position_error}, position_error_);
+ 2581 : }
+ 2582 : }
+ 2583 :
+ 2584 : // rotate the drone's z axis
+ 2585 196396 : tf2::Transform uav_state_transform = mrs_lib::AttitudeConverter(uav_state.pose.orientation);
+ 2586 196396 : tf2::Vector3 uav_z_in_world = uav_state_transform * tf2::Vector3(0, 0, 1);
+ 2587 :
+ 2588 : // calculate the angle between the drone's z axis and the world's z axis
+ 2589 196396 : double tilt_angle = acos(uav_z_in_world.dot(tf2::Vector3(0, 0, 1)));
+ 2590 :
+ 2591 : // | ------------ calculate the tilt and yaw error ------------ |
+ 2592 :
+ 2593 : // | --------------------- the tilt error --------------------- |
+ 2594 :
+ 2595 196396 : if (last_control_output.desired_orientation) {
+ 2596 :
+ 2597 : // calculate the desired drone's z axis in the world frame
+ 2598 171158 : tf2::Transform attitude_cmd_transform = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value());
+ 2599 171157 : tf2::Vector3 uav_z_in_world_desired = attitude_cmd_transform * tf2::Vector3(0, 0, 1);
+ 2600 :
+ 2601 : {
+ 2602 171158 : std::scoped_lock lock(mutex_attitude_error_);
+ 2603 :
+ 2604 : // calculate the angle between the drone's z axis and the world's z axis
+ 2605 171158 : tilt_error_ = acos(uav_z_in_world.dot(uav_z_in_world_desired));
+ 2606 :
+ 2607 : // calculate the yaw error
+ 2608 171158 : double cmd_yaw = mrs_lib::AttitudeConverter(last_control_output.desired_orientation.value()).getYaw();
+ 2609 171158 : yaw_error_ = fabs(radians::diff(cmd_yaw, uav_yaw));
+ 2610 : }
+ 2611 : }
+ 2612 :
+ 2613 196396 : auto position_error = mrs_lib::get_mutexed(mutex_position_error_, position_error_);
+ 2614 196396 : auto [tilt_error, yaw_error] = mrs_lib::get_mutexed(mutex_attitude_error_, tilt_error_, yaw_error_);
+ 2615 :
+ 2616 : // --------------------------------------------------------------
+ 2617 : // | activate the failsafe controller in case of large error |
+ 2618 : // --------------------------------------------------------------
+ 2619 :
+ 2620 196396 : if (position_error) {
+ 2621 :
+ 2622 195383 : if (position_error->norm() > _failsafe_threshold_ && !failsafe_triggered_) {
+ 2623 :
+ 2624 15 : auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+ 2625 :
+ 2626 15 : if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+ 2627 :
+ 2628 2 : if (!failsafe_triggered_) {
+ 2629 :
+ 2630 2 : ROS_ERROR("[ControlManager]: activating failsafe land: control_error=%.2f/%.2f m (x: %.2f, y: %.2f, z: %.2f)", position_error->norm(),
+ 2631 : _failsafe_threshold_, position_error.value()(0), position_error.value()(1), position_error.value()(2));
+ 2632 :
+ 2633 2 : failsafe();
+ 2634 : }
+ 2635 : }
+ 2636 : }
+ 2637 : }
+ 2638 :
+ 2639 : // --------------------------------------------------------------
+ 2640 : // | activate emergency land in case of large innovation |
+ 2641 : // --------------------------------------------------------------
+ 2642 :
+ 2643 196396 : if (_odometry_innovation_check_enabled_) {
+ 2644 :
+ 2645 196395 : std::optional<nav_msgs::Odometry::ConstPtr> innovation;
+ 2646 :
+ 2647 196396 : if (sh_odometry_innovation_.hasMsg()) {
+ 2648 196396 : innovation = {sh_odometry_innovation_.getMsg()};
+ 2649 : } else {
+ 2650 0 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: missing estimator innnovation but the innovation check is enabled!");
+ 2651 : }
+ 2652 :
+ 2653 196396 : if (innovation) {
+ 2654 :
+ 2655 196396 : auto [x, y, z] = mrs_lib::getPosition(innovation.value());
+ 2656 :
+ 2657 196396 : double heading = 0;
+ 2658 : try {
+ 2659 196396 : heading = mrs_lib::getHeading(innovation.value());
+ 2660 : }
+ 2661 0 : catch (mrs_lib::AttitudeConverter::GetHeadingException& e) {
+ 2662 0 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception caught: '%s'", e.what());
+ 2663 : }
+ 2664 :
+ 2665 196396 : double last_innovation = mrs_lib::geometry::dist(vec3_t(x, y, z), vec3_t(0, 0, 0));
+ 2666 :
+ 2667 196395 : if (last_innovation > _odometry_innovation_threshold_ || radians::diff(heading, 0) > M_PI_2) {
+ 2668 :
+ 2669 1 : auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+ 2670 :
+ 2671 1 : if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+ 2672 :
+ 2673 1 : if (!failsafe_triggered_ && !eland_triggered_) {
+ 2674 :
+ 2675 1 : ROS_ERROR("[ControlManager]: activating emergency land: odometry innovation too large: %.2f/%.2f (x: %.2f, y: %.2f, z: %.2f, heading: %.2f)",
+ 2676 : last_innovation, _odometry_innovation_threshold_, x, y, z, heading);
+ 2677 :
+ 2678 1 : eland();
+ 2679 : }
+ 2680 : }
+ 2681 : }
+ 2682 : }
+ 2683 : }
+ 2684 :
+ 2685 : // --------------------------------------------------------------
+ 2686 : // | activate emergency land in case of medium control error |
+ 2687 : // --------------------------------------------------------------
+ 2688 :
+ 2689 : // | ------------------- tilt control error ------------------- |
+ 2690 :
+ 2691 196396 : if (_tilt_limit_eland_enabled_ && tilt_angle > _tilt_limit_eland_) {
+ 2692 :
+ 2693 0 : auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+ 2694 :
+ 2695 0 : if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+ 2696 :
+ 2697 0 : if (!failsafe_triggered_ && !eland_triggered_) {
+ 2698 :
+ 2699 0 : ROS_ERROR("[ControlManager]: activating emergency land: tilt angle too large (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_angle,
+ 2700 : (180.0 / M_PI) * _tilt_limit_eland_);
+ 2701 :
+ 2702 0 : eland();
+ 2703 : }
+ 2704 : }
+ 2705 : }
+ 2706 :
+ 2707 : // | ----------------- position control error ----------------- |
+ 2708 :
+ 2709 196396 : if (position_error) {
+ 2710 :
+ 2711 195381 : double error_size = position_error->norm();
+ 2712 :
+ 2713 195383 : if (error_size > _eland_threshold_ / 2.0) {
+ 2714 :
+ 2715 545 : auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+ 2716 :
+ 2717 545 : if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+ 2718 :
+ 2719 299 : if (!failsafe_triggered_ && !eland_triggered_) {
+ 2720 :
+ 2721 67 : ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload due to large position error");
+ 2722 :
+ 2723 67 : ungripSrv();
+ 2724 : }
+ 2725 : }
+ 2726 : }
+ 2727 :
+ 2728 195383 : if (error_size > _eland_threshold_) {
+ 2729 :
+ 2730 248 : auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+ 2731 :
+ 2732 248 : if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+ 2733 :
+ 2734 55 : if (!failsafe_triggered_ && !eland_triggered_) {
+ 2735 :
+ 2736 3 : ROS_ERROR("[ControlManager]: activating emergency land: position error %.2f/%.2f m (error x: %.2f, y: %.2f, z: %.2f)", error_size, _eland_threshold_,
+ 2737 : position_error.value()(0), position_error.value()(1), position_error.value()(2));
+ 2738 :
+ 2739 3 : eland();
+ 2740 : }
+ 2741 : }
+ 2742 : }
+ 2743 : }
+ 2744 :
+ 2745 : // | -------------------- yaw control error ------------------- |
+ 2746 : // do not have to mutex the yaw_error_ here since I am filling it in this function
+ 2747 :
+ 2748 196396 : if (_yaw_error_eland_enabled_ && yaw_error) {
+ 2749 :
+ 2750 196395 : if (yaw_error.value() > (_yaw_error_eland_ / 2.0)) {
+ 2751 :
+ 2752 0 : auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+ 2753 :
+ 2754 0 : if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+ 2755 :
+ 2756 0 : if (!failsafe_triggered_ && !eland_triggered_) {
+ 2757 :
+ 2758 0 : ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: releasing payload: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+ 2759 : (180.0 / M_PI) * _yaw_error_eland_ / 2.0);
+ 2760 :
+ 2761 0 : ungripSrv();
+ 2762 : }
+ 2763 : }
+ 2764 : }
+ 2765 :
+ 2766 196395 : if (yaw_error.value() > _yaw_error_eland_) {
+ 2767 :
+ 2768 0 : auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+ 2769 :
+ 2770 0 : if ((ros::Time::now() - controller_tracker_switch_time).toSec() > 1.0) {
+ 2771 :
+ 2772 0 : if (!failsafe_triggered_ && !eland_triggered_) {
+ 2773 :
+ 2774 0 : ROS_ERROR("[ControlManager]: activating emergency land: yaw error %.2f/%.2f deg", (180.0 / M_PI) * yaw_error.value(),
+ 2775 : (180.0 / M_PI) * _yaw_error_eland_);
+ 2776 :
+ 2777 0 : eland();
+ 2778 : }
+ 2779 : }
+ 2780 : }
+ 2781 : }
+ 2782 :
+ 2783 : // --------------------------------------------------------------
+ 2784 : // | disarm the drone when the tilt exceeds the limit |
+ 2785 : // --------------------------------------------------------------
+ 2786 196391 : if (_tilt_limit_disarm_enabled_ && tilt_angle > _tilt_limit_disarm_) {
+ 2787 :
+ 2788 0 : ROS_ERROR("[ControlManager]: tilt angle too large, disarming: tilt angle=%.2f/%.2f deg", (180.0 / M_PI) * tilt_angle, (180.0 / M_PI) * _tilt_limit_disarm_);
+ 2789 :
+ 2790 0 : arming(false);
+ 2791 : }
+ 2792 :
+ 2793 : // --------------------------------------------------------------
+ 2794 : // | disarm the drone when tilt error exceeds the limit |
+ 2795 : // --------------------------------------------------------------
+ 2796 :
+ 2797 196391 : if (_tilt_error_disarm_enabled_ && tilt_error) {
+ 2798 :
+ 2799 196392 : auto controller_tracker_switch_time = mrs_lib::get_mutexed(mutex_controller_tracker_switch_time_, controller_tracker_switch_time_);
+ 2800 :
+ 2801 : // the time from the last controller/tracker switch
+ 2802 : // fyi: we should not
+ 2803 196396 : double time_from_ctrl_tracker_switch = (ros::Time::now() - controller_tracker_switch_time).toSec();
+ 2804 :
+ 2805 : // if the tile error is over the threshold
+ 2806 : // && we are not ramping up during takeoff
+ 2807 196396 : if (fabs(tilt_error.value()) > _tilt_error_disarm_threshold_ && !last_control_output.diagnostics.ramping_up) {
+ 2808 :
+ 2809 : // only account for the error if some time passed from the last tracker/controller switch
+ 2810 0 : if (time_from_ctrl_tracker_switch > 1.0) {
+ 2811 :
+ 2812 : // if the threshold was not exceeded before
+ 2813 0 : if (!tilt_error_disarm_over_thr_) {
+ 2814 :
+ 2815 0 : tilt_error_disarm_over_thr_ = true;
+ 2816 0 : tilt_error_disarm_time_ = ros::Time::now();
+ 2817 :
+ 2818 0 : ROS_WARN("[ControlManager]: tilt error exceeded threshold (%.2f/%.2f deg)", (180.0 / M_PI) * tilt_error.value(),
+ 2819 : (180.0 / M_PI) * _tilt_error_disarm_threshold_);
+ 2820 :
+ 2821 : // if it was exceeded before, just keep it
+ 2822 : } else {
+ 2823 :
+ 2824 0 : ROS_WARN_THROTTLE(0.1, "[ControlManager]: tilt error (%.2f deg) over threshold for %.2f s", (180.0 / M_PI) * tilt_error.value(),
+ 2825 : (ros::Time::now() - tilt_error_disarm_time_).toSec());
+ 2826 : }
+ 2827 :
+ 2828 : // if the tile error is bad, but the controller just switched,
+ 2829 : // don't think its bad anymore
+ 2830 : } else {
+ 2831 :
+ 2832 0 : tilt_error_disarm_over_thr_ = false;
+ 2833 0 : tilt_error_disarm_time_ = ros::Time::now();
+ 2834 : }
+ 2835 :
+ 2836 : // if the tilt error is fine
+ 2837 : } else {
+ 2838 :
+ 2839 : // make it fine
+ 2840 196396 : tilt_error_disarm_over_thr_ = false;
+ 2841 196396 : tilt_error_disarm_time_ = ros::Time::now();
+ 2842 : }
+ 2843 :
+ 2844 : // calculate the time over the threshold
+ 2845 196395 : double tot = (ros::Time::now() - tilt_error_disarm_time_).toSec();
+ 2846 :
+ 2847 : // if the tot exceeds the limit (and if we are actually over the threshold)
+ 2848 196396 : if (tilt_error_disarm_over_thr_ && (tot > _tilt_error_disarm_timeout_)) {
+ 2849 :
+ 2850 0 : bool is_flying = offboard_mode_ && active_tracker_idx != _null_tracker_idx_;
+ 2851 :
+ 2852 : // only when flying and not in failsafe
+ 2853 0 : if (is_flying) {
+ 2854 :
+ 2855 0 : ROS_ERROR("[ControlManager]: tilt error too large for %.2f s, disarming", tot);
+ 2856 :
+ 2857 0 : toggleOutput(false);
+ 2858 0 : arming(false);
+ 2859 : }
+ 2860 : }
+ 2861 : }
+ 2862 :
+ 2863 : // | --------- dropping out of OFFBOARD in mid flight --------- |
+ 2864 :
+ 2865 : // if we are not in offboard and the drone is in mid air (NullTracker is not active)
+ 2866 196394 : if (offboard_mode_was_true_ && !offboard_mode_ && active_tracker_idx != _null_tracker_idx_) {
+ 2867 :
+ 2868 0 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: we fell out of OFFBOARD in mid air, disabling output");
+ 2869 :
+ 2870 0 : toggleOutput(false);
+ 2871 : }
+ 2872 : } // namespace control_manager
+ 2873 :
+ 2874 : //}
+ 2875 :
+ 2876 : /* //{ timerEland() */
+ 2877 :
+ 2878 444 : void ControlManager::timerEland(const ros::TimerEvent& event) {
+ 2879 :
+ 2880 444 : if (!is_initialized_)
+ 2881 80 : return;
+ 2882 :
+ 2883 888 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("timerEland", _elanding_timer_rate_, 0.01, event);
+ 2884 888 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::timerEland", scope_timer_logger_, scope_timer_enabled_);
+ 2885 :
+ 2886 : // copy member variables
+ 2887 444 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 2888 :
+ 2889 444 : if (!last_control_output.control_output) {
+ 2890 0 : return;
+ 2891 : }
+ 2892 :
+ 2893 444 : auto throttle = extractThrottle(last_control_output);
+ 2894 :
+ 2895 444 : if (!throttle) {
+ 2896 80 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: TODO: implement landing detection mechanism for the current control modality");
+ 2897 80 : return;
+ 2898 : }
+ 2899 :
+ 2900 364 : if (current_state_landing_ == IDLE_STATE) {
+ 2901 :
+ 2902 0 : return;
+ 2903 :
+ 2904 364 : } else if (current_state_landing_ == LANDING_STATE) {
+ 2905 :
+ 2906 : // --------------------------------------------------------------
+ 2907 : // | TODO |
+ 2908 : // This section needs work. The throttle landing detection |
+ 2909 : // mechanism should be extracted and other mechanisms, such |
+ 2910 : // as velocity-based detection should be used for high |
+ 2911 : // modalities |
+ 2912 : // --------------------------------------------------------------
+ 2913 :
+ 2914 364 : if (!last_control_output.control_output) {
+ 2915 0 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: timerEland: last_control_output has not been initialized, returning");
+ 2916 0 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: tip: the RC eland is probably triggered");
+ 2917 0 : return;
+ 2918 : }
+ 2919 :
+ 2920 : // recalculate the mass based on the throttle
+ 2921 364 : throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+ 2922 364 : ROS_INFO_THROTTLE(1.0, "[ControlManager]: landing: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+ 2923 :
+ 2924 : // condition for automatic motor turn off
+ 2925 364 : if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_) || throttle < 0.01)) {
+ 2926 88 : if (!throttle_under_threshold_) {
+ 2927 :
+ 2928 4 : throttle_mass_estimate_first_time_ = ros::Time::now();
+ 2929 4 : throttle_under_threshold_ = true;
+ 2930 : }
+ 2931 :
+ 2932 88 : ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+ 2933 :
+ 2934 : } else {
+ 2935 276 : throttle_mass_estimate_first_time_ = ros::Time::now();
+ 2936 276 : throttle_under_threshold_ = false;
+ 2937 : }
+ 2938 :
+ 2939 364 : if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+ 2940 : // enable callbacks? ... NO
+ 2941 :
+ 2942 4 : ROS_INFO("[ControlManager]: reached cutoff throttle, disabling output");
+ 2943 4 : toggleOutput(false);
+ 2944 :
+ 2945 : // disarm the drone
+ 2946 4 : if (_eland_disarm_enabled_) {
+ 2947 :
+ 2948 4 : ROS_INFO("[ControlManager]: calling for disarm");
+ 2949 4 : arming(false);
+ 2950 : }
+ 2951 :
+ 2952 4 : changeLandingState(IDLE_STATE);
+ 2953 :
+ 2954 4 : ROS_WARN("[ControlManager]: emergency landing finished");
+ 2955 :
+ 2956 4 : ROS_DEBUG("[ControlManager]: stopping eland timer");
+ 2957 4 : timer_eland_.stop();
+ 2958 4 : ROS_DEBUG("[ControlManager]: eland timer stopped");
+ 2959 :
+ 2960 : // we should NOT set eland_triggered_=true
+ 2961 : }
+ 2962 : }
+ 2963 : }
+ 2964 :
+ 2965 : //}
+ 2966 :
+ 2967 : /* //{ timerFailsafe() */
+ 2968 :
+ 2969 10207 : void ControlManager::timerFailsafe(const ros::TimerEvent& event) {
+ 2970 :
+ 2971 10207 : if (!is_initialized_) {
+ 2972 489 : return;
+ 2973 : }
+ 2974 :
+ 2975 10207 : ROS_INFO_ONCE("[ControlManager]: timerFailsafe() spinning");
+ 2976 :
+ 2977 20414 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("timerFailsafe", _failsafe_timer_rate_, 0.01, event);
+ 2978 20414 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::timerFailsafe", scope_timer_logger_, scope_timer_enabled_);
+ 2979 :
+ 2980 : // copy member variables
+ 2981 10207 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 2982 :
+ 2983 10207 : updateControllers(uav_state);
+ 2984 :
+ 2985 10207 : publish();
+ 2986 :
+ 2987 10207 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 2988 :
+ 2989 10207 : if (!last_control_output.control_output) {
+ 2990 0 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: timerFailsafe: the control output produced by the failsafe controller is empty!");
+ 2991 0 : return;
+ 2992 : }
+ 2993 :
+ 2994 10207 : auto throttle = extractThrottle(last_control_output);
+ 2995 :
+ 2996 10207 : if (!throttle) {
+ 2997 489 : ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: FailsafeTimer: could not extract throttle out of the last control output");
+ 2998 489 : return;
+ 2999 : }
+ 3000 :
+ 3001 : // --------------------------------------------------------------
+ 3002 : // | TODO |
+ 3003 : // This section needs work. The throttle landing detection |
+ 3004 : // mechanism should be extracted and other mechanisms, such |
+ 3005 : // as velocity-based detection should be used for high |
+ 3006 : // modalities |
+ 3007 : // --------------------------------------------------------------
+ 3008 :
+ 3009 9718 : double throttle_mass_estimate_ = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value()) / common_handlers_->g;
+ 3010 9718 : ROS_INFO_THROTTLE(1.0, "[ControlManager]: failsafe: initial mass: %.2f throttle_mass_estimate: %.2f", landing_uav_mass_, throttle_mass_estimate_);
+ 3011 :
+ 3012 : // condition for automatic motor turn off
+ 3013 9718 : if (((throttle_mass_estimate_ < _elanding_cutoff_mass_factor_ * landing_uav_mass_))) {
+ 3014 :
+ 3015 1414 : if (!throttle_under_threshold_) {
+ 3016 :
+ 3017 7 : throttle_mass_estimate_first_time_ = ros::Time::now();
+ 3018 7 : throttle_under_threshold_ = true;
+ 3019 : }
+ 3020 :
+ 3021 1414 : ROS_INFO_THROTTLE(0.1, "[ControlManager]: throttle is under cutoff factor for %.2f s", (ros::Time::now() - throttle_mass_estimate_first_time_).toSec());
+ 3022 :
+ 3023 : } else {
+ 3024 :
+ 3025 8304 : throttle_mass_estimate_first_time_ = ros::Time::now();
+ 3026 8304 : throttle_under_threshold_ = false;
+ 3027 : }
+ 3028 :
+ 3029 : // condition for automatic motor turn off
+ 3030 9718 : if (throttle_under_threshold_ && ((ros::Time::now() - throttle_mass_estimate_first_time_).toSec() > _elanding_cutoff_timeout_)) {
+ 3031 :
+ 3032 7 : ROS_INFO_THROTTLE(1.0, "[ControlManager]: detecting zero throttle, disarming");
+ 3033 :
+ 3034 7 : arming(false);
+ 3035 : }
+ 3036 : }
+ 3037 :
+ 3038 : //}
+ 3039 :
+ 3040 : /* //{ timerJoystick() */
+ 3041 :
+ 3042 58565 : void ControlManager::timerJoystick(const ros::TimerEvent& event) {
+ 3043 :
+ 3044 58565 : if (!is_initialized_) {
+ 3045 0 : return;
+ 3046 : }
+ 3047 :
+ 3048 175695 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("timerJoystick", _status_timer_rate_, 0.05, event);
+ 3049 175695 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::timerJoystick", scope_timer_logger_, scope_timer_enabled_);
+ 3050 :
+ 3051 117130 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 3052 :
+ 3053 : // if start was pressed and held for > 3.0 s
+ 3054 58565 : if (joystick_start_pressed_ && joystick_start_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_start_press_time_).toSec() > 3.0) {
+ 3055 :
+ 3056 0 : joystick_start_press_time_ = ros::Time(0);
+ 3057 :
+ 3058 0 : ROS_INFO("[ControlManager]: transitioning to joystick control: activating '%s' and '%s'", _joystick_tracker_name_.c_str(),
+ 3059 : _joystick_controller_name_.c_str());
+ 3060 :
+ 3061 0 : joystick_start_pressed_ = false;
+ 3062 :
+ 3063 0 : switchTracker(_joystick_tracker_name_);
+ 3064 0 : switchController(_joystick_controller_name_);
+ 3065 : }
+ 3066 :
+ 3067 : // if RT+LT were pressed and held for > 0.1 s
+ 3068 58565 : if (joystick_failsafe_pressed_ && joystick_failsafe_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_failsafe_press_time_).toSec() > 0.1) {
+ 3069 :
+ 3070 0 : joystick_failsafe_press_time_ = ros::Time(0);
+ 3071 :
+ 3072 0 : ROS_INFO("[ControlManager]: activating failsafe by joystick");
+ 3073 :
+ 3074 0 : joystick_failsafe_pressed_ = false;
+ 3075 :
+ 3076 0 : failsafe();
+ 3077 : }
+ 3078 :
+ 3079 : // if joypads were pressed and held for > 0.1 s
+ 3080 58565 : if (joystick_eland_pressed_ && joystick_eland_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_eland_press_time_).toSec() > 0.1) {
+ 3081 :
+ 3082 0 : joystick_eland_press_time_ = ros::Time(0);
+ 3083 :
+ 3084 0 : ROS_INFO("[ControlManager]: activating eland by joystick");
+ 3085 :
+ 3086 0 : joystick_failsafe_pressed_ = false;
+ 3087 :
+ 3088 0 : eland();
+ 3089 : }
+ 3090 :
+ 3091 : // if back was pressed and held for > 0.1 s
+ 3092 58565 : if (joystick_back_pressed_ && joystick_back_press_time_ != ros::Time(0) && (ros::Time::now() - joystick_back_press_time_).toSec() > 0.1) {
+ 3093 :
+ 3094 0 : joystick_back_press_time_ = ros::Time(0);
+ 3095 :
+ 3096 : // activate/deactivate the joystick goto functionality
+ 3097 0 : joystick_goto_enabled_ = !joystick_goto_enabled_;
+ 3098 :
+ 3099 0 : ROS_INFO("[ControlManager]: joystick control %s", joystick_goto_enabled_ ? "activated" : "deactivated");
+ 3100 : }
+ 3101 :
+ 3102 : // if the GOTO functionality is enabled...
+ 3103 58565 : if (joystick_goto_enabled_ && sh_joystick_.hasMsg()) {
+ 3104 :
+ 3105 0 : auto joystick_data = sh_joystick_.getMsg();
+ 3106 :
+ 3107 : // create the reference
+ 3108 :
+ 3109 0 : mrs_msgs::Vec4::Request request;
+ 3110 :
+ 3111 0 : if (fabs(joystick_data->axes.at(_channel_pitch_)) >= 0.05 || fabs(joystick_data->axes.at(_channel_roll_)) >= 0.05 ||
+ 3112 0 : fabs(joystick_data->axes.at(_channel_heading_)) >= 0.05 || fabs(joystick_data->axes.at(_channel_throttle_)) >= 0.05) {
+ 3113 :
+ 3114 0 : if (_joystick_mode_ == 0) {
+ 3115 :
+ 3116 0 : request.goal.at(REF_X) = _channel_mult_pitch_ * joystick_data->axes.at(_channel_pitch_) * _joystick_carrot_distance_;
+ 3117 0 : request.goal.at(REF_Y) = _channel_mult_roll_ * joystick_data->axes.at(_channel_roll_) * _joystick_carrot_distance_;
+ 3118 0 : request.goal.at(REF_Z) = _channel_mult_throttle_ * joystick_data->axes.at(_channel_throttle_);
+ 3119 0 : request.goal.at(REF_HEADING) = _channel_mult_heading_ * joystick_data->axes.at(_channel_heading_);
+ 3120 :
+ 3121 0 : mrs_msgs::Vec4::Response response;
+ 3122 :
+ 3123 0 : callbackGotoFcu(request, response);
+ 3124 :
+ 3125 0 : } else if (_joystick_mode_ == 1) {
+ 3126 :
+ 3127 0 : mrs_msgs::TrajectoryReference trajectory;
+ 3128 :
+ 3129 0 : double dt = 0.2;
+ 3130 :
+ 3131 0 : trajectory.fly_now = true;
+ 3132 0 : trajectory.header.frame_id = "fcu_untilted";
+ 3133 0 : trajectory.use_heading = true;
+ 3134 0 : trajectory.dt = dt;
+ 3135 :
+ 3136 0 : mrs_msgs::Reference point;
+ 3137 0 : point.position.x = 0;
+ 3138 0 : point.position.y = 0;
+ 3139 0 : point.position.z = 0;
+ 3140 0 : point.heading = 0;
+ 3141 :
+ 3142 0 : trajectory.points.push_back(point);
+ 3143 :
+ 3144 0 : double speed = 1.0;
+ 3145 :
+ 3146 0 : for (int i = 0; i < 50; i++) {
+ 3147 :
+ 3148 0 : point.position.x += _channel_mult_pitch_ * joystick_data->axes.at(_channel_pitch_) * (speed * dt);
+ 3149 0 : point.position.y += _channel_mult_roll_ * joystick_data->axes.at(_channel_roll_) * (speed * dt);
+ 3150 0 : point.position.z += _channel_mult_throttle_ * joystick_data->axes.at(_channel_throttle_) * (speed * dt);
+ 3151 0 : point.heading = _channel_mult_heading_ * joystick_data->axes.at(_channel_heading_);
+ 3152 :
+ 3153 0 : trajectory.points.push_back(point);
+ 3154 : }
+ 3155 :
+ 3156 0 : setTrajectoryReference(trajectory);
+ 3157 : }
+ 3158 : }
+ 3159 : }
+ 3160 :
+ 3161 58565 : if (rc_goto_active_ && last_tracker_cmd && sh_hw_api_rc_.hasMsg()) {
+ 3162 :
+ 3163 : // create the reference
+ 3164 1094 : mrs_msgs::VelocityReferenceStampedSrv::Request request;
+ 3165 :
+ 3166 547 : double des_x = 0;
+ 3167 547 : double des_y = 0;
+ 3168 547 : double des_z = 0;
+ 3169 547 : double des_heading = 0;
+ 3170 :
+ 3171 547 : bool nothing_to_do = true;
+ 3172 :
+ 3173 : // copy member variables
+ 3174 1094 : mrs_msgs::HwApiRcChannelsConstPtr rc_channels = sh_hw_api_rc_.getMsg();
+ 3175 :
+ 3176 547 : if (rc_channels->channels.size() < 4) {
+ 3177 :
+ 3178 0 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC control channel numbers are out of range (the # of channels in rc/in topic is %d)",
+ 3179 : int(rc_channels->channels.size()));
+ 3180 0 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: tip: this could be caused by the RC failsafe not being configured!");
+ 3181 :
+ 3182 : } else {
+ 3183 :
+ 3184 547 : double tmp_x = RCChannelToRange(rc_channels->channels.at(_rc_channel_pitch_), _rc_horizontal_speed_, 0.1);
+ 3185 547 : double tmp_y = -RCChannelToRange(rc_channels->channels.at(_rc_channel_roll_), _rc_horizontal_speed_, 0.1);
+ 3186 547 : double tmp_z = RCChannelToRange(rc_channels->channels.at(_rc_channel_throttle_), _rc_vertical_speed_, 0.3);
+ 3187 547 : double tmp_heading = -RCChannelToRange(rc_channels->channels.at(_rc_channel_heading_), _rc_heading_rate_, 0.1);
+ 3188 :
+ 3189 547 : if (abs(tmp_x) > 1e-3) {
+ 3190 90 : des_x = tmp_x;
+ 3191 90 : nothing_to_do = false;
+ 3192 : }
+ 3193 :
+ 3194 547 : if (abs(tmp_y) > 1e-3) {
+ 3195 113 : des_y = tmp_y;
+ 3196 113 : nothing_to_do = false;
+ 3197 : }
+ 3198 :
+ 3199 547 : if (abs(tmp_z) > 1e-3) {
+ 3200 221 : des_z = tmp_z;
+ 3201 221 : nothing_to_do = false;
+ 3202 : }
+ 3203 :
+ 3204 547 : if (abs(tmp_heading) > 1e-3) {
+ 3205 48 : des_heading = tmp_heading;
+ 3206 48 : nothing_to_do = false;
+ 3207 : }
+ 3208 : }
+ 3209 :
+ 3210 547 : if (!nothing_to_do) {
+ 3211 :
+ 3212 472 : request.reference.header.frame_id = "fcu_untilted";
+ 3213 :
+ 3214 472 : request.reference.reference.use_heading_rate = true;
+ 3215 :
+ 3216 472 : request.reference.reference.velocity.x = des_x;
+ 3217 472 : request.reference.reference.velocity.y = des_y;
+ 3218 472 : request.reference.reference.velocity.z = des_z;
+ 3219 472 : request.reference.reference.heading_rate = des_heading;
+ 3220 :
+ 3221 944 : mrs_msgs::VelocityReferenceStampedSrv::Response response;
+ 3222 :
+ 3223 : // disable callbacks of all trackers
+ 3224 472 : std_srvs::SetBoolRequest req_enable_callbacks;
+ 3225 :
+ 3226 : // enable the callbacks for the active tracker
+ 3227 472 : req_enable_callbacks.data = true;
+ 3228 : {
+ 3229 472 : std::scoped_lock lock(mutex_tracker_list_);
+ 3230 :
+ 3231 944 : tracker_list_.at(active_tracker_idx_)
+ 3232 944 : ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 3233 : }
+ 3234 :
+ 3235 472 : callbacks_enabled_ = true;
+ 3236 :
+ 3237 472 : callbackVelocityReferenceService(request, response);
+ 3238 :
+ 3239 472 : callbacks_enabled_ = false;
+ 3240 :
+ 3241 472 : ROS_INFO_THROTTLE(1.0, "[ControlManager]: goto by RC with speed x=%.2f, y=%.2f, z=%.2f, heading_rate=%.2f", des_x, des_y, des_z, des_heading);
+ 3242 :
+ 3243 : // disable the callbacks back again
+ 3244 472 : req_enable_callbacks.data = false;
+ 3245 : {
+ 3246 472 : std::scoped_lock lock(mutex_tracker_list_);
+ 3247 :
+ 3248 944 : tracker_list_.at(active_tracker_idx_)
+ 3249 944 : ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 3250 : }
+ 3251 : }
+ 3252 : }
+ 3253 : }
+ 3254 :
+ 3255 : //}
+ 3256 :
+ 3257 : /* //{ timerBumper() */
+ 3258 :
+ 3259 2157 : void ControlManager::timerBumper(const ros::TimerEvent& event) {
+ 3260 :
+ 3261 2157 : if (!is_initialized_)
+ 3262 1897 : return;
+ 3263 :
+ 3264 4314 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("timerBumper", _bumper_timer_rate_, 0.05, event);
+ 3265 4314 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::timerBumper", scope_timer_logger_, scope_timer_enabled_);
+ 3266 :
+ 3267 : // | ---------------------- preconditions --------------------- |
+ 3268 :
+ 3269 2157 : if (!sh_bumper_.hasMsg()) {
+ 3270 1893 : return;
+ 3271 : }
+ 3272 :
+ 3273 264 : if (!bumper_enabled_) {
+ 3274 0 : return;
+ 3275 : }
+ 3276 :
+ 3277 264 : if (!isFlyingNormally() && !bumper_repulsing_) {
+ 3278 4 : return;
+ 3279 : }
+ 3280 :
+ 3281 260 : if (!got_uav_state_) {
+ 3282 0 : return;
+ 3283 : }
+ 3284 :
+ 3285 260 : if (sh_bumper_.hasMsg() && (ros::Time::now() - sh_bumper_.lastMsgTime()).toSec() > 1.0) {
+ 3286 :
+ 3287 0 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: obstacle bumper is missing messages although we got them before");
+ 3288 :
+ 3289 0 : return;
+ 3290 : }
+ 3291 :
+ 3292 260 : timer_bumper_.setPeriod(ros::Duration(1.0 / _bumper_timer_rate_));
+ 3293 :
+ 3294 : // | ------------------ call the bumper logic ----------------- |
+ 3295 :
+ 3296 260 : bumperPushFromObstacle();
+ 3297 : }
+ 3298 :
+ 3299 : //}
+ 3300 :
+ 3301 : /* //{ timerPirouette() */
+ 3302 :
+ 3303 0 : void ControlManager::timerPirouette(const ros::TimerEvent& event) {
+ 3304 :
+ 3305 0 : if (!is_initialized_)
+ 3306 0 : return;
+ 3307 :
+ 3308 0 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("timerPirouette", _pirouette_timer_rate_, 0.01, event);
+ 3309 0 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::timerPirouette", scope_timer_logger_, scope_timer_enabled_);
+ 3310 :
+ 3311 0 : pirouette_iterator_++;
+ 3312 :
+ 3313 0 : double pirouette_duration = (2 * M_PI) / _pirouette_speed_;
+ 3314 0 : double pirouette_n_steps = pirouette_duration * _pirouette_timer_rate_;
+ 3315 0 : double pirouette_step_size = (2 * M_PI) / pirouette_n_steps;
+ 3316 :
+ 3317 0 : if (rc_escalating_failsafe_triggered_ || failsafe_triggered_ || eland_triggered_ || (pirouette_iterator_ > pirouette_duration * _pirouette_timer_rate_)) {
+ 3318 :
+ 3319 0 : _pirouette_enabled_ = false;
+ 3320 0 : timer_pirouette_.stop();
+ 3321 :
+ 3322 0 : setCallbacks(true);
+ 3323 :
+ 3324 0 : return;
+ 3325 : }
+ 3326 :
+ 3327 : // set the reference
+ 3328 0 : mrs_msgs::ReferenceStamped reference_request;
+ 3329 :
+ 3330 0 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 3331 :
+ 3332 0 : reference_request.header.frame_id = "";
+ 3333 0 : reference_request.header.stamp = ros::Time(0);
+ 3334 0 : reference_request.reference.position.x = last_tracker_cmd->position.x;
+ 3335 0 : reference_request.reference.position.y = last_tracker_cmd->position.y;
+ 3336 0 : reference_request.reference.position.z = last_tracker_cmd->position.z;
+ 3337 0 : reference_request.reference.heading = pirouette_initial_heading_ + pirouette_iterator_ * pirouette_step_size;
+ 3338 :
+ 3339 : // enable the callbacks for the active tracker
+ 3340 : {
+ 3341 0 : std::scoped_lock lock(mutex_tracker_list_);
+ 3342 :
+ 3343 0 : std_srvs::SetBoolRequest req_enable_callbacks;
+ 3344 0 : req_enable_callbacks.data = true;
+ 3345 :
+ 3346 0 : tracker_list_.at(active_tracker_idx_)
+ 3347 : ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 3348 0 :
+ 3349 : callbacks_enabled_ = true;
+ 3350 : }
+ 3351 0 :
+ 3352 : setReference(reference_request);
+ 3353 :
+ 3354 0 : {
+ 3355 : std::scoped_lock lock(mutex_tracker_list_);
+ 3356 :
+ 3357 0 : // disable the callbacks for the active tracker
+ 3358 0 : std_srvs::SetBoolRequest req_enable_callbacks;
+ 3359 : req_enable_callbacks.data = false;
+ 3360 0 :
+ 3361 : tracker_list_.at(active_tracker_idx_)
+ 3362 0 : ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 3363 :
+ 3364 : callbacks_enabled_ = false;
+ 3365 : }
+ 3366 : }
+ 3367 :
+ 3368 : //}
+ 3369 :
+ 3370 : // --------------------------------------------------------------
+ 3371 : // | asyncs |
+ 3372 : // --------------------------------------------------------------
+ 3373 :
+ 3374 130332 : /* asyncControl() //{ */
+ 3375 :
+ 3376 130332 : void ControlManager::asyncControl(void) {
+ 3377 0 :
+ 3378 : if (!is_initialized_)
+ 3379 260664 : return;
+ 3380 :
+ 3381 390996 : mrs_lib::AtomicScopeFlag unset_running(running_async_control_);
+ 3382 390996 :
+ 3383 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("asyncControl");
+ 3384 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::asyncControl", scope_timer_logger_, scope_timer_enabled_);
+ 3385 260664 :
+ 3386 130332 : // copy member variables
+ 3387 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 3388 130332 : auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+ 3389 :
+ 3390 : if (!failsafe_triggered_) { // when failsafe is triggered, updateControllers() and publish() is called in timerFailsafe()
+ 3391 :
+ 3392 120889 : // run the safety timer
+ 3393 : // in the case of large control errors, the safety mechanisms will be triggered before the controllers and trackers are updated...
+ 3394 1011 : while (running_safety_timer_) {
+ 3395 1011 :
+ 3396 1011 : ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+ 3397 : ros::Duration wait(0.001);
+ 3398 1011 : wait.sleep();
+ 3399 1004 :
+ 3400 1004 : if (!running_safety_timer_) {
+ 3401 : ROS_DEBUG("[ControlManager]: safety timer finished");
+ 3402 : break;
+ 3403 : }
+ 3404 120882 : }
+ 3405 120882 :
+ 3406 : ros::TimerEvent safety_timer_event;
+ 3407 120882 : timerSafety(safety_timer_event);
+ 3408 :
+ 3409 120882 : updateTrackers();
+ 3410 :
+ 3411 120882 : updateControllers(uav_state);
+ 3412 :
+ 3413 : if (got_constraints_) {
+ 3414 120259 :
+ 3415 : // update the constraints to trackers, if need to
+ 3416 120259 : auto enforce = enforceControllersConstraints(current_constraints);
+ 3417 :
+ 3418 65 : if (enforce && !constraints_being_enforced_) {
+ 3419 65 :
+ 3420 : setConstraintsToTrackers(enforce.value());
+ 3421 65 : mrs_lib::set_mutexed(mutex_constraints_, enforce.value(), sanitized_constraints_);
+ 3422 :
+ 3423 65 : constraints_being_enforced_ = true;
+ 3424 :
+ 3425 65 : auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+ 3426 :
+ 3427 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' is enforcing constraints over the ConstraintManager",
+ 3428 120194 : _controller_names_.at(active_controller_idx).c_str());
+ 3429 :
+ 3430 55 : } else if (!enforce && constraints_being_enforced_) {
+ 3431 :
+ 3432 55 : ROS_INFO_THROTTLE(1.0, "[ControlManager]: constraint values returned to original values");
+ 3433 :
+ 3434 55 : constraints_being_enforced_ = false;
+ 3435 :
+ 3436 55 : mrs_lib::set_mutexed(mutex_constraints_, current_constraints, sanitized_constraints_);
+ 3437 :
+ 3438 : setConstraintsToTrackers(current_constraints);
+ 3439 : }
+ 3440 120882 : }
+ 3441 :
+ 3442 : publish();
+ 3443 : }
+ 3444 130332 :
+ 3445 : // if odometry switch happened, we finish it here and turn the safety timer back on
+ 3446 5 : if (odometry_switch_in_progress_) {
+ 3447 5 :
+ 3448 5 : ROS_DEBUG("[ControlManager]: starting safety timer");
+ 3449 5 : timer_safety_.start();
+ 3450 : ROS_DEBUG("[ControlManager]: safety timer started");
+ 3451 : odometry_switch_in_progress_ = false;
+ 3452 10 :
+ 3453 : {
+ 3454 5 : std::scoped_lock lock(mutex_uav_state_);
+ 3455 :
+ 3456 : ROS_INFO("[ControlManager]: odometry after switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state.pose.position.x, uav_state.pose.position.y,
+ 3457 : uav_state.pose.position.z, uav_heading_);
+ 3458 : }
+ 3459 : }
+ 3460 : }
+ 3461 :
+ 3462 : //}
+ 3463 :
+ 3464 : // --------------------------------------------------------------
+ 3465 : // | callbacks |
+ 3466 : // --------------------------------------------------------------
+ 3467 :
+ 3468 : // | --------------------- topic callbacks -------------------- |
+ 3469 :
+ 3470 0 : /* //{ callbackOdometry() */
+ 3471 :
+ 3472 0 : void ControlManager::callbackOdometry(const nav_msgs::Odometry::ConstPtr msg) {
+ 3473 0 :
+ 3474 : if (!is_initialized_)
+ 3475 0 : return;
+ 3476 0 :
+ 3477 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackOdometry");
+ 3478 0 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackOdometry", scope_timer_logger_, scope_timer_enabled_);
+ 3479 :
+ 3480 : nav_msgs::OdometryConstPtr odom = msg;
+ 3481 :
+ 3482 : // | ------------------ check for time stamp ------------------ |
+ 3483 0 :
+ 3484 : {
+ 3485 0 : std::scoped_lock lock(mutex_uav_state_);
+ 3486 0 :
+ 3487 : if (uav_state_.header.stamp == odom->header.stamp) {
+ 3488 : return;
+ 3489 : }
+ 3490 : }
+ 3491 :
+ 3492 0 : // | --------------------- check for nans --------------------- |
+ 3493 0 :
+ 3494 0 : if (!validateOdometry(*odom, "ControlManager", "odometry")) {
+ 3495 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'odometry' contains invalid values, throwing it away");
+ 3496 : return;
+ 3497 : }
+ 3498 :
+ 3499 : // | ---------------------- frame switch ---------------------- |
+ 3500 :
+ 3501 : /* Odometry frame switch //{ */
+ 3502 :
+ 3503 0 : // | -- prepare an OdometryConstPtr for trackers & controllers -- |
+ 3504 :
+ 3505 0 : mrs_msgs::UavState uav_state_odom;
+ 3506 0 :
+ 3507 0 : uav_state_odom.header = odom->header;
+ 3508 : uav_state_odom.pose = odom->pose.pose;
+ 3509 : uav_state_odom.velocity = odom->twist.twist;
+ 3510 :
+ 3511 0 : // | ----- check for change in odometry frame of reference ---- |
+ 3512 :
+ 3513 0 : if (got_uav_state_) {
+ 3514 :
+ 3515 0 : if (odom->header.frame_id != uav_state_.header.frame_id) {
+ 3516 :
+ 3517 0 : ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+ 3518 : {
+ 3519 0 : std::scoped_lock lock(mutex_uav_state_);
+ 3520 :
+ 3521 : ROS_INFO("[ControlManager]: odometry before switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state_.pose.position.x, uav_state_.pose.position.y,
+ 3522 : uav_state_.pose.position.z, uav_heading_);
+ 3523 0 : }
+ 3524 :
+ 3525 : odometry_switch_in_progress_ = true;
+ 3526 0 :
+ 3527 0 : // we have to stop safety timer, otherwise it will interfere
+ 3528 0 : ROS_DEBUG("[ControlManager]: stopping the safety timer");
+ 3529 : timer_safety_.stop();
+ 3530 : ROS_DEBUG("[ControlManager]: safety timer stopped");
+ 3531 0 :
+ 3532 : // wait for the safety timer to stop if its running
+ 3533 0 : while (running_safety_timer_) {
+ 3534 0 :
+ 3535 0 : ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+ 3536 : ros::Duration wait(0.001);
+ 3537 0 : wait.sleep();
+ 3538 0 :
+ 3539 0 : if (!running_safety_timer_) {
+ 3540 : ROS_DEBUG("[ControlManager]: safety timer finished");
+ 3541 : break;
+ 3542 : }
+ 3543 : }
+ 3544 0 :
+ 3545 : // we have to also for the oneshot control timer to finish
+ 3546 0 : while (running_async_control_) {
+ 3547 0 :
+ 3548 0 : ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+ 3549 : ros::Duration wait(0.001);
+ 3550 0 : wait.sleep();
+ 3551 0 :
+ 3552 0 : if (!running_async_control_) {
+ 3553 : ROS_DEBUG("[ControlManager]: control timer finished");
+ 3554 : break;
+ 3555 : }
+ 3556 : }
+ 3557 0 :
+ 3558 : {
+ 3559 0 : std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+ 3560 0 :
+ 3561 : tracker_list_.at(active_tracker_idx_)->switchOdometrySource(uav_state_odom);
+ 3562 : controller_list_.at(active_controller_idx_)->switchOdometrySource(uav_state_odom);
+ 3563 : }
+ 3564 : }
+ 3565 : }
+ 3566 :
+ 3567 : //}
+ 3568 :
+ 3569 : // | ----------- copy the odometry to the uav_state ----------- |
+ 3570 0 :
+ 3571 : {
+ 3572 0 : std::scoped_lock lock(mutex_uav_state_);
+ 3573 :
+ 3574 0 : previous_uav_state_ = uav_state_;
+ 3575 :
+ 3576 0 : uav_state_ = mrs_msgs::UavState();
+ 3577 0 :
+ 3578 0 : uav_state_.header = odom->header;
+ 3579 : uav_state_.pose = odom->pose.pose;
+ 3580 : uav_state_.velocity.angular = odom->twist.twist.angular;
+ 3581 :
+ 3582 : // transform the twist into the header's frame
+ 3583 0 : {
+ 3584 0 : // the velocity from the odometry
+ 3585 0 : geometry_msgs::Vector3Stamped speed_child_frame;
+ 3586 0 : speed_child_frame.header.frame_id = odom->child_frame_id;
+ 3587 0 : speed_child_frame.header.stamp = odom->header.stamp;
+ 3588 0 : speed_child_frame.vector.x = odom->twist.twist.linear.x;
+ 3589 : speed_child_frame.vector.y = odom->twist.twist.linear.y;
+ 3590 0 : speed_child_frame.vector.z = odom->twist.twist.linear.z;
+ 3591 :
+ 3592 0 : auto res = transformer_->transformSingle(speed_child_frame, odom->header.frame_id);
+ 3593 0 :
+ 3594 0 : if (res) {
+ 3595 0 : uav_state_.velocity.linear.x = res.value().vector.x;
+ 3596 : uav_state_.velocity.linear.y = res.value().vector.y;
+ 3597 0 : uav_state_.velocity.linear.z = res.value().vector.z;
+ 3598 : } else {
+ 3599 0 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the odometry speed from '%s' to '%s'", odom->child_frame_id.c_str(),
+ 3600 : odom->header.frame_id.c_str());
+ 3601 : return;
+ 3602 : }
+ 3603 : }
+ 3604 0 :
+ 3605 : // calculate the euler angles
+ 3606 : std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(odom->pose.pose.orientation);
+ 3607 0 :
+ 3608 : try {
+ 3609 0 : uav_heading_ = mrs_lib::AttitudeConverter(odom->pose.pose.orientation).getHeading();
+ 3610 0 : }
+ 3611 : catch (...) {
+ 3612 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading");
+ 3613 0 : }
+ 3614 :
+ 3615 0 : transformer_->setDefaultFrame(odom->header.frame_id);
+ 3616 :
+ 3617 : got_uav_state_ = true;
+ 3618 : }
+ 3619 :
+ 3620 0 : // run the control loop asynchronously in an OneShotTimer
+ 3621 : // but only if its not already running
+ 3622 0 : if (!running_async_control_) {
+ 3623 :
+ 3624 0 : running_async_control_ = true;
+ 3625 :
+ 3626 : async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+ 3627 : }
+ 3628 : }
+ 3629 :
+ 3630 : //}
+ 3631 :
+ 3632 152208 : /* //{ callbackUavState() */
+ 3633 :
+ 3634 152208 : void ControlManager::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) {
+ 3635 20159 :
+ 3636 : if (!is_initialized_)
+ 3637 304416 : return;
+ 3638 304416 :
+ 3639 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackUavState");
+ 3640 152208 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackUavState", scope_timer_logger_, scope_timer_enabled_);
+ 3641 :
+ 3642 : mrs_msgs::UavStateConstPtr uav_state = msg;
+ 3643 :
+ 3644 : // | ------------------ check for time stamp ------------------ |
+ 3645 152208 :
+ 3646 : {
+ 3647 152208 : std::scoped_lock lock(mutex_uav_state_);
+ 3648 20159 :
+ 3649 : if (uav_state_.header.stamp == uav_state->header.stamp) {
+ 3650 : return;
+ 3651 : }
+ 3652 : }
+ 3653 :
+ 3654 132049 : // | --------------------- check for nans --------------------- |
+ 3655 0 :
+ 3656 0 : if (!validateUavState(*uav_state, "ControlManager", "uav_state")) {
+ 3657 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: incoming 'uav_state' contains invalid values, throwing it away");
+ 3658 : return;
+ 3659 : }
+ 3660 :
+ 3661 : // | -------------------- check for hiccups ------------------- |
+ 3662 :
+ 3663 132049 : /* hickup detection //{ */
+ 3664 132049 :
+ 3665 132049 : double alpha = 0.99;
+ 3666 : double alpha2 = 0.666;
+ 3667 132049 : double uav_state_count_lim = 1000;
+ 3668 :
+ 3669 : double uav_state_dt = (ros::Time::now() - previous_uav_state_.header.stamp).toSec();
+ 3670 132049 :
+ 3671 : // belive only reasonable numbers
+ 3672 131867 : if (uav_state_dt <= 1.0) {
+ 3673 :
+ 3674 131867 : uav_state_avg_dt_ = alpha * uav_state_avg_dt_ + (1 - alpha) * uav_state_dt;
+ 3675 69981 :
+ 3676 : if (uav_state_count_ < uav_state_count_lim) {
+ 3677 : uav_state_count_++;
+ 3678 : }
+ 3679 132049 : }
+ 3680 :
+ 3681 : if (uav_state_count_ == uav_state_count_lim) {
+ 3682 :
+ 3683 61942 : /* ROS_INFO_STREAM("[ControlManager]: uav_state_dt = " << uav_state_dt); */
+ 3684 :
+ 3685 24704 : if (uav_state_dt < uav_state_avg_dt_ && uav_state_dt > 0.0001) {
+ 3686 :
+ 3687 37238 : uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_avg_dt_ / uav_state_dt);
+ 3688 :
+ 3689 37238 : } else if (uav_state_avg_dt_ > 0.0001) {
+ 3690 :
+ 3691 : uav_state_hiccup_factor_ = alpha2 * uav_state_hiccup_factor_ + (1 - alpha2) * (uav_state_dt / uav_state_avg_dt_);
+ 3692 61942 : }
+ 3693 :
+ 3694 : if (uav_state_hiccup_factor_ > 3.141592653) {
+ 3695 :
+ 3696 0 : /* ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: hiccup factor = " << uav_state_hiccup_factor_); */
+ 3697 0 :
+ 3698 0 : ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+ 3699 0 : ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+ 3700 0 : ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | |");
+ 3701 0 : ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | UAV_STATE has a large hiccup factor! |");
+ 3702 0 : ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | hint, hint: you are probably rosbagging |");
+ 3703 0 : ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | lot of data or publishing lot of large |");
+ 3704 0 : ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | messages without mutual nodelet managers. |");
+ 3705 0 : ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | |");
+ 3706 : ROS_WARN_THROTTLE(2.0, "[ControlManager]: // | ------------------------- WARNING ------------------------ |");
+ 3707 : ROS_WARN_THROTTLE(2.0, "[ControlManager]: ");
+ 3708 : }
+ 3709 : }
+ 3710 :
+ 3711 : //}
+ 3712 :
+ 3713 : // | ---------------------- frame switch ---------------------- |
+ 3714 :
+ 3715 : /* frame switch //{ */
+ 3716 :
+ 3717 132049 : // | ----- check for change in odometry frame of reference ---- |
+ 3718 :
+ 3719 131958 : if (got_uav_state_) {
+ 3720 :
+ 3721 5 : if (uav_state->estimator_iteration != uav_state_.estimator_iteration) {
+ 3722 :
+ 3723 10 : ROS_INFO("[ControlManager]: detecting switch of odometry frame");
+ 3724 : {
+ 3725 5 : std::scoped_lock lock(mutex_uav_state_);
+ 3726 :
+ 3727 : ROS_INFO("[ControlManager]: odometry before switch: x=%.2f, y=%.2f, z=%.2f, heading=%.2f", uav_state_.pose.position.x, uav_state_.pose.position.y,
+ 3728 : uav_state_.pose.position.z, uav_heading_);
+ 3729 5 : }
+ 3730 :
+ 3731 : odometry_switch_in_progress_ = true;
+ 3732 5 :
+ 3733 5 : // we have to stop safety timer, otherwise it will interfere
+ 3734 5 : ROS_DEBUG("[ControlManager]: stopping the safety timer");
+ 3735 : timer_safety_.stop();
+ 3736 : ROS_DEBUG("[ControlManager]: safety timer stopped");
+ 3737 5 :
+ 3738 : // wait for the safety timer to stop if its running
+ 3739 0 : while (running_safety_timer_) {
+ 3740 0 :
+ 3741 0 : ROS_DEBUG("[ControlManager]: waiting for safety timer to finish");
+ 3742 : ros::Duration wait(0.001);
+ 3743 0 : wait.sleep();
+ 3744 0 :
+ 3745 0 : if (!running_safety_timer_) {
+ 3746 : ROS_DEBUG("[ControlManager]: safety timer finished");
+ 3747 : break;
+ 3748 : }
+ 3749 : }
+ 3750 5 :
+ 3751 : // we have to also for the oneshot control timer to finish
+ 3752 0 : while (running_async_control_) {
+ 3753 0 :
+ 3754 0 : ROS_DEBUG("[ControlManager]: waiting for control timer to finish");
+ 3755 : ros::Duration wait(0.001);
+ 3756 0 : wait.sleep();
+ 3757 0 :
+ 3758 0 : if (!running_async_control_) {
+ 3759 : ROS_DEBUG("[ControlManager]: control timer finished");
+ 3760 : break;
+ 3761 : }
+ 3762 : }
+ 3763 10 :
+ 3764 : {
+ 3765 5 : std::scoped_lock lock(mutex_controller_list_, mutex_tracker_list_);
+ 3766 5 :
+ 3767 : tracker_list_.at(active_tracker_idx_)->switchOdometrySource(*uav_state);
+ 3768 : controller_list_.at(active_controller_idx_)->switchOdometrySource(*uav_state);
+ 3769 : }
+ 3770 : }
+ 3771 : }
+ 3772 :
+ 3773 : //}
+ 3774 :
+ 3775 : // --------------------------------------------------------------
+ 3776 : // | copy the UavState message for later use |
+ 3777 : // --------------------------------------------------------------
+ 3778 132049 :
+ 3779 : {
+ 3780 132049 : std::scoped_lock lock(mutex_uav_state_);
+ 3781 :
+ 3782 132049 : previous_uav_state_ = uav_state_;
+ 3783 :
+ 3784 132049 : uav_state_ = *uav_state;
+ 3785 :
+ 3786 : std::tie(uav_roll_, uav_pitch_, uav_yaw_) = mrs_lib::AttitudeConverter(uav_state_.pose.orientation);
+ 3787 132049 :
+ 3788 : try {
+ 3789 0 : uav_heading_ = mrs_lib::AttitudeConverter(uav_state_.pose.orientation).getHeading();
+ 3790 0 : }
+ 3791 : catch (...) {
+ 3792 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not calculate UAV heading, not updating it");
+ 3793 132049 : }
+ 3794 :
+ 3795 132049 : transformer_->setDefaultFrame(uav_state->header.frame_id);
+ 3796 :
+ 3797 : got_uav_state_ = true;
+ 3798 : }
+ 3799 :
+ 3800 132049 : // run the control loop asynchronously in an OneShotTimer
+ 3801 : // but only if its not already running
+ 3802 130332 : if (!running_async_control_) {
+ 3803 :
+ 3804 130332 : running_async_control_ = true;
+ 3805 :
+ 3806 : async_control_result_ = std::async(std::launch::async, &ControlManager::asyncControl, this);
+ 3807 : }
+ 3808 : }
+ 3809 :
+ 3810 : //}
+ 3811 :
+ 3812 67849 : /* //{ callbackGNSS() */
+ 3813 :
+ 3814 67849 : void ControlManager::callbackGNSS(const sensor_msgs::NavSatFix::ConstPtr msg) {
+ 3815 75 :
+ 3816 : if (!is_initialized_)
+ 3817 203322 : return;
+ 3818 203322 :
+ 3819 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackGNSS");
+ 3820 67774 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackGNSS", scope_timer_logger_, scope_timer_enabled_);
+ 3821 :
+ 3822 : transformer_->setLatLon(msg->latitude, msg->longitude);
+ 3823 : }
+ 3824 :
+ 3825 : //}
+ 3826 :
+ 3827 0 : /* callbackJoystick() //{ */
+ 3828 :
+ 3829 0 : void ControlManager::callbackJoystick(const sensor_msgs::Joy::ConstPtr msg) {
+ 3830 0 :
+ 3831 : if (!is_initialized_)
+ 3832 0 : return;
+ 3833 0 :
+ 3834 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackJoystick");
+ 3835 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackJoystick", scope_timer_logger_, scope_timer_enabled_);
+ 3836 0 :
+ 3837 0 : // copy member variables
+ 3838 : auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+ 3839 0 : auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+ 3840 :
+ 3841 : sensor_msgs::JoyConstPtr joystick_data = msg;
+ 3842 0 :
+ 3843 0 : // TODO check if the array is smaller than the largest idx
+ 3844 : if (joystick_data->buttons.size() == 0 || joystick_data->axes.size() == 0) {
+ 3845 : return;
+ 3846 : }
+ 3847 :
+ 3848 : // | ---- switching back to fallback tracker and controller --- |
+ 3849 0 :
+ 3850 0 : // if any of the A, B, X, Y buttons are pressed when flying with joystick, switch back to fallback controller and tracker
+ 3851 0 : if ((joystick_data->buttons.at(_channel_A_) == 1 || joystick_data->buttons.at(_channel_B_) == 1 || joystick_data->buttons.at(_channel_X_) == 1 ||
+ 3852 : joystick_data->buttons.at(_channel_Y_) == 1) &&
+ 3853 0 : active_tracker_idx == _joystick_tracker_idx_ && active_controller_idx == _joystick_controller_idx_) {
+ 3854 :
+ 3855 0 : ROS_INFO("[ControlManager]: switching from joystick to normal control");
+ 3856 0 :
+ 3857 : switchTracker(_joystick_fallback_tracker_name_);
+ 3858 0 : switchController(_joystick_fallback_controller_name_);
+ 3859 :
+ 3860 : joystick_goto_enabled_ = false;
+ 3861 : }
+ 3862 :
+ 3863 : // | ------- joystick control activation ------- |
+ 3864 0 :
+ 3865 : // if start button was pressed
+ 3866 0 : if (joystick_data->buttons.at(_channel_start_) == 1) {
+ 3867 :
+ 3868 0 : if (!joystick_start_pressed_) {
+ 3869 :
+ 3870 0 : ROS_INFO("[ControlManager]: joystick start button pressed");
+ 3871 0 :
+ 3872 : joystick_start_pressed_ = true;
+ 3873 : joystick_start_press_time_ = ros::Time::now();
+ 3874 0 : }
+ 3875 :
+ 3876 0 : } else if (joystick_start_pressed_) {
+ 3877 :
+ 3878 0 : ROS_INFO("[ControlManager]: joystick start button released");
+ 3879 0 :
+ 3880 : joystick_start_pressed_ = false;
+ 3881 : joystick_start_press_time_ = ros::Time(0);
+ 3882 : }
+ 3883 :
+ 3884 : // | ---------------- Joystick goto activation ---------------- |
+ 3885 0 :
+ 3886 : // if back button was pressed
+ 3887 0 : if (joystick_data->buttons.at(_channel_back_) == 1) {
+ 3888 :
+ 3889 0 : if (!joystick_back_pressed_) {
+ 3890 :
+ 3891 0 : ROS_INFO("[ControlManager]: joystick back button pressed");
+ 3892 0 :
+ 3893 : joystick_back_pressed_ = true;
+ 3894 : joystick_back_press_time_ = ros::Time::now();
+ 3895 0 : }
+ 3896 :
+ 3897 0 : } else if (joystick_back_pressed_) {
+ 3898 :
+ 3899 0 : ROS_INFO("[ControlManager]: joystick back button released");
+ 3900 0 :
+ 3901 : joystick_back_pressed_ = false;
+ 3902 : joystick_back_press_time_ = ros::Time(0);
+ 3903 : }
+ 3904 :
+ 3905 : // | ------------------------ Failsafes ----------------------- |
+ 3906 0 :
+ 3907 : // if LT and RT buttons are both pressed down
+ 3908 0 : if (joystick_data->axes.at(_channel_LT_) < -0.99 && joystick_data->axes.at(_channel_RT_) < -0.99) {
+ 3909 :
+ 3910 0 : if (!joystick_failsafe_pressed_) {
+ 3911 :
+ 3912 0 : ROS_INFO("[ControlManager]: joystick Failsafe pressed");
+ 3913 0 :
+ 3914 : joystick_failsafe_pressed_ = true;
+ 3915 : joystick_failsafe_press_time_ = ros::Time::now();
+ 3916 0 : }
+ 3917 :
+ 3918 0 : } else if (joystick_failsafe_pressed_) {
+ 3919 :
+ 3920 0 : ROS_INFO("[ControlManager]: joystick Failsafe released");
+ 3921 0 :
+ 3922 : joystick_failsafe_pressed_ = false;
+ 3923 : joystick_failsafe_press_time_ = ros::Time(0);
+ 3924 : }
+ 3925 0 :
+ 3926 : // if left and right joypads are both pressed down
+ 3927 0 : if (joystick_data->buttons.at(_channel_L_joy_) == 1 && joystick_data->buttons.at(_channel_R_joy_) == 1) {
+ 3928 :
+ 3929 0 : if (!joystick_eland_pressed_) {
+ 3930 :
+ 3931 0 : ROS_INFO("[ControlManager]: joystick eland pressed");
+ 3932 0 :
+ 3933 : joystick_eland_pressed_ = true;
+ 3934 : joystick_eland_press_time_ = ros::Time::now();
+ 3935 0 : }
+ 3936 :
+ 3937 0 : } else if (joystick_eland_pressed_) {
+ 3938 :
+ 3939 0 : ROS_INFO("[ControlManager]: joystick eland released");
+ 3940 0 :
+ 3941 : joystick_eland_pressed_ = false;
+ 3942 : joystick_eland_press_time_ = ros::Time(0);
+ 3943 : }
+ 3944 : }
+ 3945 :
+ 3946 : //}
+ 3947 :
+ 3948 389287 : /* //{ callbackHwApiStatus() */
+ 3949 :
+ 3950 389287 : void ControlManager::callbackHwApiStatus(const mrs_msgs::HwApiStatus::ConstPtr msg) {
+ 3951 369 :
+ 3952 : if (!is_initialized_)
+ 3953 1166754 : return;
+ 3954 1166754 :
+ 3955 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackHwApiStatus");
+ 3956 777836 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackHwApiStatus", scope_timer_logger_, scope_timer_enabled_);
+ 3957 :
+ 3958 : mrs_msgs::HwApiStatusConstPtr state = msg;
+ 3959 388918 :
+ 3960 : // | ------ detect and print the changes in offboard mode ----- |
+ 3961 271502 : if (state->offboard) {
+ 3962 85 :
+ 3963 85 : if (!offboard_mode_) {
+ 3964 85 : offboard_mode_ = true;
+ 3965 : offboard_mode_was_true_ = true;
+ 3966 : ROS_INFO("[ControlManager]: detected: OFFBOARD mode ON");
+ 3967 : }
+ 3968 :
+ 3969 117416 : } else {
+ 3970 0 :
+ 3971 0 : if (offboard_mode_) {
+ 3972 : offboard_mode_ = false;
+ 3973 : ROS_INFO("[ControlManager]: detected: OFFBOARD mode OFF");
+ 3974 : }
+ 3975 : }
+ 3976 388918 :
+ 3977 : // | --------- detect and print the changes in arming --------- |
+ 3978 284318 : if (state->armed == true) {
+ 3979 90 :
+ 3980 90 : if (!armed_) {
+ 3981 : armed_ = true;
+ 3982 : ROS_INFO("[ControlManager]: detected: vehicle ARMED");
+ 3983 : }
+ 3984 :
+ 3985 104600 : } else {
+ 3986 20 :
+ 3987 20 : if (armed_) {
+ 3988 : armed_ = false;
+ 3989 : ROS_INFO("[ControlManager]: detected: vehicle DISARMED");
+ 3990 : }
+ 3991 : }
+ 3992 : }
+ 3993 :
+ 3994 : //}
+ 3995 :
+ 3996 21330 : /* //{ callbackRC() */
+ 3997 :
+ 3998 21330 : void ControlManager::callbackRC(const mrs_msgs::HwApiRcChannels::ConstPtr msg) {
+ 3999 0 :
+ 4000 : if (!is_initialized_)
+ 4001 63990 : return;
+ 4002 63990 :
+ 4003 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackRC");
+ 4004 42660 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackRC", scope_timer_logger_, scope_timer_enabled_);
+ 4005 :
+ 4006 21330 : mrs_msgs::HwApiRcChannelsConstPtr rc = msg;
+ 4007 :
+ 4008 : ROS_INFO_ONCE("[ControlManager]: getting RC channels");
+ 4009 :
+ 4010 : // | ------------------- rc joystic control ------------------- |
+ 4011 21330 :
+ 4012 : // when the switch change its position
+ 4013 21330 : if (_rc_goto_enabled_) {
+ 4014 :
+ 4015 0 : if (_rc_joystick_channel_ >= int(rc->channels.size())) {
+ 4016 :
+ 4017 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC joystick activation channel number (%d) is out of range [0-%d]", _rc_joystick_channel_,
+ 4018 : int(rc->channels.size()));
+ 4019 :
+ 4020 21330 : } else {
+ 4021 21330 :
+ 4022 : bool channel_low = rc->channels.at(_rc_joystick_channel_) < (0.5 - RC_DEADBAND) ? true : false;
+ 4023 21330 : bool channel_high = rc->channels.at(_rc_joystick_channel_) > (0.5 + RC_DEADBAND) ? true : false;
+ 4024 19545 :
+ 4025 : if (channel_low) {
+ 4026 : rc_joystick_channel_was_low_ = true;
+ 4027 : }
+ 4028 21330 :
+ 4029 : // rc control activation
+ 4030 19546 : if (!rc_goto_active_) {
+ 4031 :
+ 4032 2 : if (rc_joystick_channel_last_value_ < (0.5 - RC_DEADBAND) && channel_high) {
+ 4033 :
+ 4034 2 : if (isFlyingNormally()) {
+ 4035 :
+ 4036 2 : ROS_INFO_THROTTLE(1.0, "[ControlManager]: activating RC joystick");
+ 4037 :
+ 4038 2 : callbacks_enabled_ = false;
+ 4039 2 :
+ 4040 : std_srvs::SetBoolRequest req_goto_out;
+ 4041 2 : req_goto_out.data = false;
+ 4042 2 :
+ 4043 : std_srvs::SetBoolRequest req_enable_callbacks;
+ 4044 : req_enable_callbacks.data = callbacks_enabled_;
+ 4045 4 :
+ 4046 : {
+ 4047 : std::scoped_lock lock(mutex_tracker_list_);
+ 4048 14 :
+ 4049 12 : // disable callbacks of all trackers
+ 4050 : for (int i = 0; i < int(tracker_list_.size()); i++) {
+ 4051 : tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 4052 : }
+ 4053 2 : }
+ 4054 :
+ 4055 : rc_goto_active_ = true;
+ 4056 :
+ 4057 0 : } else {
+ 4058 2 :
+ 4059 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, not flying normally");
+ 4060 19544 : }
+ 4061 :
+ 4062 0 : } else if (channel_high && !rc_joystick_channel_was_low_) {
+ 4063 :
+ 4064 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: can not activate RC joystick, the switch is ON from the beginning");
+ 4065 : }
+ 4066 : }
+ 4067 21330 :
+ 4068 : // rc control deactivation
+ 4069 1 : if (rc_goto_active_ && channel_low) {
+ 4070 :
+ 4071 1 : ROS_INFO("[ControlManager]: deactivating RC joystick");
+ 4072 :
+ 4073 1 : callbacks_enabled_ = true;
+ 4074 1 :
+ 4075 : std_srvs::SetBoolRequest req_goto_out;
+ 4076 1 : req_goto_out.data = true;
+ 4077 1 :
+ 4078 : std_srvs::SetBoolRequest req_enable_callbacks;
+ 4079 : req_enable_callbacks.data = callbacks_enabled_;
+ 4080 2 :
+ 4081 : {
+ 4082 : std::scoped_lock lock(mutex_tracker_list_);
+ 4083 7 :
+ 4084 6 : // enable callbacks of all trackers
+ 4085 : for (int i = 0; i < int(tracker_list_.size()); i++) {
+ 4086 : tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 4087 : }
+ 4088 1 : }
+ 4089 :
+ 4090 : rc_goto_active_ = false;
+ 4091 : }
+ 4092 :
+ 4093 21330 : // do not forget to update the last... variable
+ 4094 21330 : // only do that if its out of the deadband
+ 4095 : if (channel_high || channel_low) {
+ 4096 : rc_joystick_channel_last_value_ = rc->channels.at(_rc_joystick_channel_);
+ 4097 : }
+ 4098 : }
+ 4099 : }
+ 4100 21330 :
+ 4101 : // | ------------------------ rc eland ------------------------ |
+ 4102 21330 : if (_rc_escalating_failsafe_enabled_) {
+ 4103 :
+ 4104 0 : if (_rc_escalating_failsafe_channel_ >= int(rc->channels.size())) {
+ 4105 :
+ 4106 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: RC eland channel number (%d) is out of range [0-%d]", _rc_escalating_failsafe_channel_,
+ 4107 : int(rc->channels.size()));
+ 4108 :
+ 4109 21330 : } else {
+ 4110 :
+ 4111 143 : if (rc->channels.at(_rc_escalating_failsafe_channel_) >= _rc_escalating_failsafe_threshold_) {
+ 4112 :
+ 4113 286 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: triggering escalating failsafe by RC");
+ 4114 :
+ 4115 143 : auto [success, message] = escalatingFailsafe();
+ 4116 3 :
+ 4117 : if (success) {
+ 4118 : rc_escalating_failsafe_triggered_ = true;
+ 4119 : }
+ 4120 : }
+ 4121 : }
+ 4122 : }
+ 4123 : }
+ 4124 :
+ 4125 : //}
+ 4126 :
+ 4127 : // | --------------------- topic timeouts --------------------- |
+ 4128 :
+ 4129 0 : /* timeoutUavState() //{ */
+ 4130 :
+ 4131 0 : void ControlManager::timeoutUavState(const double& missing_for) {
+ 4132 :
+ 4133 0 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 4134 :
+ 4135 : if (output_enabled_ && last_control_output.control_output && !failsafe_triggered_) {
+ 4136 :
+ 4137 : // We need to fire up timerFailsafe, which will regularly trigger the controllers
+ 4138 0 : // in place of the callbackUavState/callbackOdometry().
+ 4139 :
+ 4140 0 : ROS_ERROR_THROTTLE(0.1, "[ControlManager]: not receiving uav_state/odometry for %.3f s, initiating failsafe land", missing_for);
+ 4141 :
+ 4142 0 : failsafe();
+ 4143 : }
+ 4144 : }
+ 4145 :
+ 4146 : //}
+ 4147 :
+ 4148 : // | -------------------- service callbacks ------------------- |
+ 4149 :
+ 4150 178 : /* //{ callbackSwitchTracker() */
+ 4151 :
+ 4152 178 : bool ControlManager::callbackSwitchTracker(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+ 4153 0 :
+ 4154 : if (!is_initialized_)
+ 4155 178 : return false;
+ 4156 :
+ 4157 0 : if (failsafe_triggered_ || eland_triggered_) {
+ 4158 0 :
+ 4159 : std::stringstream ss;
+ 4160 0 : ss << "can not switch tracker, eland or failsafe active";
+ 4161 0 :
+ 4162 : res.message = ss.str();
+ 4163 0 : res.success = false;
+ 4164 :
+ 4165 0 : ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+ 4166 :
+ 4167 : return true;
+ 4168 178 : }
+ 4169 :
+ 4170 178 : auto [success, response] = switchTracker(req.value);
+ 4171 178 :
+ 4172 : res.success = success;
+ 4173 178 : res.message = response;
+ 4174 :
+ 4175 : return true;
+ 4176 : }
+ 4177 :
+ 4178 : //}
+ 4179 :
+ 4180 177 : /* callbackSwitchController() //{ */
+ 4181 :
+ 4182 177 : bool ControlManager::callbackSwitchController(mrs_msgs::String::Request& req, mrs_msgs::String::Response& res) {
+ 4183 0 :
+ 4184 : if (!is_initialized_)
+ 4185 177 : return false;
+ 4186 :
+ 4187 0 : if (failsafe_triggered_ || eland_triggered_) {
+ 4188 0 :
+ 4189 : std::stringstream ss;
+ 4190 0 : ss << "can not switch controller, eland or failsafe active";
+ 4191 0 :
+ 4192 : res.message = ss.str();
+ 4193 0 : res.success = false;
+ 4194 :
+ 4195 0 : ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+ 4196 :
+ 4197 : return true;
+ 4198 177 : }
+ 4199 :
+ 4200 177 : auto [success, response] = switchController(req.value);
+ 4201 177 :
+ 4202 : res.success = success;
+ 4203 177 : res.message = response;
+ 4204 :
+ 4205 : return true;
+ 4206 : }
+ 4207 :
+ 4208 : //}
+ 4209 :
+ 4210 0 : /* //{ callbackSwitchTracker() */
+ 4211 :
+ 4212 0 : bool ControlManager::callbackTrackerResetStatic([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4213 0 :
+ 4214 : if (!is_initialized_)
+ 4215 0 : return false;
+ 4216 :
+ 4217 0 : std::stringstream message;
+ 4218 :
+ 4219 0 : if (failsafe_triggered_ || eland_triggered_) {
+ 4220 :
+ 4221 0 : message << "can not reset tracker, eland or failsafe active";
+ 4222 0 :
+ 4223 : res.message = message.str();
+ 4224 0 : res.success = false;
+ 4225 :
+ 4226 0 : ROS_WARN_STREAM("[ControlManager]: " << message.str());
+ 4227 :
+ 4228 : return true;
+ 4229 : }
+ 4230 :
+ 4231 0 : // reactivate the current tracker
+ 4232 : {
+ 4233 0 : std::scoped_lock lock(mutex_tracker_list_);
+ 4234 :
+ 4235 0 : std::string tracker_name = _tracker_names_.at(active_tracker_idx_);
+ 4236 :
+ 4237 0 : bool succ = tracker_list_.at(active_tracker_idx_)->resetStatic();
+ 4238 0 :
+ 4239 0 : if (succ) {
+ 4240 : message << "the tracker '" << tracker_name << "' was reset";
+ 4241 0 : ROS_INFO_STREAM("[ControlManager]: " << message.str());
+ 4242 0 : } else {
+ 4243 : message << "the tracker '" << tracker_name << "' reset failed!";
+ 4244 : ROS_ERROR_STREAM("[ControlManager]: " << message.str());
+ 4245 : }
+ 4246 0 : }
+ 4247 0 :
+ 4248 : res.message = message.str();
+ 4249 0 : res.success = true;
+ 4250 :
+ 4251 : return true;
+ 4252 : }
+ 4253 :
+ 4254 : //}
+ 4255 :
+ 4256 2 : /* //{ callbackEHover() */
+ 4257 :
+ 4258 2 : bool ControlManager::callbackEHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4259 0 :
+ 4260 : if (!is_initialized_)
+ 4261 2 : return false;
+ 4262 :
+ 4263 0 : if (failsafe_triggered_ || eland_triggered_) {
+ 4264 0 :
+ 4265 : std::stringstream ss;
+ 4266 0 : ss << "can not switch controller, eland or failsafe active";
+ 4267 0 :
+ 4268 : res.message = ss.str();
+ 4269 0 : res.success = false;
+ 4270 :
+ 4271 0 : ROS_WARN_STREAM("[ControlManager]: " << ss.str());
+ 4272 :
+ 4273 : return true;
+ 4274 2 : }
+ 4275 :
+ 4276 2 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: ehover trigger by callback");
+ 4277 :
+ 4278 2 : auto [success, message] = ehover();
+ 4279 2 :
+ 4280 : res.success = success;
+ 4281 2 : res.message = message;
+ 4282 :
+ 4283 : return true;
+ 4284 : }
+ 4285 :
+ 4286 : //}
+ 4287 :
+ 4288 4 : /* callbackFailsafe() //{ */
+ 4289 :
+ 4290 4 : bool ControlManager::callbackFailsafe([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4291 0 :
+ 4292 : if (!is_initialized_)
+ 4293 4 : return false;
+ 4294 :
+ 4295 0 : if (failsafe_triggered_) {
+ 4296 0 :
+ 4297 : std::stringstream ss;
+ 4298 0 : ss << "can not activate failsafe, it is already active";
+ 4299 0 :
+ 4300 : res.message = ss.str();
+ 4301 0 : res.success = false;
+ 4302 :
+ 4303 0 : ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+ 4304 :
+ 4305 : return true;
+ 4306 4 : }
+ 4307 :
+ 4308 4 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: failsafe triggered by callback");
+ 4309 :
+ 4310 4 : auto [success, message] = failsafe();
+ 4311 4 :
+ 4312 : res.success = success;
+ 4313 4 : res.message = message;
+ 4314 :
+ 4315 : return true;
+ 4316 : }
+ 4317 :
+ 4318 : //}
+ 4319 :
+ 4320 7 : /* callbackFailsafeEscalating() //{ */
+ 4321 :
+ 4322 7 : bool ControlManager::callbackFailsafeEscalating([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4323 0 :
+ 4324 : if (!is_initialized_)
+ 4325 7 : return false;
+ 4326 :
+ 4327 7 : if (_service_escalating_failsafe_enabled_) {
+ 4328 :
+ 4329 14 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: escalating failsafe triggered by callback");
+ 4330 :
+ 4331 7 : auto [success, message] = escalatingFailsafe();
+ 4332 7 :
+ 4333 : res.success = success;
+ 4334 : res.message = message;
+ 4335 :
+ 4336 0 : } else {
+ 4337 0 :
+ 4338 : std::stringstream ss;
+ 4339 0 : ss << "escalating failsafe is disabled";
+ 4340 0 :
+ 4341 : res.success = false;
+ 4342 0 : res.message = ss.str();
+ 4343 :
+ 4344 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: %s", ss.str().c_str());
+ 4345 7 : }
+ 4346 :
+ 4347 : return true;
+ 4348 : }
+ 4349 :
+ 4350 : //}
+ 4351 :
+ 4352 2 : /* //{ callbackELand() */
+ 4353 :
+ 4354 2 : bool ControlManager::callbackEland([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4355 0 :
+ 4356 : if (!is_initialized_)
+ 4357 2 : return false;
+ 4358 :
+ 4359 2 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: eland triggered by callback");
+ 4360 :
+ 4361 2 : auto [success, message] = eland();
+ 4362 2 :
+ 4363 : res.success = success;
+ 4364 2 : res.message = message;
+ 4365 :
+ 4366 : return true;
+ 4367 : }
+ 4368 :
+ 4369 : //}
+ 4370 :
+ 4371 0 : /* //{ callbackParachute() */
+ 4372 :
+ 4373 0 : bool ControlManager::callbackParachute([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4374 0 :
+ 4375 : if (!is_initialized_)
+ 4376 0 : return false;
+ 4377 :
+ 4378 0 : if (!_parachute_enabled_) {
+ 4379 0 :
+ 4380 0 : std::stringstream ss;
+ 4381 0 : ss << "parachute disabled";
+ 4382 0 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 4383 : res.message = ss.str();
+ 4384 : res.success = false;
+ 4385 0 : }
+ 4386 :
+ 4387 0 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: parachute triggered by callback");
+ 4388 :
+ 4389 0 : auto [success, message] = deployParachute();
+ 4390 0 :
+ 4391 : res.success = success;
+ 4392 0 : res.message = message;
+ 4393 :
+ 4394 : return true;
+ 4395 : }
+ 4396 :
+ 4397 : //}
+ 4398 :
+ 4399 0 : /* //{ callbackSetMinZ() */
+ 4400 :
+ 4401 0 : bool ControlManager::callbackSetMinZ([[maybe_unused]] mrs_msgs::Float64StampedSrv::Request& req, mrs_msgs::Float64StampedSrv::Response& res) {
+ 4402 0 :
+ 4403 : if (!is_initialized_)
+ 4404 0 : return false;
+ 4405 0 :
+ 4406 0 : if (!use_safety_area_) {
+ 4407 0 : res.success = true;
+ 4408 : res.message = "safety area is disabled";
+ 4409 : return true;
+ 4410 : }
+ 4411 :
+ 4412 0 : // | -------- transform min_z to the safety area frame -------- |
+ 4413 0 :
+ 4414 0 : mrs_msgs::ReferenceStamped point;
+ 4415 : point.header = req.header;
+ 4416 0 : point.reference.position.z = req.value;
+ 4417 :
+ 4418 0 : auto result = transformer_->transformSingle(point, _safety_area_vertical_frame_);
+ 4419 :
+ 4420 0 : if (result) {
+ 4421 :
+ 4422 0 : _safety_area_min_z_ = result.value().reference.position.z;
+ 4423 0 :
+ 4424 : res.success = true;
+ 4425 : res.message = "safety area's min z updated";
+ 4426 :
+ 4427 0 : } else {
+ 4428 0 :
+ 4429 : res.success = false;
+ 4430 : res.message = "could not transform the value to safety area's vertical frame";
+ 4431 0 : }
+ 4432 :
+ 4433 : return true;
+ 4434 : }
+ 4435 :
+ 4436 : //}
+ 4437 :
+ 4438 93 : /* //{ callbackToggleOutput() */
+ 4439 :
+ 4440 93 : bool ControlManager::callbackToggleOutput(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+ 4441 0 :
+ 4442 : if (!is_initialized_)
+ 4443 93 : return false;
+ 4444 :
+ 4445 : ROS_INFO("[ControlManager]: toggling output by service");
+ 4446 186 :
+ 4447 : // copy member variables
+ 4448 186 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 4449 :
+ 4450 93 : std::stringstream ss;
+ 4451 :
+ 4452 : bool prereq_check = true;
+ 4453 186 :
+ 4454 93 : {
+ 4455 93 : mrs_msgs::ReferenceStamped current_coord;
+ 4456 93 : current_coord.header.frame_id = uav_state.header.frame_id;
+ 4457 : current_coord.reference.position.x = uav_state.pose.position.x;
+ 4458 93 : current_coord.reference.position.y = uav_state.pose.position.y;
+ 4459 1 :
+ 4460 1 : if (!isPointInSafetyArea2d(current_coord)) {
+ 4461 : ss << "cannot toggle output, the UAV is outside of the safety area!";
+ 4462 : prereq_check = false;
+ 4463 : }
+ 4464 93 : }
+ 4465 0 :
+ 4466 0 : if (req.data && (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_)) {
+ 4467 : ss << "cannot toggle output ON, we landed in emergency";
+ 4468 : prereq_check = false;
+ 4469 93 : }
+ 4470 0 :
+ 4471 0 : if (!sh_hw_api_status_.hasMsg() || (ros::Time::now() - sh_hw_api_status_.lastMsgTime()).toSec() > 1.0) {
+ 4472 : ss << "cannot toggle output ON, missing HW API status!";
+ 4473 : prereq_check = false;
+ 4474 93 : }
+ 4475 :
+ 4476 1 : if (!prereq_check) {
+ 4477 1 :
+ 4478 : res.message = ss.str();
+ 4479 1 : res.success = false;
+ 4480 :
+ 4481 1 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 4482 :
+ 4483 : return false;
+ 4484 :
+ 4485 92 : } else {
+ 4486 :
+ 4487 92 : toggleOutput(req.data);
+ 4488 92 :
+ 4489 92 : ss << "Output: " << (output_enabled_ ? "ON" : "OFF");
+ 4490 : res.message = ss.str();
+ 4491 92 : res.success = true;
+ 4492 :
+ 4493 92 : ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 4494 :
+ 4495 92 : publishDiagnostics();
+ 4496 :
+ 4497 : return true;
+ 4498 : }
+ 4499 : }
+ 4500 :
+ 4501 : //}
+ 4502 :
+ 4503 7 : /* callbackArm() //{ */
+ 4504 :
+ 4505 7 : bool ControlManager::callbackArm(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+ 4506 0 :
+ 4507 : if (!is_initialized_)
+ 4508 7 : return false;
+ 4509 :
+ 4510 14 : ROS_INFO("[ControlManager]: arming by service");
+ 4511 :
+ 4512 7 : std::stringstream ss;
+ 4513 :
+ 4514 0 : if (failsafe_triggered_ || eland_triggered_) {
+ 4515 :
+ 4516 0 : ss << "can not " << (req.data ? "arm" : "disarm") << ", eland or failsafe active";
+ 4517 0 :
+ 4518 : res.message = ss.str();
+ 4519 0 : res.success = false;
+ 4520 :
+ 4521 0 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 4522 :
+ 4523 : return true;
+ 4524 7 : }
+ 4525 :
+ 4526 0 : if (req.data) {
+ 4527 0 :
+ 4528 0 : ss << "this service is not allowed to arm the UAV";
+ 4529 : res.success = false;
+ 4530 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 4531 :
+ 4532 14 : } else {
+ 4533 :
+ 4534 7 : auto [success, message] = arming(false);
+ 4535 :
+ 4536 7 : if (success) {
+ 4537 7 :
+ 4538 7 : ss << "disarmed";
+ 4539 : res.success = true;
+ 4540 : ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+ 4541 :
+ 4542 0 : } else {
+ 4543 0 :
+ 4544 0 : ss << "could not disarm: " << message;
+ 4545 : res.success = false;
+ 4546 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 4547 : }
+ 4548 7 : }
+ 4549 :
+ 4550 7 : res.message = ss.str();
+ 4551 :
+ 4552 : return true;
+ 4553 : }
+ 4554 :
+ 4555 : //}
+ 4556 :
+ 4557 93 : /* //{ callbackEnableCallbacks() */
+ 4558 :
+ 4559 93 : bool ControlManager::callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+ 4560 0 :
+ 4561 : if (!is_initialized_)
+ 4562 93 : return false;
+ 4563 :
+ 4564 93 : setCallbacks(req.data);
+ 4565 :
+ 4566 93 : std::stringstream ss;
+ 4567 :
+ 4568 93 : ss << "callbacks " << (callbacks_enabled_ ? "enabled" : "disabled");
+ 4569 93 :
+ 4570 : res.message = ss.str();
+ 4571 93 : res.success = true;
+ 4572 :
+ 4573 93 : ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+ 4574 :
+ 4575 : return true;
+ 4576 : }
+ 4577 :
+ 4578 : //}
+ 4579 :
+ 4580 94 : /* callbackSetConstraints() //{ */
+ 4581 :
+ 4582 94 : bool ControlManager::callbackSetConstraints(mrs_msgs::DynamicsConstraintsSrv::Request& req, mrs_msgs::DynamicsConstraintsSrv::Response& res) {
+ 4583 0 :
+ 4584 0 : if (!is_initialized_) {
+ 4585 0 : res.message = "not initialized";
+ 4586 : res.success = false;
+ 4587 : return true;
+ 4588 : }
+ 4589 188 :
+ 4590 : {
+ 4591 94 : std::scoped_lock lock(mutex_constraints_);
+ 4592 :
+ 4593 94 : current_constraints_ = req;
+ 4594 :
+ 4595 94 : auto enforced = enforceControllersConstraints(current_constraints_);
+ 4596 0 :
+ 4597 0 : if (enforced) {
+ 4598 : sanitized_constraints_ = enforced.value();
+ 4599 94 : constraints_being_enforced_ = true;
+ 4600 94 : } else {
+ 4601 : sanitized_constraints_ = req;
+ 4602 : constraints_being_enforced_ = false;
+ 4603 94 : }
+ 4604 :
+ 4605 94 : got_constraints_ = true;
+ 4606 94 :
+ 4607 : setConstraintsToControllers(current_constraints_);
+ 4608 : setConstraintsToTrackers(sanitized_constraints_);
+ 4609 94 : }
+ 4610 94 :
+ 4611 : res.message = "setting constraints";
+ 4612 94 : res.success = true;
+ 4613 :
+ 4614 : return true;
+ 4615 : }
+ 4616 :
+ 4617 : //}
+ 4618 :
+ 4619 92 : /* //{ callbackEmergencyReference() */
+ 4620 :
+ 4621 92 : bool ControlManager::callbackEmergencyReference(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+ 4622 0 :
+ 4623 : if (!is_initialized_)
+ 4624 184 : return false;
+ 4625 :
+ 4626 92 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 4627 :
+ 4628 92 : callbacks_enabled_ = false;
+ 4629 :
+ 4630 184 : mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+ 4631 :
+ 4632 : std::stringstream ss;
+ 4633 184 :
+ 4634 92 : // transform the reference to the current frame
+ 4635 92 : mrs_msgs::ReferenceStamped original_reference;
+ 4636 : original_reference.header = req.header;
+ 4637 184 : original_reference.reference = req.reference;
+ 4638 :
+ 4639 92 : auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+ 4640 :
+ 4641 0 : if (!ret) {
+ 4642 :
+ 4643 0 : ss << "the emergency reference could not be transformed";
+ 4644 0 :
+ 4645 0 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 4646 0 : res.message = ss.str();
+ 4647 : res.success = false;
+ 4648 : return true;
+ 4649 92 : }
+ 4650 :
+ 4651 92 : mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+ 4652 :
+ 4653 92 : std_srvs::SetBoolRequest req_enable_callbacks;
+ 4654 92 :
+ 4655 : mrs_msgs::ReferenceSrvRequest req_goto_out;
+ 4656 : req_goto_out.reference = transformed_reference.reference;
+ 4657 184 :
+ 4658 : {
+ 4659 : std::scoped_lock lock(mutex_tracker_list_);
+ 4660 92 :
+ 4661 644 : // disable callbacks of all trackers
+ 4662 552 : req_enable_callbacks.data = false;
+ 4663 : for (int i = 0; i < int(tracker_list_.size()); i++) {
+ 4664 : tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 4665 : }
+ 4666 92 :
+ 4667 92 : // enable the callbacks for the active tracker
+ 4668 : req_enable_callbacks.data = true;
+ 4669 : tracker_list_.at(active_tracker_idx_)
+ 4670 276 : ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 4671 276 :
+ 4672 : // call the setReference()
+ 4673 : tracker_response = tracker_list_.at(active_tracker_idx_)
+ 4674 92 : ->setReference(mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+ 4675 92 :
+ 4676 : // disable the callbacks back again
+ 4677 92 : req_enable_callbacks.data = false;
+ 4678 92 : tracker_list_.at(active_tracker_idx_)
+ 4679 92 : ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 4680 :
+ 4681 0 : if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+ 4682 0 : res.message = tracker_response->message;
+ 4683 0 : res.success = tracker_response->success;
+ 4684 : } else {
+ 4685 : ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setReference()' function!";
+ 4686 : res.message = ss.str();
+ 4687 92 : res.success = false;
+ 4688 : }
+ 4689 : }
+ 4690 :
+ 4691 : return true;
+ 4692 : }
+ 4693 :
+ 4694 0 : //}
+ 4695 :
+ 4696 0 : /* callbackPirouette() //{ */
+ 4697 0 :
+ 4698 : bool ControlManager::callbackPirouette([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4699 :
+ 4700 0 : if (!is_initialized_)
+ 4701 : return false;
+ 4702 :
+ 4703 : // copy member variables
+ 4704 0 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 4705 :
+ 4706 0 : double uav_heading;
+ 4707 0 : try {
+ 4708 0 : uav_heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+ 4709 : }
+ 4710 0 : catch (...) {
+ 4711 : std::stringstream ss;
+ 4712 0 : ss << "could not calculate the UAV heading to initialize the pirouette";
+ 4713 0 :
+ 4714 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 4715 0 :
+ 4716 : res.message = ss.str();
+ 4717 : res.success = false;
+ 4718 0 :
+ 4719 0 : return false;
+ 4720 0 : }
+ 4721 0 :
+ 4722 : if (_pirouette_enabled_) {
+ 4723 : res.success = false;
+ 4724 0 : res.message = "already active";
+ 4725 : return true;
+ 4726 0 : }
+ 4727 0 :
+ 4728 : if (failsafe_triggered_ || eland_triggered_ || rc_escalating_failsafe_triggered_) {
+ 4729 0 :
+ 4730 0 : std::stringstream ss;
+ 4731 : ss << "can not activate the pirouette, eland or failsafe active";
+ 4732 0 :
+ 4733 : res.message = ss.str();
+ 4734 0 : res.success = false;
+ 4735 :
+ 4736 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 4737 0 :
+ 4738 : return true;
+ 4739 0 : }
+ 4740 :
+ 4741 0 : _pirouette_enabled_ = true;
+ 4742 0 :
+ 4743 0 : setCallbacks(false);
+ 4744 :
+ 4745 0 : pirouette_initial_heading_ = uav_heading;
+ 4746 0 : pirouette_iterator_ = 0;
+ 4747 : timer_pirouette_.start();
+ 4748 0 :
+ 4749 : res.success = true;
+ 4750 : res.message = "activated";
+ 4751 :
+ 4752 : return true;
+ 4753 : }
+ 4754 :
+ 4755 0 : //}
+ 4756 :
+ 4757 0 : /* callbackUseJoystick() //{ */
+ 4758 0 :
+ 4759 : bool ControlManager::callbackUseJoystick([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4760 :
+ 4761 0 : if (!is_initialized_) {
+ 4762 : return false;
+ 4763 : }
+ 4764 0 :
+ 4765 : std::stringstream ss;
+ 4766 0 :
+ 4767 : {
+ 4768 0 : auto [success, response] = switchTracker(_joystick_tracker_name_);
+ 4769 0 :
+ 4770 : if (!success) {
+ 4771 0 :
+ 4772 0 : ss << "switching to '" << _joystick_tracker_name_ << "' was unsuccessfull: '" << response << "'";
+ 4773 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 4774 0 :
+ 4775 : res.success = false;
+ 4776 : res.message = ss.str();
+ 4777 :
+ 4778 0 : return true;
+ 4779 : }
+ 4780 0 : }
+ 4781 :
+ 4782 0 : auto [success, response] = switchController(_joystick_controller_name_);
+ 4783 0 :
+ 4784 : if (!success) {
+ 4785 0 :
+ 4786 0 : ss << "switching to '" << _joystick_controller_name_ << "' was unsuccessfull: '" << response << "'";
+ 4787 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 4788 :
+ 4789 0 : res.success = false;
+ 4790 : res.message = ss.str();
+ 4791 :
+ 4792 0 : // switch back to hover tracker
+ 4793 : switchTracker(_ehover_tracker_name_);
+ 4794 0 :
+ 4795 : // switch back to safety controller
+ 4796 0 : switchController(_eland_controller_name_);
+ 4797 :
+ 4798 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 4799 0 :
+ 4800 : return true;
+ 4801 0 : }
+ 4802 0 :
+ 4803 : ss << "switched to joystick control";
+ 4804 0 :
+ 4805 : res.success = true;
+ 4806 0 : res.message = ss.str();
+ 4807 :
+ 4808 : ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+ 4809 :
+ 4810 : return true;
+ 4811 : }
+ 4812 :
+ 4813 0 : //}
+ 4814 :
+ 4815 0 : /* //{ callbackHover() */
+ 4816 0 :
+ 4817 : bool ControlManager::callbackHover([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4818 0 :
+ 4819 : if (!is_initialized_)
+ 4820 0 : return false;
+ 4821 0 :
+ 4822 : auto [success, message] = hover();
+ 4823 0 :
+ 4824 : res.success = success;
+ 4825 : res.message = message;
+ 4826 :
+ 4827 : return true;
+ 4828 : }
+ 4829 :
+ 4830 1 : //}
+ 4831 :
+ 4832 1 : /* //{ callbackStartTrajectoryTracking() */
+ 4833 0 :
+ 4834 : bool ControlManager::callbackStartTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4835 1 :
+ 4836 : if (!is_initialized_)
+ 4837 1 : return false;
+ 4838 1 :
+ 4839 : auto [success, message] = startTrajectoryTracking();
+ 4840 1 :
+ 4841 : res.success = success;
+ 4842 : res.message = message;
+ 4843 :
+ 4844 : return true;
+ 4845 : }
+ 4846 :
+ 4847 0 : //}
+ 4848 :
+ 4849 0 : /* //{ callbackStopTrajectoryTracking() */
+ 4850 0 :
+ 4851 : bool ControlManager::callbackStopTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4852 0 :
+ 4853 : if (!is_initialized_)
+ 4854 0 : return false;
+ 4855 0 :
+ 4856 : auto [success, message] = stopTrajectoryTracking();
+ 4857 0 :
+ 4858 : res.success = success;
+ 4859 : res.message = message;
+ 4860 :
+ 4861 : return true;
+ 4862 : }
+ 4863 :
+ 4864 0 : //}
+ 4865 :
+ 4866 0 : /* //{ callbackResumeTrajectoryTracking() */
+ 4867 0 :
+ 4868 : bool ControlManager::callbackResumeTrajectoryTracking([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4869 0 :
+ 4870 : if (!is_initialized_)
+ 4871 0 : return false;
+ 4872 0 :
+ 4873 : auto [success, message] = resumeTrajectoryTracking();
+ 4874 0 :
+ 4875 : res.success = success;
+ 4876 : res.message = message;
+ 4877 :
+ 4878 : return true;
+ 4879 : }
+ 4880 :
+ 4881 1 : //}
+ 4882 :
+ 4883 1 : /* //{ callbackGotoTrajectoryStart() */
+ 4884 0 :
+ 4885 : bool ControlManager::callbackGotoTrajectoryStart([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) {
+ 4886 1 :
+ 4887 : if (!is_initialized_)
+ 4888 1 : return false;
+ 4889 1 :
+ 4890 : auto [success, message] = gotoTrajectoryStart();
+ 4891 1 :
+ 4892 : res.success = success;
+ 4893 : res.message = message;
+ 4894 :
+ 4895 : return true;
+ 4896 : }
+ 4897 :
+ 4898 0 : //}
+ 4899 :
+ 4900 0 : /* //{ callbackTransformReference() */
+ 4901 0 :
+ 4902 : bool ControlManager::callbackTransformReference(mrs_msgs::TransformReferenceSrv::Request& req, mrs_msgs::TransformReferenceSrv::Response& res) {
+ 4903 :
+ 4904 0 : if (!is_initialized_)
+ 4905 : return false;
+ 4906 0 :
+ 4907 : // transform the reference to the current frame
+ 4908 0 : mrs_msgs::ReferenceStamped transformed_reference = req.reference;
+ 4909 0 :
+ 4910 0 : if (auto ret = transformer_->transformSingle(transformed_reference, req.frame_id)) {
+ 4911 0 :
+ 4912 : res.reference = ret.value();
+ 4913 : res.message = "transformation successful";
+ 4914 : res.success = true;
+ 4915 0 : return true;
+ 4916 0 :
+ 4917 0 : } else {
+ 4918 :
+ 4919 : res.message = "the reference could not be transformed";
+ 4920 : res.success = false;
+ 4921 : return true;
+ 4922 : }
+ 4923 :
+ 4924 : return true;
+ 4925 : }
+ 4926 :
+ 4927 0 : //}
+ 4928 :
+ 4929 0 : /* //{ callbackTransformPose() */
+ 4930 0 :
+ 4931 : bool ControlManager::callbackTransformPose(mrs_msgs::TransformPoseSrv::Request& req, mrs_msgs::TransformPoseSrv::Response& res) {
+ 4932 :
+ 4933 0 : if (!is_initialized_)
+ 4934 : return false;
+ 4935 0 :
+ 4936 : // transform the reference to the current frame
+ 4937 0 : geometry_msgs::PoseStamped transformed_pose = req.pose;
+ 4938 0 :
+ 4939 0 : if (auto ret = transformer_->transformSingle(transformed_pose, req.frame_id)) {
+ 4940 0 :
+ 4941 : res.pose = ret.value();
+ 4942 : res.message = "transformation successful";
+ 4943 : res.success = true;
+ 4944 0 : return true;
+ 4945 0 :
+ 4946 0 : } else {
+ 4947 :
+ 4948 : res.message = "the pose could not be transformed";
+ 4949 : res.success = false;
+ 4950 : return true;
+ 4951 : }
+ 4952 :
+ 4953 : return true;
+ 4954 : }
+ 4955 :
+ 4956 0 : //}
+ 4957 :
+ 4958 0 : /* //{ callbackTransformVector3() */
+ 4959 0 :
+ 4960 : bool ControlManager::callbackTransformVector3(mrs_msgs::TransformVector3Srv::Request& req, mrs_msgs::TransformVector3Srv::Response& res) {
+ 4961 :
+ 4962 0 : if (!is_initialized_)
+ 4963 : return false;
+ 4964 0 :
+ 4965 : // transform the reference to the current frame
+ 4966 0 : geometry_msgs::Vector3Stamped transformed_vector3 = req.vector;
+ 4967 0 :
+ 4968 0 : if (auto ret = transformer_->transformSingle(transformed_vector3, req.frame_id)) {
+ 4969 0 :
+ 4970 : res.vector = ret.value();
+ 4971 : res.message = "transformation successful";
+ 4972 : res.success = true;
+ 4973 0 : return true;
+ 4974 0 :
+ 4975 0 : } else {
+ 4976 :
+ 4977 : res.message = "the twist could not be transformed";
+ 4978 : res.success = false;
+ 4979 : return true;
+ 4980 : }
+ 4981 :
+ 4982 : return true;
+ 4983 : }
+ 4984 :
+ 4985 0 : //}
+ 4986 :
+ 4987 0 : /* //{ callbackEnableBumper() */
+ 4988 0 :
+ 4989 : bool ControlManager::callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+ 4990 0 :
+ 4991 : if (!is_initialized_)
+ 4992 0 : return false;
+ 4993 :
+ 4994 0 : bumper_enabled_ = req.data;
+ 4995 :
+ 4996 0 : std::stringstream ss;
+ 4997 :
+ 4998 0 : ss << "bumper " << (bumper_enabled_ ? "enalbed" : "disabled");
+ 4999 0 :
+ 5000 : ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+ 5001 0 :
+ 5002 : res.success = true;
+ 5003 : res.message = ss.str();
+ 5004 :
+ 5005 : return true;
+ 5006 : }
+ 5007 :
+ 5008 0 : //}
+ 5009 :
+ 5010 0 : /* //{ callbackUseSafetyArea() */
+ 5011 0 :
+ 5012 : bool ControlManager::callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+ 5013 0 :
+ 5014 : if (!is_initialized_)
+ 5015 0 : return false;
+ 5016 :
+ 5017 0 : use_safety_area_ = req.data;
+ 5018 :
+ 5019 0 : std::stringstream ss;
+ 5020 :
+ 5021 0 : ss << "safety area " << (use_safety_area_ ? "enabled" : "disabled");
+ 5022 0 :
+ 5023 : ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+ 5024 0 :
+ 5025 : res.success = true;
+ 5026 : res.message = ss.str();
+ 5027 :
+ 5028 : return true;
+ 5029 : }
+ 5030 :
+ 5031 0 : //}
+ 5032 :
+ 5033 0 : /* //{ callbackGetMinZ() */
+ 5034 0 :
+ 5035 : bool ControlManager::callbackGetMinZ([[maybe_unused]] mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res) {
+ 5036 :
+ 5037 0 : if (!is_initialized_) {
+ 5038 : return false;
+ 5039 0 : }
+ 5040 0 :
+ 5041 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 5042 0 :
+ 5043 : res.success = true;
+ 5044 : res.value = getMinZ(uav_state.header.frame_id);
+ 5045 :
+ 5046 : return true;
+ 5047 : }
+ 5048 :
+ 5049 0 : //}
+ 5050 :
+ 5051 0 : /* //{ callbackValidateReference() */
+ 5052 0 :
+ 5053 0 : bool ControlManager::callbackValidateReference(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+ 5054 0 :
+ 5055 : if (!is_initialized_) {
+ 5056 : res.message = "not initialized";
+ 5057 0 : res.success = false;
+ 5058 0 : return true;
+ 5059 0 : }
+ 5060 0 :
+ 5061 0 : if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+ 5062 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+ 5063 : res.message = "NaNs/infs in input!";
+ 5064 : res.success = false;
+ 5065 0 : return true;
+ 5066 0 : }
+ 5067 :
+ 5068 : // copy member variables
+ 5069 0 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 5070 0 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 5071 0 :
+ 5072 : // transform the reference to the current frame
+ 5073 0 : mrs_msgs::ReferenceStamped original_reference;
+ 5074 : original_reference.header = req.reference.header;
+ 5075 0 : original_reference.reference = req.reference.reference;
+ 5076 :
+ 5077 0 : auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+ 5078 0 :
+ 5079 0 : if (!ret) {
+ 5080 0 :
+ 5081 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+ 5082 : res.message = "the reference could not be transformed";
+ 5083 0 : res.success = false;
+ 5084 : return true;
+ 5085 0 : }
+ 5086 0 :
+ 5087 0 : mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+ 5088 0 :
+ 5089 0 : if (!isPointInSafetyArea3d(transformed_reference)) {
+ 5090 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+ 5091 : res.message = "the point is outside of the safety area";
+ 5092 0 : res.success = false;
+ 5093 : return true;
+ 5094 0 : }
+ 5095 0 :
+ 5096 0 : if (last_tracker_cmd) {
+ 5097 0 :
+ 5098 0 : mrs_msgs::ReferenceStamped from_point;
+ 5099 : from_point.header.frame_id = uav_state.header.frame_id;
+ 5100 0 : from_point.reference.position.x = last_tracker_cmd->position.x;
+ 5101 0 : from_point.reference.position.y = last_tracker_cmd->position.y;
+ 5102 0 : from_point.reference.position.z = last_tracker_cmd->position.z;
+ 5103 0 :
+ 5104 0 : if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+ 5105 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+ 5106 : res.message = "the path is going outside the safety area";
+ 5107 : res.success = false;
+ 5108 0 : return true;
+ 5109 0 : }
+ 5110 0 : }
+ 5111 :
+ 5112 : res.message = "the reference is ok";
+ 5113 : res.success = true;
+ 5114 : return true;
+ 5115 : }
+ 5116 :
+ 5117 9768 : //}
+ 5118 :
+ 5119 9768 : /* //{ callbackValidateReference2d() */
+ 5120 0 :
+ 5121 0 : bool ControlManager::callbackValidateReference2d(mrs_msgs::ValidateReference::Request& req, mrs_msgs::ValidateReference::Response& res) {
+ 5122 0 :
+ 5123 : if (!is_initialized_) {
+ 5124 : res.message = "not initialized";
+ 5125 9768 : res.success = false;
+ 5126 0 : return true;
+ 5127 0 : }
+ 5128 0 :
+ 5129 0 : if (!validateReference(req.reference.reference, "ControlManager", "reference_for_validation")) {
+ 5130 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: NaN detected in variable 'req.reference'!!!");
+ 5131 : res.message = "NaNs/infs in input!";
+ 5132 : res.success = false;
+ 5133 19536 : return true;
+ 5134 19536 : }
+ 5135 :
+ 5136 : // copy member variables
+ 5137 19536 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 5138 9768 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 5139 9768 :
+ 5140 : // transform the reference to the current frame
+ 5141 19536 : mrs_msgs::ReferenceStamped original_reference;
+ 5142 : original_reference.header = req.reference.header;
+ 5143 9768 : original_reference.reference = req.reference.reference;
+ 5144 :
+ 5145 0 : auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+ 5146 0 :
+ 5147 0 : if (!ret) {
+ 5148 0 :
+ 5149 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: the reference could not be transformed");
+ 5150 : res.message = "the reference could not be transformed";
+ 5151 19536 : res.success = false;
+ 5152 : return true;
+ 5153 9768 : }
+ 5154 76 :
+ 5155 76 : mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+ 5156 76 :
+ 5157 76 : if (!isPointInSafetyArea2d(transformed_reference)) {
+ 5158 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the point is outside of the safety area!");
+ 5159 : res.message = "the point is outside of the safety area";
+ 5160 9692 : res.success = false;
+ 5161 : return true;
+ 5162 31 : }
+ 5163 31 :
+ 5164 31 : if (last_tracker_cmd) {
+ 5165 31 :
+ 5166 31 : mrs_msgs::ReferenceStamped from_point;
+ 5167 : from_point.header.frame_id = uav_state.header.frame_id;
+ 5168 31 : from_point.reference.position.x = last_tracker_cmd->position.x;
+ 5169 0 : from_point.reference.position.y = last_tracker_cmd->position.y;
+ 5170 0 : from_point.reference.position.z = last_tracker_cmd->position.z;
+ 5171 0 :
+ 5172 0 : if (!isPathToPointInSafetyArea2d(from_point, transformed_reference)) {
+ 5173 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: reference validation failed, the path is going outside the safety area!");
+ 5174 : res.message = "the path is going outside the safety area";
+ 5175 : res.success = false;
+ 5176 9692 : return true;
+ 5177 9692 : }
+ 5178 9692 : }
+ 5179 :
+ 5180 : res.message = "the reference is ok";
+ 5181 : res.success = true;
+ 5182 : return true;
+ 5183 : }
+ 5184 :
+ 5185 0 : //}
+ 5186 :
+ 5187 0 : /* //{ callbackValidateReferenceList() */
+ 5188 0 :
+ 5189 0 : bool ControlManager::callbackValidateReferenceList(mrs_msgs::ValidateReferenceList::Request& req, mrs_msgs::ValidateReferenceList::Response& res) {
+ 5190 :
+ 5191 : if (!is_initialized_) {
+ 5192 : res.message = "not initialized";
+ 5193 0 : return false;
+ 5194 0 : }
+ 5195 :
+ 5196 : // copy member variables
+ 5197 0 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 5198 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 5199 0 :
+ 5200 : // get the transformer
+ 5201 0 : auto ret = transformer_->getTransform(uav_state.header.frame_id, req.list.header.frame_id, req.list.header.stamp);
+ 5202 0 :
+ 5203 0 : if (!ret) {
+ 5204 :
+ 5205 : ROS_DEBUG("[ControlManager]: could not find transform for the reference");
+ 5206 0 : res.message = "could not find transform";
+ 5207 : return false;
+ 5208 0 : }
+ 5209 :
+ 5210 0 : geometry_msgs::TransformStamped tf = ret.value();
+ 5211 :
+ 5212 0 : for (int i = 0; i < int(req.list.list.size()); i++) {
+ 5213 0 :
+ 5214 0 : res.success.push_back(true);
+ 5215 :
+ 5216 0 : mrs_msgs::ReferenceStamped original_reference;
+ 5217 : original_reference.header = req.list.header;
+ 5218 0 : original_reference.reference = req.list.list.at(i);
+ 5219 :
+ 5220 0 : res.success.at(i) = validateReference(original_reference.reference, "ControlManager", "reference_list");
+ 5221 :
+ 5222 0 : auto ret = transformer_->transformSingle(original_reference, uav_state.header.frame_id);
+ 5223 0 :
+ 5224 : if (!ret) {
+ 5225 :
+ 5226 0 : ROS_DEBUG("[ControlManager]: the reference could not be transformed");
+ 5227 : res.success.at(i) = false;
+ 5228 0 : }
+ 5229 0 :
+ 5230 : mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+ 5231 :
+ 5232 0 : if (!isPointInSafetyArea3d(transformed_reference)) {
+ 5233 : res.success.at(i) = false;
+ 5234 0 : }
+ 5235 0 :
+ 5236 0 : if (last_tracker_cmd) {
+ 5237 0 :
+ 5238 0 : mrs_msgs::ReferenceStamped from_point;
+ 5239 : from_point.header.frame_id = uav_state.header.frame_id;
+ 5240 0 : from_point.reference.position.x = last_tracker_cmd->position.x;
+ 5241 0 : from_point.reference.position.y = last_tracker_cmd->position.y;
+ 5242 : from_point.reference.position.z = last_tracker_cmd->position.z;
+ 5243 :
+ 5244 : if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+ 5245 : res.success.at(i) = false;
+ 5246 0 : }
+ 5247 0 : }
+ 5248 : }
+ 5249 :
+ 5250 : res.message = "references were checked";
+ 5251 : return true;
+ 5252 : }
+ 5253 :
+ 5254 : //}
+ 5255 :
+ 5256 0 : // | -------------- setpoint topics and services -------------- |
+ 5257 :
+ 5258 0 : /* //{ callbackReferenceService() */
+ 5259 0 :
+ 5260 0 : bool ControlManager::callbackReferenceService(mrs_msgs::ReferenceStampedSrv::Request& req, mrs_msgs::ReferenceStampedSrv::Response& res) {
+ 5261 0 :
+ 5262 : if (!is_initialized_) {
+ 5263 : res.message = "not initialized";
+ 5264 0 : res.success = false;
+ 5265 0 : return true;
+ 5266 : }
+ 5267 0 :
+ 5268 0 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackReferenceService");
+ 5269 0 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackReferenceService", scope_timer_logger_, scope_timer_enabled_);
+ 5270 :
+ 5271 0 : mrs_msgs::ReferenceStamped des_reference;
+ 5272 : des_reference.header = req.header;
+ 5273 0 : des_reference.reference = req.reference;
+ 5274 0 :
+ 5275 : auto [success, message] = setReference(des_reference);
+ 5276 0 :
+ 5277 : res.success = success;
+ 5278 : res.message = message;
+ 5279 :
+ 5280 : return true;
+ 5281 : }
+ 5282 :
+ 5283 0 : //}
+ 5284 :
+ 5285 0 : /* //{ callbackReferenceTopic() */
+ 5286 0 :
+ 5287 : void ControlManager::callbackReferenceTopic(const mrs_msgs::ReferenceStamped::ConstPtr msg) {
+ 5288 0 :
+ 5289 0 : if (!is_initialized_)
+ 5290 : return;
+ 5291 0 :
+ 5292 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackReferenceTopic");
+ 5293 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+ 5294 :
+ 5295 : setReference(*msg);
+ 5296 : }
+ 5297 :
+ 5298 472 : //}
+ 5299 :
+ 5300 : /* //{ callbackVelocityReferenceService() */
+ 5301 472 :
+ 5302 0 : bool ControlManager::callbackVelocityReferenceService(mrs_msgs::VelocityReferenceStampedSrv::Request& req,
+ 5303 0 : mrs_msgs::VelocityReferenceStampedSrv::Response& res) {
+ 5304 0 :
+ 5305 : if (!is_initialized_) {
+ 5306 : res.message = "not initialized";
+ 5307 1416 : res.success = false;
+ 5308 1416 : return true;
+ 5309 : }
+ 5310 944 :
+ 5311 472 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackVelocityReferenceService");
+ 5312 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceService", scope_timer_logger_, scope_timer_enabled_);
+ 5313 472 :
+ 5314 : mrs_msgs::VelocityReferenceStamped des_reference;
+ 5315 472 : des_reference = req.reference;
+ 5316 472 :
+ 5317 : auto [success, message] = setVelocityReference(des_reference);
+ 5318 472 :
+ 5319 : res.success = success;
+ 5320 : res.message = message;
+ 5321 :
+ 5322 : return true;
+ 5323 : }
+ 5324 :
+ 5325 0 : //}
+ 5326 :
+ 5327 0 : /* //{ callbackVelocityReferenceTopic() */
+ 5328 0 :
+ 5329 : void ControlManager::callbackVelocityReferenceTopic(const mrs_msgs::VelocityReferenceStamped::ConstPtr msg) {
+ 5330 0 :
+ 5331 0 : if (!is_initialized_)
+ 5332 : return;
+ 5333 0 :
+ 5334 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackVelocityReferenceTopic");
+ 5335 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackVelocityReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+ 5336 :
+ 5337 : setVelocityReference(*msg);
+ 5338 : }
+ 5339 :
+ 5340 4 : //}
+ 5341 :
+ 5342 4 : /* //{ callbackTrajectoryReferenceService() */
+ 5343 0 :
+ 5344 0 : bool ControlManager::callbackTrajectoryReferenceService(mrs_msgs::TrajectoryReferenceSrv::Request& req, mrs_msgs::TrajectoryReferenceSrv::Response& res) {
+ 5345 0 :
+ 5346 : if (!is_initialized_) {
+ 5347 : res.message = "not initialized";
+ 5348 12 : res.success = false;
+ 5349 12 : return true;
+ 5350 : }
+ 5351 8 :
+ 5352 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceService");
+ 5353 4 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceService", scope_timer_logger_, scope_timer_enabled_);
+ 5354 4 :
+ 5355 4 : auto [success, message, modified, tracker_names, tracker_successes, tracker_messages] = setTrajectoryReference(req.trajectory);
+ 5356 4 :
+ 5357 4 : res.success = success;
+ 5358 : res.message = message;
+ 5359 28 : res.modified = modified;
+ 5360 24 : res.tracker_names = tracker_names;
+ 5361 : res.tracker_messages = tracker_messages;
+ 5362 :
+ 5363 4 : for (size_t i = 0; i < tracker_successes.size(); i++) {
+ 5364 : res.tracker_successes.push_back(tracker_successes.at(i));
+ 5365 : }
+ 5366 :
+ 5367 : return true;
+ 5368 : }
+ 5369 :
+ 5370 0 : //}
+ 5371 :
+ 5372 0 : /* //{ callbackTrajectoryReferenceTopic() */
+ 5373 0 :
+ 5374 : void ControlManager::callbackTrajectoryReferenceTopic(const mrs_msgs::TrajectoryReference::ConstPtr msg) {
+ 5375 0 :
+ 5376 0 : if (!is_initialized_)
+ 5377 : return;
+ 5378 0 :
+ 5379 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackTrajectoryReferenceTopic");
+ 5380 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackTrajectoryReferenceTopic", scope_timer_logger_, scope_timer_enabled_);
+ 5381 :
+ 5382 : setTrajectoryReference(*msg);
+ 5383 : }
+ 5384 :
+ 5385 : //}
+ 5386 :
+ 5387 26 : // | ------------- human-callable "goto" services ------------- |
+ 5388 :
+ 5389 26 : /* //{ callbackGoto() */
+ 5390 0 :
+ 5391 0 : bool ControlManager::callbackGoto(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+ 5392 0 :
+ 5393 : if (!is_initialized_) {
+ 5394 : res.message = "not initialized";
+ 5395 78 : res.success = false;
+ 5396 78 : return true;
+ 5397 : }
+ 5398 52 :
+ 5399 26 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackGoto");
+ 5400 26 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackGoto", scope_timer_logger_, scope_timer_enabled_);
+ 5401 26 :
+ 5402 26 : mrs_msgs::ReferenceStamped des_reference;
+ 5403 26 : des_reference.header.frame_id = "";
+ 5404 26 : des_reference.header.stamp = ros::Time(0);
+ 5405 : des_reference.reference.position.x = req.goal.at(REF_X);
+ 5406 52 : des_reference.reference.position.y = req.goal.at(REF_Y);
+ 5407 : des_reference.reference.position.z = req.goal.at(REF_Z);
+ 5408 26 : des_reference.reference.heading = req.goal.at(REF_HEADING);
+ 5409 26 :
+ 5410 : auto [success, message] = setReference(des_reference);
+ 5411 26 :
+ 5412 : res.success = success;
+ 5413 : res.message = message;
+ 5414 :
+ 5415 : return true;
+ 5416 : }
+ 5417 :
+ 5418 0 : //}
+ 5419 :
+ 5420 0 : /* //{ callbackGotoFcu() */
+ 5421 0 :
+ 5422 0 : bool ControlManager::callbackGotoFcu(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+ 5423 0 :
+ 5424 : if (!is_initialized_) {
+ 5425 : res.message = "not initialized";
+ 5426 0 : res.success = false;
+ 5427 0 : return true;
+ 5428 : }
+ 5429 0 :
+ 5430 0 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackGotoFcu");
+ 5431 0 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackGotoFcu", scope_timer_logger_, scope_timer_enabled_);
+ 5432 0 :
+ 5433 0 : mrs_msgs::ReferenceStamped des_reference;
+ 5434 0 : des_reference.header.frame_id = "fcu_untilted";
+ 5435 0 : des_reference.header.stamp = ros::Time(0);
+ 5436 : des_reference.reference.position.x = req.goal.at(REF_X);
+ 5437 0 : des_reference.reference.position.y = req.goal.at(REF_Y);
+ 5438 : des_reference.reference.position.z = req.goal.at(REF_Z);
+ 5439 0 : des_reference.reference.heading = req.goal.at(REF_HEADING);
+ 5440 0 :
+ 5441 : auto [success, message] = setReference(des_reference);
+ 5442 0 :
+ 5443 : res.success = success;
+ 5444 : res.message = message;
+ 5445 :
+ 5446 : return true;
+ 5447 : }
+ 5448 :
+ 5449 23 : //}
+ 5450 :
+ 5451 23 : /* //{ callbackGotoRelative() */
+ 5452 0 :
+ 5453 0 : bool ControlManager::callbackGotoRelative(mrs_msgs::Vec4::Request& req, mrs_msgs::Vec4::Response& res) {
+ 5454 0 :
+ 5455 : if (!is_initialized_) {
+ 5456 : res.message = "not initialized";
+ 5457 69 : res.success = false;
+ 5458 69 : return true;
+ 5459 : }
+ 5460 46 :
+ 5461 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackGotoRelative");
+ 5462 23 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackGotoRelative", scope_timer_logger_, scope_timer_enabled_);
+ 5463 0 :
+ 5464 0 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 5465 0 :
+ 5466 : if (!last_tracker_cmd) {
+ 5467 : res.message = "not flying";
+ 5468 46 : res.success = false;
+ 5469 23 : return true;
+ 5470 23 : }
+ 5471 23 :
+ 5472 23 : mrs_msgs::ReferenceStamped des_reference;
+ 5473 23 : des_reference.header.frame_id = "";
+ 5474 23 : des_reference.header.stamp = ros::Time(0);
+ 5475 : des_reference.reference.position.x = last_tracker_cmd->position.x + req.goal.at(REF_X);
+ 5476 46 : des_reference.reference.position.y = last_tracker_cmd->position.y + req.goal.at(REF_Y);
+ 5477 : des_reference.reference.position.z = last_tracker_cmd->position.z + req.goal.at(REF_Z);
+ 5478 23 : des_reference.reference.heading = last_tracker_cmd->heading + req.goal.at(REF_HEADING);
+ 5479 23 :
+ 5480 : auto [success, message] = setReference(des_reference);
+ 5481 23 :
+ 5482 : res.success = success;
+ 5483 : res.message = message;
+ 5484 :
+ 5485 : return true;
+ 5486 : }
+ 5487 :
+ 5488 0 : //}
+ 5489 :
+ 5490 0 : /* //{ callbackGotoAltitude() */
+ 5491 0 :
+ 5492 0 : bool ControlManager::callbackGotoAltitude(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+ 5493 0 :
+ 5494 : if (!is_initialized_) {
+ 5495 : res.message = "not initialized";
+ 5496 0 : res.success = false;
+ 5497 0 : return true;
+ 5498 : }
+ 5499 0 :
+ 5500 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackGotoAltitude");
+ 5501 0 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackGotoAltitude", scope_timer_logger_, scope_timer_enabled_);
+ 5502 0 :
+ 5503 0 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 5504 0 :
+ 5505 : if (!last_tracker_cmd) {
+ 5506 : res.message = "not flying";
+ 5507 0 : res.success = false;
+ 5508 0 : return true;
+ 5509 0 : }
+ 5510 0 :
+ 5511 0 : mrs_msgs::ReferenceStamped des_reference;
+ 5512 0 : des_reference.header.frame_id = "";
+ 5513 0 : des_reference.header.stamp = ros::Time(0);
+ 5514 : des_reference.reference.position.x = last_tracker_cmd->position.x;
+ 5515 0 : des_reference.reference.position.y = last_tracker_cmd->position.y;
+ 5516 : des_reference.reference.position.z = req.goal;
+ 5517 0 : des_reference.reference.heading = last_tracker_cmd->heading;
+ 5518 0 :
+ 5519 : auto [success, message] = setReference(des_reference);
+ 5520 0 :
+ 5521 : res.success = success;
+ 5522 : res.message = message;
+ 5523 :
+ 5524 : return true;
+ 5525 : }
+ 5526 :
+ 5527 0 : //}
+ 5528 :
+ 5529 0 : /* //{ callbackSetHeading() */
+ 5530 0 :
+ 5531 0 : bool ControlManager::callbackSetHeading(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+ 5532 0 :
+ 5533 : if (!is_initialized_) {
+ 5534 : res.message = "not initialized";
+ 5535 0 : res.success = false;
+ 5536 0 : return true;
+ 5537 : }
+ 5538 0 :
+ 5539 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackSetHeading");
+ 5540 0 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackSetHeading", scope_timer_logger_, scope_timer_enabled_);
+ 5541 0 :
+ 5542 0 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 5543 0 :
+ 5544 : if (!last_tracker_cmd) {
+ 5545 : res.message = "not flying";
+ 5546 0 : res.success = false;
+ 5547 0 : return true;
+ 5548 0 : }
+ 5549 0 :
+ 5550 0 : mrs_msgs::ReferenceStamped des_reference;
+ 5551 0 : des_reference.header.frame_id = "";
+ 5552 0 : des_reference.header.stamp = ros::Time(0);
+ 5553 : des_reference.reference.position.x = last_tracker_cmd->position.x;
+ 5554 0 : des_reference.reference.position.y = last_tracker_cmd->position.y;
+ 5555 : des_reference.reference.position.z = last_tracker_cmd->position.z;
+ 5556 0 : des_reference.reference.heading = req.goal;
+ 5557 0 :
+ 5558 : auto [success, message] = setReference(des_reference);
+ 5559 0 :
+ 5560 : res.success = success;
+ 5561 : res.message = message;
+ 5562 :
+ 5563 : return true;
+ 5564 : }
+ 5565 :
+ 5566 0 : //}
+ 5567 :
+ 5568 0 : /* //{ callbackSetHeadingRelative() */
+ 5569 0 :
+ 5570 0 : bool ControlManager::callbackSetHeadingRelative(mrs_msgs::Vec1::Request& req, mrs_msgs::Vec1::Response& res) {
+ 5571 0 :
+ 5572 : if (!is_initialized_) {
+ 5573 : res.message = "not initialized";
+ 5574 0 : res.success = false;
+ 5575 0 : return true;
+ 5576 : }
+ 5577 0 :
+ 5578 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("callbackSetHeadingRelative");
+ 5579 0 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::callbackSetHeadingRelative", scope_timer_logger_, scope_timer_enabled_);
+ 5580 0 :
+ 5581 0 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 5582 0 :
+ 5583 : if (!last_tracker_cmd) {
+ 5584 : res.message = "not flying";
+ 5585 0 : res.success = false;
+ 5586 0 : return true;
+ 5587 0 : }
+ 5588 0 :
+ 5589 0 : mrs_msgs::ReferenceStamped des_reference;
+ 5590 0 : des_reference.header.frame_id = "";
+ 5591 0 : des_reference.header.stamp = ros::Time(0);
+ 5592 : des_reference.reference.position.x = last_tracker_cmd->position.x;
+ 5593 0 : des_reference.reference.position.y = last_tracker_cmd->position.y;
+ 5594 : des_reference.reference.position.z = last_tracker_cmd->position.z;
+ 5595 0 : des_reference.reference.heading = last_tracker_cmd->heading + req.goal;
+ 5596 0 :
+ 5597 : auto [success, message] = setReference(des_reference);
+ 5598 0 :
+ 5599 : res.success = success;
+ 5600 : res.message = message;
+ 5601 :
+ 5602 : return true;
+ 5603 : }
+ 5604 :
+ 5605 : //}
+ 5606 :
+ 5607 : // --------------------------------------------------------------
+ 5608 : // | routines |
+ 5609 49 : // --------------------------------------------------------------
+ 5610 :
+ 5611 98 : /* setReference() //{ */
+ 5612 :
+ 5613 49 : std::tuple<bool, std::string> ControlManager::setReference(const mrs_msgs::ReferenceStamped reference_in) {
+ 5614 0 :
+ 5615 0 : std::stringstream ss;
+ 5616 0 :
+ 5617 : if (!callbacks_enabled_) {
+ 5618 : ss << "can not set the reference, the callbacks are disabled";
+ 5619 49 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5620 0 : return std::tuple(false, ss.str());
+ 5621 0 : }
+ 5622 0 :
+ 5623 : if (!validateReference(reference_in.reference, "ControlManager", "reference")) {
+ 5624 : ss << "incoming reference is not finite!!!";
+ 5625 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5626 98 : return std::tuple(false, ss.str());
+ 5627 98 : }
+ 5628 :
+ 5629 : // copy member variables
+ 5630 98 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 5631 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 5632 49 :
+ 5633 : // transform the reference to the current frame
+ 5634 0 : auto ret = transformer_->transformSingle(reference_in, uav_state.header.frame_id);
+ 5635 0 :
+ 5636 0 : if (!ret) {
+ 5637 :
+ 5638 : ss << "the reference could not be transformed";
+ 5639 98 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5640 : return std::tuple(false, ss.str());
+ 5641 : }
+ 5642 49 :
+ 5643 0 : mrs_msgs::ReferenceStamped transformed_reference = ret.value();
+ 5644 0 :
+ 5645 0 : // safety area check
+ 5646 : if (!isPointInSafetyArea3d(transformed_reference)) {
+ 5647 : ss << "failed to set the reference, the point is outside of the safety area!";
+ 5648 49 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5649 : return std::tuple(false, ss.str());
+ 5650 49 : }
+ 5651 49 :
+ 5652 49 : if (last_tracker_cmd) {
+ 5653 49 :
+ 5654 49 : mrs_msgs::ReferenceStamped from_point;
+ 5655 : from_point.header.frame_id = uav_state.header.frame_id;
+ 5656 49 : from_point.reference.position.x = last_tracker_cmd->position.x;
+ 5657 0 : from_point.reference.position.y = last_tracker_cmd->position.y;
+ 5658 0 : from_point.reference.position.z = last_tracker_cmd->position.z;
+ 5659 0 :
+ 5660 : if (!isPathToPointInSafetyArea3d(from_point, transformed_reference)) {
+ 5661 : ss << "failed to set the reference, the path is going outside the safety area!";
+ 5662 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5663 49 : return std::tuple(false, ss.str());
+ 5664 : }
+ 5665 : }
+ 5666 49 :
+ 5667 49 : mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+ 5668 :
+ 5669 : // prepare the message for current tracker
+ 5670 98 : mrs_msgs::ReferenceSrvRequest reference_request;
+ 5671 : reference_request.reference = transformed_reference.reference;
+ 5672 147 :
+ 5673 147 : {
+ 5674 : std::scoped_lock lock(mutex_tracker_list_);
+ 5675 49 :
+ 5676 : tracker_response = tracker_list_.at(active_tracker_idx_)
+ 5677 98 : ->setReference(mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(reference_request)));
+ 5678 :
+ 5679 : if (tracker_response != mrs_msgs::ReferenceSrvResponse::Ptr()) {
+ 5680 :
+ 5681 0 : return std::tuple(tracker_response->success, tracker_response->message);
+ 5682 0 :
+ 5683 0 : } else {
+ 5684 :
+ 5685 : ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setReference()' function!";
+ 5686 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the reference: " << ss.str());
+ 5687 : return std::tuple(false, ss.str());
+ 5688 : }
+ 5689 : }
+ 5690 : }
+ 5691 :
+ 5692 472 : //}
+ 5693 :
+ 5694 944 : /* setVelocityReference() //{ */
+ 5695 :
+ 5696 472 : std::tuple<bool, std::string> ControlManager::setVelocityReference(const mrs_msgs::VelocityReferenceStamped& reference_in) {
+ 5697 0 :
+ 5698 0 : std::stringstream ss;
+ 5699 0 :
+ 5700 : if (!callbacks_enabled_) {
+ 5701 : ss << "can not set the reference, the callbacks are disabled";
+ 5702 472 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5703 0 : return std::tuple(false, ss.str());
+ 5704 0 : }
+ 5705 0 :
+ 5706 : if (!validateVelocityReference(reference_in.reference, "ControlManager", "velocity_reference")) {
+ 5707 : ss << "velocity command is not valid!";
+ 5708 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5709 472 : return std::tuple(false, ss.str());
+ 5710 : }
+ 5711 472 :
+ 5712 0 : {
+ 5713 0 : std::scoped_lock lock(mutex_last_tracker_cmd_);
+ 5714 0 :
+ 5715 : if (!last_tracker_cmd_) {
+ 5716 : ss << "could not set velocity command, not flying!";
+ 5717 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5718 : return std::tuple(false, ss.str());
+ 5719 944 : }
+ 5720 944 : }
+ 5721 :
+ 5722 : // copy member variables
+ 5723 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 5724 944 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 5725 :
+ 5726 944 : // | -- transform the velocity reference to the current frame - |
+ 5727 :
+ 5728 944 : mrs_msgs::VelocityReferenceStamped transformed_reference = reference_in;
+ 5729 :
+ 5730 472 : auto ret = transformer_->getTransform(reference_in.header.frame_id, uav_state.header.frame_id, reference_in.header.stamp);
+ 5731 0 :
+ 5732 0 : geometry_msgs::TransformStamped tf;
+ 5733 0 :
+ 5734 : if (!ret) {
+ 5735 472 : ss << "could not find tf from " << reference_in.header.frame_id << " to " << uav_state.header.frame_id;
+ 5736 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5737 : return std::tuple(false, ss.str());
+ 5738 : } else {
+ 5739 : tf = ret.value();
+ 5740 472 : }
+ 5741 472 :
+ 5742 472 : // transform the velocity
+ 5743 472 : {
+ 5744 472 : geometry_msgs::Vector3Stamped velocity;
+ 5745 : velocity.header = reference_in.header;
+ 5746 472 : velocity.vector.x = reference_in.reference.velocity.x;
+ 5747 : velocity.vector.y = reference_in.reference.velocity.y;
+ 5748 472 : velocity.vector.z = reference_in.reference.velocity.z;
+ 5749 :
+ 5750 0 : auto ret = transformer_->transform(velocity, tf);
+ 5751 0 :
+ 5752 0 : if (!ret) {
+ 5753 :
+ 5754 : ss << "the velocity reference could not be transformed";
+ 5755 472 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5756 472 : return std::tuple(false, ss.str());
+ 5757 472 :
+ 5758 : } else {
+ 5759 : transformed_reference.reference.velocity.x = ret->vector.x;
+ 5760 : transformed_reference.reference.velocity.y = ret->vector.y;
+ 5761 : transformed_reference.reference.velocity.z = ret->vector.z;
+ 5762 : }
+ 5763 472 : }
+ 5764 472 :
+ 5765 472 : // transform the z and the heading
+ 5766 472 : {
+ 5767 472 : geometry_msgs::PoseStamped pose;
+ 5768 472 : pose.header = reference_in.header;
+ 5769 : pose.pose.position.x = 0;
+ 5770 472 : pose.pose.position.y = 0;
+ 5771 : pose.pose.position.z = reference_in.reference.altitude;
+ 5772 472 : pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, reference_in.reference.heading);
+ 5773 :
+ 5774 0 : auto ret = transformer_->transform(pose, tf);
+ 5775 0 :
+ 5776 0 : if (!ret) {
+ 5777 :
+ 5778 : ss << "the velocity reference could not be transformed";
+ 5779 472 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5780 472 : return std::tuple(false, ss.str());
+ 5781 :
+ 5782 : } else {
+ 5783 : transformed_reference.reference.altitude = ret->pose.position.z;
+ 5784 : transformed_reference.reference.heading = mrs_lib::AttitudeConverter(ret->pose.orientation).getHeading();
+ 5785 472 : }
+ 5786 : }
+ 5787 472 :
+ 5788 472 : // the heading rate doees not need to be transformed
+ 5789 : transformed_reference.reference.heading_rate = reference_in.reference.heading_rate;
+ 5790 944 :
+ 5791 : transformed_reference.header.stamp = tf.header.stamp;
+ 5792 472 : transformed_reference.header.frame_id = transformer_->frame_to(tf);
+ 5793 :
+ 5794 : mrs_msgs::ReferenceStamped eqivalent_reference = velocityReferenceToReference(transformed_reference);
+ 5795 :
+ 5796 472 : ROS_DEBUG("[ControlManager]: equivalent reference: %.2f, %.2f, %.2f, %.2f", eqivalent_reference.reference.position.x,
+ 5797 0 : eqivalent_reference.reference.position.y, eqivalent_reference.reference.position.z, eqivalent_reference.reference.heading);
+ 5798 0 :
+ 5799 0 : // safety area check
+ 5800 : if (!isPointInSafetyArea3d(eqivalent_reference)) {
+ 5801 : ss << "failed to set the reference, the point is outside of the safety area!";
+ 5802 472 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5803 : return std::tuple(false, ss.str());
+ 5804 472 : }
+ 5805 472 :
+ 5806 472 : if (last_tracker_cmd) {
+ 5807 472 :
+ 5808 472 : mrs_msgs::ReferenceStamped from_point;
+ 5809 : from_point.header.frame_id = uav_state.header.frame_id;
+ 5810 472 : from_point.reference.position.x = last_tracker_cmd->position.x;
+ 5811 0 : from_point.reference.position.y = last_tracker_cmd->position.y;
+ 5812 0 : from_point.reference.position.z = last_tracker_cmd->position.z;
+ 5813 0 :
+ 5814 : if (!isPathToPointInSafetyArea3d(from_point, eqivalent_reference)) {
+ 5815 : ss << "failed to set the reference, the path is going outside the safety area!";
+ 5816 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5817 472 : return std::tuple(false, ss.str());
+ 5818 : }
+ 5819 : }
+ 5820 472 :
+ 5821 472 : mrs_msgs::VelocityReferenceSrvResponse::ConstPtr tracker_response;
+ 5822 :
+ 5823 : // prepare the message for current tracker
+ 5824 944 : mrs_msgs::VelocityReferenceSrvRequest reference_request;
+ 5825 : reference_request.reference = transformed_reference.reference;
+ 5826 1416 :
+ 5827 1416 : {
+ 5828 : std::scoped_lock lock(mutex_tracker_list_);
+ 5829 472 :
+ 5830 : tracker_response =
+ 5831 944 : tracker_list_.at(active_tracker_idx_)
+ 5832 : ->setVelocityReference(mrs_msgs::VelocityReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::VelocityReferenceSrvRequest>(reference_request)));
+ 5833 :
+ 5834 : if (tracker_response != mrs_msgs::VelocityReferenceSrvResponse::Ptr()) {
+ 5835 0 :
+ 5836 0 : return std::tuple(tracker_response->success, tracker_response->message);
+ 5837 0 :
+ 5838 : } else {
+ 5839 :
+ 5840 : ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setVelocityReference()' function!";
+ 5841 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: failed to set the velocity reference: " << ss.str());
+ 5842 : return std::tuple(false, ss.str());
+ 5843 : }
+ 5844 : }
+ 5845 : }
+ 5846 4 :
+ 5847 : //}
+ 5848 :
+ 5849 8 : /* setTrajectoryReference() //{ */
+ 5850 :
+ 5851 8 : std::tuple<bool, std::string, bool, std::vector<std::string>, std::vector<bool>, std::vector<std::string>> ControlManager::setTrajectoryReference(
+ 5852 : const mrs_msgs::TrajectoryReference trajectory_in) {
+ 5853 4 :
+ 5854 0 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 5855 0 :
+ 5856 0 : std::stringstream ss;
+ 5857 :
+ 5858 : if (!callbacks_enabled_) {
+ 5859 : ss << "can not set the reference, the callbacks are disabled";
+ 5860 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5861 : return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+ 5862 4 : }
+ 5863 :
+ 5864 0 : /* validate the size and check for NaNs //{ */
+ 5865 0 :
+ 5866 0 : // check for the size 0, which is invalid
+ 5867 : if (trajectory_in.points.size() == 0) {
+ 5868 :
+ 5869 584 : ss << "can not load trajectory with size 0";
+ 5870 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5871 : return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+ 5872 580 : }
+ 5873 :
+ 5874 580 : for (int i = 0; i < int(trajectory_in.points.size()); i++) {
+ 5875 :
+ 5876 0 : // check the point for NaN/inf
+ 5877 0 : bool valid = validateReference(trajectory_in.points.at(i), "ControlManager", "trajectory_in.points");
+ 5878 0 :
+ 5879 : if (!valid) {
+ 5880 :
+ 5881 : ss << "trajectory contains NaNs/infs.";
+ 5882 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 5883 : return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+ 5884 : }
+ 5885 : }
+ 5886 :
+ 5887 : //}
+ 5888 8 :
+ 5889 4 : /* publish the debugging topics of the original trajectory //{ */
+ 5890 :
+ 5891 4 : {
+ 5892 :
+ 5893 4 : geometry_msgs::PoseArray debug_trajectory_out;
+ 5894 2 : debug_trajectory_out.header = trajectory_in.header;
+ 5895 :
+ 5896 : debug_trajectory_out.header.frame_id = transformer_->resolveFrame(debug_trajectory_out.header.frame_id);
+ 5897 580 :
+ 5898 : if (debug_trajectory_out.header.stamp == ros::Time(0)) {
+ 5899 576 : debug_trajectory_out.header.stamp = ros::Time::now();
+ 5900 : }
+ 5901 576 :
+ 5902 576 : for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+ 5903 576 :
+ 5904 : geometry_msgs::Pose new_pose;
+ 5905 576 :
+ 5906 : new_pose.position.x = trajectory_in.points.at(i).position.x;
+ 5907 576 : new_pose.position.y = trajectory_in.points.at(i).position.y;
+ 5908 : new_pose.position.z = trajectory_in.points.at(i).position.z;
+ 5909 :
+ 5910 4 : new_pose.orientation = mrs_lib::AttitudeConverter(0, 0, trajectory_in.points.at(i).heading);
+ 5911 :
+ 5912 8 : debug_trajectory_out.poses.push_back(new_pose);
+ 5913 : }
+ 5914 8 :
+ 5915 : pub_debug_original_trajectory_poses_.publish(debug_trajectory_out);
+ 5916 4 :
+ 5917 : visualization_msgs::MarkerArray msg_out;
+ 5918 4 :
+ 5919 : visualization_msgs::Marker marker;
+ 5920 4 :
+ 5921 0 : marker.header = trajectory_in.header;
+ 5922 :
+ 5923 : marker.header.frame_id = transformer_->resolveFrame(marker.header.frame_id);
+ 5924 4 :
+ 5925 2 : if (marker.header.frame_id == "") {
+ 5926 : marker.header.frame_id = uav_state.header.frame_id;
+ 5927 : }
+ 5928 4 :
+ 5929 4 : if (marker.header.stamp == ros::Time(0)) {
+ 5930 4 : marker.header.stamp = ros::Time::now();
+ 5931 4 : }
+ 5932 4 :
+ 5933 4 : marker.type = visualization_msgs::Marker::LINE_LIST;
+ 5934 4 : marker.color.a = 1;
+ 5935 : marker.scale.x = 0.05;
+ 5936 580 : marker.color.r = 0;
+ 5937 : marker.color.g = 1;
+ 5938 576 : marker.color.b = 0;
+ 5939 : marker.pose.orientation = mrs_lib::AttitudeConverter(0, 0, 0);
+ 5940 576 :
+ 5941 576 : for (int i = 0; i < int(trajectory_in.points.size()) - 1; i++) {
+ 5942 576 :
+ 5943 : geometry_msgs::Point point1;
+ 5944 576 :
+ 5945 : point1.x = trajectory_in.points.at(i).position.x;
+ 5946 576 : point1.y = trajectory_in.points.at(i).position.y;
+ 5947 : point1.z = trajectory_in.points.at(i).position.z;
+ 5948 576 :
+ 5949 576 : marker.points.push_back(point1);
+ 5950 576 :
+ 5951 : geometry_msgs::Point point2;
+ 5952 576 :
+ 5953 : point2.x = trajectory_in.points.at(i + 1).position.x;
+ 5954 : point2.y = trajectory_in.points.at(i + 1).position.y;
+ 5955 4 : point2.z = trajectory_in.points.at(i + 1).position.z;
+ 5956 :
+ 5957 4 : marker.points.push_back(point2);
+ 5958 : }
+ 5959 :
+ 5960 : msg_out.markers.push_back(marker);
+ 5961 :
+ 5962 8 : pub_debug_original_trajectory_markers_.publish(msg_out);
+ 5963 : }
+ 5964 4 :
+ 5965 : //}
+ 5966 4 :
+ 5967 : mrs_msgs::TrajectoryReference processed_trajectory = trajectory_in;
+ 5968 :
+ 5969 : int trajectory_size = int(processed_trajectory.points.size());
+ 5970 4 :
+ 5971 : bool trajectory_modified = false;
+ 5972 4 :
+ 5973 4 : /* safety area check //{ */
+ 5974 :
+ 5975 4 : if (use_safety_area_) {
+ 5976 4 :
+ 5977 : int last_valid_idx = 0;
+ 5978 584 : int first_invalid_idx = -1;
+ 5979 :
+ 5980 580 : double min_z = getMinZ(processed_trajectory.header.frame_id);
+ 5981 : double max_z = getMaxZ(processed_trajectory.header.frame_id);
+ 5982 :
+ 5983 0 : for (int i = 0; i < trajectory_size; i++) {
+ 5984 :
+ 5985 0 : if (_snap_trajectory_to_safety_area_) {
+ 5986 0 :
+ 5987 0 : // saturate the trajectory to min and max Z
+ 5988 : if (processed_trajectory.points.at(i).position.z < min_z) {
+ 5989 :
+ 5990 0 : processed_trajectory.points.at(i).position.z = min_z;
+ 5991 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the minimum Z!");
+ 5992 0 : trajectory_modified = true;
+ 5993 0 : }
+ 5994 0 :
+ 5995 : if (processed_trajectory.points.at(i).position.z > max_z) {
+ 5996 :
+ 5997 : processed_trajectory.points.at(i).position.z = max_z;
+ 5998 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory violates the maximum Z!");
+ 5999 580 : trajectory_modified = true;
+ 6000 580 : }
+ 6001 580 : }
+ 6002 :
+ 6003 580 : // check the point against the safety area
+ 6004 : mrs_msgs::ReferenceStamped des_reference;
+ 6005 0 : des_reference.header = processed_trajectory.header;
+ 6006 0 : des_reference.reference = processed_trajectory.points.at(i);
+ 6007 :
+ 6008 : if (!isPointInSafetyArea3d(des_reference)) {
+ 6009 0 :
+ 6010 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: the trajectory contains points outside of the safety area!");
+ 6011 0 : trajectory_modified = true;
+ 6012 :
+ 6013 0 : // the first invalid point
+ 6014 : if (first_invalid_idx == -1) {
+ 6015 :
+ 6016 : first_invalid_idx = i;
+ 6017 :
+ 6018 : last_valid_idx = i - 1;
+ 6019 : }
+ 6020 580 :
+ 6021 : // the point is ok
+ 6022 : } else {
+ 6023 0 :
+ 6024 : // we found a point, which is ok, after finding a point which was not ok
+ 6025 0 : if (first_invalid_idx != -1) {
+ 6026 0 :
+ 6027 0 : // special case, we had no valid point so far
+ 6028 : if (last_valid_idx == -1) {
+ 6029 :
+ 6030 : ss << "the trajectory starts outside of the safety area!";
+ 6031 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 6032 0 : return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+ 6033 0 :
+ 6034 : // we have a valid point in the past
+ 6035 : } else {
+ 6036 0 :
+ 6037 : if (!_snap_trajectory_to_safety_area_) {
+ 6038 : break;
+ 6039 0 : }
+ 6040 0 :
+ 6041 : bool interpolation_success = true;
+ 6042 :
+ 6043 0 : // iterpolate between the last valid point and this new valid point
+ 6044 0 : double angle = atan2((processed_trajectory.points.at(i).position.y - processed_trajectory.points.at(last_valid_idx).position.y),
+ 6045 0 : (processed_trajectory.points.at(i).position.x - processed_trajectory.points.at(last_valid_idx).position.x));
+ 6046 :
+ 6047 0 : double dist_two_points = mrs_lib::geometry::dist(
+ 6048 : vec2_t(processed_trajectory.points.at(i).position.x, processed_trajectory.points.at(i).position.y),
+ 6049 0 : vec2_t(processed_trajectory.points.at(last_valid_idx).position.x, processed_trajectory.points.at(last_valid_idx).position.y));
+ 6050 0 : double step = dist_two_points / (i - last_valid_idx);
+ 6051 0 :
+ 6052 0 : for (int j = last_valid_idx; j < i; j++) {
+ 6053 :
+ 6054 0 : mrs_msgs::ReferenceStamped temp_point;
+ 6055 : temp_point.header.frame_id = processed_trajectory.header.frame_id;
+ 6056 0 : temp_point.reference.position.x = processed_trajectory.points.at(last_valid_idx).position.x + (j - last_valid_idx) * cos(angle) * step;
+ 6057 0 : temp_point.reference.position.y = processed_trajectory.points.at(last_valid_idx).position.y + (j - last_valid_idx) * sin(angle) * step;
+ 6058 :
+ 6059 : if (!isPointInSafetyArea2d(temp_point)) {
+ 6060 :
+ 6061 0 : interpolation_success = false;
+ 6062 0 : break;
+ 6063 :
+ 6064 : } else {
+ 6065 :
+ 6066 0 : processed_trajectory.points.at(j).position.x = temp_point.reference.position.x;
+ 6067 0 : processed_trajectory.points.at(j).position.y = temp_point.reference.position.y;
+ 6068 : }
+ 6069 : }
+ 6070 :
+ 6071 0 : if (!interpolation_success) {
+ 6072 : break;
+ 6073 : }
+ 6074 : }
+ 6075 :
+ 6076 : first_invalid_idx = -1;
+ 6077 4 : }
+ 6078 : }
+ 6079 : }
+ 6080 0 :
+ 6081 : // special case, the trajectory does not end with a valid point
+ 6082 0 : if (first_invalid_idx != -1) {
+ 6083 0 :
+ 6084 0 : // super special case, the whole trajectory is invalid
+ 6085 : if (first_invalid_idx == 0) {
+ 6086 :
+ 6087 : ss << "the whole trajectory is outside of the safety area!";
+ 6088 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 6089 0 : return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+ 6090 0 :
+ 6091 0 : // there is a good portion of the trajectory in the beginning
+ 6092 : } else {
+ 6093 :
+ 6094 : trajectory_size = last_valid_idx + 1;
+ 6095 : processed_trajectory.points.resize(trajectory_size);
+ 6096 4 : trajectory_modified = true;
+ 6097 : }
+ 6098 0 : }
+ 6099 0 : }
+ 6100 0 :
+ 6101 : if (trajectory_size == 0) {
+ 6102 :
+ 6103 : ss << "the trajectory happened to be empty after all the checks! This message should not appear!";
+ 6104 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 6105 : return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+ 6106 : }
+ 6107 4 :
+ 6108 : //}
+ 6109 4 :
+ 6110 0 : /* transform the trajectory to the current control frame //{ */
+ 6111 :
+ 6112 4 : std::optional<geometry_msgs::TransformStamped> tf_traj_state;
+ 6113 :
+ 6114 : if (processed_trajectory.header.stamp > ros::Time::now()) {
+ 6115 4 : tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", processed_trajectory.header.stamp);
+ 6116 0 : } else {
+ 6117 0 : tf_traj_state = transformer_->getTransform(processed_trajectory.header.frame_id, "", uav_state_.header.stamp);
+ 6118 0 : }
+ 6119 :
+ 6120 : if (!tf_traj_state) {
+ 6121 4 : ss << "could not create TF transformer for the trajectory";
+ 6122 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 6123 584 : return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+ 6124 : }
+ 6125 580 :
+ 6126 580 : processed_trajectory.header.frame_id = transformer_->frame_to(*tf_traj_state);
+ 6127 580 :
+ 6128 : for (int i = 0; i < trajectory_size; i++) {
+ 6129 580 :
+ 6130 : mrs_msgs::ReferenceStamped trajectory_point;
+ 6131 580 : trajectory_point.header = processed_trajectory.header;
+ 6132 : trajectory_point.reference = processed_trajectory.points.at(i);
+ 6133 0 :
+ 6134 0 : auto ret = transformer_->transform(trajectory_point, *tf_traj_state);
+ 6135 0 :
+ 6136 : if (!ret) {
+ 6137 :
+ 6138 : ss << "trajectory cannnot be transformed";
+ 6139 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 6140 580 : return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+ 6141 :
+ 6142 : } else {
+ 6143 :
+ 6144 : // transform the points in the trajectory to the current frame
+ 6145 : processed_trajectory.points.at(i) = ret.value().reference;
+ 6146 4 : }
+ 6147 8 : }
+ 6148 :
+ 6149 : //}
+ 6150 4 :
+ 6151 0 : mrs_msgs::TrajectoryReferenceSrvResponse::ConstPtr response;
+ 6152 0 : mrs_msgs::TrajectoryReferenceSrvRequest request;
+ 6153 0 :
+ 6154 : // check for empty trajectory
+ 6155 : if (processed_trajectory.points.size() == 0) {
+ 6156 : ss << "reference trajectory was processing and it is now empty, this should not happen!";
+ 6157 4 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 6158 : return std::tuple(false, ss.str(), false, std::vector<std::string>(), std::vector<bool>(), std::vector<std::string>());
+ 6159 : }
+ 6160 8 :
+ 6161 : // prepare the message for current tracker
+ 6162 8 : request.trajectory = processed_trajectory;
+ 6163 8 :
+ 6164 8 : bool success;
+ 6165 : std::string message;
+ 6166 : bool modified;
+ 6167 8 : std::vector<std::string> tracker_names;
+ 6168 : std::vector<bool> tracker_successes;
+ 6169 : std::vector<std::string> tracker_messages;
+ 6170 12 :
+ 6171 12 : {
+ 6172 : std::scoped_lock lock(mutex_tracker_list_);
+ 6173 4 :
+ 6174 : // set the trajectory to the currently active tracker
+ 6175 4 : response =
+ 6176 : tracker_list_.at(active_tracker_idx_)
+ 6177 3 : ->setTrajectoryReference(mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+ 6178 3 :
+ 6179 3 : tracker_names.push_back(_tracker_names_.at(active_tracker_idx_));
+ 6180 3 :
+ 6181 3 : if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+ 6182 :
+ 6183 : success = response->success;
+ 6184 : message = response->message;
+ 6185 1 : modified = response->modified || trajectory_modified;
+ 6186 1 : tracker_successes.push_back(response->success);
+ 6187 : tracker_messages.push_back(response->message);
+ 6188 1 :
+ 6189 1 : } else {
+ 6190 1 :
+ 6191 1 : ss << "the active tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'setTrajectoryReference()' function!";
+ 6192 1 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 6193 :
+ 6194 : success = true;
+ 6195 : message = ss.str();
+ 6196 28 : modified = false;
+ 6197 : tracker_successes.push_back(false);
+ 6198 24 : tracker_messages.push_back(ss.str());
+ 6199 : }
+ 6200 20 :
+ 6201 : // set the trajectory to the non-active trackers
+ 6202 60 : for (size_t i = 0; i < tracker_list_.size(); i++) {
+ 6203 60 :
+ 6204 : if (i != active_tracker_idx_) {
+ 6205 20 :
+ 6206 : tracker_names.push_back(_tracker_names_.at(i));
+ 6207 1 :
+ 6208 1 : response = tracker_list_.at(i)->setTrajectoryReference(
+ 6209 : mrs_msgs::TrajectoryReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::TrajectoryReferenceSrvRequest>(request)));
+ 6210 1 :
+ 6211 2 : if (response != mrs_msgs::TrajectoryReferenceSrvResponse::Ptr()) {
+ 6212 1 :
+ 6213 1 : tracker_successes.push_back(response->success);
+ 6214 : tracker_messages.push_back(response->message);
+ 6215 :
+ 6216 : if (response->success) {
+ 6217 : std::stringstream ss;
+ 6218 19 : ss << "trajectory loaded to non-active tracker '" << _tracker_names_.at(i);
+ 6219 19 : ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 6220 19 : }
+ 6221 19 :
+ 6222 : } else {
+ 6223 :
+ 6224 : std::stringstream ss;
+ 6225 : ss << "the tracker \"" << _tracker_names_.at(i) << "\" does not implement setTrajectoryReference()";
+ 6226 : tracker_successes.push_back(false);
+ 6227 4 : tracker_messages.push_back(ss.str());
+ 6228 : }
+ 6229 : }
+ 6230 : }
+ 6231 : }
+ 6232 :
+ 6233 : return std::tuple(success, message, modified, tracker_names, tracker_successes, tracker_messages);
+ 6234 18 : }
+ 6235 :
+ 6236 18 : //}
+ 6237 0 :
+ 6238 : /* isOffboard() //{ */
+ 6239 :
+ 6240 18 : bool ControlManager::isOffboard(void) {
+ 6241 :
+ 6242 18 : if (!sh_hw_api_status_.hasMsg()) {
+ 6243 : return false;
+ 6244 : }
+ 6245 :
+ 6246 : mrs_msgs::HwApiStatusConstPtr hw_api_status = sh_hw_api_status_.getMsg();
+ 6247 :
+ 6248 : return hw_api_status->connected && hw_api_status->offboard;
+ 6249 93 : }
+ 6250 :
+ 6251 93 : //}
+ 6252 :
+ 6253 93 : /* setCallbacks() //{ */
+ 6254 93 :
+ 6255 : void ControlManager::setCallbacks(bool in) {
+ 6256 :
+ 6257 186 : callbacks_enabled_ = in;
+ 6258 :
+ 6259 : std_srvs::SetBoolRequest req_enable_callbacks;
+ 6260 651 : req_enable_callbacks.data = callbacks_enabled_;
+ 6261 558 :
+ 6262 : {
+ 6263 : std::scoped_lock lock(mutex_tracker_list_);
+ 6264 93 :
+ 6265 : // set callbacks to all trackers
+ 6266 : for (int i = 0; i < int(tracker_list_.size()); i++) {
+ 6267 : tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 6268 : }
+ 6269 : }
+ 6270 16471 : }
+ 6271 :
+ 6272 16471 : //}
+ 6273 0 :
+ 6274 : /* publishDiagnostics() //{ */
+ 6275 :
+ 6276 49413 : void ControlManager::publishDiagnostics(void) {
+ 6277 49413 :
+ 6278 : if (!is_initialized_) {
+ 6279 32942 : return;
+ 6280 : }
+ 6281 32942 :
+ 6282 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("publishDiagnostics");
+ 6283 16471 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::publishDiagnostics", scope_timer_logger_, scope_timer_enabled_);
+ 6284 16471 :
+ 6285 : std::scoped_lock lock(mutex_diagnostics_);
+ 6286 16471 :
+ 6287 : mrs_msgs::ControlManagerDiagnostics diagnostics_msg;
+ 6288 16471 :
+ 6289 : diagnostics_msg.stamp = ros::Time::now();
+ 6290 16471 : diagnostics_msg.uav_name = _uav_name_;
+ 6291 :
+ 6292 : diagnostics_msg.desired_uav_state_rate = desired_uav_state_rate_;
+ 6293 16471 :
+ 6294 : diagnostics_msg.output_enabled = output_enabled_;
+ 6295 16471 :
+ 6296 : diagnostics_msg.joystick_active = rc_goto_active_;
+ 6297 :
+ 6298 16471 : {
+ 6299 : std::scoped_lock lock(mutex_tracker_list_, mutex_controller_list_);
+ 6300 :
+ 6301 : diagnostics_msg.flying_normally = isFlyingNormally();
+ 6302 : }
+ 6303 32942 :
+ 6304 : diagnostics_msg.bumper_active = bumper_repulsing_;
+ 6305 16471 :
+ 6306 : // | ----------------- fill the tracker status ---------------- |
+ 6307 16471 :
+ 6308 16471 : {
+ 6309 : std::scoped_lock lock(mutex_tracker_list_);
+ 6310 :
+ 6311 : mrs_msgs::TrackerStatus tracker_status;
+ 6312 :
+ 6313 : diagnostics_msg.active_tracker = _tracker_names_.at(active_tracker_idx_);
+ 6314 32942 : diagnostics_msg.tracker_status = tracker_list_.at(active_tracker_idx_)->getStatus();
+ 6315 : }
+ 6316 16471 :
+ 6317 : // | --------------- fill the controller status --------------- |
+ 6318 16471 :
+ 6319 16471 : {
+ 6320 : std::scoped_lock lock(mutex_controller_list_);
+ 6321 :
+ 6322 : mrs_msgs::ControllerStatus controller_status;
+ 6323 :
+ 6324 98826 : diagnostics_msg.active_controller = _controller_names_.at(active_controller_idx_);
+ 6325 82355 : diagnostics_msg.controller_status = controller_list_.at(active_controller_idx_)->getStatus();
+ 6326 49413 : }
+ 6327 49413 :
+ 6328 : // | ------------ fill in the available controllers ----------- |
+ 6329 :
+ 6330 : for (int i = 0; i < int(_controller_names_.size()); i++) {
+ 6331 : if ((_controller_names_.at(i) != _failsafe_controller_name_) && (_controller_names_.at(i) != _eland_controller_name_)) {
+ 6332 : diagnostics_msg.available_controllers.push_back(_controller_names_.at(i));
+ 6333 115520 : diagnostics_msg.human_switchable_controllers.push_back(controllers_.at(_controller_names_.at(i)).human_switchable);
+ 6334 99049 : }
+ 6335 82578 : }
+ 6336 82578 :
+ 6337 : // | ------------- fill in the available trackers ------------- |
+ 6338 :
+ 6339 : for (int i = 0; i < int(_tracker_names_.size()); i++) {
+ 6340 : if (_tracker_names_.at(i) != _null_tracker_name_) {
+ 6341 : diagnostics_msg.available_trackers.push_back(_tracker_names_.at(i));
+ 6342 16471 : diagnostics_msg.human_switchable_trackers.push_back(trackers_.at(_tracker_names_.at(i)).human_switchable);
+ 6343 : }
+ 6344 : }
+ 6345 :
+ 6346 : // | ------------------------- publish ------------------------ |
+ 6347 :
+ 6348 : ph_diagnostics_.publish(diagnostics_msg);
+ 6349 305 : }
+ 6350 :
+ 6351 915 : //}
+ 6352 915 :
+ 6353 : /* setConstraintsToTrackers() //{ */
+ 6354 305 :
+ 6355 : void ControlManager::setConstraintsToTrackers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+ 6356 :
+ 6357 610 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("setConstraintsToTrackers");
+ 6358 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::setConstraintsToTrackers", scope_timer_logger_, scope_timer_enabled_);
+ 6359 :
+ 6360 2138 : mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+ 6361 :
+ 6362 : {
+ 6363 5499 : std::scoped_lock lock(mutex_tracker_list_);
+ 6364 5499 :
+ 6365 : // for each tracker
+ 6366 : for (int i = 0; i < int(tracker_list_.size()); i++) {
+ 6367 305 :
+ 6368 : // if it is the active one, update and retrieve the command
+ 6369 : response = tracker_list_.at(i)->setConstraints(
+ 6370 : mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+ 6371 : }
+ 6372 : }
+ 6373 360 : }
+ 6374 :
+ 6375 1080 : //}
+ 6376 1080 :
+ 6377 : /* setConstraintsToControllers() //{ */
+ 6378 360 :
+ 6379 : void ControlManager::setConstraintsToControllers(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+ 6380 :
+ 6381 720 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("setConstraintsToControllers");
+ 6382 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::setConstraintsToControllers", scope_timer_logger_, scope_timer_enabled_);
+ 6383 :
+ 6384 2160 : mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+ 6385 :
+ 6386 : {
+ 6387 5400 : std::scoped_lock lock(mutex_controller_list_);
+ 6388 5400 :
+ 6389 : // for each controller
+ 6390 : for (int i = 0; i < int(controller_list_.size()); i++) {
+ 6391 360 :
+ 6392 : // if it is the active one, update and retrieve the command
+ 6393 : response = controller_list_.at(i)->setConstraints(
+ 6394 : mrs_msgs::DynamicsConstraintsSrvRequest::ConstPtr(std::make_unique<mrs_msgs::DynamicsConstraintsSrvRequest>(constraints)));
+ 6395 : }
+ 6396 : }
+ 6397 91 : }
+ 6398 :
+ 6399 273 : //}
+ 6400 273 :
+ 6401 : /* setConstraints() //{ */
+ 6402 91 :
+ 6403 : void ControlManager::setConstraints(const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+ 6404 91 :
+ 6405 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("setConstraints");
+ 6406 91 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::setConstraints", scope_timer_logger_, scope_timer_enabled_);
+ 6407 91 :
+ 6408 : mrs_msgs::DynamicsConstraintsSrvResponse::ConstPtr response;
+ 6409 :
+ 6410 : setConstraintsToTrackers(constraints);
+ 6411 :
+ 6412 : setConstraintsToControllers(constraints);
+ 6413 : }
+ 6414 120353 :
+ 6415 : //}
+ 6416 :
+ 6417 : /* enforceControllerConstraints() //{ */
+ 6418 240706 :
+ 6419 : std::optional<mrs_msgs::DynamicsConstraintsSrvRequest> ControlManager::enforceControllersConstraints(
+ 6420 120353 : const mrs_msgs::DynamicsConstraintsSrvRequest& constraints) {
+ 6421 101205 :
+ 6422 : // copy member variables
+ 6423 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 6424 19148 :
+ 6425 : if (!last_control_output.control_output || !last_control_output.diagnostics.controller_enforcing_constraints) {
+ 6426 19148 : return {};
+ 6427 : }
+ 6428 38296 :
+ 6429 : bool enforcing = false;
+ 6430 :
+ 6431 19148 : auto constraints_out = constraints;
+ 6432 13808 :
+ 6433 : std::scoped_lock lock(mutex_tracker_list_);
+ 6434 13808 :
+ 6435 : // enforce horizontal speed
+ 6436 : if (last_control_output.diagnostics.horizontal_speed_constraint < constraints.constraints.horizontal_speed) {
+ 6437 : constraints_out.constraints.horizontal_speed = last_control_output.diagnostics.horizontal_speed_constraint;
+ 6438 19148 :
+ 6439 17434 : enforcing = true;
+ 6440 : }
+ 6441 17434 :
+ 6442 : // enforce horizontal acceleration
+ 6443 : if (last_control_output.diagnostics.horizontal_acc_constraint < constraints.constraints.horizontal_acceleration) {
+ 6444 : constraints_out.constraints.horizontal_acceleration = last_control_output.diagnostics.horizontal_acc_constraint;
+ 6445 19148 :
+ 6446 13808 : enforcing = true;
+ 6447 : }
+ 6448 13808 :
+ 6449 : // enforce vertical ascending speed
+ 6450 : if (last_control_output.diagnostics.vertical_asc_speed_constraint < constraints.constraints.vertical_ascending_speed) {
+ 6451 : constraints_out.constraints.vertical_ascending_speed = last_control_output.diagnostics.vertical_asc_speed_constraint;
+ 6452 19148 :
+ 6453 0 : enforcing = true;
+ 6454 : }
+ 6455 0 :
+ 6456 : // enforce vertical ascending acceleration
+ 6457 : if (last_control_output.diagnostics.vertical_asc_acc_constraint < constraints.constraints.vertical_ascending_acceleration) {
+ 6458 : constraints_out.constraints.vertical_ascending_acceleration = last_control_output.diagnostics.vertical_asc_acc_constraint;
+ 6459 19148 :
+ 6460 13808 : enforcing = true;
+ 6461 : }
+ 6462 13808 :
+ 6463 : // enforce vertical descending speed
+ 6464 : if (last_control_output.diagnostics.vertical_desc_speed_constraint < constraints.constraints.vertical_descending_speed) {
+ 6465 : constraints_out.constraints.vertical_descending_speed = last_control_output.diagnostics.vertical_desc_speed_constraint;
+ 6466 19148 :
+ 6467 0 : enforcing = true;
+ 6468 : }
+ 6469 0 :
+ 6470 : // enforce vertical descending acceleration
+ 6471 : if (last_control_output.diagnostics.vertical_desc_acc_constraint < constraints.constraints.vertical_descending_acceleration) {
+ 6472 19148 : constraints_out.constraints.vertical_descending_acceleration = last_control_output.diagnostics.vertical_desc_acc_constraint;
+ 6473 17434 :
+ 6474 : enforcing = true;
+ 6475 1714 : }
+ 6476 :
+ 6477 : if (enforcing) {
+ 6478 : return {constraints_out};
+ 6479 : } else {
+ 6480 : return {};
+ 6481 : }
+ 6482 : }
+ 6483 16737 :
+ 6484 : //}
+ 6485 14126 :
+ 6486 8782 : /* isFlyingNormally() //{ */
+ 6487 8782 :
+ 6488 33053 : bool ControlManager::isFlyingNormally(void) {
+ 6489 23329 :
+ 6490 : return callbacks_enabled_ && (output_enabled_) && (offboard_mode_) && (armed_) &&
+ 6491 : (((active_tracker_idx_ != _ehover_tracker_idx_) && (active_controller_idx_ != _eland_controller_idx_) &&
+ 6492 : (active_controller_idx_ != _failsafe_controller_idx_)) ||
+ 6493 : _controller_names_.size() == 1) &&
+ 6494 : (((active_tracker_idx_ != _null_tracker_idx_) && (active_tracker_idx_ != _landoff_tracker_idx_)) || _tracker_names_.size() == 1);
+ 6495 : }
+ 6496 471 :
+ 6497 : //}
+ 6498 942 :
+ 6499 : /* //{ getMass() */
+ 6500 471 :
+ 6501 13 : double ControlManager::getMass(void) {
+ 6502 :
+ 6503 458 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 6504 :
+ 6505 : if (last_control_output.diagnostics.mass_estimator) {
+ 6506 : return _uav_mass_ + last_control_output.diagnostics.mass_difference;
+ 6507 : } else {
+ 6508 : return _uav_mass_;
+ 6509 : }
+ 6510 : }
+ 6511 0 :
+ 6512 : //}
+ 6513 0 :
+ 6514 : /* loadConfigFile() //{ */
+ 6515 0 :
+ 6516 : bool ControlManager::loadConfigFile(const std::string& file_path, const std::string ns) {
+ 6517 :
+ 6518 : const std::string name_space = nh_.getNamespace() + "/" + ns;
+ 6519 0 :
+ 6520 0 : ROS_INFO("[ControlManager]: loading '%s' under the namespace '%s'", file_path.c_str(), name_space.c_str());
+ 6521 :
+ 6522 0 : // load the user-requested file
+ 6523 0 : {
+ 6524 0 : std::string command = "rosparam load " + file_path + " " + name_space;
+ 6525 : int result = std::system(command.c_str());
+ 6526 :
+ 6527 : if (result != 0) {
+ 6528 : ROS_ERROR("[ControlManager]: failed to load '%s'", file_path.c_str());
+ 6529 0 : return false;
+ 6530 0 : }
+ 6531 0 : }
+ 6532 :
+ 6533 0 : // load the platform config
+ 6534 0 : if (_platform_config_ != "") {
+ 6535 0 : std::string command = "rosparam load " + _platform_config_ + " " + name_space;
+ 6536 : int result = std::system(command.c_str());
+ 6537 :
+ 6538 : if (result != 0) {
+ 6539 : ROS_ERROR("[ControlManager]: failed to load the platform config file '%s'", _platform_config_.c_str());
+ 6540 0 : return false;
+ 6541 0 : }
+ 6542 0 : }
+ 6543 :
+ 6544 0 : // load the custom config
+ 6545 0 : if (_custom_config_ != "") {
+ 6546 0 : std::string command = "rosparam load " + _custom_config_ + " " + name_space;
+ 6547 : int result = std::system(command.c_str());
+ 6548 :
+ 6549 : if (result != 0) {
+ 6550 0 : ROS_ERROR("[ControlManager]: failed to load the custom config file '%s'", _custom_config_.c_str());
+ 6551 : return false;
+ 6552 : }
+ 6553 : }
+ 6554 :
+ 6555 : return true;
+ 6556 : }
+ 6557 :
+ 6558 : //}
+ 6559 1165 :
+ 6560 : // | ----------------------- safety area ---------------------- |
+ 6561 1165 :
+ 6562 489 : /* //{ isPointInSafetyArea3d() */
+ 6563 :
+ 6564 : bool ControlManager::isPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& point) {
+ 6565 1352 :
+ 6566 : if (!use_safety_area_) {
+ 6567 676 : return true;
+ 6568 0 : }
+ 6569 0 :
+ 6570 : auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+ 6571 :
+ 6572 676 : if (!tfed_horizontal) {
+ 6573 0 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+ 6574 : return false;
+ 6575 : }
+ 6576 676 :
+ 6577 0 : if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+ 6578 : return false;
+ 6579 : }
+ 6580 676 :
+ 6581 : if (point.reference.position.z < getMinZ(point.header.frame_id) || point.reference.position.z > getMaxZ(point.header.frame_id)) {
+ 6582 : return false;
+ 6583 : }
+ 6584 :
+ 6585 : return true;
+ 6586 : }
+ 6587 9923 :
+ 6588 : //}
+ 6589 9923 :
+ 6590 593 : /* //{ isPointInSafetyArea2d() */
+ 6591 :
+ 6592 : bool ControlManager::isPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& point) {
+ 6593 18660 :
+ 6594 : if (!use_safety_area_) {
+ 6595 9330 : return true;
+ 6596 0 : }
+ 6597 0 :
+ 6598 : auto tfed_horizontal = transformer_->transformSingle(point, _safety_area_horizontal_frame_);
+ 6599 :
+ 6600 9330 : if (!tfed_horizontal) {
+ 6601 77 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform the point to the safety area horizontal frame");
+ 6602 : return false;
+ 6603 : }
+ 6604 9253 :
+ 6605 : if (!safety_zone_->isPointValid(tfed_horizontal->reference.position.x, tfed_horizontal->reference.position.y)) {
+ 6606 : return false;
+ 6607 : }
+ 6608 :
+ 6609 : return true;
+ 6610 : }
+ 6611 521 :
+ 6612 : //}
+ 6613 521 :
+ 6614 489 : /* //{ isPathToPointInSafetyArea3d() */
+ 6615 :
+ 6616 : bool ControlManager::isPathToPointInSafetyArea3d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+ 6617 32 :
+ 6618 0 : if (!use_safety_area_) {
+ 6619 : return true;
+ 6620 : }
+ 6621 64 :
+ 6622 : if (!isPointInSafetyArea3d(start) || !isPointInSafetyArea3d(end)) {
+ 6623 : return false;
+ 6624 32 : }
+ 6625 :
+ 6626 32 : mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+ 6627 :
+ 6628 0 : {
+ 6629 : auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+ 6630 0 :
+ 6631 : if (!ret) {
+ 6632 :
+ 6633 32 : ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+ 6634 :
+ 6635 : return false;
+ 6636 : }
+ 6637 32 :
+ 6638 : start_transformed = ret.value();
+ 6639 32 : }
+ 6640 :
+ 6641 0 : {
+ 6642 : auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+ 6643 0 :
+ 6644 : if (!ret) {
+ 6645 :
+ 6646 32 : ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+ 6647 :
+ 6648 : return false;
+ 6649 32 : }
+ 6650 32 :
+ 6651 : end_transformed = ret.value();
+ 6652 : }
+ 6653 :
+ 6654 : return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+ 6655 : end_transformed.reference.position.y);
+ 6656 : }
+ 6657 31 :
+ 6658 : //}
+ 6659 31 :
+ 6660 0 : /* //{ isPathToPointInSafetyArea2d() */
+ 6661 :
+ 6662 : bool ControlManager::isPathToPointInSafetyArea2d(const mrs_msgs::ReferenceStamped& start, const mrs_msgs::ReferenceStamped& end) {
+ 6663 62 :
+ 6664 : if (!use_safety_area_) {
+ 6665 31 : return true;
+ 6666 0 : }
+ 6667 :
+ 6668 : mrs_msgs::ReferenceStamped start_transformed, end_transformed;
+ 6669 :
+ 6670 31 : if (!isPointInSafetyArea2d(start) || !isPointInSafetyArea2d(end)) {
+ 6671 : return false;
+ 6672 31 : }
+ 6673 :
+ 6674 0 : {
+ 6675 : auto ret = transformer_->transformSingle(start, _safety_area_horizontal_frame_);
+ 6676 0 :
+ 6677 : if (!ret) {
+ 6678 :
+ 6679 31 : ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+ 6680 :
+ 6681 : return false;
+ 6682 : }
+ 6683 31 :
+ 6684 : start_transformed = ret.value();
+ 6685 31 : }
+ 6686 :
+ 6687 0 : {
+ 6688 : auto ret = transformer_->transformSingle(end, _safety_area_horizontal_frame_);
+ 6689 0 :
+ 6690 : if (!ret) {
+ 6691 :
+ 6692 31 : ROS_ERROR("[ControlManager]: SafetyArea: Could not transform the first point in the path");
+ 6693 :
+ 6694 : return false;
+ 6695 31 : }
+ 6696 31 :
+ 6697 : end_transformed = ret.value();
+ 6698 : }
+ 6699 :
+ 6700 : return safety_zone_->isPathValid(start_transformed.reference.position.x, start_transformed.reference.position.y, end_transformed.reference.position.x,
+ 6701 : end_transformed.reference.position.y);
+ 6702 : }
+ 6703 11661 :
+ 6704 : //}
+ 6705 :
+ 6706 : /* //{ getMaxZ() */
+ 6707 11661 :
+ 6708 : double ControlManager::getMaxZ(const std::string& frame_id) {
+ 6709 :
+ 6710 : // | ------- first, calculate max_z from the safety area ------ |
+ 6711 23322 :
+ 6712 : double safety_area_max_z = std::numeric_limits<float>::max();
+ 6713 11661 :
+ 6714 11661 : {
+ 6715 11661 :
+ 6716 11661 : geometry_msgs::PointStamped point;
+ 6717 :
+ 6718 11661 : point.header.frame_id = _safety_area_vertical_frame_;
+ 6719 : point.point.x = 0;
+ 6720 11661 : point.point.y = 0;
+ 6721 0 : point.point.z = _safety_area_max_z_;
+ 6722 :
+ 6723 : auto ret = transformer_->transformSingle(point, frame_id);
+ 6724 11661 :
+ 6725 : if (!ret) {
+ 6726 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's max_z to '%s'", frame_id.c_str());
+ 6727 : }
+ 6728 :
+ 6729 11661 : safety_area_max_z = ret->point.z;
+ 6730 : }
+ 6731 :
+ 6732 : // | ------------ overwrite from estimation manager ----------- |
+ 6733 11661 :
+ 6734 : double estimation_manager_max_z = std::numeric_limits<float>::max();
+ 6735 23284 :
+ 6736 : {
+ 6737 : // if possible, override it with max z from the estimation manager
+ 6738 23284 : if (sh_max_z_.hasMsg()) {
+ 6739 11642 :
+ 6740 11642 : auto msg = sh_max_z_.getMsg();
+ 6741 11642 :
+ 6742 11642 : // transform it into the safety area frame
+ 6743 : geometry_msgs::PointStamped point;
+ 6744 11642 : point.header = msg->header;
+ 6745 : point.point.x = 0;
+ 6746 11642 : point.point.y = 0;
+ 6747 0 : point.point.z = msg->value;
+ 6748 :
+ 6749 : auto ret = transformer_->transformSingle(point, frame_id);
+ 6750 11642 :
+ 6751 : if (!ret) {
+ 6752 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform estimation manager's max_z to the current control frame");
+ 6753 : }
+ 6754 11661 :
+ 6755 10860 : estimation_manager_max_z = ret->point.z;
+ 6756 : }
+ 6757 801 : }
+ 6758 :
+ 6759 : if (estimation_manager_max_z < safety_area_max_z) {
+ 6760 : return estimation_manager_max_z;
+ 6761 : } else {
+ 6762 : return safety_area_max_z;
+ 6763 : }
+ 6764 : }
+ 6765 11661 :
+ 6766 : //}
+ 6767 11661 :
+ 6768 0 : /* //{ getMinZ() */
+ 6769 :
+ 6770 : double ControlManager::getMinZ(const std::string& frame_id) {
+ 6771 23322 :
+ 6772 : if (!use_safety_area_) {
+ 6773 11661 : return std::numeric_limits<double>::lowest();
+ 6774 11661 : }
+ 6775 11661 :
+ 6776 11661 : geometry_msgs::PointStamped point;
+ 6777 :
+ 6778 23322 : point.header.frame_id = _safety_area_vertical_frame_;
+ 6779 : point.point.x = 0;
+ 6780 11661 : point.point.y = 0;
+ 6781 0 : point.point.z = _safety_area_min_z_;
+ 6782 0 :
+ 6783 : auto ret = transformer_->transformSingle(point, frame_id);
+ 6784 :
+ 6785 11661 : if (!ret) {
+ 6786 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: SafetyArea: Could not transform safety area's min_z to '%s'", frame_id.c_str());
+ 6787 : return std::numeric_limits<double>::lowest();
+ 6788 : }
+ 6789 :
+ 6790 : return ret->point.z;
+ 6791 : }
+ 6792 :
+ 6793 : //}
+ 6794 260 :
+ 6795 : // | --------------------- obstacle bumper -------------------- |
+ 6796 :
+ 6797 : /* bumperPushFromObstacle() //{ */
+ 6798 260 :
+ 6799 260 : void ControlManager::bumperPushFromObstacle(void) {
+ 6800 :
+ 6801 260 : // | --------------- fabricate the min distances -------------- |
+ 6802 :
+ 6803 260 : double min_distance_horizontal = _bumper_horizontal_distance_;
+ 6804 : double min_distance_vertical = _bumper_vertical_distance_;
+ 6805 260 :
+ 6806 : if (_bumper_horizontal_derive_from_dynamics_ || _bumper_vertical_derive_from_dynamics_) {
+ 6807 260 :
+ 6808 260 : auto constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+ 6809 :
+ 6810 260 : if (_bumper_horizontal_derive_from_dynamics_) {
+ 6811 :
+ 6812 : const double horizontal_t_stop = constraints.constraints.horizontal_speed / constraints.constraints.horizontal_acceleration;
+ 6813 260 : const double horizontal_stop_dist = (horizontal_t_stop * constraints.constraints.horizontal_speed) / 2.0;
+ 6814 :
+ 6815 : min_distance_horizontal += 1.5 * horizontal_stop_dist;
+ 6816 : }
+ 6817 520 :
+ 6818 260 : if (_bumper_vertical_derive_from_dynamics_) {
+ 6819 :
+ 6820 :
+ 6821 : // larger from the two accelerations
+ 6822 520 : const double vert_acc = constraints.constraints.vertical_ascending_acceleration > constraints.constraints.vertical_descending_acceleration
+ 6823 260 : ? constraints.constraints.vertical_ascending_acceleration
+ 6824 : : constraints.constraints.vertical_descending_acceleration;
+ 6825 :
+ 6826 260 : // larger from the two speeds
+ 6827 260 : const double vert_speed = constraints.constraints.vertical_ascending_speed > constraints.constraints.vertical_descending_speed
+ 6828 : ? constraints.constraints.vertical_ascending_speed
+ 6829 260 : : constraints.constraints.vertical_descending_speed;
+ 6830 :
+ 6831 : const double vertical_t_stop = vert_speed / vert_acc;
+ 6832 : const double vertical_stop_dist = (vertical_t_stop * vert_speed) / 2.0;
+ 6833 :
+ 6834 : min_distance_vertical += 1.5 * vertical_stop_dist;
+ 6835 : }
+ 6836 260 : }
+ 6837 260 :
+ 6838 : // | ---------------------------- ---------------------------- |
+ 6839 260 :
+ 6840 : // copy member variables
+ 6841 260 : mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+ 6842 260 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 6843 :
+ 6844 260 : double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+ 6845 260 :
+ 6846 : double direction = 0;
+ 6847 260 : double repulsion_distance = std::numeric_limits<double>::max();
+ 6848 260 :
+ 6849 : bool horizontal_collision_detected = false;
+ 6850 2340 : bool vertical_collision_detected = false;
+ 6851 :
+ 6852 2080 : double min_horizontal_sector_distance = std::numeric_limits<double>::max();
+ 6853 0 : size_t min_sector_id = 0;
+ 6854 :
+ 6855 : for (unsigned long i = 0; i < bumper_data->n_horizontal_sectors; i++) {
+ 6856 2080 :
+ 6857 260 : if (bumper_data->sectors.at(i) < 0) {
+ 6858 260 : continue;
+ 6859 : }
+ 6860 :
+ 6861 : if (bumper_data->sectors.at(i) < min_horizontal_sector_distance) {
+ 6862 : min_horizontal_sector_distance = bumper_data->sectors.at(i);
+ 6863 260 : min_sector_id = i;
+ 6864 : }
+ 6865 : }
+ 6866 104 :
+ 6867 104 : // if the sector is under the threshold distance
+ 6868 : if (min_horizontal_sector_distance < min_distance_horizontal) {
+ 6869 :
+ 6870 104 : // get the desired direction of motion
+ 6871 : double oposite_direction = double(min_sector_id) * sector_size + M_PI;
+ 6872 104 : int oposite_sector_idx = bumperGetSectorId(cos(oposite_direction), sin(oposite_direction), 0);
+ 6873 :
+ 6874 : // get the id of the oposite sector
+ 6875 104 : direction = oposite_direction;
+ 6876 :
+ 6877 104 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: found potential collision (sector %lu vs. %d), obstacle distance: %.2f, repulsing", min_sector_id,
+ 6878 : oposite_sector_idx, bumper_data->sectors.at(min_sector_id));
+ 6879 :
+ 6880 260 : repulsion_distance = min_distance_horizontal + _bumper_horizontal_overshoot_ - bumper_data->sectors.at(min_sector_id);
+ 6881 :
+ 6882 : horizontal_collision_detected = true;
+ 6883 260 : }
+ 6884 :
+ 6885 0 : double vertical_repulsion_distance = 0;
+ 6886 0 :
+ 6887 0 : // check for vertical collision down
+ 6888 : if (bumper_data->sectors.at(bumper_data->n_horizontal_sectors) > 0 && bumper_data->sectors.at(bumper_data->n_horizontal_sectors) <= min_distance_vertical) {
+ 6889 :
+ 6890 : ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision below");
+ 6891 260 : vertical_collision_detected = true;
+ 6892 : vertical_repulsion_distance = min_distance_vertical - bumper_data->sectors.at(bumper_data->n_horizontal_sectors);
+ 6893 0 : }
+ 6894 0 :
+ 6895 0 : // check for vertical collision up
+ 6896 : if (bumper_data->sectors.at(bumper_data->n_horizontal_sectors + 1) > 0 &&
+ 6897 : bumper_data->sectors.at(bumper_data->n_horizontal_sectors + 1) <= min_distance_vertical) {
+ 6898 :
+ 6899 260 : ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: potential collision above");
+ 6900 : vertical_collision_detected = true;
+ 6901 104 : vertical_repulsion_distance = -(min_distance_vertical - bumper_data->sectors.at(bumper_data->n_horizontal_sectors + 1));
+ 6902 : }
+ 6903 1 :
+ 6904 : // if potential collision was detected and we should start the repulsing_
+ 6905 1 : if (horizontal_collision_detected || vertical_collision_detected) {
+ 6906 2 :
+ 6907 : if (!bumper_repulsing_) {
+ 6908 :
+ 6909 1 : if (_bumper_switch_tracker_) {
+ 6910 :
+ 6911 1 : auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+ 6912 : std::string active_tracker_name = _tracker_names_.at(active_tracker_idx);
+ 6913 0 :
+ 6914 : // remember the previously active tracker
+ 6915 : bumper_previous_tracker_ = active_tracker_name;
+ 6916 :
+ 6917 1 : if (active_tracker_name != _bumper_tracker_name_) {
+ 6918 :
+ 6919 1 : switchTracker(_bumper_tracker_name_);
+ 6920 2 : }
+ 6921 : }
+ 6922 :
+ 6923 1 : if (_bumper_switch_controller_) {
+ 6924 :
+ 6925 1 : auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+ 6926 : std::string active_controller_name = _controller_names_.at(active_controller_idx);
+ 6927 1 :
+ 6928 : // remember the previously active controller
+ 6929 : bumper_previous_controller_ = active_controller_name;
+ 6930 :
+ 6931 : if (active_controller_name != _bumper_controller_name_) {
+ 6932 104 :
+ 6933 : switchController(_bumper_controller_name_);
+ 6934 104 : }
+ 6935 : }
+ 6936 0 : }
+ 6937 :
+ 6938 104 : bumper_repulsing_ = true;
+ 6939 :
+ 6940 : callbacks_enabled_ = false;
+ 6941 104 :
+ 6942 : mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;
+ 6943 104 :
+ 6944 : std_srvs::SetBoolRequest req_enable_callbacks;
+ 6945 104 :
+ 6946 104 : // create the reference in the fcu_untilted frame
+ 6947 104 : mrs_msgs::ReferenceStamped reference_fcu_untilted;
+ 6948 :
+ 6949 0 : reference_fcu_untilted.header.frame_id = "fcu_untilted";
+ 6950 0 :
+ 6951 : if (horizontal_collision_detected) {
+ 6952 : reference_fcu_untilted.reference.position.x = cos(direction) * repulsion_distance;
+ 6953 104 : reference_fcu_untilted.reference.position.y = sin(direction) * repulsion_distance;
+ 6954 : } else {
+ 6955 104 : reference_fcu_untilted.reference.position.x = 0;
+ 6956 0 : reference_fcu_untilted.reference.position.y = 0;
+ 6957 : }
+ 6958 104 :
+ 6959 : reference_fcu_untilted.reference.heading = 0;
+ 6960 :
+ 6961 : if (vertical_collision_detected) {
+ 6962 104 : reference_fcu_untilted.reference.position.z = vertical_repulsion_distance;
+ 6963 : } else {
+ 6964 : reference_fcu_untilted.reference.position.z = 0;
+ 6965 : }
+ 6966 :
+ 6967 : {
+ 6968 104 : std::scoped_lock lock(mutex_tracker_list_);
+ 6969 :
+ 6970 104 : // transform the reference into the currently used frame
+ 6971 0 : // this is under the mutex_tracker_list since we don't won't the odometry switch to happen
+ 6972 0 : // to the tracker before we actually call the goto service
+ 6973 :
+ 6974 : auto ret = transformer_->transformSingle(reference_fcu_untilted, uav_state.header.frame_id);
+ 6975 104 :
+ 6976 : if (!ret) {
+ 6977 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: Bumper: bumper reference could not be transformed");
+ 6978 104 : return;
+ 6979 104 : }
+ 6980 :
+ 6981 : reference_fcu_untilted = ret.value();
+ 6982 104 :
+ 6983 728 : // copy the reference into the service type message
+ 6984 624 : mrs_msgs::ReferenceSrvRequest req_goto_out;
+ 6985 : req_goto_out.reference = reference_fcu_untilted.reference;
+ 6986 :
+ 6987 : // disable callbacks of all trackers
+ 6988 104 : req_enable_callbacks.data = false;
+ 6989 104 : for (size_t i = 0; i < tracker_list_.size(); i++) {
+ 6990 : tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 6991 : }
+ 6992 312 :
+ 6993 312 : // enable the callbacks for the active tracker
+ 6994 : req_enable_callbacks.data = true;
+ 6995 : tracker_list_.at(active_tracker_idx_)
+ 6996 104 : ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 6997 104 :
+ 6998 : // call the goto
+ 6999 : tracker_response = tracker_list_.at(active_tracker_idx_)
+ 7000 : ->setReference(mrs_msgs::ReferenceSrvRequest::ConstPtr(std::make_unique<mrs_msgs::ReferenceSrvRequest>(req_goto_out)));
+ 7001 :
+ 7002 260 : // disable the callbacks back again
+ 7003 : req_enable_callbacks.data = false;
+ 7004 1 : tracker_list_.at(active_tracker_idx_)
+ 7005 : ->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 7006 1 : }
+ 7007 : }
+ 7008 1 :
+ 7009 2 : // if repulsing_ and the distance is safe once again
+ 7010 : if (bumper_repulsing_ && !horizontal_collision_detected && !vertical_collision_detected) {
+ 7011 1 :
+ 7012 : ROS_INFO_THROTTLE(1.0, "[ControlManager]: Bumper: no more collision, stopping repulsion");
+ 7013 0 :
+ 7014 : if (_bumper_switch_tracker_) {
+ 7015 :
+ 7016 : auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+ 7017 1 : std::string active_tracker_name = _tracker_names_.at(active_tracker_idx);
+ 7018 :
+ 7019 1 : if (active_tracker_name != bumper_previous_tracker_) {
+ 7020 2 :
+ 7021 : switchTracker(bumper_previous_tracker_);
+ 7022 1 : }
+ 7023 : }
+ 7024 1 :
+ 7025 : if (_bumper_switch_controller_) {
+ 7026 :
+ 7027 : auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+ 7028 1 : std::string active_controller_name = _controller_names_.at(active_controller_idx);
+ 7029 :
+ 7030 : if (active_controller_name != bumper_previous_controller_) {
+ 7031 2 :
+ 7032 : switchController(bumper_previous_controller_);
+ 7033 : }
+ 7034 1 : }
+ 7035 7 :
+ 7036 6 : std_srvs::SetBoolRequest req_enable_callbacks;
+ 7037 :
+ 7038 : {
+ 7039 : std::scoped_lock lock(mutex_tracker_list_);
+ 7040 1 :
+ 7041 : // enable callbacks of all trackers
+ 7042 1 : req_enable_callbacks.data = true;
+ 7043 : for (size_t i = 0; i < tracker_list_.size(); i++) {
+ 7044 : tracker_list_.at(i)->enableCallbacks(std_srvs::SetBoolRequest::ConstPtr(std::make_unique<std_srvs::SetBoolRequest>(req_enable_callbacks)));
+ 7045 : }
+ 7046 : }
+ 7047 :
+ 7048 : callbacks_enabled_ = true;
+ 7049 :
+ 7050 104 : bumper_repulsing_ = false;
+ 7051 : }
+ 7052 : }
+ 7053 104 :
+ 7054 : //}
+ 7055 :
+ 7056 104 : /* bumperGetSectorId() //{ */
+ 7057 :
+ 7058 104 : int ControlManager::bumperGetSectorId(const double& x, const double& y, [[maybe_unused]] const double& z) {
+ 7059 :
+ 7060 : // copy member variables
+ 7061 104 : mrs_msgs::ObstacleSectorsConstPtr bumper_data = sh_bumper_.getMsg();
+ 7062 104 :
+ 7063 : // heading of the point in drone frame
+ 7064 : double point_heading_horizontal = atan2(y, x);
+ 7065 :
+ 7066 104 : point_heading_horizontal += TAU;
+ 7067 :
+ 7068 : // if point_heading_horizontal is greater then 2*M_PI mod it
+ 7069 104 : if (fabs(point_heading_horizontal) >= TAU) {
+ 7070 : point_heading_horizontal = fmod(point_heading_horizontal, TAU);
+ 7071 104 : }
+ 7072 0 :
+ 7073 : // heading of the right edge of the first sector
+ 7074 : double sector_size = TAU / double(bumper_data->n_horizontal_sectors);
+ 7075 208 :
+ 7076 : // calculate the idx
+ 7077 : int idx = floor((point_heading_horizontal + (sector_size / 2.0)) / sector_size);
+ 7078 :
+ 7079 : if (idx > int(bumper_data->n_horizontal_sectors) - 1) {
+ 7080 : idx -= bumper_data->n_horizontal_sectors;
+ 7081 : }
+ 7082 :
+ 7083 : return idx;
+ 7084 12 : }
+ 7085 :
+ 7086 : //}
+ 7087 24 :
+ 7088 : // | ------------------------- safety ------------------------- |
+ 7089 :
+ 7090 12 : /* //{ changeLandingState() */
+ 7091 :
+ 7092 12 : void ControlManager::changeLandingState(LandingStates_t new_state) {
+ 7093 12 :
+ 7094 : // copy member variables
+ 7095 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 7096 12 :
+ 7097 : {
+ 7098 4 : std::scoped_lock lock(mutex_landing_state_machine_);
+ 7099 4 :
+ 7100 8 : previous_state_landing_ = current_state_landing_;
+ 7101 : current_state_landing_ = new_state;
+ 7102 8 : }
+ 7103 8 :
+ 7104 8 : switch (current_state_landing_) {
+ 7105 8 :
+ 7106 8 : case IDLE_STATE:
+ 7107 : break;
+ 7108 8 : case LANDING_STATE: {
+ 7109 :
+ 7110 : ROS_DEBUG("[ControlManager]: starting eland timer");
+ 7111 8 : timer_eland_.start();
+ 7112 : ROS_DEBUG("[ControlManager]: eland timer started");
+ 7113 : eland_triggered_ = true;
+ 7114 12 : bumper_enabled_ = false;
+ 7115 12 :
+ 7116 : landing_uav_mass_ = getMass();
+ 7117 : }
+ 7118 :
+ 7119 : break;
+ 7120 : }
+ 7121 0 :
+ 7122 0 : ROS_INFO("[ControlManager]: switching emergency landing state %s -> %s", state_names[previous_state_landing_], state_names[current_state_landing_]);
+ 7123 0 : }
+ 7124 :
+ 7125 0 : //}
+ 7126 0 :
+ 7127 : /* hover() //{ */
+ 7128 0 :
+ 7129 0 : std::tuple<bool, std::string> ControlManager::hover(void) {
+ 7130 : if (!is_initialized_)
+ 7131 : return std::tuple(false, "the ControlManager is not initialized");
+ 7132 0 :
+ 7133 : if (eland_triggered_)
+ 7134 0 : return std::tuple(false, "cannot hover, eland already triggered");
+ 7135 0 :
+ 7136 : if (failsafe_triggered_)
+ 7137 0 : return std::tuple(false, "cannot hover, failsafe already triggered");
+ 7138 :
+ 7139 0 : {
+ 7140 : std::scoped_lock lock(mutex_tracker_list_);
+ 7141 0 :
+ 7142 : std_srvs::TriggerResponse::ConstPtr response;
+ 7143 : std_srvs::TriggerRequest request;
+ 7144 :
+ 7145 0 : response = tracker_list_.at(active_tracker_idx_)->hover(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+ 7146 0 :
+ 7147 : if (response != std_srvs::TriggerResponse::Ptr()) {
+ 7148 0 :
+ 7149 : return std::tuple(response->success, response->message);
+ 7150 :
+ 7151 : } else {
+ 7152 :
+ 7153 : std::stringstream ss;
+ 7154 : ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'hover()' function!";
+ 7155 :
+ 7156 : return std::tuple(false, ss.str());
+ 7157 4 : }
+ 7158 : }
+ 7159 4 : }
+ 7160 0 :
+ 7161 : //}
+ 7162 4 :
+ 7163 0 : /* //{ ehover() */
+ 7164 :
+ 7165 4 : std::tuple<bool, std::string> ControlManager::ehover(void) {
+ 7166 0 :
+ 7167 : if (!is_initialized_)
+ 7168 : return std::tuple(false, "the ControlManager is not initialized");
+ 7169 8 :
+ 7170 4 : if (eland_triggered_)
+ 7171 : return std::tuple(false, "cannot ehover, eland already triggered");
+ 7172 4 :
+ 7173 : if (failsafe_triggered_)
+ 7174 0 : return std::tuple(false, "cannot ehover, failsafe already triggered");
+ 7175 0 :
+ 7176 0 : // copy the member variables
+ 7177 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 7178 0 : auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+ 7179 :
+ 7180 : if (active_tracker_idx == _null_tracker_idx_) {
+ 7181 4 :
+ 7182 : std::stringstream ss;
+ 7183 : ss << "can not trigger ehover while not flying";
+ 7184 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7185 4 :
+ 7186 : return std::tuple(false, ss.str());
+ 7187 : }
+ 7188 :
+ 7189 4 : ungripSrv();
+ 7190 :
+ 7191 0 : {
+ 7192 0 :
+ 7193 0 : auto [success, message] = switchTracker(_ehover_tracker_name_);
+ 7194 :
+ 7195 0 : // check if the tracker was successfully switched
+ 7196 : // this is vital, that is the core of the hover
+ 7197 : if (!success) {
+ 7198 :
+ 7199 : std::stringstream ss;
+ 7200 8 : ss << "error during switching to ehover tracker: '" << message << "'";
+ 7201 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7202 :
+ 7203 : return std::tuple(success, ss.str());
+ 7204 4 : }
+ 7205 : }
+ 7206 0 :
+ 7207 0 : {
+ 7208 0 : auto [success, message] = switchController(_eland_controller_name_);
+ 7209 :
+ 7210 : // check if the controller was successfully switched
+ 7211 : // this is not vital, we can continue without that
+ 7212 4 : if (!success) {
+ 7213 4 :
+ 7214 4 : std::stringstream ss;
+ 7215 : ss << "error during switching to ehover controller: '" << message << "'";
+ 7216 4 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7217 : }
+ 7218 4 : }
+ 7219 :
+ 7220 : std::stringstream ss;
+ 7221 : ss << "ehover activated";
+ 7222 : ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7223 :
+ 7224 : callbacks_enabled_ = false;
+ 7225 8 :
+ 7226 : return std::tuple(true, ss.str());
+ 7227 8 : }
+ 7228 0 :
+ 7229 : //}
+ 7230 8 :
+ 7231 0 : /* eland() //{ */
+ 7232 :
+ 7233 8 : std::tuple<bool, std::string> ControlManager::eland(void) {
+ 7234 0 :
+ 7235 : if (!is_initialized_)
+ 7236 : return std::tuple(false, "the ControlManager is not initialized");
+ 7237 16 :
+ 7238 8 : if (eland_triggered_)
+ 7239 : return std::tuple(false, "cannot eland, eland already triggered");
+ 7240 8 :
+ 7241 : if (failsafe_triggered_)
+ 7242 0 : return std::tuple(false, "cannot eland, failsafe already triggered");
+ 7243 0 :
+ 7244 0 : // copy member variables
+ 7245 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 7246 0 : auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+ 7247 :
+ 7248 : if (active_tracker_idx == _null_tracker_idx_) {
+ 7249 8 :
+ 7250 : std::stringstream ss;
+ 7251 0 : ss << "can not trigger eland while not flying";
+ 7252 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7253 0 :
+ 7254 : return std::tuple(false, ss.str());
+ 7255 : }
+ 7256 :
+ 7257 8 : if (_rc_emergency_handoff_) {
+ 7258 :
+ 7259 : toggleOutput(false);
+ 7260 :
+ 7261 8 : return std::tuple(true, "RC emergency handoff is ON, disabling output");
+ 7262 : }
+ 7263 0 :
+ 7264 0 : {
+ 7265 0 : auto [success, message] = switchTracker(_ehover_tracker_name_);
+ 7266 :
+ 7267 0 : // check if the tracker was successfully switched
+ 7268 : // this is vital
+ 7269 : if (!success) {
+ 7270 :
+ 7271 : std::stringstream ss;
+ 7272 16 : ss << "error during switching to eland tracker: '" << message << "'";
+ 7273 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7274 :
+ 7275 : return std::tuple(success, ss.str());
+ 7276 8 : }
+ 7277 : }
+ 7278 0 :
+ 7279 0 : {
+ 7280 0 : auto [success, message] = switchController(_eland_controller_name_);
+ 7281 :
+ 7282 : // check if the controller was successfully switched
+ 7283 : // this is not vital, we can continue without it
+ 7284 : if (!success) {
+ 7285 :
+ 7286 8 : std::stringstream ss;
+ 7287 : ss << "error during switching to eland controller: '" << message << "'";
+ 7288 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7289 8 : }
+ 7290 : }
+ 7291 8 :
+ 7292 : // | ----------------- call the eland service ----------------- |
+ 7293 8 :
+ 7294 : std::stringstream ss;
+ 7295 8 : bool success;
+ 7296 8 :
+ 7297 : if (elandSrv()) {
+ 7298 8 :
+ 7299 : changeLandingState(LANDING_STATE);
+ 7300 8 :
+ 7301 : odometryCallbacksSrv(false);
+ 7302 :
+ 7303 : ss << "eland activated";
+ 7304 0 : ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7305 0 :
+ 7306 : success = true;
+ 7307 0 :
+ 7308 : callbacks_enabled_ = false;
+ 7309 :
+ 7310 16 : } else {
+ 7311 :
+ 7312 : ss << "error during activation of eland";
+ 7313 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7314 :
+ 7315 : success = false;
+ 7316 : }
+ 7317 8 :
+ 7318 : return std::tuple(success, ss.str());
+ 7319 : }
+ 7320 16 :
+ 7321 8 : //}
+ 7322 8 :
+ 7323 : /* failsafe() //{ */
+ 7324 8 :
+ 7325 0 : std::tuple<bool, std::string> ControlManager::failsafe(void) {
+ 7326 :
+ 7327 : // copy member variables
+ 7328 8 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 7329 0 : auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+ 7330 : auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+ 7331 :
+ 7332 8 : if (!is_initialized_) {
+ 7333 : return std::tuple(false, "the ControlManager is not initialized");
+ 7334 0 : }
+ 7335 0 :
+ 7336 0 : if (failsafe_triggered_) {
+ 7337 0 : return std::tuple(false, "cannot, failsafe already triggered");
+ 7338 : }
+ 7339 :
+ 7340 8 : if (active_tracker_idx == _null_tracker_idx_) {
+ 7341 :
+ 7342 0 : std::stringstream ss;
+ 7343 : ss << "can not trigger failsafe while not flying";
+ 7344 0 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7345 : return std::tuple(false, ss.str());
+ 7346 : }
+ 7347 8 :
+ 7348 0 : if (_rc_emergency_handoff_) {
+ 7349 :
+ 7350 : toggleOutput(false);
+ 7351 8 :
+ 7352 : return std::tuple(true, "RC emergency handoff is ON, disabling output");
+ 7353 0 : }
+ 7354 :
+ 7355 0 : if (getLowestOuput(_hw_api_inputs_) == POSITION) {
+ 7356 : return eland();
+ 7357 0 : }
+ 7358 0 :
+ 7359 0 : if (_parachute_enabled_) {
+ 7360 :
+ 7361 0 : auto [success, message] = deployParachute();
+ 7362 :
+ 7363 : if (success) {
+ 7364 :
+ 7365 0 : std::stringstream ss;
+ 7366 0 : ss << "failsafe activated (parachute): '" << message << "'";
+ 7367 0 : ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+ 7368 :
+ 7369 : return std::tuple(true, ss.str());
+ 7370 :
+ 7371 8 : } else {
+ 7372 :
+ 7373 : std::stringstream ss;
+ 7374 : ss << "could not deploy parachute: '" << message << "', continuing with normal failsafe";
+ 7375 16 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7376 : }
+ 7377 8 : }
+ 7378 8 :
+ 7379 : if (_failsafe_controller_idx_ != active_controller_idx) {
+ 7380 :
+ 7381 16 : try {
+ 7382 :
+ 7383 : std::scoped_lock lock(mutex_controller_list_);
+ 7384 8 :
+ 7385 : ROS_INFO("[ControlManager]: activating the controller '%s'", _failsafe_controller_name_.c_str());
+ 7386 : controller_list_.at(_failsafe_controller_idx_)->activate(last_control_output);
+ 7387 8 :
+ 7388 8 : {
+ 7389 8 : std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+ 7390 8 :
+ 7391 : // update the time (used in failsafe)
+ 7392 8 : controller_tracker_switch_time_ = ros::Time::now();
+ 7393 : }
+ 7394 8 :
+ 7395 8 : failsafe_triggered_ = true;
+ 7396 8 : ROS_DEBUG("[ControlManager]: stopping eland timer");
+ 7397 8 : timer_eland_.stop();
+ 7398 : ROS_DEBUG("[ControlManager]: eland timer stopped");
+ 7399 8 :
+ 7400 : landing_uav_mass_ = getMass();
+ 7401 8 :
+ 7402 : eland_triggered_ = false;
+ 7403 8 : ROS_DEBUG("[ControlManager]: starting failsafe timer");
+ 7404 : timer_failsafe_.start();
+ 7405 8 : ROS_DEBUG("[ControlManager]: failsafe timer started");
+ 7406 :
+ 7407 : bumper_enabled_ = false;
+ 7408 :
+ 7409 8 : odometryCallbacksSrv(false);
+ 7410 8 :
+ 7411 : callbacks_enabled_ = false;
+ 7412 0 :
+ 7413 0 : ROS_INFO_THROTTLE(1.0, "[ControlManager]: the controller '%s' was activated", _failsafe_controller_name_.c_str());
+ 7414 :
+ 7415 : // super important, switch the active controller idx
+ 7416 0 : try {
+ 7417 0 : controller_list_.at(active_controller_idx_)->deactivate();
+ 7418 0 : active_controller_idx_ = _failsafe_controller_idx_;
+ 7419 : }
+ 7420 : catch (std::runtime_error& exrun) {
+ 7421 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not deactivate the controller '%s'", _controller_names_.at(active_controller_idx_).c_str());
+ 7422 16 : }
+ 7423 : }
+ 7424 : catch (std::runtime_error& exrun) {
+ 7425 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: error during activation of the controller '%s'", _failsafe_controller_name_.c_str());
+ 7426 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+ 7427 : }
+ 7428 : }
+ 7429 150 :
+ 7430 300 : return std::tuple(true, "failsafe activated");
+ 7431 : }
+ 7432 150 :
+ 7433 : //}
+ 7434 142 :
+ 7435 142 : /* escalatingFailsafe() //{ */
+ 7436 :
+ 7437 142 : std::tuple<bool, std::string> ControlManager::escalatingFailsafe(void) {
+ 7438 : std::stringstream ss;
+ 7439 :
+ 7440 8 : if ((ros::Time::now() - escalating_failsafe_time_).toSec() < _escalating_failsafe_timeout_) {
+ 7441 :
+ 7442 0 : ss << "too soon for escalating failsafe";
+ 7443 0 : ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+ 7444 :
+ 7445 0 : return std::tuple(false, ss.str());
+ 7446 : }
+ 7447 :
+ 7448 8 : if (!output_enabled_) {
+ 7449 :
+ 7450 8 : ss << "not escalating failsafe, output is disabled";
+ 7451 8 : ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+ 7452 :
+ 7453 16 : return std::tuple(false, ss.str());
+ 7454 16 : }
+ 7455 :
+ 7456 8 : ROS_WARN("[ControlManager]: escalating failsafe triggered");
+ 7457 :
+ 7458 8 : auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+ 7459 : auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+ 7460 8 :
+ 7461 : std::string active_tracker_name = _tracker_names_.at(active_tracker_idx);
+ 7462 0 : std::string active_controller_name = _controller_names_.at(active_controller_idx);
+ 7463 :
+ 7464 0 : EscalatingFailsafeStates_t next_state = getNextEscFailsafeState();
+ 7465 0 :
+ 7466 : escalating_failsafe_time_ = ros::Time::now();
+ 7467 0 :
+ 7468 : switch (next_state) {
+ 7469 :
+ 7470 : case ESC_NONE_STATE: {
+ 7471 :
+ 7472 2 : ss << "escalating failsafe has run to impossible situation";
+ 7473 : ROS_ERROR_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+ 7474 2 :
+ 7475 2 : return std::tuple(false, "escalating failsafe has run to impossible situation");
+ 7476 :
+ 7477 4 : break;
+ 7478 : }
+ 7479 2 :
+ 7480 2 : case ESC_EHOVER_STATE: {
+ 7481 :
+ 7482 : ss << "escalating failsafe escalates to ehover";
+ 7483 2 : ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+ 7484 :
+ 7485 : auto [success, message] = ehover();
+ 7486 :
+ 7487 : if (success) {
+ 7488 2 : state_escalating_failsafe_ = ESC_EHOVER_STATE;
+ 7489 : }
+ 7490 2 :
+ 7491 2 : return {success, message};
+ 7492 :
+ 7493 4 : break;
+ 7494 : }
+ 7495 2 :
+ 7496 2 : case ESC_ELAND_STATE: {
+ 7497 :
+ 7498 : ss << "escalating failsafe escalates to eland";
+ 7499 2 : ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+ 7500 :
+ 7501 : auto [success, message] = eland();
+ 7502 :
+ 7503 : if (success) {
+ 7504 2 : state_escalating_failsafe_ = ESC_ELAND_STATE;
+ 7505 : }
+ 7506 2 :
+ 7507 : return {success, message};
+ 7508 2 :
+ 7509 2 : break;
+ 7510 : }
+ 7511 4 :
+ 7512 : case ESC_FAILSAFE_STATE: {
+ 7513 2 :
+ 7514 2 : escalating_failsafe_time_ = ros::Time::now();
+ 7515 :
+ 7516 : ss << "escalating failsafe escalates to failsafe";
+ 7517 2 : ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+ 7518 :
+ 7519 : auto [success, message] = failsafe();
+ 7520 :
+ 7521 : if (success) {
+ 7522 2 : state_escalating_failsafe_ = ESC_FINISHED_STATE;
+ 7523 : }
+ 7524 2 :
+ 7525 : return {success, message};
+ 7526 2 :
+ 7527 2 : break;
+ 7528 : }
+ 7529 4 :
+ 7530 : case ESC_FINISHED_STATE: {
+ 7531 :
+ 7532 : escalating_failsafe_time_ = ros::Time::now();
+ 7533 :
+ 7534 0 : ss << "escalating failsafe has nothing more to do";
+ 7535 : ROS_WARN_STREAM_THROTTLE(0.1, "[ControlManager]: " << ss.str());
+ 7536 0 :
+ 7537 : return std::tuple(false, "escalating failsafe has nothing more to do");
+ 7538 :
+ 7539 : break;
+ 7540 0 : }
+ 7541 :
+ 7542 0 : default: {
+ 7543 :
+ 7544 : break;
+ 7545 : }
+ 7546 : }
+ 7547 :
+ 7548 : ROS_ERROR("[ControlManager]: escalatingFailsafe() reached the final return, this should not happen!");
+ 7549 8 :
+ 7550 8 : return std::tuple(false, "escalating failsafe exception");
+ 7551 : }
+ 7552 8 :
+ 7553 : //}
+ 7554 2 :
+ 7555 : /* getNextEscFailsafeState() //{ */
+ 7556 2 :
+ 7557 : EscalatingFailsafeStates_t ControlManager::getNextEscFailsafeState(void) {
+ 7558 : EscalatingFailsafeStates_t current_state = state_escalating_failsafe_;
+ 7559 :
+ 7560 : switch (current_state) {
+ 7561 2 :
+ 7562 : case ESC_FINISHED_STATE: {
+ 7563 2 :
+ 7564 2 : return ESC_FINISHED_STATE;
+ 7565 0 :
+ 7566 0 : break;
+ 7567 0 : }
+ 7568 0 :
+ 7569 : case ESC_NONE_STATE: {
+ 7570 0 :
+ 7571 : if (_escalating_failsafe_ehover_) {
+ 7572 : return ESC_EHOVER_STATE;
+ 7573 : } else if (_escalating_failsafe_eland_) {
+ 7574 : return ESC_ELAND_STATE;
+ 7575 : } else if (_escalating_failsafe_failsafe_) {
+ 7576 2 : return ESC_FAILSAFE_STATE;
+ 7577 : } else {
+ 7578 2 : return ESC_FINISHED_STATE;
+ 7579 2 : }
+ 7580 0 :
+ 7581 0 : break;
+ 7582 : }
+ 7583 0 :
+ 7584 : case ESC_EHOVER_STATE: {
+ 7585 :
+ 7586 : if (_escalating_failsafe_eland_) {
+ 7587 : return ESC_ELAND_STATE;
+ 7588 : } else if (_escalating_failsafe_failsafe_) {
+ 7589 2 : return ESC_FAILSAFE_STATE;
+ 7590 : } else {
+ 7591 2 : return ESC_FINISHED_STATE;
+ 7592 2 : }
+ 7593 :
+ 7594 0 : break;
+ 7595 : }
+ 7596 :
+ 7597 : case ESC_ELAND_STATE: {
+ 7598 :
+ 7599 : if (_escalating_failsafe_failsafe_) {
+ 7600 0 : return ESC_FAILSAFE_STATE;
+ 7601 : } else {
+ 7602 0 : return ESC_FINISHED_STATE;
+ 7603 : }
+ 7604 :
+ 7605 : break;
+ 7606 : }
+ 7607 :
+ 7608 0 : case ESC_FAILSAFE_STATE: {
+ 7609 :
+ 7610 0 : return ESC_FINISHED_STATE;
+ 7611 :
+ 7612 : break;
+ 7613 : }
+ 7614 : }
+ 7615 :
+ 7616 : ROS_ERROR("[ControlManager]: getNextEscFailsafeState() reached the final return, this should not happen!");
+ 7617 :
+ 7618 : return ESC_NONE_STATE;
+ 7619 1 : }
+ 7620 1 :
+ 7621 0 : //}
+ 7622 :
+ 7623 : // | ------------------- trajectory tracking ------------------ |
+ 7624 2 :
+ 7625 : /* startTrajectoryTracking() //{ */
+ 7626 1 :
+ 7627 1 : std::tuple<bool, std::string> ControlManager::startTrajectoryTracking(void) {
+ 7628 : if (!is_initialized_)
+ 7629 : return std::tuple(false, "the ControlManager is not initialized");
+ 7630 1 :
+ 7631 : {
+ 7632 1 : std::scoped_lock lock(mutex_tracker_list_);
+ 7633 :
+ 7634 2 : std_srvs::TriggerResponse::ConstPtr response;
+ 7635 : std_srvs::TriggerRequest request;
+ 7636 :
+ 7637 : response =
+ 7638 0 : tracker_list_.at(active_tracker_idx_)->startTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+ 7639 0 :
+ 7640 : if (response != std_srvs::TriggerResponse::Ptr()) {
+ 7641 0 :
+ 7642 : return std::tuple(response->success, response->message);
+ 7643 :
+ 7644 : } else {
+ 7645 :
+ 7646 : std::stringstream ss;
+ 7647 : ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'startTrajectoryTracking()' function!";
+ 7648 :
+ 7649 : return std::tuple(false, ss.str());
+ 7650 0 : }
+ 7651 0 : }
+ 7652 0 : }
+ 7653 :
+ 7654 : //}
+ 7655 0 :
+ 7656 : /* stopTrajectoryTracking() //{ */
+ 7657 0 :
+ 7658 0 : std::tuple<bool, std::string> ControlManager::stopTrajectoryTracking(void) {
+ 7659 : if (!is_initialized_)
+ 7660 : return std::tuple(false, "the ControlManager is not initialized");
+ 7661 0 :
+ 7662 : {
+ 7663 0 : std::scoped_lock lock(mutex_tracker_list_);
+ 7664 :
+ 7665 0 : std_srvs::TriggerResponse::ConstPtr response;
+ 7666 : std_srvs::TriggerRequest request;
+ 7667 :
+ 7668 : response =
+ 7669 0 : tracker_list_.at(active_tracker_idx_)->stopTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+ 7670 0 :
+ 7671 : if (response != std_srvs::TriggerResponse::Ptr()) {
+ 7672 0 :
+ 7673 : return std::tuple(response->success, response->message);
+ 7674 :
+ 7675 : } else {
+ 7676 :
+ 7677 : std::stringstream ss;
+ 7678 : ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'stopTrajectoryTracking()' function!";
+ 7679 :
+ 7680 : return std::tuple(false, ss.str());
+ 7681 0 : }
+ 7682 0 : }
+ 7683 0 : }
+ 7684 :
+ 7685 : //}
+ 7686 0 :
+ 7687 : /* resumeTrajectoryTracking() //{ */
+ 7688 0 :
+ 7689 0 : std::tuple<bool, std::string> ControlManager::resumeTrajectoryTracking(void) {
+ 7690 : if (!is_initialized_)
+ 7691 : return std::tuple(false, "the ControlManager is not initialized");
+ 7692 0 :
+ 7693 : {
+ 7694 0 : std::scoped_lock lock(mutex_tracker_list_);
+ 7695 :
+ 7696 0 : std_srvs::TriggerResponse::ConstPtr response;
+ 7697 : std_srvs::TriggerRequest request;
+ 7698 :
+ 7699 : response = tracker_list_.at(active_tracker_idx_)
+ 7700 0 : ->resumeTrajectoryTracking(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+ 7701 0 :
+ 7702 : if (response != std_srvs::TriggerResponse::Ptr()) {
+ 7703 0 :
+ 7704 : return std::tuple(response->success, response->message);
+ 7705 :
+ 7706 : } else {
+ 7707 :
+ 7708 : std::stringstream ss;
+ 7709 : ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'resumeTrajectoryTracking()' function!";
+ 7710 :
+ 7711 : return std::tuple(false, ss.str());
+ 7712 1 : }
+ 7713 1 : }
+ 7714 0 : }
+ 7715 :
+ 7716 : //}
+ 7717 2 :
+ 7718 : /* gotoTrajectoryStart() //{ */
+ 7719 1 :
+ 7720 1 : std::tuple<bool, std::string> ControlManager::gotoTrajectoryStart(void) {
+ 7721 : if (!is_initialized_)
+ 7722 1 : return std::tuple(false, "the ControlManager is not initialized");
+ 7723 :
+ 7724 1 : {
+ 7725 : std::scoped_lock lock(mutex_tracker_list_);
+ 7726 2 :
+ 7727 : std_srvs::TriggerResponse::ConstPtr response;
+ 7728 : std_srvs::TriggerRequest request;
+ 7729 :
+ 7730 0 : response =
+ 7731 0 : tracker_list_.at(active_tracker_idx_)->gotoTrajectoryStart(std_srvs::TriggerRequest::ConstPtr(std::make_unique<std_srvs::TriggerRequest>(request)));
+ 7732 :
+ 7733 0 : if (response != std_srvs::TriggerResponse::Ptr()) {
+ 7734 :
+ 7735 : return std::tuple(response->success, response->message);
+ 7736 :
+ 7737 : } else {
+ 7738 :
+ 7739 : std::stringstream ss;
+ 7740 : ss << "the tracker '" << _tracker_names_.at(active_tracker_idx_) << "' does not implement the 'gotoTrajectoryStart()' function!";
+ 7741 :
+ 7742 : return std::tuple(false, ss.str());
+ 7743 : }
+ 7744 18 : }
+ 7745 36 : }
+ 7746 :
+ 7747 18 : //}
+ 7748 :
+ 7749 0 : // | ----------------- service client wrappers ---------------- |
+ 7750 0 :
+ 7751 0 : /* arming() //{ */
+ 7752 :
+ 7753 : std::tuple<bool, std::string> ControlManager::arming(const bool input) {
+ 7754 18 : std::stringstream ss;
+ 7755 :
+ 7756 0 : if (input) {
+ 7757 0 :
+ 7758 0 : ss << "not allowed to arm using the ControlManager, maybe later when we don't do bugs";
+ 7759 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7760 : return std::tuple(false, ss.str());
+ 7761 18 : }
+ 7762 :
+ 7763 0 : if (!input && !isOffboard()) {
+ 7764 :
+ 7765 0 : ss << "can not disarm, not in OFFBOARD mode";
+ 7766 : ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7767 : return std::tuple(false, ss.str());
+ 7768 18 : }
+ 7769 :
+ 7770 18 : if (!input && _rc_emergency_handoff_) {
+ 7771 :
+ 7772 18 : toggleOutput(false);
+ 7773 :
+ 7774 18 : return std::tuple(true, "RC emergency handoff is ON, disabling output");
+ 7775 : }
+ 7776 18 :
+ 7777 : std_srvs::SetBool srv_out;
+ 7778 18 :
+ 7779 18 : srv_out.request.data = input ? 1 : 0; // arm or disarm?
+ 7780 :
+ 7781 18 : ROS_INFO("[ControlManager]: calling for %s", input ? "arming" : "disarming");
+ 7782 :
+ 7783 18 : if (sch_arming_.call(srv_out)) {
+ 7784 :
+ 7785 18 : if (srv_out.response.success) {
+ 7786 18 :
+ 7787 18 : ss << "service call for " << (input ? "arming" : "disarming") << " was successful";
+ 7788 : ROS_INFO_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7789 18 :
+ 7790 18 : if (!input) {
+ 7791 18 :
+ 7792 : toggleOutput(false);
+ 7793 :
+ 7794 : ROS_DEBUG("[ControlManager]: stopping failsafe timer");
+ 7795 0 : timer_failsafe_.stop();
+ 7796 0 : ROS_DEBUG("[ControlManager]: failsafe timer stopped");
+ 7797 :
+ 7798 : ROS_DEBUG("[ControlManager]: stopping the eland timer");
+ 7799 : timer_eland_.stop();
+ 7800 0 : ROS_DEBUG("[ControlManager]: eland timer stopped");
+ 7801 0 : }
+ 7802 :
+ 7803 : } else {
+ 7804 36 : ss << "service call for " << (input ? "arming" : "disarming") << " failed";
+ 7805 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7806 : }
+ 7807 :
+ 7808 : } else {
+ 7809 : ss << "calling for " << (input ? "arming" : "disarming") << " resulted in failure: '" << srv_out.response.message << "'";
+ 7810 : ROS_ERROR_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str());
+ 7811 16 : }
+ 7812 16 :
+ 7813 : return std::tuple(srv_out.response.success, ss.str());
+ 7814 32 : }
+ 7815 :
+ 7816 16 : //}
+ 7817 :
+ 7818 16 : /* odometryCallbacksSrv() //{ */
+ 7819 :
+ 7820 16 : void ControlManager::odometryCallbacksSrv(const bool input) {
+ 7821 : ROS_INFO("[ControlManager]: switching odometry callbacks to %s", input ? "ON" : "OFF");
+ 7822 16 :
+ 7823 0 : std_srvs::SetBool srv;
+ 7824 :
+ 7825 : srv.request.data = input;
+ 7826 :
+ 7827 0 : bool res = sch_set_odometry_callbacks_.call(srv);
+ 7828 :
+ 7829 16 : if (res) {
+ 7830 :
+ 7831 : if (!srv.response.success) {
+ 7832 : ROS_WARN("[ControlManager]: service call for toggle odometry callbacks returned: '%s'", srv.response.message.c_str());
+ 7833 : }
+ 7834 :
+ 7835 8 : } else {
+ 7836 8 : ROS_ERROR("[ControlManager]: service call for toggle odometry callbacks failed!");
+ 7837 : }
+ 7838 16 : }
+ 7839 :
+ 7840 8 : //}
+ 7841 :
+ 7842 8 : /* elandSrv() //{ */
+ 7843 :
+ 7844 8 : bool ControlManager::elandSrv(void) {
+ 7845 0 : ROS_INFO("[ControlManager]: calling for eland");
+ 7846 :
+ 7847 : std_srvs::Trigger srv;
+ 7848 8 :
+ 7849 : bool res = sch_eland_.call(srv);
+ 7850 :
+ 7851 : if (res) {
+ 7852 0 :
+ 7853 : if (!srv.response.success) {
+ 7854 0 : ROS_WARN("[ControlManager]: service call for eland returned: '%s'", srv.response.message.c_str());
+ 7855 : }
+ 7856 :
+ 7857 : return srv.response.success;
+ 7858 :
+ 7859 : } else {
+ 7860 :
+ 7861 : ROS_ERROR("[ControlManager]: service call for eland failed!");
+ 7862 0 :
+ 7863 0 : return false;
+ 7864 : }
+ 7865 0 : }
+ 7866 :
+ 7867 0 : //}
+ 7868 :
+ 7869 0 : /* parachuteSrv() //{ */
+ 7870 :
+ 7871 0 : bool ControlManager::parachuteSrv(void) {
+ 7872 0 : ROS_INFO("[ControlManager]: calling for parachute deployment");
+ 7873 :
+ 7874 : std_srvs::Trigger srv;
+ 7875 0 :
+ 7876 : bool res = sch_parachute_.call(srv);
+ 7877 :
+ 7878 : if (res) {
+ 7879 0 :
+ 7880 : if (!srv.response.success) {
+ 7881 0 : ROS_WARN("[ControlManager]: service call for parachute deployment returned: '%s'", srv.response.message.c_str());
+ 7882 : }
+ 7883 :
+ 7884 : return srv.response.success;
+ 7885 :
+ 7886 : } else {
+ 7887 :
+ 7888 : ROS_ERROR("[ControlManager]: service call for parachute deployment failed!");
+ 7889 71 :
+ 7890 142 : return false;
+ 7891 : }
+ 7892 71 : }
+ 7893 :
+ 7894 71 : //}
+ 7895 :
+ 7896 0 : /* ungripSrv() //{ */
+ 7897 0 :
+ 7898 : void ControlManager::ungripSrv(void) {
+ 7899 : std_srvs::Trigger srv;
+ 7900 :
+ 7901 71 : bool res = sch_ungrip_.call(srv);
+ 7902 :
+ 7903 71 : if (res) {
+ 7904 :
+ 7905 : if (!srv.response.success) {
+ 7906 : ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload returned: '%s'", srv.response.message.c_str());
+ 7907 : }
+ 7908 :
+ 7909 : } else {
+ 7910 : ROS_DEBUG_THROTTLE(1.0, "[ControlManager]: service call for ungripping payload failed!");
+ 7911 114 : }
+ 7912 : }
+ 7913 114 :
+ 7914 6 : //}
+ 7915 6 :
+ 7916 : // | ------------------------ routines ------------------------ |
+ 7917 :
+ 7918 108 : /* toggleOutput() //{ */
+ 7919 :
+ 7920 108 : void ControlManager::toggleOutput(const bool& input) {
+ 7921 :
+ 7922 : if (input == output_enabled_) {
+ 7923 108 : ROS_WARN_THROTTLE(0.1, "[ControlManager]: output is already %s", input ? "ON" : "OFF");
+ 7924 : return;
+ 7925 20 : }
+ 7926 :
+ 7927 20 : ROS_INFO("[ControlManager]: switching output %s", input ? "ON" : "OFF");
+ 7928 :
+ 7929 20 : output_enabled_ = input;
+ 7930 :
+ 7931 20 : // if switching output off, switch to NullTracker
+ 7932 : if (!output_enabled_) {
+ 7933 :
+ 7934 : ROS_INFO("[ControlManager]: switching to 'NullTracker' after switching output off");
+ 7935 140 :
+ 7936 : switchTracker(_null_tracker_name_);
+ 7937 120 :
+ 7938 120 : ROS_INFO_STREAM("[ControlManager]: switching to the controller '" << _eland_controller_name_ << "' after switching output off");
+ 7939 :
+ 7940 : switchController(_eland_controller_name_);
+ 7941 120 :
+ 7942 120 : // | --------- deactivate all trackers and controllers -------- |
+ 7943 :
+ 7944 0 : for (int i = 0; i < int(tracker_list_.size()); i++) {
+ 7945 0 :
+ 7946 : std::map<std::string, TrackerParams>::iterator it;
+ 7947 : it = trackers_.find(_tracker_names_.at(i));
+ 7948 :
+ 7949 120 : try {
+ 7950 : ROS_INFO("[ControlManager]: deactivating the tracker '%s'", it->second.address.c_str());
+ 7951 100 : tracker_list_.at(i)->deactivate();
+ 7952 100 : }
+ 7953 : catch (std::runtime_error& ex) {
+ 7954 : ROS_ERROR("[ControlManager]: exception caught during tracker deactivation: '%s'", ex.what());
+ 7955 100 : }
+ 7956 100 : }
+ 7957 :
+ 7958 0 : for (int i = 0; i < int(controller_list_.size()); i++) {
+ 7959 0 :
+ 7960 : std::map<std::string, ControllerParams>::iterator it;
+ 7961 : it = controllers_.find(_controller_names_.at(i));
+ 7962 :
+ 7963 20 : try {
+ 7964 20 : ROS_INFO("[ControlManager]: deactivating the controller '%s'", it->second.address.c_str());
+ 7965 20 : controller_list_.at(i)->deactivate();
+ 7966 : }
+ 7967 20 : catch (std::runtime_error& ex) {
+ 7968 : ROS_ERROR("[ControlManager]: exception caught during controller deactivation: '%s'", ex.what());
+ 7969 : }
+ 7970 : }
+ 7971 :
+ 7972 : timer_failsafe_.stop();
+ 7973 : timer_eland_.stop();
+ 7974 : timer_pirouette_.stop();
+ 7975 210 :
+ 7976 : offboard_mode_was_true_ = false;
+ 7977 630 : }
+ 7978 630 : }
+ 7979 :
+ 7980 : //}
+ 7981 420 :
+ 7982 210 : /* switchTracker() //{ */
+ 7983 :
+ 7984 420 : std::tuple<bool, std::string> ControlManager::switchTracker(const std::string& tracker_name) {
+ 7985 :
+ 7986 210 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("switchTracker");
+ 7987 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::switchTracker", scope_timer_logger_, scope_timer_enabled_);
+ 7988 0 :
+ 7989 0 : // copy member variables
+ 7990 0 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 7991 : auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+ 7992 :
+ 7993 210 : std::stringstream ss;
+ 7994 :
+ 7995 0 : if (!got_uav_state_) {
+ 7996 0 :
+ 7997 0 : ss << "can not switch tracker, missing odometry!";
+ 7998 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 7999 : return std::tuple(false, ss.str());
+ 8000 210 : }
+ 8001 :
+ 8002 : if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+ 8003 210 :
+ 8004 2 : ss << "can not switch tracker, missing odometry innovation!";
+ 8005 2 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 8006 2 : return std::tuple(false, ss.str());
+ 8007 : }
+ 8008 :
+ 8009 : auto new_tracker_idx = idxInVector(tracker_name, _tracker_names_);
+ 8010 208 :
+ 8011 11 : // check if the tracker exists
+ 8012 11 : if (!new_tracker_idx) {
+ 8013 11 : ss << "the tracker '" << tracker_name << "' does not exist!";
+ 8014 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 8015 : return std::tuple(false, ss.str());
+ 8016 : }
+ 8017 197 :
+ 8018 : // check if the tracker is already active
+ 8019 : if (new_tracker_idx.value() == active_tracker_idx) {
+ 8020 : ss << "not switching, the tracker '" << tracker_name << "' is already active!";
+ 8021 197 : ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+ 8022 : return std::tuple(true, ss.str());
+ 8023 197 : }
+ 8024 :
+ 8025 197 : {
+ 8026 : std::scoped_lock lock(mutex_tracker_list_);
+ 8027 0 :
+ 8028 0 : try {
+ 8029 0 :
+ 8030 : ROS_INFO("[ControlManager]: activating the tracker '%s'", _tracker_names_.at(new_tracker_idx.value()).c_str());
+ 8031 :
+ 8032 : auto [success, message] = tracker_list_.at(new_tracker_idx.value())->activate(last_tracker_cmd);
+ 8033 197 :
+ 8034 197 : if (!success) {
+ 8035 :
+ 8036 : ss << "the tracker '" << tracker_name << "' could not be activated: '" << message << "'";
+ 8037 394 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 8038 : return std::tuple(false, ss.str());
+ 8039 :
+ 8040 197 : } else {
+ 8041 :
+ 8042 : ss << "the tracker '" << tracker_name << "' was activated";
+ 8043 : ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+ 8044 :
+ 8045 : {
+ 8046 197 : std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+ 8047 197 :
+ 8048 : // update the time (used in failsafe)
+ 8049 : controller_tracker_switch_time_ = ros::Time::now();
+ 8050 197 : }
+ 8051 :
+ 8052 84 : // super important, switch the active tracker idx
+ 8053 : try {
+ 8054 168 :
+ 8055 : ROS_INFO("[ControlManager]: deactivating '%s'", _tracker_names_.at(active_tracker_idx_).c_str());
+ 8056 84 : tracker_list_.at(active_tracker_idx_)->deactivate();
+ 8057 :
+ 8058 168 : // if switching from null tracker, re-activate the already active the controller
+ 8059 : if (active_tracker_idx_ == _null_tracker_idx_) {
+ 8060 84 :
+ 8061 : ROS_INFO("[ControlManager]: reactivating '%s' due to switching from 'NullTracker'", _controller_names_.at(active_controller_idx_).c_str());
+ 8062 : {
+ 8063 168 : std::scoped_lock lock(mutex_controller_list_);
+ 8064 :
+ 8065 : initializeControlOutput();
+ 8066 84 :
+ 8067 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 8068 :
+ 8069 : controller_list_.at(active_controller_idx_)->activate(last_control_output);
+ 8070 :
+ 8071 113 : {
+ 8072 : std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+ 8073 17 :
+ 8074 : // update the time (used in failsafe)
+ 8075 34 : controller_tracker_switch_time_ = ros::Time::now();
+ 8076 : }
+ 8077 17 : }
+ 8078 :
+ 8079 : // if switching to null tracker, deactivate the active controller
+ 8080 : } else if (new_tracker_idx == _null_tracker_idx_) {
+ 8081 17 :
+ 8082 : ROS_INFO("[ControlManager]: deactivating '%s' due to switching to 'NullTracker'", _controller_names_.at(active_controller_idx_).c_str());
+ 8083 17 : {
+ 8084 : std::scoped_lock lock(mutex_controller_list_);
+ 8085 :
+ 8086 17 : controller_list_.at(active_controller_idx_)->deactivate();
+ 8087 : }
+ 8088 :
+ 8089 197 : {
+ 8090 : std::scoped_lock lock(mutex_last_tracker_cmd_);
+ 8091 0 :
+ 8092 0 : last_tracker_cmd_ = {};
+ 8093 : }
+ 8094 :
+ 8095 : initializeControlOutput();
+ 8096 0 : }
+ 8097 0 :
+ 8098 0 : active_tracker_idx_ = new_tracker_idx.value();
+ 8099 : }
+ 8100 : catch (std::runtime_error& exrun) {
+ 8101 : ROS_ERROR("[ControlManager]: could not deactivate the tracker '%s'", _tracker_names_.at(active_tracker_idx_).c_str());
+ 8102 197 : }
+ 8103 : }
+ 8104 : }
+ 8105 : catch (std::runtime_error& exrun) {
+ 8106 : ROS_ERROR("[ControlManager]: error during activation of the tracker '%s'", tracker_name.c_str());
+ 8107 : ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+ 8108 : }
+ 8109 211 : }
+ 8110 :
+ 8111 633 : return std::tuple(true, ss.str());
+ 8112 633 : }
+ 8113 :
+ 8114 : //}
+ 8115 422 :
+ 8116 211 : /* switchController() //{ */
+ 8117 :
+ 8118 422 : std::tuple<bool, std::string> ControlManager::switchController(const std::string& controller_name) {
+ 8119 :
+ 8120 211 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("switchController");
+ 8121 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::switchController", scope_timer_logger_, scope_timer_enabled_);
+ 8122 0 :
+ 8123 0 : // copy member variables
+ 8124 0 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 8125 : auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+ 8126 :
+ 8127 211 : std::stringstream ss;
+ 8128 :
+ 8129 0 : if (!got_uav_state_) {
+ 8130 0 :
+ 8131 0 : ss << "can not switch controller, missing odometry!";
+ 8132 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 8133 : return std::tuple(false, ss.str());
+ 8134 211 : }
+ 8135 :
+ 8136 : if (_state_input_ == INPUT_UAV_STATE && _odometry_innovation_check_enabled_ && !sh_odometry_innovation_.hasMsg()) {
+ 8137 211 :
+ 8138 2 : ss << "can not switch controller, missing odometry innovation!";
+ 8139 2 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 8140 2 : return std::tuple(false, ss.str());
+ 8141 : }
+ 8142 :
+ 8143 : auto new_controller_idx = idxInVector(controller_name, _controller_names_);
+ 8144 209 :
+ 8145 : // check if the controller exists
+ 8146 34 : if (!new_controller_idx) {
+ 8147 34 : ss << "the controller '" << controller_name << "' does not exist!";
+ 8148 34 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 8149 : return std::tuple(false, ss.str());
+ 8150 : }
+ 8151 :
+ 8152 175 : // check if the controller is not active
+ 8153 : if (new_controller_idx.value() == active_controller_idx) {
+ 8154 :
+ 8155 : ss << "not switching, the controller '" << controller_name << "' is already active!";
+ 8156 175 : ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+ 8157 175 : return std::tuple(true, ss.str());
+ 8158 : }
+ 8159 0 :
+ 8160 0 : {
+ 8161 0 : std::scoped_lock lock(mutex_controller_list_);
+ 8162 :
+ 8163 : try {
+ 8164 :
+ 8165 175 : ROS_INFO("[ControlManager]: activating the controller '%s'", _controller_names_.at(new_controller_idx.value()).c_str());
+ 8166 175 : if (!controller_list_.at(new_controller_idx.value())->activate(last_control_output)) {
+ 8167 :
+ 8168 175 : ss << "the controller '" << controller_name << "' was not activated";
+ 8169 : ROS_ERROR_STREAM("[ControlManager]: " << ss.str());
+ 8170 : return std::tuple(false, ss.str());
+ 8171 :
+ 8172 : } else {
+ 8173 :
+ 8174 : ss << "the controller '" << controller_name << "' was activated";
+ 8175 175 : ROS_INFO_STREAM("[ControlManager]: " << ss.str());
+ 8176 :
+ 8177 175 : ROS_INFO("[ControlManager]: triggering hover after switching to '%s', re-activating '%s'", _controller_names_.at(new_controller_idx.value()).c_str(),
+ 8178 175 : _tracker_names_.at(active_tracker_idx_).c_str());
+ 8179 :
+ 8180 : // reactivate the current tracker
+ 8181 : // TODO this is not the most elegant way to restart the tracker after a controller switch
+ 8182 350 : // but it serves the purpose
+ 8183 : {
+ 8184 : std::scoped_lock lock(mutex_tracker_list_);
+ 8185 175 :
+ 8186 : tracker_list_.at(active_tracker_idx_)->deactivate();
+ 8187 : tracker_list_.at(active_tracker_idx_)->activate({});
+ 8188 : }
+ 8189 :
+ 8190 : {
+ 8191 175 : std::scoped_lock lock(mutex_controller_tracker_switch_time_);
+ 8192 175 :
+ 8193 : // update the time (used in failsafe)
+ 8194 0 : controller_tracker_switch_time_ = ros::Time::now();
+ 8195 0 : }
+ 8196 :
+ 8197 : // super important, switch the active controller idx
+ 8198 : try {
+ 8199 0 :
+ 8200 0 : controller_list_.at(active_controller_idx_)->deactivate();
+ 8201 0 : active_controller_idx_ = new_controller_idx.value();
+ 8202 : }
+ 8203 : catch (std::runtime_error& exrun) {
+ 8204 : ROS_ERROR("[ControlManager]: could not deactivate controller '%s'", _controller_names_.at(active_controller_idx_).c_str());
+ 8205 175 : }
+ 8206 : }
+ 8207 : }
+ 8208 175 : catch (std::runtime_error& exrun) {
+ 8209 : ROS_ERROR("[ControlManager]: error during activation of controller '%s'", controller_name.c_str());
+ 8210 175 : ROS_ERROR("[ControlManager]: exception: '%s'", exrun.what());
+ 8211 175 : }
+ 8212 : }
+ 8213 :
+ 8214 175 : mrs_msgs::DynamicsConstraintsSrvRequest sanitized_constraints;
+ 8215 :
+ 8216 175 : {
+ 8217 : std::scoped_lock lock(mutex_constraints_);
+ 8218 :
+ 8219 : sanitized_constraints_ = current_constraints_;
+ 8220 : sanitized_constraints = sanitized_constraints_;
+ 8221 : }
+ 8222 :
+ 8223 120882 : setConstraintsToControllers(sanitized_constraints);
+ 8224 :
+ 8225 241764 : return std::tuple(true, ss.str());
+ 8226 241764 : }
+ 8227 :
+ 8228 : //}
+ 8229 120882 :
+ 8230 120882 : /* updateTrackers() //{ */
+ 8231 :
+ 8232 : void ControlManager::updateTrackers(void) {
+ 8233 :
+ 8234 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("updateTrackers");
+ 8235 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::updateTrackers", scope_timer_logger_, scope_timer_enabled_);
+ 8236 0 :
+ 8237 : // copy member variables
+ 8238 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 8239 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 8240 :
+ 8241 120882 : // --------------------------------------------------------------
+ 8242 : // | Update the trackers |
+ 8243 120882 : // --------------------------------------------------------------
+ 8244 :
+ 8245 : std::optional<mrs_msgs::TrackerCommand> tracker_command;
+ 8246 847929 :
+ 8247 : unsigned int active_tracker_idx;
+ 8248 727047 :
+ 8249 : {
+ 8250 : std::scoped_lock lock(mutex_tracker_list_);
+ 8251 :
+ 8252 120882 : active_tracker_idx = active_tracker_idx_;
+ 8253 :
+ 8254 0 : // for each tracker
+ 8255 0 : for (size_t i = 0; i < tracker_list_.size(); i++) {
+ 8256 0 :
+ 8257 0 : if (i == active_tracker_idx) {
+ 8258 :
+ 8259 : try {
+ 8260 : // active tracker => update and retrieve the command
+ 8261 : tracker_command = tracker_list_.at(i)->update(uav_state, last_control_output);
+ 8262 : }
+ 8263 : catch (std::runtime_error& exrun) {
+ 8264 606165 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: caught an exception while updating the active tracker (%s)",
+ 8265 : _tracker_names_.at(active_tracker_idx).c_str());
+ 8266 0 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+ 8267 0 : tracker_command = {};
+ 8268 0 : }
+ 8269 :
+ 8270 : } else {
+ 8271 :
+ 8272 : try {
+ 8273 120882 : // nonactive tracker => just update without retrieving the command
+ 8274 35617 : tracker_list_.at(i)->update(uav_state, last_control_output);
+ 8275 : }
+ 8276 : catch (std::runtime_error& exrun) {
+ 8277 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: caught an exception while updating the tracker '%s'", _tracker_names_.at(i).c_str());
+ 8278 85265 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+ 8279 : }
+ 8280 170530 : }
+ 8281 : }
+ 8282 85265 :
+ 8283 : if (active_tracker_idx == _null_tracker_idx_) {
+ 8284 : return;
+ 8285 : }
+ 8286 85265 : }
+ 8287 0 :
+ 8288 : if (validateTrackerCommand(tracker_command, "ControlManager", "tracker_command")) {
+ 8289 :
+ 8290 85265 : std::scoped_lock lock(mutex_last_tracker_cmd_);
+ 8291 0 :
+ 8292 : last_tracker_cmd_ = tracker_command;
+ 8293 :
+ 8294 : // | --------- fill in the potentially missing header --------- |
+ 8295 :
+ 8296 0 : if (last_tracker_cmd_->header.frame_id == "") {
+ 8297 : last_tracker_cmd_->header.frame_id = uav_state.header.frame_id;
+ 8298 0 : }
+ 8299 0 :
+ 8300 : if (last_tracker_cmd_->header.stamp == ros::Time(0)) {
+ 8301 : last_tracker_cmd_->header.stamp = ros::Time::now();
+ 8302 : }
+ 8303 0 :
+ 8304 : } else {
+ 8305 0 :
+ 8306 0 : if (active_tracker_idx == _ehover_tracker_idx_) {
+ 8307 0 :
+ 8308 0 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the emergency tracker '%s' returned empty or invalid command!",
+ 8309 : _tracker_names_.at(active_tracker_idx).c_str());
+ 8310 0 : failsafe();
+ 8311 :
+ 8312 : } else {
+ 8313 :
+ 8314 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the tracker '%s' returned empty or invalid command!", _tracker_names_.at(active_tracker_idx).c_str());
+ 8315 :
+ 8316 : if (_tracker_error_action_ == ELAND_STR) {
+ 8317 : eland();
+ 8318 : } else if (_tracker_error_action_ == EHOVER_STR) {
+ 8319 : ehover();
+ 8320 131089 : } else {
+ 8321 : failsafe();
+ 8322 262178 : }
+ 8323 262178 : }
+ 8324 : }
+ 8325 : }
+ 8326 131089 :
+ 8327 : //}
+ 8328 :
+ 8329 : /* updateControllers() //{ */
+ 8330 :
+ 8331 131089 : void ControlManager::updateControllers(const mrs_msgs::UavState& uav_state) {
+ 8332 :
+ 8333 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("updateControllers");
+ 8334 71234 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::updateControllers", scope_timer_logger_, scope_timer_enabled_);
+ 8335 :
+ 8336 : // copy member variables
+ 8337 213702 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 8338 178085 :
+ 8339 : // | ----------------- update the controllers ----------------- |
+ 8340 :
+ 8341 : // the trackers are not running
+ 8342 35617 : if (!last_tracker_cmd) {
+ 8343 :
+ 8344 : {
+ 8345 190944 : std::scoped_lock lock(mutex_controller_list_);
+ 8346 :
+ 8347 : // nonactive controller => just update without retrieving the command
+ 8348 : for (int i = 0; i < int(controller_list_.size()); i++) {
+ 8349 : controller_list_.at(i)->updateInactive(uav_state, last_tracker_cmd);
+ 8350 190944 : }
+ 8351 : }
+ 8352 95472 :
+ 8353 : return;
+ 8354 : }
+ 8355 572832 :
+ 8356 : Controller::ControlOutput control_output;
+ 8357 477360 :
+ 8358 : unsigned int active_controller_idx;
+ 8359 :
+ 8360 : {
+ 8361 95472 : std::scoped_lock lock(mutex_controller_list_);
+ 8362 :
+ 8363 0 : active_controller_idx = active_controller_idx_;
+ 8364 :
+ 8365 0 : // for each controller
+ 8366 : for (size_t i = 0; i < controller_list_.size(); i++) {
+ 8367 0 :
+ 8368 : if (i == active_controller_idx) {
+ 8369 :
+ 8370 : try {
+ 8371 : // active controller => update and retrieve the command
+ 8372 : control_output = controller_list_.at(active_controller_idx)->updateActive(uav_state, last_tracker_cmd.value());
+ 8373 : }
+ 8374 381888 : catch (std::runtime_error& exrun) {
+ 8375 :
+ 8376 0 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: an exception while updating the active controller (%s)",
+ 8377 : _controller_names_.at(active_controller_idx).c_str());
+ 8378 0 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the exception: '%s'", exrun.what());
+ 8379 0 : }
+ 8380 :
+ 8381 : } else {
+ 8382 :
+ 8383 : try {
+ 8384 : // nonactive controller => just update without retrieving the command
+ 8385 : controller_list_.at(i)->updateInactive(uav_state, last_tracker_cmd);
+ 8386 95472 : }
+ 8387 : catch (std::runtime_error& exrun) {
+ 8388 95472 :
+ 8389 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception while updating the controller '%s'", _controller_names_.at(i).c_str());
+ 8390 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: exception: '%s'", exrun.what());
+ 8391 : }
+ 8392 : }
+ 8393 : }
+ 8394 : }
+ 8395 :
+ 8396 : // normally, the active controller returns a valid command
+ 8397 : if (validateControlOutput(control_output, _hw_api_inputs_, "ControlManager", "control_output")) {
+ 8398 0 :
+ 8399 : mrs_lib::set_mutexed(mutex_last_control_output_, control_output, last_control_output_);
+ 8400 :
+ 8401 0 : // but it can return an empty command, due to some critical internal error
+ 8402 : // which means we should trigger the failsafe landing
+ 8403 0 : } else {
+ 8404 :
+ 8405 : // only if the controller is still active, trigger escalating failsafe
+ 8406 0 : // if not active, we don't care, we should not ask the controller for
+ 8407 : // the result anyway -> this could mean a race condition occured
+ 8408 0 : // like it once happend during landing
+ 8409 : bool controller_status = false;
+ 8410 0 :
+ 8411 : {
+ 8412 0 : std::scoped_lock lock(mutex_controller_list_);
+ 8413 :
+ 8414 0 : controller_status = controller_list_.at(active_controller_idx)->getStatus().active;
+ 8415 : }
+ 8416 0 :
+ 8417 : if (controller_status) {
+ 8418 0 :
+ 8419 : if (failsafe_triggered_) {
+ 8420 :
+ 8421 : ROS_ERROR("[ControlManager]: disabling control, the active controller returned an empty command when failsafe was active");
+ 8422 0 :
+ 8423 : toggleOutput(false);
+ 8424 0 :
+ 8425 : } else if (eland_triggered_) {
+ 8426 :
+ 8427 : ROS_ERROR("[ControlManager]: triggering failsafe, the active controller returned an empty command when eland was active");
+ 8428 :
+ 8429 : failsafe();
+ 8430 :
+ 8431 : } else {
+ 8432 :
+ 8433 : ROS_ERROR("[ControlManager]: triggering eland, the active controller returned an empty command");
+ 8434 131089 :
+ 8435 : eland();
+ 8436 262178 : }
+ 8437 262178 : }
+ 8438 : }
+ 8439 : }
+ 8440 131089 :
+ 8441 131089 : //}
+ 8442 131089 :
+ 8443 131089 : /* publish() //{ */
+ 8444 131089 :
+ 8445 : void ControlManager::publish(void) {
+ 8446 131089 :
+ 8447 : mrs_lib::Routine profiler_routine = profiler_.createRoutine("publish");
+ 8448 : mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("ControlManager::publish", scope_timer_logger_, scope_timer_enabled_);
+ 8449 :
+ 8450 : // copy member variables
+ 8451 : auto last_control_output = mrs_lib::get_mutexed(mutex_last_control_output_, last_control_output_);
+ 8452 131089 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 8453 131089 : auto active_tracker_idx = mrs_lib::get_mutexed(mutex_tracker_list_, active_tracker_idx_);
+ 8454 : auto active_controller_idx = mrs_lib::get_mutexed(mutex_controller_list_, active_controller_idx_);
+ 8455 131089 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 8456 131089 :
+ 8457 : publishControlReferenceOdom(last_tracker_cmd, last_control_output);
+ 8458 131089 :
+ 8459 : // --------------------------------------------------------------
+ 8460 21499 : // | Publish the control command |
+ 8461 : // --------------------------------------------------------------
+ 8462 109590 :
+ 8463 : mrs_msgs::HwApiAttitudeCmd attitude_target;
+ 8464 14122 : attitude_target.stamp = ros::Time::now();
+ 8465 :
+ 8466 : mrs_msgs::HwApiAttitudeRateCmd attitude_rate_target;
+ 8467 28244 : attitude_rate_target.stamp = ros::Time::now();
+ 8468 :
+ 8469 : if (!output_enabled_) {
+ 8470 28244 :
+ 8471 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: output is disabled");
+ 8472 14122 :
+ 8473 : } else if (active_tracker_idx == _null_tracker_idx_) {
+ 8474 :
+ 8475 14122 : ROS_WARN_THROTTLE(5.0, "[ControlManager]: 'NullTracker' is active, not controlling");
+ 8476 :
+ 8477 95468 : Controller::HwApiOutputVariant output =
+ 8478 : initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+ 8479 0 :
+ 8480 : {
+ 8481 : std::scoped_lock lock(mutex_last_control_output_);
+ 8482 :
+ 8483 0 : last_control_output_.control_output = output;
+ 8484 : }
+ 8485 0 :
+ 8486 : control_output_publisher_.publish(output);
+ 8487 95468 :
+ 8488 : } else if (active_tracker_idx != _null_tracker_idx_ && !last_control_output.control_output) {
+ 8489 95468 :
+ 8490 95468 : ROS_WARN_THROTTLE(1.0, "[ControlManager]: the controller '%s' returned nil command, not publishing anything",
+ 8491 : _controller_names_.at(active_controller_idx).c_str());
+ 8492 0 :
+ 8493 0 : Controller::HwApiOutputVariant output =
+ 8494 : initializeDefaultOutput(_hw_api_inputs_, uav_state, _min_throttle_null_tracker_, common_handlers_->throttle_model.n_motors);
+ 8495 :
+ 8496 : control_output_publisher_.publish(output);
+ 8497 0 :
+ 8498 : } else if (last_control_output.control_output) {
+ 8499 :
+ 8500 : if (validateHwApiAttitudeCmd(attitude_target, "ControlManager", "last_control_output.control_output")) {
+ 8501 : control_output_publisher_.publish(last_control_output.control_output.value());
+ 8502 131089 : } else {
+ 8503 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: the attitude cmd is not valid just before publishing!");
+ 8504 : return;
+ 8505 : }
+ 8506 131089 :
+ 8507 : } else {
+ 8508 131089 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: not publishing a control command");
+ 8509 : }
+ 8510 :
+ 8511 103923 : // | ----------- publish the controller diagnostics ----------- |
+ 8512 103923 :
+ 8513 103923 : ph_controller_diagnostics_.publish(last_control_output.diagnostics);
+ 8514 :
+ 8515 : // | --------- publish the applied throttle and thrust -------- |
+ 8516 103923 :
+ 8517 : auto throttle = extractThrottle(last_control_output);
+ 8518 :
+ 8519 103923 : if (throttle) {
+ 8520 103923 :
+ 8521 103923 : {
+ 8522 : std_msgs::Float64 msg;
+ 8523 : msg.data = throttle.value();
+ 8524 : ph_throttle_.publish(msg);
+ 8525 : }
+ 8526 :
+ 8527 131089 : double thrust = mrs_lib::quadratic_throttle_model::throttleToForce(common_handlers_->throttle_model, throttle.value());
+ 8528 95468 :
+ 8529 : {
+ 8530 : std_msgs::Float64 msg;
+ 8531 : msg.data = thrust;
+ 8532 : ph_thrust_.publish(msg);
+ 8533 131089 : }
+ 8534 : }
+ 8535 221026 :
+ 8536 : // | ----------------- publish tracker command ---------------- |
+ 8537 110513 :
+ 8538 110513 : if (last_tracker_cmd) {
+ 8539 : ph_tracker_cmd_.publish(last_tracker_cmd.value());
+ 8540 110513 : }
+ 8541 80816 :
+ 8542 80816 : // | --------------- publish the odometry input --------------- |
+ 8543 80816 :
+ 8544 : if (last_control_output.control_output) {
+ 8545 :
+ 8546 110513 : mrs_msgs::EstimatorInput msg;
+ 8547 77501 :
+ 8548 : msg.header.frame_id = _uav_name_ + "/fcu";
+ 8549 : msg.header.stamp = ros::Time::now();
+ 8550 110513 :
+ 8551 80816 : if (last_control_output.desired_unbiased_acceleration) {
+ 8552 : msg.control_acceleration.x = last_control_output.desired_unbiased_acceleration.value()(0);
+ 8553 : msg.control_acceleration.y = last_control_output.desired_unbiased_acceleration.value()(1);
+ 8554 : msg.control_acceleration.z = last_control_output.desired_unbiased_acceleration.value()(2);
+ 8555 : }
+ 8556 :
+ 8557 : if (last_control_output.desired_heading_rate) {
+ 8558 : msg.control_hdg_rate = last_control_output.desired_heading_rate.value();
+ 8559 : }
+ 8560 0 :
+ 8561 : if (last_control_output.desired_unbiased_acceleration) {
+ 8562 0 : ph_mrs_odom_input_.publish(msg);
+ 8563 : }
+ 8564 0 : }
+ 8565 0 : }
+ 8566 0 :
+ 8567 : //}
+ 8568 :
+ 8569 : /* deployParachute() //{ */
+ 8570 :
+ 8571 0 : std::tuple<bool, std::string> ControlManager::deployParachute(void) {
+ 8572 : // if not enabled, return false
+ 8573 0 : if (!_parachute_enabled_) {
+ 8574 0 :
+ 8575 0 : std::stringstream ss;
+ 8576 : ss << "can not deploy parachute, it is disabled";
+ 8577 : return std::tuple(false, ss.str());
+ 8578 : }
+ 8579 0 :
+ 8580 : // we can not disarm if the drone is not in offboard mode
+ 8581 : // this is super important!
+ 8582 0 : if (!isOffboard()) {
+ 8583 :
+ 8584 0 : std::stringstream ss;
+ 8585 : ss << "can not deploy parachute, not in offboard mode";
+ 8586 0 : return std::tuple(false, ss.str());
+ 8587 0 : }
+ 8588 :
+ 8589 0 : // call the parachute service
+ 8590 : bool succ = parachuteSrv();
+ 8591 :
+ 8592 : // if the deployment was successful,
+ 8593 0 : if (succ) {
+ 8594 0 :
+ 8595 : arming(false);
+ 8596 0 :
+ 8597 : std::stringstream ss;
+ 8598 : ss << "parachute deployed";
+ 8599 :
+ 8600 : return std::tuple(true, ss.str());
+ 8601 :
+ 8602 : } else {
+ 8603 :
+ 8604 472 : std::stringstream ss;
+ 8605 : ss << "error during deployment of parachute";
+ 8606 944 :
+ 8607 944 : return std::tuple(false, ss.str());
+ 8608 472 : }
+ 8609 : }
+ 8610 472 :
+ 8611 : //}
+ 8612 472 :
+ 8613 : /* velocityReferenceToReference() //{ */
+ 8614 472 :
+ 8615 0 : mrs_msgs::ReferenceStamped ControlManager::velocityReferenceToReference(const mrs_msgs::VelocityReferenceStamped& vel_reference) {
+ 8616 472 :
+ 8617 472 : auto last_tracker_cmd = mrs_lib::get_mutexed(mutex_last_tracker_cmd_, last_tracker_cmd_);
+ 8618 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 8619 0 : auto current_constraints = mrs_lib::get_mutexed(mutex_constraints_, current_constraints_);
+ 8620 :
+ 8621 : mrs_msgs::ReferenceStamped reference_out;
+ 8622 472 :
+ 8623 0 : reference_out.header = vel_reference.header;
+ 8624 :
+ 8625 : if (vel_reference.reference.use_heading) {
+ 8626 472 : reference_out.reference.heading = vel_reference.reference.heading;
+ 8627 : } else if (vel_reference.reference.use_heading_rate) {
+ 8628 472 : reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading() + vel_reference.reference.use_heading_rate;
+ 8629 238 : } else {
+ 8630 : reference_out.reference.heading = mrs_lib::AttitudeConverter(uav_state.pose.orientation).getHeading();
+ 8631 234 : }
+ 8632 :
+ 8633 : if (vel_reference.reference.use_altitude) {
+ 8634 472 : reference_out.reference.position.z = vel_reference.reference.altitude;
+ 8635 : } else {
+ 8636 :
+ 8637 : double stopping_time_z = 0;
+ 8638 472 :
+ 8639 472 : if (vel_reference.reference.velocity.x >= 0) {
+ 8640 : stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_ascending_acceleration) + 1.0;
+ 8641 472 : } else {
+ 8642 472 : stopping_time_z = 1.5 * (fabs(vel_reference.reference.velocity.z) / current_constraints.constraints.vertical_descending_acceleration) + 1.0;
+ 8643 : }
+ 8644 :
+ 8645 944 : reference_out.reference.position.z = last_tracker_cmd->position.z + vel_reference.reference.velocity.z * stopping_time_z;
+ 8646 : }
+ 8647 :
+ 8648 : {
+ 8649 : double stopping_time_x = 1.5 * (fabs(vel_reference.reference.velocity.x) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+ 8650 : double stopping_time_y = 1.5 * (fabs(vel_reference.reference.velocity.y) / current_constraints.constraints.horizontal_acceleration) + 1.0;
+ 8651 :
+ 8652 131089 : reference_out.reference.position.x = last_tracker_cmd->position.x + vel_reference.reference.velocity.x * stopping_time_x;
+ 8653 : reference_out.reference.position.y = last_tracker_cmd->position.y + vel_reference.reference.velocity.y * stopping_time_y;
+ 8654 131089 : }
+ 8655 35621 :
+ 8656 : return reference_out;
+ 8657 : }
+ 8658 190936 :
+ 8659 : //}
+ 8660 190936 :
+ 8661 : /* publishControlReferenceOdom() //{ */
+ 8662 95468 :
+ 8663 : void ControlManager::publishControlReferenceOdom([[maybe_unused]] const std::optional<mrs_msgs::TrackerCommand>& tracker_command,
+ 8664 95468 : [[maybe_unused]] const Controller::ControlOutput& control_output) {
+ 8665 95468 : if (!tracker_command || !control_output.control_output) {
+ 8666 95468 : return;
+ 8667 : }
+ 8668 0 :
+ 8669 0 : auto uav_state = mrs_lib::get_mutexed(mutex_uav_state_, uav_state_);
+ 8670 :
+ 8671 : nav_msgs::Odometry msg;
+ 8672 95468 :
+ 8673 95468 : msg.header = tracker_command->header;
+ 8674 :
+ 8675 0 : if (tracker_command->use_position_horizontal) {
+ 8676 : msg.pose.pose.position.x = tracker_command->position.x;
+ 8677 : msg.pose.pose.position.y = tracker_command->position.y;
+ 8678 : } else {
+ 8679 95468 : msg.pose.pose.position.x = uav_state.pose.position.x;
+ 8680 : msg.pose.pose.position.y = uav_state.pose.position.y;
+ 8681 95468 : }
+ 8682 :
+ 8683 190936 : if (tracker_command->use_position_vertical) {
+ 8684 95468 : msg.pose.pose.position.z = tracker_command->position.z;
+ 8685 : } else {
+ 8686 95468 : msg.pose.pose.position.z = uav_state.pose.position.z;
+ 8687 95468 : }
+ 8688 95468 :
+ 8689 : // transform the velocity in the reference to the child_frame
+ 8690 : if (tracker_command->use_velocity_horizontal || tracker_command->use_velocity_vertical) {
+ 8691 95468 :
+ 8692 95468 : msg.child_frame_id = _uav_name_ + "/" + _body_frame_;
+ 8693 :
+ 8694 : geometry_msgs::Vector3Stamped velocity;
+ 8695 190936 : velocity.header = tracker_command->header;
+ 8696 :
+ 8697 95468 : if (tracker_command->use_velocity_horizontal) {
+ 8698 95468 : velocity.vector.x = tracker_command->velocity.x;
+ 8699 95468 : velocity.vector.y = tracker_command->velocity.y;
+ 8700 95468 : }
+ 8701 :
+ 8702 0 : if (tracker_command->use_velocity_vertical) {
+ 8703 : velocity.vector.z = tracker_command->velocity.z;
+ 8704 : }
+ 8705 :
+ 8706 : auto res = transformer_->transformSingle(velocity, msg.child_frame_id);
+ 8707 :
+ 8708 95468 : if (res) {
+ 8709 78822 : msg.twist.twist.linear.x = res.value().vector.x;
+ 8710 16646 : msg.twist.twist.linear.y = res.value().vector.y;
+ 8711 16646 : msg.twist.twist.linear.z = res.value().vector.z;
+ 8712 : } else {
+ 8713 : ROS_ERROR_THROTTLE(1.0, "[ControlManager]: could not transform the reference speed from '%s' to '%s'", velocity.header.frame_id.c_str(),
+ 8714 : msg.child_frame_id.c_str());
+ 8715 95468 : }
+ 8716 : }
+ 8717 73240 :
+ 8718 : // fill in the orientation or heading
+ 8719 73240 : if (control_output.desired_orientation) {
+ 8720 73240 : msg.pose.pose.orientation = mrs_lib::AttitudeConverter(control_output.desired_orientation.value());
+ 8721 73240 : } else if (tracker_command->use_heading) {
+ 8722 : msg.pose.pose.orientation = mrs_lib::AttitudeConverter(0, 0, tracker_command->heading);
+ 8723 : }
+ 8724 95468 :
+ 8725 : // fill in the attitude rate
+ 8726 : if (std::holds_alternative<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value())) {
+ 8727 :
+ 8728 : auto attitude_cmd = std::get<mrs_msgs::HwApiAttitudeRateCmd>(control_output.control_output.value());
+ 8729 :
+ 8730 : msg.twist.twist.angular.x = attitude_cmd.body_rate.x;
+ 8731 192 : msg.twist.twist.angular.y = attitude_cmd.body_rate.y;
+ 8732 : msg.twist.twist.angular.z = attitude_cmd.body_rate.z;
+ 8733 192 : }
+ 8734 :
+ 8735 192 : ph_control_reference_odom_.publish(msg);
+ 8736 192 : }
+ 8737 :
+ 8738 192 : //}
+ 8739 192 :
+ 8740 : /* initializeControlOutput() //{ */
+ 8741 192 :
+ 8742 0 : void ControlManager::initializeControlOutput(void) {
+ 8743 :
+ 8744 : Controller::ControlOutput controller_output;
+ 8745 192 :
+ 8746 192 : controller_output.diagnostics.total_mass = _uav_mass_;
+ 8747 : controller_output.diagnostics.mass_difference = 0.0;
+ 8748 192 :
+ 8749 192 : controller_output.diagnostics.disturbance_bx_b = _initial_body_disturbance_x_;
+ 8750 : controller_output.diagnostics.disturbance_by_b = _initial_body_disturbance_y_;
+ 8751 192 :
+ 8752 : if (std::abs(_initial_body_disturbance_x_) > 0.001 || std::abs(_initial_body_disturbance_y_) > 0.001) {
+ 8753 192 : controller_output.diagnostics.disturbance_estimator = true;
+ 8754 192 : }
+ 8755 :
+ 8756 : controller_output.diagnostics.disturbance_wx_w = 0.0;
+ 8757 : controller_output.diagnostics.disturbance_wy_w = 0.0;
+ 8758 :
+ 8759 : controller_output.diagnostics.disturbance_bx_w = 0.0;
+ 8760 : controller_output.diagnostics.disturbance_by_w = 0.0;
+ 8761 :
+ 8762 : controller_output.diagnostics.controller = "none";
+ 8763 91 :
+ 8764 : mrs_lib::set_mutexed(mutex_last_control_output_, controller_output, last_control_output_);
+ 8765 : }
+ 8766 :
+ 8767 : //}
+ 8768 :
+ 8769 : } // namespace control_manager
+ 8770 :
+ 8771 : } // namespace mrs_uav_managers
+ 8772 :
+ 8773 : #include <pluginlib/class_list_macros.h>
+ 8774 : PLUGINLIB_EXPORT_CLASS(mrs_uav_managers::control_manager::ControlManager, nodelet::Nodelet)
+
+ |
+
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
