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