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