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