From 4d12e18b8a02c2ac8582ec743b57a98828f41fc3 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Mon, 15 Jan 2024 14:07:43 +0100 Subject: [PATCH] added tests for getPath srv --- launch/trajectory_generation.launch | 1 + src/mrs_trajectory_generation.cpp | 3379 ++++++++--------- test/CMakeLists.txt | 4 + test/get_path_after_takeoff/CMakeLists.txt | 16 + .../config/mrs_simulator.yaml | 14 + .../get_path_after_takeoff.test | 36 + test/get_path_after_takeoff/test.cpp | 104 + test/get_path_before_takeoff/CMakeLists.txt | 16 + .../config/mrs_simulator.yaml | 14 + .../get_path_before_takeoff.test | 36 + test/get_path_before_takeoff/test.cpp | 111 + test/include/get_path_test.h | 63 + 12 files changed, 2008 insertions(+), 1786 deletions(-) create mode 100644 test/get_path_after_takeoff/CMakeLists.txt create mode 100644 test/get_path_after_takeoff/config/mrs_simulator.yaml create mode 100644 test/get_path_after_takeoff/get_path_after_takeoff.test create mode 100644 test/get_path_after_takeoff/test.cpp create mode 100644 test/get_path_before_takeoff/CMakeLists.txt create mode 100644 test/get_path_before_takeoff/config/mrs_simulator.yaml create mode 100644 test/get_path_before_takeoff/get_path_before_takeoff.test create mode 100644 test/get_path_before_takeoff/test.cpp create mode 100644 test/include/get_path_test.h diff --git a/launch/trajectory_generation.launch b/launch/trajectory_generation.launch index 8277efc..9d74b81 100644 --- a/launch/trajectory_generation.launch +++ b/launch/trajectory_generation.launch @@ -48,6 +48,7 @@ + diff --git a/src/mrs_trajectory_generation.cpp b/src/mrs_trajectory_generation.cpp index 8c41146..d9a0edb 100644 --- a/src/mrs_trajectory_generation.cpp +++ b/src/mrs_trajectory_generation.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -49,7 +50,7 @@ using vec2_t = mrs_lib::geometry::vec_t<2>; using vec3_t = mrs_lib::geometry::vec_t<3>; using mat3_t = Eigen::Matrix3Xd; -using radians = mrs_lib::geometry::radians; +using radians = mrs_lib::geometry::radians; using sradians = mrs_lib::geometry::sradians; //} @@ -65,7 +66,7 @@ using sradians = mrs_lib::geometry::sradians; typedef struct { Eigen::Vector4d coords; - bool stop_at; + bool stop_at; } Waypoint_t; //} @@ -73,2462 +74,2268 @@ typedef struct namespace mrs_uav_trajectory_generation { - /* class MrsTrajectoryGeneration //{ */ - class MrsTrajectoryGeneration : public nodelet::Nodelet - { +/* class MrsTrajectoryGeneration //{ */ +class MrsTrajectoryGeneration : public nodelet::Nodelet { - public: - virtual void onInit(); +public: + virtual void onInit(); - private: - ros::NodeHandle nh_; +private: + ros::NodeHandle nh_; - bool is_initialized_ = false; + bool is_initialized_ = false; - // | ----------------------- parameters ----------------------- | + // | ----------------------- parameters ----------------------- | - double _sampling_dt_; + double _sampling_dt_; - double _max_trajectory_len_factor_; - double _min_trajectory_len_factor_; + double _max_trajectory_len_factor_; + double _min_trajectory_len_factor_; - int _n_attempts_; + int _n_attempts_; - double _min_waypoint_distance_; + double _min_waypoint_distance_; - bool _fallback_sampling_enabled_; - double _fallback_sampling_speed_factor_; - double _fallback_sampling_accel_factor_; - double _fallback_sampling_stopping_time_; - bool _fallback_sampling_first_waypoint_additional_stop_; + bool _fallback_sampling_enabled_; + double _fallback_sampling_speed_factor_; + double _fallback_sampling_accel_factor_; + double _fallback_sampling_stopping_time_; + bool _fallback_sampling_first_waypoint_additional_stop_; - std::string _uav_name_; + std::string _uav_name_; - bool _trajectory_max_segment_deviation_enabled_; - double _trajectory_max_segment_deviation_; - int _trajectory_max_segment_deviation_max_iterations_; + bool _trajectory_max_segment_deviation_enabled_; + double _trajectory_max_segment_deviation_; + int _trajectory_max_segment_deviation_max_iterations_; - bool _path_straightener_enabled_; - double _path_straightener_max_deviation_; - double _path_straightener_max_hdg_deviation_; + bool _path_straightener_enabled_; + double _path_straightener_max_deviation_; + double _path_straightener_max_hdg_deviation_; - // | -------- variable parameters (come with the path) -------- | + // | -------- variable parameters (come with the path) -------- | - std::string frame_id_; - bool fly_now_ = false; - bool use_heading_ = false; - bool stop_at_waypoints_ = false; - bool override_constraints_ = false; - bool loop_ = false; - double override_max_velocity_horizontal_ = 0.0; - double override_max_velocity_vertical_ = 0.0; - double override_max_acceleration_horizontal_ = 0.0; - double override_max_acceleration_vertical_ = 0.0; - double override_max_jerk_horizontal_ = 0.0; - double override_max_jerk_vertical_ = 0.0; + std::string frame_id_; + bool fly_now_ = false; + bool use_heading_ = false; + bool stop_at_waypoints_ = false; + bool override_constraints_ = false; + bool loop_ = false; + double override_max_velocity_horizontal_ = 0.0; + double override_max_velocity_vertical_ = 0.0; + double override_max_acceleration_horizontal_ = 0.0; + double override_max_acceleration_vertical_ = 0.0; + double override_max_jerk_horizontal_ = 0.0; + double override_max_jerk_vertical_ = 0.0; - // | -------------- variable parameters (deduced) ------------- | + // | -------------- variable parameters (deduced) ------------- | - double max_execution_time_ = 0; - std::mutex mutex_max_execution_time_; + double max_execution_time_ = 0; + std::mutex mutex_max_execution_time_; - bool max_deviation_first_segment_; + bool max_deviation_first_segment_; - // | -------------------- the transformer -------------------- | + // | -------------------- the transformer -------------------- | - std::shared_ptr transformer_; + std::shared_ptr transformer_; - // | ------------------- scope timer logger ------------------- | + // | ------------------- scope timer logger ------------------- | - bool scope_timer_enabled_ = false; - std::shared_ptr scope_timer_logger_; + bool scope_timer_enabled_ = false; + std::shared_ptr scope_timer_logger_; - // service client for input - bool callbackPathSrv(mrs_msgs::PathSrv::Request& req, mrs_msgs::PathSrv::Response& res); - ros::ServiceServer service_server_path_; + // service client for input + bool callbackPathSrv(mrs_msgs::PathSrv::Request& req, mrs_msgs::PathSrv::Response& res); + ros::ServiceServer service_server_path_; - // service client for returning result to the user - bool callbackGetPathSrv(mrs_msgs::GetPathSrv::Request& req, mrs_msgs::GetPathSrv::Response& res); - ros::ServiceServer service_server_get_path_; + // service client for returning result to the user + bool callbackGetPathSrv(mrs_msgs::GetPathSrv::Request& req, mrs_msgs::GetPathSrv::Response& res); + ros::ServiceServer service_server_get_path_; - // subscriber for input - void callbackPath(const mrs_msgs::PathConstPtr& msg); - ros::Subscriber subscriber_path_; + // subscriber for input + void callbackPath(const mrs_msgs::PathConstPtr& msg); + ros::Subscriber subscriber_path_; - void callbackConstraints(const mrs_msgs::DynamicsConstraintsConstPtr& msg); - ros::Subscriber subscriber_constraints_; - bool got_constraints_ = false; - mrs_msgs::DynamicsConstraints constraints_; - std::mutex mutex_constraints_; + void callbackConstraints(const mrs_msgs::DynamicsConstraintsConstPtr& msg); + ros::Subscriber subscriber_constraints_; + bool got_constraints_ = false; + mrs_msgs::DynamicsConstraints constraints_; + std::mutex mutex_constraints_; - void callbackTrackerCmd(const mrs_msgs::TrackerCommand::ConstPtr msg); - mrs_lib::SubscribeHandler sh_tracker_cmd_; + mrs_lib::SubscribeHandler sh_tracker_cmd_; - void callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnosticsConstPtr& msg); - ros::Subscriber subscriber_control_manager_diag_; - bool got_control_manager_diag_ = false; - mrs_msgs::ControlManagerDiagnostics control_manager_diag_; - std::mutex mutex_control_manager_diag_; + void callbackUavState(const mrs_msgs::UavState::ConstPtr msg); + mrs_lib::SubscribeHandler sh_uav_state_; - // service client for publishing trajectory out - ros::ServiceClient service_client_trajectory_reference_; + void callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnosticsConstPtr& msg); + ros::Subscriber subscriber_control_manager_diag_; + bool got_control_manager_diag_ = false; + mrs_msgs::ControlManagerDiagnostics control_manager_diag_; + std::mutex mutex_control_manager_diag_; - // solve the whole problem - std::tuple optimize(const std::vector& waypoints_in, const std_msgs::Header& waypoints_stamp, - bool fallback_sampling, const bool relax_heading); + // service client for publishing trajectory out + ros::ServiceClient service_client_trajectory_reference_; - std::tuple, bool, int> prepareInitialCondition(const ros::Time path_time); + // solve the whole problem + std::tuple optimize(const std::vector& waypoints_in, const std_msgs::Header& waypoints_stamp, + bool fallback_sampling, const bool relax_heading); - // batch vizualizer - mrs_lib::BatchVisualizer bw_original_; - mrs_lib::BatchVisualizer bw_final_; + std::tuple, bool, int> prepareInitialCondition(const ros::Time path_time); - // transforming TrackerCommand - std::optional transformPath(const mrs_msgs::Path& path, const std::string& target_frame); + // batch vizualizer + mrs_lib::BatchVisualizer bw_original_; + mrs_lib::BatchVisualizer bw_final_; - // | ------------------ trajectory validation ----------------- | + // transforming TrackerCommand + std::optional transformPath(const mrs_msgs::Path& path, const std::string& target_frame); - /** - * @brief validates samples of a trajectory agains a path of waypoints - * - * @param trajectory - * @param segments - * - * @return - */ - std::tuple, double> validateTrajectorySpatial(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, - const std::vector& waypoints); + // | ------------------ trajectory validation ----------------- | - std::optional findTrajectory(const std::vector& waypoints, - const std::optional& initial_state, - const double& sampling_dt, const bool& relax_heading); + /** + * @brief validates samples of a trajectory agains a path of waypoints + * + * @param trajectory + * @param segments + * + * @return + */ + std::tuple, double> validateTrajectorySpatial(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, + const std::vector& waypoints); - std::optional findTrajectoryFallback(const std::vector& waypoints, const double& sampling_dt, - const bool& relax_heading); + std::optional findTrajectory(const std::vector& waypoints, + const std::optional& initial_state, + const double& sampling_dt, const bool& relax_heading); - std::future> future_trajectory_result_; - std::atomic running_async_planning_ = false; + std::optional findTrajectoryFallback(const std::vector& waypoints, const double& sampling_dt, + const bool& relax_heading); - std::optional findTrajectoryAsync(const std::vector& waypoints, - const std::optional& initial_state, - const double& sampling_dt, const bool& relax_heading); + std::future> future_trajectory_result_; + std::atomic running_async_planning_ = false; - std::vector preprocessPath(const std::vector& waypoints_in); + std::optional findTrajectoryAsync(const std::vector& waypoints, + const std::optional& initial_state, + const double& sampling_dt, const bool& relax_heading); - mrs_msgs::TrajectoryReference getTrajectoryReference(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, - const std::optional& initial_condition, const double& sampling_dt); + std::vector preprocessPath(const std::vector& waypoints_in); - Waypoint_t interpolatePoint(const Waypoint_t& a, const Waypoint_t& b, const double& coeff); + mrs_msgs::TrajectoryReference getTrajectoryReference(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, + const std::optional& initial_condition, const double& sampling_dt); - bool checkNaN(const Waypoint_t& a); + Waypoint_t interpolatePoint(const Waypoint_t& a, const Waypoint_t& b, const double& coeff); - double distFromSegment(const vec3_t& point, const vec3_t& seg1, const vec3_t& seg2); + bool checkNaN(const Waypoint_t& a); - bool trajectorySrv(const mrs_msgs::TrajectoryReference& msg); + double distFromSegment(const vec3_t& point, const vec3_t& seg1, const vec3_t& seg2); - // | --------------- dynamic reconfigure server --------------- | + bool trajectorySrv(const mrs_msgs::TrajectoryReference& msg); - boost::recursive_mutex mutex_drs_; - typedef mrs_uav_trajectory_generation::drsConfig DrsParams_t; - typedef dynamic_reconfigure::Server Drs_t; - boost::shared_ptr drs_; - void callbackDrs(mrs_uav_trajectory_generation::drsConfig& params, uint32_t level); - DrsParams_t params_; - std::mutex mutex_params_; + // | --------------- dynamic reconfigure server --------------- | - // | ------------ Republisher for the desired path ------------ | + boost::recursive_mutex mutex_drs_; + typedef mrs_uav_trajectory_generation::drsConfig DrsParams_t; + typedef dynamic_reconfigure::Server Drs_t; + boost::shared_ptr drs_; + void callbackDrs(mrs_uav_trajectory_generation::drsConfig& params, uint32_t level); + DrsParams_t params_; + std::mutex mutex_params_; - mrs_lib::PublisherHandler ph_original_path_; + // | ------------ Republisher for the desired path ------------ | - // | ------------- measuring the time of execution ------------ | + mrs_lib::PublisherHandler ph_original_path_; - ros::Time start_time_total_; - std::mutex mutex_start_time_total_; - bool overtime(void); - double timeLeft(void); - }; + // | ------------- measuring the time of execution ------------ | - //} + ros::Time start_time_total_; + std::mutex mutex_start_time_total_; + bool overtime(void); + double timeLeft(void); +}; - /* onInit() //{ */ +//} - void MrsTrajectoryGeneration::onInit() - { +/* onInit() //{ */ - /* obtain node handle */ - nh_ = nodelet::Nodelet::getMTPrivateNodeHandle(); +void MrsTrajectoryGeneration::onInit() { - /* waits for the ROS to publish clock */ - ros::Time::waitForValid(); + /* obtain node handle */ + nh_ = nodelet::Nodelet::getMTPrivateNodeHandle(); - // | ----------------------- publishers ----------------------- | + /* waits for the ROS to publish clock */ + ros::Time::waitForValid(); - ph_original_path_ = mrs_lib::PublisherHandler(nh_, "original_path_out", 1); + // | ----------------------- publishers ----------------------- | - // | ----------------------- subscribers ---------------------- | + ph_original_path_ = mrs_lib::PublisherHandler(nh_, "original_path_out", 1); - mrs_lib::SubscribeHandlerOptions shopts; - shopts.nh = nh_; - shopts.node_name = "TrajectoryGeneration"; - shopts.no_message_timeout = mrs_lib::no_timeout; - shopts.threadsafe = true; - shopts.autostart = true; - shopts.queue_size = 10; - shopts.transport_hints = ros::TransportHints().tcpNoDelay(); + // | ----------------------- subscribers ---------------------- | - subscriber_constraints_ = nh_.subscribe("constraints_in", 1, &MrsTrajectoryGeneration::callbackConstraints, this, ros::TransportHints().tcpNoDelay()); - sh_tracker_cmd_ = mrs_lib::SubscribeHandler(shopts, "tracker_cmd_in", &MrsTrajectoryGeneration::callbackTrackerCmd, this); - subscriber_control_manager_diag_ = - nh_.subscribe("control_manager_diag_in", 1, &MrsTrajectoryGeneration::callbackControlManagerDiag, this, ros::TransportHints().tcpNoDelay()); - subscriber_path_ = nh_.subscribe("path_in", 1, &MrsTrajectoryGeneration::callbackPath, this, ros::TransportHints().tcpNoDelay()); + mrs_lib::SubscribeHandlerOptions shopts; + shopts.nh = nh_; + shopts.node_name = "TrajectoryGeneration"; + shopts.no_message_timeout = mrs_lib::no_timeout; + shopts.threadsafe = true; + shopts.autostart = true; + shopts.queue_size = 10; + shopts.transport_hints = ros::TransportHints().tcpNoDelay(); - // | --------------------- service servers -------------------- | + subscriber_constraints_ = nh_.subscribe("constraints_in", 1, &MrsTrajectoryGeneration::callbackConstraints, this, ros::TransportHints().tcpNoDelay()); - service_server_path_ = nh_.advertiseService("path_in", &MrsTrajectoryGeneration::callbackPathSrv, this); + sh_tracker_cmd_ = mrs_lib::SubscribeHandler(shopts, "tracker_cmd_in"); + sh_uav_state_ = mrs_lib::SubscribeHandler(shopts, "uav_state_in", &MrsTrajectoryGeneration::callbackUavState, this); - service_server_get_path_ = nh_.advertiseService("get_path_in", &MrsTrajectoryGeneration::callbackGetPathSrv, this); + subscriber_control_manager_diag_ = + nh_.subscribe("control_manager_diag_in", 1, &MrsTrajectoryGeneration::callbackControlManagerDiag, this, ros::TransportHints().tcpNoDelay()); + subscriber_path_ = nh_.subscribe("path_in", 1, &MrsTrajectoryGeneration::callbackPath, this, ros::TransportHints().tcpNoDelay()); - service_client_trajectory_reference_ = nh_.serviceClient("trajectory_reference_out"); + // | --------------------- service servers -------------------- | - // | ----------------------- parameters ----------------------- | + service_server_path_ = nh_.advertiseService("path_in", &MrsTrajectoryGeneration::callbackPathSrv, this); - mrs_lib::ParamLoader param_loader(nh_, "MrsTrajectoryGeneration"); + service_server_get_path_ = nh_.advertiseService("get_path_in", &MrsTrajectoryGeneration::callbackGetPathSrv, this); - std::string custom_config_path; - std::string platform_config_path; + service_client_trajectory_reference_ = nh_.serviceClient("trajectory_reference_out"); - param_loader.loadParam("custom_config", custom_config_path); - param_loader.loadParam("platform_config", platform_config_path); + // | ----------------------- parameters ----------------------- | - if (custom_config_path != "") - { - param_loader.addYamlFile(custom_config_path); - } + mrs_lib::ParamLoader param_loader(nh_, "MrsTrajectoryGeneration"); - if (platform_config_path != "") - { - param_loader.addYamlFile(platform_config_path); - } + std::string custom_config_path; + std::string platform_config_path; - param_loader.addYamlFileFromParam("private_config"); - param_loader.addYamlFileFromParam("public_config"); + param_loader.loadParam("custom_config", custom_config_path); + param_loader.loadParam("platform_config", platform_config_path); - const std::string yaml_prefix = "mrs_uav_trajectory_generation/"; + if (custom_config_path != "") { + param_loader.addYamlFile(custom_config_path); + } - param_loader.loadParam("uav_name", _uav_name_); + if (platform_config_path != "") { + param_loader.addYamlFile(platform_config_path); + } - param_loader.loadParam(yaml_prefix + "sampling_dt", _sampling_dt_); + param_loader.addYamlFileFromParam("private_config"); + param_loader.addYamlFileFromParam("public_config"); - param_loader.loadParam(yaml_prefix + "enforce_fallback_solver", params_.enforce_fallback_solver); + const std::string yaml_prefix = "mrs_uav_trajectory_generation/"; - param_loader.loadParam(yaml_prefix + "max_trajectory_len_factor", _max_trajectory_len_factor_); - param_loader.loadParam(yaml_prefix + "min_trajectory_len_factor", _min_trajectory_len_factor_); + param_loader.loadParam("uav_name", _uav_name_); - param_loader.loadParam(yaml_prefix + "n_attempts", _n_attempts_); - param_loader.loadParam(yaml_prefix + "fallback_sampling/enabled", _fallback_sampling_enabled_); - param_loader.loadParam(yaml_prefix + "fallback_sampling/speed_factor", _fallback_sampling_speed_factor_); - param_loader.loadParam(yaml_prefix + "fallback_sampling/accel_factor", _fallback_sampling_accel_factor_); - param_loader.loadParam(yaml_prefix + "fallback_sampling/stopping_time", _fallback_sampling_stopping_time_); - param_loader.loadParam(yaml_prefix + "fallback_sampling/first_waypoint_additional_stop", _fallback_sampling_first_waypoint_additional_stop_); + param_loader.loadParam(yaml_prefix + "sampling_dt", _sampling_dt_); - param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/enabled", _trajectory_max_segment_deviation_enabled_); - param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/max_deviation", _trajectory_max_segment_deviation_); - param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/max_iterations", _trajectory_max_segment_deviation_max_iterations_); + param_loader.loadParam(yaml_prefix + "enforce_fallback_solver", params_.enforce_fallback_solver); - param_loader.loadParam(yaml_prefix + "path_straightener/enabled", _path_straightener_enabled_); - param_loader.loadParam(yaml_prefix + "path_straightener/max_deviation", _path_straightener_max_deviation_); - param_loader.loadParam(yaml_prefix + "path_straightener/max_hdg_deviation", _path_straightener_max_hdg_deviation_); + param_loader.loadParam(yaml_prefix + "max_trajectory_len_factor", _max_trajectory_len_factor_); + param_loader.loadParam(yaml_prefix + "min_trajectory_len_factor", _min_trajectory_len_factor_); - param_loader.loadParam(yaml_prefix + "min_waypoint_distance", _min_waypoint_distance_); + param_loader.loadParam(yaml_prefix + "n_attempts", _n_attempts_); + param_loader.loadParam(yaml_prefix + "fallback_sampling/enabled", _fallback_sampling_enabled_); + param_loader.loadParam(yaml_prefix + "fallback_sampling/speed_factor", _fallback_sampling_speed_factor_); + param_loader.loadParam(yaml_prefix + "fallback_sampling/accel_factor", _fallback_sampling_accel_factor_); + param_loader.loadParam(yaml_prefix + "fallback_sampling/stopping_time", _fallback_sampling_stopping_time_); + param_loader.loadParam(yaml_prefix + "fallback_sampling/first_waypoint_additional_stop", _fallback_sampling_first_waypoint_additional_stop_); - // | --------------------- tf transformer --------------------- | + param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/enabled", _trajectory_max_segment_deviation_enabled_); + param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/max_deviation", _trajectory_max_segment_deviation_); + param_loader.loadParam(yaml_prefix + "check_trajectory_deviation/max_iterations", _trajectory_max_segment_deviation_max_iterations_); - transformer_ = std::make_shared(nh_, "TrajectoryGeneration"); - transformer_->setDefaultPrefix(_uav_name_); - transformer_->retryLookupNewest(true); + param_loader.loadParam(yaml_prefix + "path_straightener/enabled", _path_straightener_enabled_); + param_loader.loadParam(yaml_prefix + "path_straightener/max_deviation", _path_straightener_max_deviation_); + param_loader.loadParam(yaml_prefix + "path_straightener/max_hdg_deviation", _path_straightener_max_hdg_deviation_); - // | ------------------- scope timer logger ------------------- | + param_loader.loadParam(yaml_prefix + "min_waypoint_distance", _min_waypoint_distance_); - param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_); - const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string("")); - scope_timer_logger_ = std::make_shared(scope_timer_log_filename, scope_timer_enabled_); + // | --------------------- tf transformer --------------------- | - // | --------------------- service clients -------------------- | + transformer_ = std::make_shared(nh_, "TrajectoryGeneration"); + transformer_->setDefaultPrefix(_uav_name_); + transformer_->retryLookupNewest(true); - param_loader.loadParam(yaml_prefix + "time_penalty", params_.time_penalty); - param_loader.loadParam(yaml_prefix + "soft_constraints_enabled", params_.soft_constraints_enabled); - param_loader.loadParam(yaml_prefix + "soft_constraints_weight", params_.soft_constraints_weight); - param_loader.loadParam(yaml_prefix + "time_allocation", params_.time_allocation); - param_loader.loadParam(yaml_prefix + "equality_constraint_tolerance", params_.equality_constraint_tolerance); - param_loader.loadParam(yaml_prefix + "inequality_constraint_tolerance", params_.inequality_constraint_tolerance); - param_loader.loadParam(yaml_prefix + "max_iterations", params_.max_iterations); - param_loader.loadParam(yaml_prefix + "derivative_to_optimize", params_.derivative_to_optimize); + // | ------------------- scope timer logger ------------------- | - param_loader.loadParam(yaml_prefix + "max_time", params_.max_time); - max_execution_time_ = params_.max_time; + param_loader.loadParam(yaml_prefix + "scope_timer/enabled", scope_timer_enabled_); + const std::string scope_timer_log_filename = param_loader.loadParam2("scope_timer/log_filename", std::string("")); + scope_timer_logger_ = std::make_shared(scope_timer_log_filename, scope_timer_enabled_); - if (!param_loader.loadedSuccessfully()) - { - ROS_ERROR("[MrsTrajectoryGeneration]: could not load all parameters!"); - ros::shutdown(); - } + // | --------------------- service clients -------------------- | - // | -------------------- batch visualizer -------------------- | + param_loader.loadParam(yaml_prefix + "time_penalty", params_.time_penalty); + param_loader.loadParam(yaml_prefix + "soft_constraints_enabled", params_.soft_constraints_enabled); + param_loader.loadParam(yaml_prefix + "soft_constraints_weight", params_.soft_constraints_weight); + param_loader.loadParam(yaml_prefix + "time_allocation", params_.time_allocation); + param_loader.loadParam(yaml_prefix + "equality_constraint_tolerance", params_.equality_constraint_tolerance); + param_loader.loadParam(yaml_prefix + "inequality_constraint_tolerance", params_.inequality_constraint_tolerance); + param_loader.loadParam(yaml_prefix + "max_iterations", params_.max_iterations); + param_loader.loadParam(yaml_prefix + "derivative_to_optimize", params_.derivative_to_optimize); - bw_original_ = mrs_lib::BatchVisualizer(nh_, "markers/original", ""); + param_loader.loadParam(yaml_prefix + "max_time", params_.max_time); + max_execution_time_ = params_.max_time; - bw_original_.clearBuffers(); - bw_original_.clearVisuals(); + if (!param_loader.loadedSuccessfully()) { + ROS_ERROR("[MrsTrajectoryGeneration]: could not load all parameters!"); + ros::shutdown(); + } - bw_final_ = mrs_lib::BatchVisualizer(nh_, "markers/final", ""); + // | -------------------- batch visualizer -------------------- | - bw_final_.clearBuffers(); - bw_final_.clearVisuals(); + bw_original_ = mrs_lib::BatchVisualizer(nh_, "markers/original", ""); - // | --------------- dynamic reconfigure server --------------- | + bw_original_.clearBuffers(); + bw_original_.clearVisuals(); - drs_.reset(new Drs_t(mutex_drs_, nh_)); - drs_->updateConfig(params_); - Drs_t::CallbackType f = boost::bind(&MrsTrajectoryGeneration::callbackDrs, this, _1, _2); - drs_->setCallback(f); + bw_final_ = mrs_lib::BatchVisualizer(nh_, "markers/final", ""); - // | --------------------- finish the init -------------------- | + bw_final_.clearBuffers(); + bw_final_.clearVisuals(); - ROS_INFO_ONCE("[MrsTrajectoryGeneration]: initialized"); + // | --------------- dynamic reconfigure server --------------- | - is_initialized_ = true; - } + drs_.reset(new Drs_t(mutex_drs_, nh_)); + drs_->updateConfig(params_); + Drs_t::CallbackType f = boost::bind(&MrsTrajectoryGeneration::callbackDrs, this, _1, _2); + drs_->setCallback(f); - //} + // | --------------------- finish the init -------------------- | - // | ---------------------- main routines --------------------- | - - /* - * 1. preprocessPath(): preprocessing the incoming path - * - throughs away too close waypoints - * - straightness path by neglecting waypoints close to segments - * 2. optimize(): solves the whole problem including - * - subdivision for satisfying max deviation - * 3. findTrajectory(): solves single instance by the ETH tool - * 4. findTrajectoryFallback(): Baca's sampling for backup solution - * 5. validateTrajectorySpatial(): checks for the spatial soundness of a trajectory vs. the original path - */ + ROS_INFO_ONCE("[MrsTrajectoryGeneration]: initialized"); - /* preprocessPath() //{ */ + is_initialized_ = true; +} - std::vector MrsTrajectoryGeneration::preprocessPath(const std::vector& waypoints_in) - { +//} - mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::preprocessPath", scope_timer_logger_, scope_timer_enabled_); +// | ---------------------- main routines --------------------- | - std::vector waypoints; +/* + * 1. preprocessPath(): preprocessing the incoming path + * - throughs away too close waypoints + * - straightness path by neglecting waypoints close to segments + * 2. optimize(): solves the whole problem including + * - subdivision for satisfying max deviation + * 3. findTrajectory(): solves single instance by the ETH tool + * 4. findTrajectoryFallback(): Baca's sampling for backup solution + * 5. validateTrajectorySpatial(): checks for the spatial soundness of a trajectory vs. the original path + */ - size_t last_added_idx = 0; // in "waypoints_in" +/* preprocessPath() //{ */ - for (size_t i = 0; i < waypoints_in.size(); i++) - { +std::vector MrsTrajectoryGeneration::preprocessPath(const std::vector& waypoints_in) { - double x = waypoints_in.at(i).coords[0]; - double y = waypoints_in.at(i).coords[1]; - double z = waypoints_in.at(i).coords[2]; - double heading = waypoints_in.at(i).coords[3]; + mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::preprocessPath", scope_timer_logger_, scope_timer_enabled_); - bw_original_.addPoint(vec3_t(x, y, z), 1.0, 0.0, 0.0, 1.0); + std::vector waypoints; - if (_path_straightener_enabled_ && waypoints_in.size() >= 3 && i > 0 && i < (waypoints_in.size() - 1)) - { + size_t last_added_idx = 0; // in "waypoints_in" - vec3_t first(waypoints_in.at(last_added_idx).coords[0], waypoints_in.at(last_added_idx).coords[1], waypoints_in.at(last_added_idx).coords[2]); - vec3_t last(waypoints_in.at(i + 1).coords[0], waypoints_in.at(i + 1).coords[1], waypoints_in.at(i + 1).coords[2]); + for (size_t i = 0; i < waypoints_in.size(); i++) { - double first_hdg = waypoints_in.at(last_added_idx).coords[3]; - double last_hdg = waypoints_in.at(i + 1).coords[3]; + double x = waypoints_in.at(i).coords[0]; + double y = waypoints_in.at(i).coords[1]; + double z = waypoints_in.at(i).coords[2]; + double heading = waypoints_in.at(i).coords[3]; - size_t next_point = last_added_idx + 1; + bw_original_.addPoint(vec3_t(x, y, z), 1.0, 0.0, 0.0, 1.0); - bool segment_is_ok = true; + if (_path_straightener_enabled_ && waypoints_in.size() >= 3 && i > 0 && i < (waypoints_in.size() - 1)) { - for (size_t j = next_point; j < i + 1; j++) - { + vec3_t first(waypoints_in.at(last_added_idx).coords[0], waypoints_in.at(last_added_idx).coords[1], waypoints_in.at(last_added_idx).coords[2]); + vec3_t last(waypoints_in.at(i + 1).coords[0], waypoints_in.at(i + 1).coords[1], waypoints_in.at(i + 1).coords[2]); - vec3_t mid(waypoints_in.at(j).coords[0], waypoints_in.at(j).coords[1], waypoints_in.at(j).coords[2]); - double mid_hdg = waypoints_in.at(j).coords[3]; + double first_hdg = waypoints_in.at(last_added_idx).coords[3]; + double last_hdg = waypoints_in.at(i + 1).coords[3]; - double dist_from_segment = distFromSegment(mid, first, last); + size_t next_point = last_added_idx + 1; - if (dist_from_segment > _path_straightener_max_deviation_ || fabs(radians::diff(first_hdg, mid_hdg) > _path_straightener_max_hdg_deviation_) - || fabs(radians::diff(last_hdg, mid_hdg) > _path_straightener_max_hdg_deviation_)) - { - segment_is_ok = false; - break; - } - } + bool segment_is_ok = true; - if (segment_is_ok) - { - continue; - } - } + for (size_t j = next_point; j < i + 1; j++) { - if (i > 0 && i < (waypoints_in.size() - 1)) - { + vec3_t mid(waypoints_in.at(j).coords[0], waypoints_in.at(j).coords[1], waypoints_in.at(j).coords[2]); + double mid_hdg = waypoints_in.at(j).coords[3]; - vec3_t first(waypoints_in.at(last_added_idx).coords[0], waypoints_in.at(last_added_idx).coords[1], waypoints_in.at(last_added_idx).coords[2]); - vec3_t last(waypoints_in.at(i).coords[0], waypoints_in.at(i).coords[1], waypoints_in.at(i).coords[2]); + double dist_from_segment = distFromSegment(mid, first, last); - if (mrs_lib::geometry::dist(first, last) < _min_waypoint_distance_) - { - ROS_INFO("[MrsTrajectoryGeneration]: waypoint #%d too close (< %.3f m) to the previous one (#%d), throwing it away", int(i), _min_waypoint_distance_, - int(last_added_idx)); - continue; + if (dist_from_segment > _path_straightener_max_deviation_ || fabs(radians::diff(first_hdg, mid_hdg) > _path_straightener_max_hdg_deviation_) || + fabs(radians::diff(last_hdg, mid_hdg) > _path_straightener_max_hdg_deviation_)) { + segment_is_ok = false; + break; } } - Waypoint_t wp; - wp.coords = Eigen::Vector4d(x, y, z, heading); - wp.stop_at = waypoints_in.at(i).stop_at; - waypoints.push_back(wp); - - last_added_idx = i; + if (segment_is_ok) { + continue; + } } - return waypoints; - } + if (i > 0 && i < (waypoints_in.size() - 1)) { - //} + vec3_t first(waypoints_in.at(last_added_idx).coords[0], waypoints_in.at(last_added_idx).coords[1], waypoints_in.at(last_added_idx).coords[2]); + vec3_t last(waypoints_in.at(i).coords[0], waypoints_in.at(i).coords[1], waypoints_in.at(i).coords[2]); - /* prepareInitialCondition() //{ */ + if (mrs_lib::geometry::dist(first, last) < _min_waypoint_distance_) { + ROS_INFO("[MrsTrajectoryGeneration]: waypoint #%d too close (< %.3f m) to the previous one (#%d), throwing it away", int(i), _min_waypoint_distance_, + int(last_added_idx)); + continue; + } + } - std::tuple, bool, int> MrsTrajectoryGeneration::prepareInitialCondition(const ros::Time path_time) - { + Waypoint_t wp; + wp.coords = Eigen::Vector4d(x, y, z, heading); + wp.stop_at = waypoints_in.at(i).stop_at; + waypoints.push_back(wp); - if (!sh_tracker_cmd_.hasMsg()) - { - return {{}, false, 0}; - } + last_added_idx = i; + } + + return waypoints; +} + +//} - auto tracker_cmd = sh_tracker_cmd_.getMsg(); +/* prepareInitialCondition() //{ */ - // | ------------- prepare the initial conditions ------------- | +std::tuple, bool, int> MrsTrajectoryGeneration::prepareInitialCondition(const ros::Time path_time) { - mrs_msgs::TrackerCommand initial_condition; + if (!sh_tracker_cmd_.hasMsg()) { + return {{}, false, 0}; + } - bool path_from_future = false; + auto tracker_cmd = sh_tracker_cmd_.getMsg(); - // positive = in the future - double path_time_offset = 0; + // | ------------- prepare the initial conditions ------------- | - if (path_time != ros::Time(0)) - { - path_time_offset = (path_time - ros::Time::now()).toSec(); - } + mrs_msgs::TrackerCommand initial_condition; - int path_sample_offset = 0; + bool path_from_future = false; - // if the desired path starts in the future, more than one MPC step ahead - if (path_time_offset > 0.2) - { + // positive = in the future + double path_time_offset = 0; - ROS_INFO("[MrsTrajectoryGeneration]: desired path is from the future by %.2f s", path_time_offset); + if (path_time != ros::Time(0)) { + path_time_offset = (path_time - ros::Time::now()).toSec(); + } - // calculate the offset in samples in the predicted trajectory - // 0.01 is subtracted for the first sample, which is smaller - // +1 is added due to the first sample, which was subtarcted - path_sample_offset = int(ceil((path_time_offset * FUTURIZATION_FIRST_WAYPOINT_FACTOR - 0.01) / 0.2)) + 1; + int path_sample_offset = 0; - if (path_sample_offset > (int(tracker_cmd->full_state_prediction.position.size()) - 1)) - { + // if the desired path starts in the future, more than one MPC step ahead + if (path_time_offset > 0.2) { - ROS_ERROR("[MrsTrajectoryGeneration]: can not extrapolate into the waypoints, using tracker_cmd instead"); - initial_condition = *tracker_cmd; + ROS_INFO("[MrsTrajectoryGeneration]: desired path is from the future by %.2f s", path_time_offset); - } else - { + // calculate the offset in samples in the predicted trajectory + // 0.01 is subtracted for the first sample, which is smaller + // +1 is added due to the first sample, which was subtarcted + path_sample_offset = int(ceil((path_time_offset * FUTURIZATION_FIRST_WAYPOINT_FACTOR - 0.01) / 0.2)) + 1; - // copy the sample from the current prediction into TrackerCommand, so that we can easily transform it - mrs_msgs::TrackerCommand full_state; + if (path_sample_offset > (int(tracker_cmd->full_state_prediction.position.size()) - 1)) { - full_state.header = tracker_cmd->full_state_prediction.header; + ROS_ERROR("[MrsTrajectoryGeneration]: can not extrapolate into the waypoints, using tracker_cmd instead"); + initial_condition = *tracker_cmd; - full_state.position = tracker_cmd->full_state_prediction.position[path_sample_offset]; - full_state.velocity = tracker_cmd->full_state_prediction.velocity[path_sample_offset]; - full_state.acceleration = tracker_cmd->full_state_prediction.acceleration[path_sample_offset]; - full_state.jerk = tracker_cmd->full_state_prediction.jerk[path_sample_offset]; + } else { - full_state.heading = tracker_cmd->full_state_prediction.heading[path_sample_offset]; - full_state.heading_rate = tracker_cmd->full_state_prediction.heading_rate[path_sample_offset]; - full_state.heading_acceleration = tracker_cmd->full_state_prediction.heading_acceleration[path_sample_offset]; - full_state.heading_jerk = tracker_cmd->full_state_prediction.heading_jerk[path_sample_offset]; + // copy the sample from the current prediction into TrackerCommand, so that we can easily transform it + mrs_msgs::TrackerCommand full_state; - ROS_INFO("[MrsTrajectoryGeneration]: getting initial condition from the %d-th sample of the MPC prediction", path_sample_offset); + full_state.header = tracker_cmd->full_state_prediction.header; - initial_condition.header = full_state.header; + full_state.position = tracker_cmd->full_state_prediction.position[path_sample_offset]; + full_state.velocity = tracker_cmd->full_state_prediction.velocity[path_sample_offset]; + full_state.acceleration = tracker_cmd->full_state_prediction.acceleration[path_sample_offset]; + full_state.jerk = tracker_cmd->full_state_prediction.jerk[path_sample_offset]; - initial_condition.position = full_state.position; - initial_condition.velocity = full_state.velocity; - initial_condition.acceleration = full_state.acceleration; - initial_condition.jerk = full_state.jerk; + full_state.heading = tracker_cmd->full_state_prediction.heading[path_sample_offset]; + full_state.heading_rate = tracker_cmd->full_state_prediction.heading_rate[path_sample_offset]; + full_state.heading_acceleration = tracker_cmd->full_state_prediction.heading_acceleration[path_sample_offset]; + full_state.heading_jerk = tracker_cmd->full_state_prediction.heading_jerk[path_sample_offset]; - initial_condition.heading = full_state.heading; - initial_condition.heading_rate = full_state.heading_rate; - initial_condition.heading_acceleration = full_state.heading_acceleration; - initial_condition.heading_jerk = full_state.heading_jerk; + ROS_INFO("[MrsTrajectoryGeneration]: getting initial condition from the %d-th sample of the MPC prediction", path_sample_offset); - path_from_future = true; - } + initial_condition.header = full_state.header; - } else - { + initial_condition.position = full_state.position; + initial_condition.velocity = full_state.velocity; + initial_condition.acceleration = full_state.acceleration; + initial_condition.jerk = full_state.jerk; - ROS_INFO("[MrsTrajectoryGeneration]: desired path is NOT from the future, using tracker_cmd as the initial condition"); + initial_condition.heading = full_state.heading; + initial_condition.heading_rate = full_state.heading_rate; + initial_condition.heading_acceleration = full_state.heading_acceleration; + initial_condition.heading_jerk = full_state.heading_jerk; - initial_condition = *tracker_cmd; + path_from_future = true; } - auto control_manager_diag = mrs_lib::get_mutexed(mutex_control_manager_diag_, control_manager_diag_); + } else { - if (path_time == ros::Time(0)) - { - if (!control_manager_diag.tracker_status.have_goal) - { - initial_condition.header.stamp = ros::Time(0); - } - } + ROS_INFO("[MrsTrajectoryGeneration]: desired path is NOT from the future, using tracker_cmd as the initial condition"); - return {{initial_condition}, path_from_future, path_sample_offset}; + initial_condition = *tracker_cmd; } - //} + auto control_manager_diag = mrs_lib::get_mutexed(mutex_control_manager_diag_, control_manager_diag_); - /* optimize() //{ */ + if (path_time == ros::Time(0)) { + if (!control_manager_diag.tracker_status.have_goal) { + initial_condition.header.stamp = ros::Time(0); + } + } - std::tuple MrsTrajectoryGeneration::optimize(const std::vector& waypoints_in, - const std_msgs::Header& waypoints_header, - const bool fallback_sampling, const bool relax_heading) - { + return {{initial_condition}, path_from_future, path_sample_offset}; +} - mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::optimize", scope_timer_logger_, scope_timer_enabled_); +//} - ros::Time optimize_time_start = ros::Time::now(); +/* optimize() //{ */ - // | ---------------- reset the visual markers ---------------- | +std::tuple MrsTrajectoryGeneration::optimize(const std::vector& waypoints_in, + const std_msgs::Header& waypoints_header, + const bool fallback_sampling, const bool relax_heading) { - bw_original_.clearBuffers(); - bw_original_.clearVisuals(); - bw_final_.clearBuffers(); - bw_final_.clearVisuals(); + mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::optimize", scope_timer_logger_, scope_timer_enabled_); - bw_original_.setParentFrame(transformer_->resolveFrame(frame_id_)); - bw_final_.setParentFrame(transformer_->resolveFrame(frame_id_)); + ros::Time optimize_time_start = ros::Time::now(); - bw_original_.setPointsScale(0.4); - bw_final_.setPointsScale(0.35); + // | ---------------- reset the visual markers ---------------- | - // empty path is invalid - if (waypoints_in.size() == 0) - { - std::stringstream ss; - ss << "the path is empty (before postprocessing)"; - ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); - return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference()); - } + bw_original_.clearBuffers(); + bw_original_.clearVisuals(); + bw_final_.clearBuffers(); + bw_final_.clearVisuals(); - std::vector waypoints_in_with_init = waypoints_in; + bw_original_.setParentFrame(transformer_->resolveFrame(frame_id_)); + bw_final_.setParentFrame(transformer_->resolveFrame(frame_id_)); - double path_time_offset = (waypoints_header.stamp - ros::Time::now()).toSec(); + bw_original_.setPointsScale(0.4); + bw_final_.setPointsScale(0.35); - if (path_time_offset > 0.2 && waypoints_in_with_init.size() >= 2) - { - waypoints_in_with_init.erase(waypoints_in_with_init.begin()); - } + // empty path is invalid + if (waypoints_in.size() == 0) { + std::stringstream ss; + ss << "the path is empty (before postprocessing)"; + ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); + return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference()); + } - auto [initial_condition, path_from_future, path_sample_offset] = prepareInitialCondition(waypoints_header.stamp); + std::vector waypoints_in_with_init = waypoints_in; - // prepend the initial condition - if (initial_condition) - { + double path_time_offset = (waypoints_header.stamp - ros::Time::now()).toSec(); - Waypoint_t initial_waypoint; - initial_waypoint.coords = - Eigen::Vector4d(initial_condition->position.x, initial_condition->position.y, initial_condition->position.z, initial_condition->heading); - initial_waypoint.stop_at = false; - waypoints_in_with_init.insert(waypoints_in_with_init.begin(), initial_waypoint); + if (path_time_offset > 0.2 && waypoints_in_with_init.size() >= 2) { + waypoints_in_with_init.erase(waypoints_in_with_init.begin()); + } - } else - { - fly_now_ = false; - } + auto [initial_condition, path_from_future, path_sample_offset] = prepareInitialCondition(waypoints_header.stamp); - std::vector waypoints = preprocessPath(waypoints_in_with_init); + // prepend the initial condition + if (initial_condition) { - if (waypoints.size() <= 1) - { - std::stringstream ss; - ss << "the path is empty (after postprocessing)"; - ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); - return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference()); - } + Waypoint_t initial_waypoint; + initial_waypoint.coords = + Eigen::Vector4d(initial_condition->position.x, initial_condition->position.y, initial_condition->position.z, initial_condition->heading); + initial_waypoint.stop_at = false; + waypoints_in_with_init.insert(waypoints_in_with_init.begin(), initial_waypoint); - bool safe = false; - int traj_idx; - std::vector segment_safeness; - double max_deviation = 0; + } else { + fly_now_ = false; + } - eth_mav_msgs::EigenTrajectoryPoint::Vector trajectory; + std::vector waypoints = preprocessPath(waypoints_in_with_init); - double sampling_dt = 0; + if (waypoints.size() <= 1) { + std::stringstream ss; + ss << "the path is empty (after postprocessing)"; + ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); + return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference()); + } - if (path_from_future) - { - ROS_INFO("[MrsTrajectoryGeneration]: changing dt = 0.2, cause the path is from the future"); - sampling_dt = 0.2; - } else - { - sampling_dt = _sampling_dt_; - } + bool safe = false; + int traj_idx; + std::vector segment_safeness; + double max_deviation = 0; - std::optional result; - - auto params = mrs_lib::get_mutexed(mutex_params_, params_); - - if (params.enforce_fallback_solver) - { - ROS_WARN("[MrsTrajectoryGeneration]: fallback sampling enforced"); - result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); - } else if (fallback_sampling) - { - ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling"); - result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); - } else if (running_async_planning_) - { - ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, the previous async task is still running"); - result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); - } else if (overtime()) - { - ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, we are running over time"); - result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); - } else - { - - result = findTrajectoryAsync(waypoints, initial_condition, sampling_dt, relax_heading); - } + eth_mav_msgs::EigenTrajectoryPoint::Vector trajectory; - if (result) - { - trajectory = result.value(); - } else - { - std::stringstream ss; - ss << "failed to find trajectory"; - ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); - return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference()); - } + double sampling_dt = 0; - for (int k = 0; k < _trajectory_max_segment_deviation_max_iterations_; k++) - { + if (path_from_future) { + ROS_INFO("[MrsTrajectoryGeneration]: changing dt = 0.2, cause the path is from the future"); + sampling_dt = 0.2; + } else { + sampling_dt = _sampling_dt_; + } - ROS_DEBUG("[MrsTrajectoryGeneration]: revalidation cycle #%d", k); + std::optional result; + + auto params = mrs_lib::get_mutexed(mutex_params_, params_); + + if (params.enforce_fallback_solver) { + ROS_WARN("[MrsTrajectoryGeneration]: fallback sampling enforced"); + result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); + } else if (fallback_sampling) { + ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling"); + result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); + } else if (running_async_planning_) { + ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, the previous async task is still running"); + result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); + } else if (overtime()) { + ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, we are running over time"); + result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); + } else { + + result = findTrajectoryAsync(waypoints, initial_condition, sampling_dt, relax_heading); + } - std::tie(safe, traj_idx, segment_safeness, max_deviation) = validateTrajectorySpatial(trajectory, waypoints); + if (result) { + trajectory = result.value(); + } else { + std::stringstream ss; + ss << "failed to find trajectory"; + ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); + return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference()); + } - if (_trajectory_max_segment_deviation_enabled_ && !safe) - { + for (int k = 0; k < _trajectory_max_segment_deviation_max_iterations_; k++) { - ROS_DEBUG("[MrsTrajectoryGeneration]: trajectory is not safe, max deviation %.3f m", max_deviation); + ROS_DEBUG("[MrsTrajectoryGeneration]: revalidation cycle #%d", k); - std::vector::iterator waypoint = waypoints.begin(); - std::vector::iterator safeness = segment_safeness.begin(); + std::tie(safe, traj_idx, segment_safeness, max_deviation) = validateTrajectorySpatial(trajectory, waypoints); - for (; waypoint < waypoints.end() - 1; waypoint++) - { + if (_trajectory_max_segment_deviation_enabled_ && !safe) { - if (!(*safeness)) - { + ROS_DEBUG("[MrsTrajectoryGeneration]: trajectory is not safe, max deviation %.3f m", max_deviation); - if (waypoint > waypoints.begin() || max_deviation_first_segment_ || int(waypoints.size()) <= 2) - { - Waypoint_t midpoint2 = interpolatePoint(*waypoint, *(waypoint + 1), 0.5); - waypoint = waypoints.insert(waypoint + 1, midpoint2); - } - } + std::vector::iterator waypoint = waypoints.begin(); + std::vector::iterator safeness = segment_safeness.begin(); - safeness++; - } + for (; waypoint < waypoints.end() - 1; waypoint++) { - if (params.enforce_fallback_solver) - { - ROS_WARN("[MrsTrajectoryGeneration]: fallback sampling enforced"); - result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); - } else if (fallback_sampling) - { - ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling"); - result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); - } else if (running_async_planning_) - { - ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, the previous async task is still running"); - result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); - } else if (overtime()) - { - ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, we are running over time"); - result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); - } else - { - result = findTrajectoryAsync(waypoints, initial_condition, sampling_dt, relax_heading); - } + if (!(*safeness)) { - if (result) - { - trajectory = result.value(); - } else - { - std::stringstream ss; - ss << "failed to find trajectory"; - ROS_WARN_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); - return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference()); + if (waypoint > waypoints.begin() || max_deviation_first_segment_ || int(waypoints.size()) <= 2) { + Waypoint_t midpoint2 = interpolatePoint(*waypoint, *(waypoint + 1), 0.5); + waypoint = waypoints.insert(waypoint + 1, midpoint2); + } } - } else - { - ROS_DEBUG("[MrsTrajectoryGeneration]: trajectory is safe (%.2f)", max_deviation); - safe = true; - break; + safeness++; } - } - ROS_INFO("[MrsTrajectoryGeneration]: final max trajectory-path deviation: %.2f m, total trajectory time: %.2fs ", max_deviation, - trajectory.size() * sampling_dt); + if (params.enforce_fallback_solver) { + ROS_WARN("[MrsTrajectoryGeneration]: fallback sampling enforced"); + result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); + } else if (fallback_sampling) { + ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling"); + result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); + } else if (running_async_planning_) { + ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, the previous async task is still running"); + result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); + } else if (overtime()) { + ROS_WARN("[MrsTrajectoryGeneration]: executing fallback sampling, we are running over time"); + result = findTrajectoryFallback(waypoints, sampling_dt, relax_heading); + } else { + result = findTrajectoryAsync(waypoints, initial_condition, sampling_dt, relax_heading); + } + + if (result) { + trajectory = result.value(); + } else { + std::stringstream ss; + ss << "failed to find trajectory"; + ROS_WARN_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); + return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference()); + } - // prepare rviz markers - for (int i = 0; i < int(waypoints.size()); i++) - { - bw_final_.addPoint(vec3_t(waypoints.at(i).coords[0], waypoints.at(i).coords[1], waypoints.at(i).coords[2]), 0.0, 1.0, 0.0, 1.0); + } else { + ROS_DEBUG("[MrsTrajectoryGeneration]: trajectory is safe (%.2f)", max_deviation); + safe = true; + break; } + } + + ROS_INFO("[MrsTrajectoryGeneration]: final max trajectory-path deviation: %.2f m, total trajectory time: %.2fs ", max_deviation, + trajectory.size() * sampling_dt); - mrs_msgs::TrajectoryReference mrs_trajectory; + // prepare rviz markers + for (int i = 0; i < int(waypoints.size()); i++) { + bw_final_.addPoint(vec3_t(waypoints.at(i).coords[0], waypoints.at(i).coords[1], waypoints.at(i).coords[2]), 0.0, 1.0, 0.0, 1.0); + } - // convert the optimized trajectory to mrs_msgs::TrajectoryReference - mrs_trajectory = getTrajectoryReference(trajectory, initial_condition, sampling_dt); + mrs_msgs::TrajectoryReference mrs_trajectory; - // insert part of the MPC prediction in the front of the generated trajectory to compensate for the future - if (path_from_future) - { + // convert the optimized trajectory to mrs_msgs::TrajectoryReference + mrs_trajectory = getTrajectoryReference(trajectory, initial_condition, sampling_dt); - auto current_prediction = sh_tracker_cmd_.getMsg()->full_state_prediction; + // insert part of the MPC prediction in the front of the generated trajectory to compensate for the future + if (path_from_future) { - // calculate the starting idx that we will use from the current_prediction - double path_time_offset_2 = (ros::Time::now() - current_prediction.header.stamp).toSec(); // = how long did it take to optimize - int path_sample_offset_2 = int(floor((path_time_offset_2 - 0.01) / 0.2)) + 1; + auto current_prediction = sh_tracker_cmd_.getMsg()->full_state_prediction; - // if there is anything to insert - if (path_sample_offset > path_sample_offset_2) - { + // calculate the starting idx that we will use from the current_prediction + double path_time_offset_2 = (ros::Time::now() - current_prediction.header.stamp).toSec(); // = how long did it take to optimize + int path_sample_offset_2 = int(floor((path_time_offset_2 - 0.01) / 0.2)) + 1; - ROS_INFO("[MrsTrajectoryGeneration]: inserting pre-trajectory from the prediction, idxs %d to %d", path_sample_offset_2, path_sample_offset); + // if there is anything to insert + if (path_sample_offset > path_sample_offset_2) { - for (int i = path_sample_offset - 1; i >= 0; i--) - { + ROS_INFO("[MrsTrajectoryGeneration]: inserting pre-trajectory from the prediction, idxs %d to %d", path_sample_offset_2, path_sample_offset); - ROS_DEBUG("[MrsTrajectoryGeneration]: inserting idx %d", i); + for (int i = path_sample_offset - 1; i >= 0; i--) { - mrs_msgs::ReferenceStamped reference; + ROS_DEBUG("[MrsTrajectoryGeneration]: inserting idx %d", i); - reference.header = current_prediction.header; + mrs_msgs::ReferenceStamped reference; - reference.reference.heading = current_prediction.heading[i]; - reference.reference.position = current_prediction.position[i]; + reference.header = current_prediction.header; - auto res = transformer_->transformSingle(reference, waypoints_header.frame_id); + reference.reference.heading = current_prediction.heading[i]; + reference.reference.position = current_prediction.position[i]; - if (res) - { - reference = res.value(); - } else - { - std::stringstream ss; - ss << "could not transform reference to the path frame"; - ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); - return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference()); - } + auto res = transformer_->transformSingle(reference, waypoints_header.frame_id); - mrs_trajectory.points.insert(mrs_trajectory.points.begin(), reference.reference); + if (res) { + reference = res.value(); + } else { + std::stringstream ss; + ss << "could not transform reference to the path frame"; + ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); + return std::tuple(false, ss.str(), mrs_msgs::TrajectoryReference()); } + + mrs_trajectory.points.insert(mrs_trajectory.points.begin(), reference.reference); } } + } - bw_original_.publish(); - bw_final_.publish(); + bw_original_.publish(); + bw_final_.publish(); - std::stringstream ss; - ss << "trajectory generated"; + std::stringstream ss; + ss << "trajectory generated"; - ROS_DEBUG("[MrsTrajectoryGeneration]: trajectory generated, took %.3f s", (ros::Time::now() - optimize_time_start).toSec()); + ROS_DEBUG("[MrsTrajectoryGeneration]: trajectory generated, took %.3f s", (ros::Time::now() - optimize_time_start).toSec()); - return std::tuple(true, ss.str(), mrs_trajectory); - } + return std::tuple(true, ss.str(), mrs_trajectory); +} - //} +//} - /* findTrajectory() //{ */ +/* findTrajectory() //{ */ - std::optional MrsTrajectoryGeneration::findTrajectory( - const std::vector& waypoints, const std::optional& initial_state, const double& sampling_dt, - const bool& relax_heading) - { +std::optional MrsTrajectoryGeneration::findTrajectory(const std::vector& waypoints, + const std::optional& initial_state, + const double& sampling_dt, const bool& relax_heading) { - mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::findTrajectory", scope_timer_logger_, scope_timer_enabled_); + mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::findTrajectory", scope_timer_logger_, scope_timer_enabled_); - mrs_lib::AtomicScopeFlag unset_running(running_async_planning_); + mrs_lib::AtomicScopeFlag unset_running(running_async_planning_); - ROS_DEBUG("[MrsTrajectoryGeneration]: findTrajectory() started"); + ROS_DEBUG("[MrsTrajectoryGeneration]: findTrajectory() started"); - ros::Time find_trajectory_time_start = ros::Time::now(); + ros::Time find_trajectory_time_start = ros::Time::now(); - auto params = mrs_lib::get_mutexed(mutex_params_, params_); - auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_); + auto params = mrs_lib::get_mutexed(mutex_params_, params_); + auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_); - auto control_manager_diag = mrs_lib::get_mutexed(mutex_control_manager_diag_, control_manager_diag_); + auto control_manager_diag = mrs_lib::get_mutexed(mutex_control_manager_diag_, control_manager_diag_); - if (initial_state && (initial_state->header.stamp - ros::Time::now()).toSec() < 0.2 && control_manager_diag.tracker_status.have_goal) - { - max_deviation_first_segment_ = false; - } else - { - max_deviation_first_segment_ = true; - } + if (initial_state && (initial_state->header.stamp - ros::Time::now()).toSec() < 0.2 && control_manager_diag.tracker_status.have_goal) { + max_deviation_first_segment_ = false; + } else { + max_deviation_first_segment_ = true; + } + + // optimizer - // optimizer + eth_trajectory_generation::NonlinearOptimizationParameters parameters; - eth_trajectory_generation::NonlinearOptimizationParameters parameters; + parameters.f_rel = 0.05; + parameters.x_rel = 0.1; + parameters.time_penalty = params.time_penalty; + parameters.use_soft_constraints = params.soft_constraints_enabled; + parameters.soft_constraint_weight = params.soft_constraints_weight; + parameters.time_alloc_method = static_cast(params.time_allocation); + if (params.time_allocation == 2) { + parameters.algorithm = nlopt::LD_LBFGS; + } + parameters.initial_stepsize_rel = 0.1; + parameters.inequality_constraint_tolerance = params.inequality_constraint_tolerance; + parameters.equality_constraint_tolerance = params.equality_constraint_tolerance; + parameters.max_iterations = params.max_iterations; + parameters.max_time = NLOPT_EXEC_TIME_FACTOR * timeLeft(); + + eth_trajectory_generation::Vertex::Vector vertices; + const int dimension = 4; - parameters.f_rel = 0.05; - parameters.x_rel = 0.1; - parameters.time_penalty = params.time_penalty; - parameters.use_soft_constraints = params.soft_constraints_enabled; - parameters.soft_constraint_weight = params.soft_constraints_weight; - parameters.time_alloc_method = static_cast(params.time_allocation); - if (params.time_allocation == 2) - { - parameters.algorithm = nlopt::LD_LBFGS; + int derivative_to_optimize = eth_trajectory_generation::derivative_order::ACCELERATION; + + switch (params.derivative_to_optimize) { + case 0: { + derivative_to_optimize = eth_trajectory_generation::derivative_order::ACCELERATION; + break; } - parameters.initial_stepsize_rel = 0.1; - parameters.inequality_constraint_tolerance = params.inequality_constraint_tolerance; - parameters.equality_constraint_tolerance = params.equality_constraint_tolerance; - parameters.max_iterations = params.max_iterations; - parameters.max_time = NLOPT_EXEC_TIME_FACTOR * timeLeft(); - - eth_trajectory_generation::Vertex::Vector vertices; - const int dimension = 4; - - int derivative_to_optimize = eth_trajectory_generation::derivative_order::ACCELERATION; - - switch (params.derivative_to_optimize) - { - case 0: { - derivative_to_optimize = eth_trajectory_generation::derivative_order::ACCELERATION; - break; - } - case 1: { - derivative_to_optimize = eth_trajectory_generation::derivative_order::JERK; - break; - } - case 2: { - derivative_to_optimize = eth_trajectory_generation::derivative_order::SNAP; - break; - } + case 1: { + derivative_to_optimize = eth_trajectory_generation::derivative_order::JERK; + break; } + case 2: { + derivative_to_optimize = eth_trajectory_generation::derivative_order::SNAP; + break; + } + } - // | --------------- add constraints to vertices -------------- | + // | --------------- add constraints to vertices -------------- | - double last_heading; + double last_heading; - if (initial_state) - { - last_heading = initial_state->heading; - } else - { - last_heading = waypoints.at(0).coords[3]; - } + if (initial_state) { + last_heading = initial_state->heading; + } else { + last_heading = waypoints.at(0).coords[3]; + } - for (size_t i = 0; i < waypoints.size(); i++) - { - double x = waypoints.at(i).coords[0]; - double y = waypoints.at(i).coords[1]; - double z = waypoints.at(i).coords[2]; - double heading = sradians::unwrap(waypoints.at(i).coords[3], last_heading); - last_heading = heading; + for (size_t i = 0; i < waypoints.size(); i++) { + double x = waypoints.at(i).coords[0]; + double y = waypoints.at(i).coords[1]; + double z = waypoints.at(i).coords[2]; + double heading = sradians::unwrap(waypoints.at(i).coords[3], last_heading); + last_heading = heading; - eth_trajectory_generation::Vertex vertex(dimension); + eth_trajectory_generation::Vertex vertex(dimension); - if (i == 0) - { + if (i == 0) { - vertex.makeStartOrEnd(Eigen::Vector4d(x, y, z, heading), derivative_to_optimize); + vertex.makeStartOrEnd(Eigen::Vector4d(x, y, z, heading), derivative_to_optimize); - vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading)); + vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading)); - if (initial_state) - { + if (initial_state) { - vertex.addConstraint(eth_trajectory_generation::derivative_order::VELOCITY, - Eigen::Vector4d(initial_state->velocity.x, initial_state->velocity.y, initial_state->velocity.z, initial_state->heading_rate)); + vertex.addConstraint(eth_trajectory_generation::derivative_order::VELOCITY, + Eigen::Vector4d(initial_state->velocity.x, initial_state->velocity.y, initial_state->velocity.z, initial_state->heading_rate)); - vertex.addConstraint(eth_trajectory_generation::derivative_order::ACCELERATION, - Eigen::Vector4d(initial_state->acceleration.x, initial_state->acceleration.y, initial_state->acceleration.z, - initial_state->heading_acceleration)); + vertex.addConstraint( + eth_trajectory_generation::derivative_order::ACCELERATION, + Eigen::Vector4d(initial_state->acceleration.x, initial_state->acceleration.y, initial_state->acceleration.z, initial_state->heading_acceleration)); - vertex.addConstraint(eth_trajectory_generation::derivative_order::JERK, - Eigen::Vector4d(initial_state->jerk.x, initial_state->jerk.y, initial_state->jerk.z, initial_state->heading_jerk)); - } + vertex.addConstraint(eth_trajectory_generation::derivative_order::JERK, + Eigen::Vector4d(initial_state->jerk.x, initial_state->jerk.y, initial_state->jerk.z, initial_state->heading_jerk)); + } - } else if (i == (waypoints.size() - 1)) - { // the last point + } else if (i == (waypoints.size() - 1)) { // the last point - vertex.makeStartOrEnd(Eigen::Vector4d(x, y, z, heading), derivative_to_optimize); + vertex.makeStartOrEnd(Eigen::Vector4d(x, y, z, heading), derivative_to_optimize); - vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading)); + vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading)); - } else - { // mid points + } else { // mid points - vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading)); + vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading)); - if (waypoints.at(i).stop_at) - { - vertex.addConstraint(eth_trajectory_generation::derivative_order::VELOCITY, Eigen::Vector4d(0, 0, 0, 0)); - vertex.addConstraint(eth_trajectory_generation::derivative_order::ACCELERATION, Eigen::Vector4d(0, 0, 0, 0)); - vertex.addConstraint(eth_trajectory_generation::derivative_order::JERK, Eigen::Vector4d(0, 0, 0, 0)); - } + if (waypoints.at(i).stop_at) { + vertex.addConstraint(eth_trajectory_generation::derivative_order::VELOCITY, Eigen::Vector4d(0, 0, 0, 0)); + vertex.addConstraint(eth_trajectory_generation::derivative_order::ACCELERATION, Eigen::Vector4d(0, 0, 0, 0)); + vertex.addConstraint(eth_trajectory_generation::derivative_order::JERK, Eigen::Vector4d(0, 0, 0, 0)); } - - vertices.push_back(vertex); } - // | ---------------- compute the segment times --------------- | - - double v_max_horizontal, a_max_horizontal, j_max_horizontal; - double v_max_vertical, a_max_vertical, j_max_vertical; + vertices.push_back(vertex); + } - // use the small of the ascending/descending values - double vertical_speed_lim = std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed); - double vertical_acceleration_lim = std::min(constraints.vertical_ascending_acceleration, constraints.vertical_descending_acceleration); + // | ---------------- compute the segment times --------------- | - v_max_horizontal = constraints.horizontal_speed; - a_max_horizontal = constraints.horizontal_acceleration; + double v_max_horizontal, a_max_horizontal, j_max_horizontal; + double v_max_vertical, a_max_vertical, j_max_vertical; - v_max_vertical = vertical_speed_lim; - a_max_vertical = vertical_acceleration_lim; + // use the small of the ascending/descending values + double vertical_speed_lim = std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed); + double vertical_acceleration_lim = std::min(constraints.vertical_ascending_acceleration, constraints.vertical_descending_acceleration); - j_max_horizontal = constraints.horizontal_jerk; - j_max_vertical = std::min(constraints.vertical_ascending_jerk, constraints.vertical_descending_jerk); + v_max_horizontal = constraints.horizontal_speed; + a_max_horizontal = constraints.horizontal_acceleration; - if (override_constraints_) - { + v_max_vertical = vertical_speed_lim; + a_max_vertical = vertical_acceleration_lim; - bool can_change = true; + j_max_horizontal = constraints.horizontal_jerk; + j_max_vertical = std::min(constraints.vertical_ascending_jerk, constraints.vertical_descending_jerk); + if (override_constraints_) { - if (initial_state) - { - can_change = (hypot(initial_state->velocity.x, initial_state->velocity.y) < override_max_velocity_horizontal_) - && (hypot(initial_state->acceleration.x, initial_state->acceleration.y) < override_max_acceleration_horizontal_) - && (hypot(initial_state->jerk.x, initial_state->jerk.y) < override_max_jerk_horizontal_) - && (fabs(initial_state->velocity.z) < override_max_velocity_vertical_) - && (fabs(initial_state->acceleration.z) < override_max_acceleration_vertical_) - && (fabs(initial_state->jerk.z) < override_max_jerk_vertical_); - } + bool can_change = true; - if (can_change) - { - v_max_horizontal = override_max_velocity_horizontal_; - a_max_horizontal = override_max_acceleration_horizontal_; - j_max_horizontal = override_max_jerk_horizontal_; + if (initial_state) { + can_change = (hypot(initial_state->velocity.x, initial_state->velocity.y) < override_max_velocity_horizontal_) && + (hypot(initial_state->acceleration.x, initial_state->acceleration.y) < override_max_acceleration_horizontal_) && + (hypot(initial_state->jerk.x, initial_state->jerk.y) < override_max_jerk_horizontal_) && + (fabs(initial_state->velocity.z) < override_max_velocity_vertical_) && + (fabs(initial_state->acceleration.z) < override_max_acceleration_vertical_) && (fabs(initial_state->jerk.z) < override_max_jerk_vertical_); + } - v_max_vertical = override_max_velocity_vertical_; - a_max_vertical = override_max_acceleration_vertical_; - j_max_vertical = override_max_jerk_vertical_; + if (can_change) { - ROS_DEBUG("[MrsTrajectoryGeneration]: overriding constraints by a user"); + v_max_horizontal = override_max_velocity_horizontal_; + a_max_horizontal = override_max_acceleration_horizontal_; + j_max_horizontal = override_max_jerk_horizontal_; - } else - { + v_max_vertical = override_max_velocity_vertical_; + a_max_vertical = override_max_acceleration_vertical_; + j_max_vertical = override_max_jerk_vertical_; - ROS_WARN("[MrsTrajectoryGeneration]: overrifing constraints refused due to possible infeasibility"); - } - } + ROS_DEBUG("[MrsTrajectoryGeneration]: overriding constraints by a user"); - double v_max_heading, a_max_heading, j_max_heading; - - if (relax_heading) - { - v_max_heading = std::numeric_limits::max(); - a_max_heading = std::numeric_limits::max(); - j_max_heading = std::numeric_limits::max(); - } else - { - v_max_heading = constraints.heading_speed; - a_max_heading = constraints.heading_acceleration; - j_max_heading = constraints.heading_jerk; - } + } else { - ROS_DEBUG("[MrsTrajectoryGeneration]: using constraints:"); - ROS_DEBUG("[MrsTrajectoryGeneration]: horizontal: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_horizontal, a_max_horizontal, j_max_horizontal); - ROS_DEBUG("[MrsTrajectoryGeneration]: vertical: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_vertical, a_max_vertical, j_max_vertical); - ROS_DEBUG("[MrsTrajectoryGeneration]: heading: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_heading, a_max_heading, j_max_heading); - - std::vector segment_times, segment_times_baca; - segment_times = estimateSegmentTimes(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical, - v_max_heading, a_max_heading); - segment_times_baca = estimateSegmentTimesBaca(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, - j_max_vertical, v_max_heading, a_max_heading); - - double initial_total_time = 0; - double initial_total_time_baca = 0; - for (int i = 0; i < int(segment_times_baca.size()); i++) - { - initial_total_time += segment_times[i]; - initial_total_time_baca += segment_times_baca[i]; + ROS_WARN("[MrsTrajectoryGeneration]: overrifing constraints refused due to possible infeasibility"); } + } - ROS_DEBUG("[MrsTrajectoryGeneration]: initial total time (Euclidean): %.2f", initial_total_time); - ROS_DEBUG("[MrsTrajectoryGeneration]: initial total time (Baca): %.2f", initial_total_time_baca); + double v_max_heading, a_max_heading, j_max_heading; - // | --------- create an optimizer object and solve it -------- | + if (relax_heading) { + v_max_heading = std::numeric_limits::max(); + a_max_heading = std::numeric_limits::max(); + j_max_heading = std::numeric_limits::max(); + } else { + v_max_heading = constraints.heading_speed; + a_max_heading = constraints.heading_acceleration; + j_max_heading = constraints.heading_jerk; + } - const int N = 10; - eth_trajectory_generation::PolynomialOptimizationNonLinear opt(dimension, parameters); - opt.setupFromVertices(vertices, segment_times, derivative_to_optimize); + ROS_DEBUG("[MrsTrajectoryGeneration]: using constraints:"); + ROS_DEBUG("[MrsTrajectoryGeneration]: horizontal: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_horizontal, a_max_horizontal, j_max_horizontal); + ROS_DEBUG("[MrsTrajectoryGeneration]: vertical: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_vertical, a_max_vertical, j_max_vertical); + ROS_DEBUG("[MrsTrajectoryGeneration]: heading: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_heading, a_max_heading, j_max_heading); + + std::vector segment_times, segment_times_baca; + segment_times = estimateSegmentTimes(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical, + v_max_heading, a_max_heading); + segment_times_baca = estimateSegmentTimesBaca(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical, + v_max_heading, a_max_heading); + + double initial_total_time = 0; + double initial_total_time_baca = 0; + for (int i = 0; i < int(segment_times_baca.size()); i++) { + initial_total_time += segment_times[i]; + initial_total_time_baca += segment_times_baca[i]; + } - opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::VELOCITY, v_max_horizontal); - opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_horizontal); - opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::JERK, j_max_horizontal); + ROS_DEBUG("[MrsTrajectoryGeneration]: initial total time (Euclidean): %.2f", initial_total_time); + ROS_DEBUG("[MrsTrajectoryGeneration]: initial total time (Baca): %.2f", initial_total_time_baca); - opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::VELOCITY, v_max_horizontal); - opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_horizontal); - opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::JERK, j_max_horizontal); + // | --------- create an optimizer object and solve it -------- | - opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::VELOCITY, v_max_vertical); - opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_vertical); - opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::JERK, j_max_vertical); + const int N = 10; + eth_trajectory_generation::PolynomialOptimizationNonLinear opt(dimension, parameters); + opt.setupFromVertices(vertices, segment_times, derivative_to_optimize); - opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::VELOCITY, v_max_heading); - opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_heading); - opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::JERK, j_max_heading); + opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::VELOCITY, v_max_horizontal); + opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_horizontal); + opt.addMaximumMagnitudeConstraint(0, eth_trajectory_generation::derivative_order::JERK, j_max_horizontal); - opt.optimize(); + opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::VELOCITY, v_max_horizontal); + opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_horizontal); + opt.addMaximumMagnitudeConstraint(1, eth_trajectory_generation::derivative_order::JERK, j_max_horizontal); - if (overtime()) - { - return {}; - } + opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::VELOCITY, v_max_vertical); + opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_vertical); + opt.addMaximumMagnitudeConstraint(2, eth_trajectory_generation::derivative_order::JERK, j_max_vertical); - std::string result_str; + opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::VELOCITY, v_max_heading); + opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::ACCELERATION, a_max_heading); + opt.addMaximumMagnitudeConstraint(3, eth_trajectory_generation::derivative_order::JERK, j_max_heading); - switch (opt.getOptimizationInfo().stopping_reason) - { - case nlopt::FAILURE: { - result_str = "generic failure"; - break; - } - case nlopt::INVALID_ARGS: { - result_str = "invalid args"; - break; - } - case nlopt::OUT_OF_MEMORY: { - result_str = "out of memory"; - break; - } - case nlopt::ROUNDOFF_LIMITED: { - result_str = "roundoff limited"; - break; - } - case nlopt::FORCED_STOP: { - result_str = "forced stop"; - break; - } - case nlopt::STOPVAL_REACHED: { - result_str = "stopval reached"; - break; - } - case nlopt::FTOL_REACHED: { - result_str = "ftol reached"; - break; - } - case nlopt::XTOL_REACHED: { - result_str = "xtol reached"; - break; - } - case nlopt::MAXEVAL_REACHED: { - result_str = "maxeval reached"; - break; - } - case nlopt::MAXTIME_REACHED: { - result_str = "maxtime reached"; - break; - } - default: { - result_str = "UNKNOWN FAILURE CODE"; - break; - } - } + opt.optimize(); - if (opt.getOptimizationInfo().stopping_reason >= 1 && opt.getOptimizationInfo().stopping_reason != 6) - { - ROS_DEBUG("[MrsTrajectoryGeneration]: optimization finished successfully with code %d, '%s'", opt.getOptimizationInfo().stopping_reason, - result_str.c_str()); + if (overtime()) { + return {}; + } - } else if (opt.getOptimizationInfo().stopping_reason == -1) - { - ROS_DEBUG("[MrsTrajectoryGeneration]: optimization finished with a generic error code %d, '%s'", opt.getOptimizationInfo().stopping_reason, - result_str.c_str()); + std::string result_str; - } else - { - ROS_WARN("[MrsTrajectoryGeneration]: optimization failed with code %d, '%s', took %.3f s", opt.getOptimizationInfo().stopping_reason, result_str.c_str(), - (ros::Time::now() - find_trajectory_time_start).toSec()); - return {}; + switch (opt.getOptimizationInfo().stopping_reason) { + case nlopt::FAILURE: { + result_str = "generic failure"; + break; + } + case nlopt::INVALID_ARGS: { + result_str = "invalid args"; + break; + } + case nlopt::OUT_OF_MEMORY: { + result_str = "out of memory"; + break; + } + case nlopt::ROUNDOFF_LIMITED: { + result_str = "roundoff limited"; + break; + } + case nlopt::FORCED_STOP: { + result_str = "forced stop"; + break; + } + case nlopt::STOPVAL_REACHED: { + result_str = "stopval reached"; + break; + } + case nlopt::FTOL_REACHED: { + result_str = "ftol reached"; + break; + } + case nlopt::XTOL_REACHED: { + result_str = "xtol reached"; + break; + } + case nlopt::MAXEVAL_REACHED: { + result_str = "maxeval reached"; + break; + } + case nlopt::MAXTIME_REACHED: { + result_str = "maxtime reached"; + break; } + default: { + result_str = "UNKNOWN FAILURE CODE"; + break; + } + } - // | ------------- obtain the polynomial segments ------------- | + if (opt.getOptimizationInfo().stopping_reason >= 1 && opt.getOptimizationInfo().stopping_reason != 6) { + ROS_DEBUG("[MrsTrajectoryGeneration]: optimization finished successfully with code %d, '%s'", opt.getOptimizationInfo().stopping_reason, + result_str.c_str()); - eth_trajectory_generation::Segment::Vector segments; - opt.getPolynomialOptimizationRef().getSegments(&segments); + } else if (opt.getOptimizationInfo().stopping_reason == -1) { + ROS_DEBUG("[MrsTrajectoryGeneration]: optimization finished with a generic error code %d, '%s'", opt.getOptimizationInfo().stopping_reason, + result_str.c_str()); - if (overtime()) - { - return {}; - } + } else { + ROS_WARN("[MrsTrajectoryGeneration]: optimization failed with code %d, '%s', took %.3f s", opt.getOptimizationInfo().stopping_reason, result_str.c_str(), + (ros::Time::now() - find_trajectory_time_start).toSec()); + return {}; + } - // | --------------- create the trajectory class -------------- | + // | ------------- obtain the polynomial segments ------------- | - eth_trajectory_generation::Trajectory trajectory; - opt.getTrajectory(&trajectory); + eth_trajectory_generation::Segment::Vector segments; + opt.getPolynomialOptimizationRef().getSegments(&segments); - eth_mav_msgs::EigenTrajectoryPoint::Vector states; + if (overtime()) { + return {}; + } - ROS_DEBUG("[MrsTrajectoryGeneration]: starting eth sampling with dt = %.2f s ", sampling_dt); + // | --------------- create the trajectory class -------------- | - bool success = eth_trajectory_generation::sampleWholeTrajectory(trajectory, sampling_dt, &states); + eth_trajectory_generation::Trajectory trajectory; + opt.getTrajectory(&trajectory); - if (overtime()) - { - return {}; - } + eth_mav_msgs::EigenTrajectoryPoint::Vector states; - // validate the temporal sampling of the trajectory + ROS_DEBUG("[MrsTrajectoryGeneration]: starting eth sampling with dt = %.2f s ", sampling_dt); - // only check this if the trajectory is > 1.0 sec, this check does not make much sense for the short ones - if ((states.size() * sampling_dt) > 1.0 && (states.size() * sampling_dt) > (_max_trajectory_len_factor_ * initial_total_time_baca)) - { - ROS_ERROR("[MrsTrajectoryGeneration]: the final trajectory sampling is too long = %.2f, initial 'baca' estimate = %.2f, allowed factor %.2f, aborting", - (states.size() * sampling_dt), initial_total_time_baca, _max_trajectory_len_factor_); + bool success = eth_trajectory_generation::sampleWholeTrajectory(trajectory, sampling_dt, &states); - std::stringstream ss; - ss << "trajectory sampling failed"; - ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); - return {}; + if (overtime()) { + return {}; + } - } else if ((states.size() * sampling_dt) > 1.0 && (states.size() * sampling_dt) < (_min_trajectory_len_factor_ * initial_total_time_baca)) - { - ROS_ERROR("[MrsTrajectoryGeneration]: the final trajectory sampling is too short = %.2f, initial 'baca' estimate = %.2f, allowed factor %.2f, aborting", - (states.size() * sampling_dt), initial_total_time_baca, _min_trajectory_len_factor_); + // validate the temporal sampling of the trajectory - std::stringstream ss; - ss << "trajectory sampling failed"; - ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); - return {}; + // only check this if the trajectory is > 1.0 sec, this check does not make much sense for the short ones + if ((states.size() * sampling_dt) > 1.0 && (states.size() * sampling_dt) > (_max_trajectory_len_factor_ * initial_total_time_baca)) { + ROS_ERROR("[MrsTrajectoryGeneration]: the final trajectory sampling is too long = %.2f, initial 'baca' estimate = %.2f, allowed factor %.2f, aborting", + (states.size() * sampling_dt), initial_total_time_baca, _max_trajectory_len_factor_); - } else - { - ROS_DEBUG("[MrsTrajectoryGeneration]: estimated/final trajectory length ratio (final/estimated) %.2f", - (states.size() * sampling_dt) / initial_total_time_baca); - } + std::stringstream ss; + ss << "trajectory sampling failed"; + ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); + return {}; - if (success) - { - ROS_DEBUG("[MrsTrajectoryGeneration]: eth sampling finished, took %.3f s", (ros::Time::now() - find_trajectory_time_start).toSec()); - return std::optional(states); + } else if ((states.size() * sampling_dt) > 1.0 && (states.size() * sampling_dt) < (_min_trajectory_len_factor_ * initial_total_time_baca)) { + ROS_ERROR("[MrsTrajectoryGeneration]: the final trajectory sampling is too short = %.2f, initial 'baca' estimate = %.2f, allowed factor %.2f, aborting", + (states.size() * sampling_dt), initial_total_time_baca, _min_trajectory_len_factor_); - } else - { - ROS_ERROR("[MrsTrajectoryGeneration]: eth could not sample the trajectory, took %.3f s", (ros::Time::now() - find_trajectory_time_start).toSec()); - return {}; - } + std::stringstream ss; + ss << "trajectory sampling failed"; + ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); + return {}; + + } else { + ROS_DEBUG("[MrsTrajectoryGeneration]: estimated/final trajectory length ratio (final/estimated) %.2f", + (states.size() * sampling_dt) / initial_total_time_baca); } - //} + if (success) { + ROS_DEBUG("[MrsTrajectoryGeneration]: eth sampling finished, took %.3f s", (ros::Time::now() - find_trajectory_time_start).toSec()); + return std::optional(states); - /* findTrajectoryFallback() //{ */ + } else { + ROS_ERROR("[MrsTrajectoryGeneration]: eth could not sample the trajectory, took %.3f s", (ros::Time::now() - find_trajectory_time_start).toSec()); + return {}; + } +} - std::optional MrsTrajectoryGeneration::findTrajectoryFallback(const std::vector& waypoints, - const double& sampling_dt, - const bool& relax_heading) - { +//} - mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::findTrajectoryFallback", scope_timer_logger_, scope_timer_enabled_); +/* findTrajectoryFallback() //{ */ - ros::Time time_start = ros::Time::now(); +std::optional MrsTrajectoryGeneration::findTrajectoryFallback(const std::vector& waypoints, + const double& sampling_dt, + const bool& relax_heading) { - ROS_WARN("[MrsTrajectoryGeneration]: fallback sampling started"); + mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::findTrajectoryFallback", scope_timer_logger_, scope_timer_enabled_); - auto params = mrs_lib::get_mutexed(mutex_params_, params_); - auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_); + ros::Time time_start = ros::Time::now(); - eth_trajectory_generation::Vertex::Vector vertices; - const int dimension = 4; + ROS_WARN("[MrsTrajectoryGeneration]: fallback sampling started"); - // | --------------- add constraints to vertices -------------- | + auto params = mrs_lib::get_mutexed(mutex_params_, params_); + auto constraints = mrs_lib::get_mutexed(mutex_constraints_, constraints_); - double last_heading = waypoints.at(0).coords[3]; + eth_trajectory_generation::Vertex::Vector vertices; + const int dimension = 4; - for (size_t i = 0; i < waypoints.size(); i++) - { + // | --------------- add constraints to vertices -------------- | - double x = waypoints.at(i).coords[0]; - double y = waypoints.at(i).coords[1]; - double z = waypoints.at(i).coords[2]; - double heading = sradians::unwrap(waypoints.at(i).coords[3], last_heading); - last_heading = heading; + double last_heading = waypoints.at(0).coords[3]; - eth_trajectory_generation::Vertex vertex(dimension); + for (size_t i = 0; i < waypoints.size(); i++) { - vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading)); + double x = waypoints.at(i).coords[0]; + double y = waypoints.at(i).coords[1]; + double z = waypoints.at(i).coords[2]; + double heading = sradians::unwrap(waypoints.at(i).coords[3], last_heading); + last_heading = heading; - vertices.push_back(vertex); - } + eth_trajectory_generation::Vertex vertex(dimension); - // | ---------------- compute the segment times --------------- | + vertex.addConstraint(eth_trajectory_generation::derivative_order::POSITION, Eigen::Vector4d(x, y, z, heading)); - double v_max_horizontal, a_max_horizontal, j_max_horizontal; - double v_max_vertical, a_max_vertical, j_max_vertical; + vertices.push_back(vertex); + } - // use the small of the ascending/descending values - double vertical_speed_lim = std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed); - double vertical_acceleration_lim = std::min(constraints.vertical_ascending_acceleration, constraints.vertical_descending_acceleration); + // | ---------------- compute the segment times --------------- | - if (override_constraints_) - { + double v_max_horizontal, a_max_horizontal, j_max_horizontal; + double v_max_vertical, a_max_vertical, j_max_vertical; - v_max_horizontal = override_max_velocity_horizontal_; - a_max_horizontal = override_max_acceleration_horizontal_; - j_max_horizontal = override_max_jerk_horizontal_; + // use the small of the ascending/descending values + double vertical_speed_lim = std::min(constraints.vertical_ascending_speed, constraints.vertical_descending_speed); + double vertical_acceleration_lim = std::min(constraints.vertical_ascending_acceleration, constraints.vertical_descending_acceleration); - v_max_vertical = override_max_velocity_vertical_; - a_max_vertical = override_max_acceleration_vertical_; - j_max_vertical = override_max_jerk_vertical_; + if (override_constraints_) { - ROS_DEBUG("[MrsTrajectoryGeneration]: overriding constraints by a user"); - } else - { + v_max_horizontal = override_max_velocity_horizontal_; + a_max_horizontal = override_max_acceleration_horizontal_; + j_max_horizontal = override_max_jerk_horizontal_; - v_max_horizontal = constraints.horizontal_speed; - a_max_horizontal = constraints.horizontal_acceleration; + v_max_vertical = override_max_velocity_vertical_; + a_max_vertical = override_max_acceleration_vertical_; + j_max_vertical = override_max_jerk_vertical_; - v_max_vertical = vertical_speed_lim; - a_max_vertical = vertical_acceleration_lim; + ROS_DEBUG("[MrsTrajectoryGeneration]: overriding constraints by a user"); + } else { - j_max_horizontal = constraints.horizontal_jerk; - j_max_vertical = std::min(constraints.vertical_ascending_jerk, constraints.vertical_descending_jerk); - } + v_max_horizontal = constraints.horizontal_speed; + a_max_horizontal = constraints.horizontal_acceleration; + + v_max_vertical = vertical_speed_lim; + a_max_vertical = vertical_acceleration_lim; + j_max_horizontal = constraints.horizontal_jerk; + j_max_vertical = std::min(constraints.vertical_ascending_jerk, constraints.vertical_descending_jerk); + } - double v_max_heading, a_max_heading, j_max_heading; - if (relax_heading) - { - v_max_heading = std::numeric_limits::max(); - a_max_heading = std::numeric_limits::max(); - j_max_heading = std::numeric_limits::max(); - } else - { - v_max_heading = constraints.heading_speed; - a_max_heading = constraints.heading_acceleration; - j_max_heading = constraints.heading_jerk; - } + double v_max_heading, a_max_heading, j_max_heading; - v_max_horizontal *= _fallback_sampling_speed_factor_; - v_max_vertical *= _fallback_sampling_speed_factor_; + if (relax_heading) { + v_max_heading = std::numeric_limits::max(); + a_max_heading = std::numeric_limits::max(); + j_max_heading = std::numeric_limits::max(); + } else { + v_max_heading = constraints.heading_speed; + a_max_heading = constraints.heading_acceleration; + j_max_heading = constraints.heading_jerk; + } - a_max_horizontal *= _fallback_sampling_accel_factor_; - a_max_vertical *= _fallback_sampling_accel_factor_; + v_max_horizontal *= _fallback_sampling_speed_factor_; + v_max_vertical *= _fallback_sampling_speed_factor_; - ROS_DEBUG("[MrsTrajectoryGeneration]: using constraints:"); - ROS_DEBUG("[MrsTrajectoryGeneration]: horizontal: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_horizontal, a_max_horizontal, j_max_horizontal); - ROS_DEBUG("[MrsTrajectoryGeneration]: vertical: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_vertical, a_max_vertical, j_max_vertical); - ROS_DEBUG("[MrsTrajectoryGeneration]: heading: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_heading, a_max_heading, j_max_heading); + a_max_horizontal *= _fallback_sampling_accel_factor_; + a_max_vertical *= _fallback_sampling_accel_factor_; - std::vector segment_times, segment_times_baca; - segment_times = estimateSegmentTimes(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical, - v_max_heading, a_max_heading); - segment_times_baca = estimateSegmentTimesBaca(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, - j_max_vertical, v_max_heading, a_max_heading); + ROS_DEBUG("[MrsTrajectoryGeneration]: using constraints:"); + ROS_DEBUG("[MrsTrajectoryGeneration]: horizontal: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_horizontal, a_max_horizontal, j_max_horizontal); + ROS_DEBUG("[MrsTrajectoryGeneration]: vertical: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_vertical, a_max_vertical, j_max_vertical); + ROS_DEBUG("[MrsTrajectoryGeneration]: heading: vel = %.2f, acc = %.2f, jerk = %.2f", v_max_heading, a_max_heading, j_max_heading); - double initial_total_time = 0; - double initial_total_time_baca = 0; - for (int i = 0; i < int(segment_times_baca.size()); i++) - { - initial_total_time += segment_times[i]; - initial_total_time_baca += segment_times_baca[i]; + std::vector segment_times, segment_times_baca; + segment_times = estimateSegmentTimes(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical, + v_max_heading, a_max_heading); + segment_times_baca = estimateSegmentTimesBaca(vertices, v_max_horizontal, v_max_vertical, a_max_horizontal, a_max_vertical, j_max_horizontal, j_max_vertical, + v_max_heading, a_max_heading); - ROS_DEBUG("[MrsTrajectoryGeneration]: segment time [%d] = %.2f", i, segment_times_baca[i]); - } + double initial_total_time = 0; + double initial_total_time_baca = 0; + for (int i = 0; i < int(segment_times_baca.size()); i++) { + initial_total_time += segment_times[i]; + initial_total_time_baca += segment_times_baca[i]; - ROS_WARN("[MrsTrajectoryGeneration]: fallback: initial total time (Euclidean): %.2f", initial_total_time); - ROS_WARN("[MrsTrajectoryGeneration]: fallback: initial total time (Baca): %.2f", initial_total_time_baca); + ROS_DEBUG("[MrsTrajectoryGeneration]: segment time [%d] = %.2f", i, segment_times_baca[i]); + } - eth_mav_msgs::EigenTrajectoryPoint::Vector states; + ROS_WARN("[MrsTrajectoryGeneration]: fallback: initial total time (Euclidean): %.2f", initial_total_time); + ROS_WARN("[MrsTrajectoryGeneration]: fallback: initial total time (Baca): %.2f", initial_total_time_baca); - // interpolate each segment - for (size_t i = 0; i < waypoints.size() - 1; i++) - { + eth_mav_msgs::EigenTrajectoryPoint::Vector states; - Eigen::VectorXd start, end; + // interpolate each segment + for (size_t i = 0; i < waypoints.size() - 1; i++) { - const double segment_time = segment_times_baca[i]; + Eigen::VectorXd start, end; - int n_samples; - double interp_step; + const double segment_time = segment_times_baca[i]; - if (segment_time > 1e-1) - { + int n_samples; + double interp_step; - n_samples = ceil(segment_time / sampling_dt); + if (segment_time > 1e-1) { - // important - if (n_samples > 0) - { - interp_step = 1.0 / double(n_samples); - } else - { - interp_step = 0.5; - } + n_samples = ceil(segment_time / sampling_dt); - } else - { - n_samples = 0; - interp_step = 0; + // important + if (n_samples > 0) { + interp_step = 1.0 / double(n_samples); + } else { + interp_step = 0.5; } - ROS_DEBUG("[MrsTrajectoryGeneration]: segment n_samples [%lu] = %d", i, n_samples); + } else { + n_samples = 0; + interp_step = 0; + } - // for the last segment, hit the last waypoint completely - // otherwise, it is hit as the first sample of the following segment - if (n_samples > 0 && i == waypoints.size() - 2) - { - n_samples++; - } + ROS_DEBUG("[MrsTrajectoryGeneration]: segment n_samples [%lu] = %d", i, n_samples); - for (int j = 0; j < n_samples; j++) - { + // for the last segment, hit the last waypoint completely + // otherwise, it is hit as the first sample of the following segment + if (n_samples > 0 && i == waypoints.size() - 2) { + n_samples++; + } - Waypoint_t point = interpolatePoint(waypoints[i], waypoints[i + 1], j * interp_step); + for (int j = 0; j < n_samples; j++) { - eth_mav_msgs::EigenTrajectoryPoint eth_point; - eth_point.position_W[0] = point.coords[0]; - eth_point.position_W[1] = point.coords[1]; - eth_point.position_W[2] = point.coords[2]; - eth_point.setFromYaw(point.coords[3]); + Waypoint_t point = interpolatePoint(waypoints[i], waypoints[i + 1], j * interp_step); - states.push_back(eth_point); + eth_mav_msgs::EigenTrajectoryPoint eth_point; + eth_point.position_W[0] = point.coords[0]; + eth_point.position_W[1] = point.coords[1]; + eth_point.position_W[2] = point.coords[2]; + eth_point.setFromYaw(point.coords[3]); - auto control_manager_diag = mrs_lib::get_mutexed(mutex_control_manager_diag_, control_manager_diag_); + states.push_back(eth_point); - if (j == 0 && i > 0 && waypoints[i].stop_at) - { + auto control_manager_diag = mrs_lib::get_mutexed(mutex_control_manager_diag_, control_manager_diag_); - int insert_samples = int(round(_fallback_sampling_stopping_time_ / sampling_dt)); + if (j == 0 && i > 0 && waypoints[i].stop_at) { - for (int k = 0; k < insert_samples; k++) - { - states.push_back(eth_point); - } + int insert_samples = int(round(_fallback_sampling_stopping_time_ / sampling_dt)); + + for (int k = 0; k < insert_samples; k++) { + states.push_back(eth_point); } } } + } - bool success = true; + bool success = true; - ROS_WARN("[MrsTrajectoryGeneration]: fallback: sampling finished, took %.3f s", (ros::Time::now() - time_start).toSec()); + ROS_WARN("[MrsTrajectoryGeneration]: fallback: sampling finished, took %.3f s", (ros::Time::now() - time_start).toSec()); - // | --------------- create the trajectory class -------------- | + // | --------------- create the trajectory class -------------- | - if (success) - { - return std::optional(states); - } else - { - ROS_ERROR("[MrsTrajectoryGeneration]: fallback: sampling failed"); - return {}; - } + if (success) { + return std::optional(states); + } else { + ROS_ERROR("[MrsTrajectoryGeneration]: fallback: sampling failed"); + return {}; } +} - //} - - /* validateTrajectorySpatial() //{ */ +//} - std::tuple, double> MrsTrajectoryGeneration::validateTrajectorySpatial( - const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, const std::vector& waypoints) - { +/* validateTrajectorySpatial() //{ */ - mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::validateTrajectorySpatial", scope_timer_logger_, scope_timer_enabled_); +std::tuple, double> MrsTrajectoryGeneration::validateTrajectorySpatial( + const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, const std::vector& waypoints) { - // prepare the output + mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("MrsTrajectoryGeneration::validateTrajectorySpatial", scope_timer_logger_, scope_timer_enabled_); - std::vector segments; - for (size_t i = 0; i < waypoints.size() - 1; i++) - { - segments.push_back(true); - } + // prepare the output - int waypoint_idx = 0; + std::vector segments; + for (size_t i = 0; i < waypoints.size() - 1; i++) { + segments.push_back(true); + } - bool is_safe = true; - double max_deviation = 0; + int waypoint_idx = 0; - for (size_t i = 0; i < trajectory.size() - 1; i++) - { + bool is_safe = true; + double max_deviation = 0; - // the trajectory sample - const vec3_t sample = vec3_t(trajectory[i].position_W[0], trajectory[i].position_W[1], trajectory[i].position_W[2]); + for (size_t i = 0; i < trajectory.size() - 1; i++) { - // next sample - const vec3_t next_sample = vec3_t(trajectory[i + 1].position_W[0], trajectory[i + 1].position_W[1], trajectory[i + 1].position_W[2]); + // the trajectory sample + const vec3_t sample = vec3_t(trajectory[i].position_W[0], trajectory[i].position_W[1], trajectory[i].position_W[2]); - // segment start - const vec3_t segment_start = vec3_t(waypoints.at(waypoint_idx).coords[0], waypoints.at(waypoint_idx).coords[1], waypoints.at(waypoint_idx).coords[2]); + // next sample + const vec3_t next_sample = vec3_t(trajectory[i + 1].position_W[0], trajectory[i + 1].position_W[1], trajectory[i + 1].position_W[2]); - // segment end - const vec3_t segment_end = - vec3_t(waypoints.at(waypoint_idx + 1).coords[0], waypoints.at(waypoint_idx + 1).coords[1], waypoints.at(waypoint_idx + 1).coords[2]); + // segment start + const vec3_t segment_start = vec3_t(waypoints.at(waypoint_idx).coords[0], waypoints.at(waypoint_idx).coords[1], waypoints.at(waypoint_idx).coords[2]); - const double distance_from_segment = distFromSegment(sample, segment_start, segment_end); + // segment end + const vec3_t segment_end = + vec3_t(waypoints.at(waypoint_idx + 1).coords[0], waypoints.at(waypoint_idx + 1).coords[1], waypoints.at(waypoint_idx + 1).coords[2]); - const double segment_end_dist = distFromSegment(segment_end, sample, next_sample); + const double distance_from_segment = distFromSegment(sample, segment_start, segment_end); - if (waypoint_idx > 0 || max_deviation_first_segment_ || int(waypoints.size()) <= 2) - { + const double segment_end_dist = distFromSegment(segment_end, sample, next_sample); - if (distance_from_segment > max_deviation) - { - max_deviation = distance_from_segment; - } + if (waypoint_idx > 0 || max_deviation_first_segment_ || int(waypoints.size()) <= 2) { - if (distance_from_segment > _trajectory_max_segment_deviation_) - { - segments.at(waypoint_idx) = false; - is_safe = false; - } + if (distance_from_segment > max_deviation) { + max_deviation = distance_from_segment; } - if (segment_end_dist < 0.05 && waypoint_idx < (int(waypoints.size()) - 2)) - { - waypoint_idx++; + if (distance_from_segment > _trajectory_max_segment_deviation_) { + segments.at(waypoint_idx) = false; + is_safe = false; } } - return std::tuple(is_safe, trajectory.size(), segments, max_deviation); + if (segment_end_dist < 0.05 && waypoint_idx < (int(waypoints.size()) - 2)) { + waypoint_idx++; + } } - //} + return std::tuple(is_safe, trajectory.size(), segments, max_deviation); +} - // | --------------------- minor routines --------------------- | +//} - /* findTrajectoryAsync() //{ */ +// | --------------------- minor routines --------------------- | - std::optional MrsTrajectoryGeneration::findTrajectoryAsync( - const std::vector& waypoints, const std::optional& initial_state, const double& sampling_dt, - const bool& relax_heading) - { +/* findTrajectoryAsync() //{ */ - ROS_DEBUG("[MrsTrajectoryGeneration]: starting the async planning task"); +std::optional MrsTrajectoryGeneration::findTrajectoryAsync( + const std::vector& waypoints, const std::optional& initial_state, const double& sampling_dt, + const bool& relax_heading) { - future_trajectory_result_ = - std::async(std::launch::async, &MrsTrajectoryGeneration::findTrajectory, this, waypoints, initial_state, sampling_dt, relax_heading); + ROS_DEBUG("[MrsTrajectoryGeneration]: starting the async planning task"); - while (ros::ok() && future_trajectory_result_.wait_for(std::chrono::milliseconds(1)) != std::future_status::ready) - { + future_trajectory_result_ = + std::async(std::launch::async, &MrsTrajectoryGeneration::findTrajectory, this, waypoints, initial_state, sampling_dt, relax_heading); - if (overtime()) - { - ROS_WARN("[MrsTrajectoryGeneration]: async task planning timeout, breaking"); - return {}; - } + while (ros::ok() && future_trajectory_result_.wait_for(std::chrono::milliseconds(1)) != std::future_status::ready) { + + if (overtime()) { + ROS_WARN("[MrsTrajectoryGeneration]: async task planning timeout, breaking"); + return {}; } + } - ROS_DEBUG("[MrsTrajectoryGeneration]: async planning task finished successfully"); + ROS_DEBUG("[MrsTrajectoryGeneration]: async planning task finished successfully"); - return future_trajectory_result_.get(); - } + return future_trajectory_result_.get(); +} - //} +//} - /* distFromSegment() //{ */ +/* distFromSegment() //{ */ - double MrsTrajectoryGeneration::distFromSegment(const vec3_t& point, const vec3_t& seg1, const vec3_t& seg2) - { +double MrsTrajectoryGeneration::distFromSegment(const vec3_t& point, const vec3_t& seg1, const vec3_t& seg2) { - vec3_t segment_vector = seg2 - seg1; - double segment_len = segment_vector.norm(); + vec3_t segment_vector = seg2 - seg1; + double segment_len = segment_vector.norm(); - vec3_t segment_vector_norm = segment_vector; - segment_vector_norm.normalize(); + vec3_t segment_vector_norm = segment_vector; + segment_vector_norm.normalize(); - double point_coordinate = segment_vector_norm.dot(point - seg1); + double point_coordinate = segment_vector_norm.dot(point - seg1); - if (point_coordinate < 0) - { - return (point - seg1).norm(); - } else if (point_coordinate > segment_len) - { - return (point - seg2).norm(); - } else - { + if (point_coordinate < 0) { + return (point - seg1).norm(); + } else if (point_coordinate > segment_len) { + return (point - seg2).norm(); + } else { - mat3_t segment_projector = segment_vector_norm * segment_vector_norm.transpose(); - vec3_t projection = seg1 + segment_projector * (point - seg1); + mat3_t segment_projector = segment_vector_norm * segment_vector_norm.transpose(); + vec3_t projection = seg1 + segment_projector * (point - seg1); - return (point - projection).norm(); - } + return (point - projection).norm(); } +} - //} - - /* getTrajectoryReference() //{ */ +//} - mrs_msgs::TrajectoryReference MrsTrajectoryGeneration::getTrajectoryReference(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, - const std::optional& initial_condition, - const double& sampling_dt) - { +/* getTrajectoryReference() //{ */ - mrs_msgs::TrajectoryReference msg; +mrs_msgs::TrajectoryReference MrsTrajectoryGeneration::getTrajectoryReference(const eth_mav_msgs::EigenTrajectoryPoint::Vector& trajectory, + const std::optional& initial_condition, + const double& sampling_dt) { - if (initial_condition) - { - msg.header.stamp = initial_condition->header.stamp; - } else - { - msg.header.stamp = ros::Time::now(); - } + mrs_msgs::TrajectoryReference msg; - msg.header.frame_id = frame_id_; - msg.fly_now = fly_now_; - msg.loop = loop_; - msg.use_heading = use_heading_; - msg.dt = sampling_dt; + if (initial_condition) { + msg.header.stamp = initial_condition->header.stamp; + } else { + msg.header.stamp = ros::Time::now(); + } - for (size_t it = 0; it < trajectory.size(); it++) - { + msg.header.frame_id = frame_id_; + msg.fly_now = fly_now_; + msg.loop = loop_; + msg.use_heading = use_heading_; + msg.dt = sampling_dt; - mrs_msgs::Reference point; - point.heading = 0; - point.position.x = trajectory[it].position_W[0]; - point.position.y = trajectory[it].position_W[1]; - point.position.z = trajectory[it].position_W[2]; - point.heading = trajectory[it].getYaw(); + for (size_t it = 0; it < trajectory.size(); it++) { - msg.points.push_back(point); - } + mrs_msgs::Reference point; + point.heading = 0; + point.position.x = trajectory[it].position_W[0]; + point.position.y = trajectory[it].position_W[1]; + point.position.z = trajectory[it].position_W[2]; + point.heading = trajectory[it].getYaw(); - return msg; + msg.points.push_back(point); } - //} + return msg; +} - /* interpolatePoint() //{ */ +//} - Waypoint_t MrsTrajectoryGeneration::interpolatePoint(const Waypoint_t& a, const Waypoint_t& b, const double& coeff) - { +/* interpolatePoint() //{ */ - Waypoint_t out; - Eigen::Vector4d diff = b.coords - a.coords; +Waypoint_t MrsTrajectoryGeneration::interpolatePoint(const Waypoint_t& a, const Waypoint_t& b, const double& coeff) { - out.coords[0] = a.coords[0] + coeff * diff[0]; - out.coords[1] = a.coords[1] + coeff * diff[1]; - out.coords[2] = a.coords[2] + coeff * diff[2]; - out.coords[3] = radians::interp(a.coords[3], b.coords[3], coeff); + Waypoint_t out; + Eigen::Vector4d diff = b.coords - a.coords; - out.stop_at = false; + out.coords[0] = a.coords[0] + coeff * diff[0]; + out.coords[1] = a.coords[1] + coeff * diff[1]; + out.coords[2] = a.coords[2] + coeff * diff[2]; + out.coords[3] = radians::interp(a.coords[3], b.coords[3], coeff); - return out; - } + out.stop_at = false; - //} + return out; +} - /* checkNaN() //{ */ +//} - bool MrsTrajectoryGeneration::checkNaN(const Waypoint_t& a) - { +/* checkNaN() //{ */ - if (!std::isfinite(a.coords[0])) - { - ROS_ERROR("NaN detected in variable \"a.coords[0]\"!!!"); - return false; - } +bool MrsTrajectoryGeneration::checkNaN(const Waypoint_t& a) { - if (!std::isfinite(a.coords[1])) - { - ROS_ERROR("NaN detected in variable \"a.coords[1]\"!!!"); - return false; - } + if (!std::isfinite(a.coords[0])) { + ROS_ERROR("NaN detected in variable \"a.coords[0]\"!!!"); + return false; + } - if (!std::isfinite(a.coords[2])) - { - ROS_ERROR("NaN detected in variable \"a.coords[2]\"!!!"); - return false; - } + if (!std::isfinite(a.coords[1])) { + ROS_ERROR("NaN detected in variable \"a.coords[1]\"!!!"); + return false; + } - if (!std::isfinite(a.coords[3])) - { - ROS_ERROR("NaN detected in variable \"a.coords[3]\"!!!"); - return false; - } + if (!std::isfinite(a.coords[2])) { + ROS_ERROR("NaN detected in variable \"a.coords[2]\"!!!"); + return false; + } - return true; + if (!std::isfinite(a.coords[3])) { + ROS_ERROR("NaN detected in variable \"a.coords[3]\"!!!"); + return false; } - //} + return true; +} + +//} - /* trajectorySrv() //{ */ +/* trajectorySrv() //{ */ - bool MrsTrajectoryGeneration::trajectorySrv(const mrs_msgs::TrajectoryReference& msg) - { +bool MrsTrajectoryGeneration::trajectorySrv(const mrs_msgs::TrajectoryReference& msg) { - mrs_msgs::TrajectoryReferenceSrv srv; - srv.request.trajectory = msg; + mrs_msgs::TrajectoryReferenceSrv srv; + srv.request.trajectory = msg; - bool res = service_client_trajectory_reference_.call(srv); + bool res = service_client_trajectory_reference_.call(srv); - if (res) - { + if (res) { - if (!srv.response.success) - { - ROS_WARN("[MrsTrajectoryGeneration]: service call for trajectory_reference returned: '%s'", srv.response.message.c_str()); - } + if (!srv.response.success) { + ROS_WARN("[MrsTrajectoryGeneration]: service call for trajectory_reference returned: '%s'", srv.response.message.c_str()); + } - return srv.response.success; + return srv.response.success; - } else - { + } else { - ROS_ERROR("[MrsTrajectoryGeneration]: service call for trajectory_reference failed!"); + ROS_ERROR("[MrsTrajectoryGeneration]: service call for trajectory_reference failed!"); - return false; - } + return false; } +} - //} +//} - /* transformTrackerCmd() //{ */ +/* transformTrackerCmd() //{ */ - std::optional MrsTrajectoryGeneration::transformPath(const mrs_msgs::Path& path_in, const std::string& target_frame) - { +std::optional MrsTrajectoryGeneration::transformPath(const mrs_msgs::Path& path_in, const std::string& target_frame) { - // if we transform to the current control frame, which is in fact the same frame as the tracker_cmd is in - if (target_frame == path_in.header.frame_id) - { - return path_in; - } + // if we transform to the current control frame, which is in fact the same frame as the tracker_cmd is in + if (target_frame == path_in.header.frame_id) { + return path_in; + } - // find the transformation - auto tf = transformer_->getTransform(path_in.header.frame_id, target_frame, path_in.header.stamp); + // find the transformation + auto tf = transformer_->getTransform(path_in.header.frame_id, target_frame, path_in.header.stamp); - if (!tf) - { - ROS_ERROR("[MrsTrajectoryGeneration]: could not find transform from '%s' to '%s' in time %f", path_in.header.frame_id.c_str(), target_frame.c_str(), - path_in.header.stamp.toSec()); - return {}; - } + if (!tf) { + ROS_ERROR("[MrsTrajectoryGeneration]: could not find transform from '%s' to '%s' in time %f", path_in.header.frame_id.c_str(), target_frame.c_str(), + path_in.header.stamp.toSec()); + return {}; + } - mrs_msgs::Path path_out = path_in; + mrs_msgs::Path path_out = path_in; - path_out.header.stamp = tf.value().header.stamp; - path_out.header.frame_id = transformer_->frame_to(tf.value()); + path_out.header.stamp = tf.value().header.stamp; + path_out.header.frame_id = transformer_->frame_to(tf.value()); - for (size_t i = 0; i < path_in.points.size(); i++) - { + for (size_t i = 0; i < path_in.points.size(); i++) { - mrs_msgs::ReferenceStamped waypoint; + mrs_msgs::ReferenceStamped waypoint; - waypoint.header = path_in.header; - waypoint.reference = path_in.points[i]; + waypoint.header = path_in.header; + waypoint.reference = path_in.points[i]; - if (auto ret = transformer_->transform(waypoint, tf.value())) - { + if (auto ret = transformer_->transform(waypoint, tf.value())) { - path_out.points[i] = ret.value().reference; + path_out.points[i] = ret.value().reference; - } else - { - return {}; - } + } else { + return {}; } - - return path_out; } - //} + return path_out; +} - /* overtime() //{ */ +//} - bool MrsTrajectoryGeneration::overtime(void) - { +/* overtime() //{ */ - auto start_time_total = mrs_lib::get_mutexed(mutex_start_time_total_, start_time_total_); - auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_); +bool MrsTrajectoryGeneration::overtime(void) { - double overtime = (ros::Time::now() - start_time_total).toSec(); + auto start_time_total = mrs_lib::get_mutexed(mutex_start_time_total_, start_time_total_); + auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_); - if (overtime > (OVERTIME_SAFETY_FACTOR * max_execution_time - OVERTIME_SAFETY_OFFSET)) - { - return true; - } + double overtime = (ros::Time::now() - start_time_total).toSec(); - return false; + if (overtime > (OVERTIME_SAFETY_FACTOR * max_execution_time - OVERTIME_SAFETY_OFFSET)) { + return true; } - //} + return false; +} + +//} - /* timeLeft() //{ */ +/* timeLeft() //{ */ - double MrsTrajectoryGeneration::timeLeft(void) - { +double MrsTrajectoryGeneration::timeLeft(void) { - auto start_time_total = mrs_lib::get_mutexed(mutex_start_time_total_, start_time_total_); - auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_); + auto start_time_total = mrs_lib::get_mutexed(mutex_start_time_total_, start_time_total_); + auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_); - double current_execution_time = (ros::Time::now() - start_time_total).toSec(); + double current_execution_time = (ros::Time::now() - start_time_total).toSec(); - if (current_execution_time >= max_execution_time) - { - return 0; - } else - { - return max_execution_time - current_execution_time; - } + if (current_execution_time >= max_execution_time) { + return 0; + } else { + return max_execution_time - current_execution_time; } +} - //} +//} - // | ------------------------ callbacks ----------------------- | +// | ------------------------ callbacks ----------------------- | - /* callbackPath() //{ */ +/* callbackPath() //{ */ - void MrsTrajectoryGeneration::callbackPath(const mrs_msgs::PathConstPtr& msg) - { +void MrsTrajectoryGeneration::callbackPath(const mrs_msgs::PathConstPtr& msg) { - if (!is_initialized_) - { - return; - } + if (!is_initialized_) { + return; + } - /* preconditions //{ */ + /* preconditions //{ */ - if (!got_constraints_) - { - std::stringstream ss; - ss << "missing constraints"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); - return; - } + if (!got_constraints_) { + std::stringstream ss; + ss << "missing constraints"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + return; + } - if (!got_control_manager_diag_) - { - std::stringstream ss; - ss << "missing control manager diagnostics"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); - return; - } + if (!got_control_manager_diag_) { + std::stringstream ss; + ss << "missing control manager diagnostics"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + return; + } - //} + if (!sh_uav_state_.hasMsg()) { + std::stringstream ss; + ss << "missing UAV state"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + return; + } - { - std::scoped_lock lock(mutex_start_time_total_); + //} - start_time_total_ = ros::Time::now(); - } + { + std::scoped_lock lock(mutex_start_time_total_); - double path_time_offset = 0; + start_time_total_ = ros::Time::now(); + } - if (msg->header.stamp != ros::Time(0)) - { - path_time_offset = (msg->header.stamp - ros::Time::now()).toSec(); - } + double path_time_offset = 0; - if (path_time_offset > 1e-3) - { + if (msg->header.stamp != ros::Time(0)) { + path_time_offset = (msg->header.stamp - ros::Time::now()).toSec(); + } - std::scoped_lock lock(mutex_max_execution_time_); + if (path_time_offset > 1e-3) { - max_execution_time_ = std::min(FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset, params_.max_time); + std::scoped_lock lock(mutex_max_execution_time_); - ROS_INFO("[MrsTrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR, - path_time_offset); - } else - { + max_execution_time_ = std::min(FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset, params_.max_time); - std::scoped_lock lock(mutex_max_execution_time_, mutex_params_); + ROS_INFO("[MrsTrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR, + path_time_offset); + } else { - max_execution_time_ = params_.max_time; - } + std::scoped_lock lock(mutex_max_execution_time_, mutex_params_); - ROS_INFO("[MrsTrajectoryGeneration]: got path from message"); + max_execution_time_ = params_.max_time; + } - ph_original_path_.publish(msg); + ROS_INFO("[MrsTrajectoryGeneration]: got path from message"); - if (msg->points.empty()) - { - std::stringstream ss; - ss << "received an empty message"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); - return; - } + ph_original_path_.publish(msg); + + if (msg->points.empty()) { + std::stringstream ss; + ss << "received an empty message"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + return; + } - auto transformed_path = transformPath(*msg, ""); + auto transformed_path = transformPath(*msg, ""); - if (!transformed_path) - { - std::stringstream ss; - ss << "could not transform the path to the current control frame"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + if (!transformed_path) { + std::stringstream ss; + ss << "could not transform the path to the current control frame"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + return; + } + + fly_now_ = transformed_path->fly_now; + use_heading_ = transformed_path->use_heading; + frame_id_ = transformed_path->header.frame_id; + override_constraints_ = transformed_path->override_constraints; + loop_ = transformed_path->loop; + override_max_velocity_horizontal_ = transformed_path->override_max_velocity_horizontal; + override_max_velocity_vertical_ = transformed_path->override_max_velocity_vertical; + override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal; + override_max_acceleration_vertical_ = transformed_path->override_max_acceleration_vertical; + override_max_jerk_horizontal_ = transformed_path->override_max_jerk_horizontal; + override_max_jerk_vertical_ = transformed_path->override_max_jerk_horizontal; + stop_at_waypoints_ = transformed_path->stop_at_waypoints; + + std::vector waypoints; + + for (size_t i = 0; i < transformed_path->points.size(); i++) { + + double x = transformed_path->points[i].position.x; + double y = transformed_path->points[i].position.y; + double z = transformed_path->points[i].position.z; + double heading = transformed_path->points[i].heading; + + Waypoint_t wp; + wp.coords = Eigen::Vector4d(x, y, z, heading); + wp.stop_at = stop_at_waypoints_; + + if (!checkNaN(wp)) { + ROS_ERROR("[MrsTrajectoryGeneration]: NaN detected in waypoint #%d", int(i)); return; } - fly_now_ = transformed_path->fly_now; - use_heading_ = transformed_path->use_heading; - frame_id_ = transformed_path->header.frame_id; - override_constraints_ = transformed_path->override_constraints; - loop_ = transformed_path->loop; - override_max_velocity_horizontal_ = transformed_path->override_max_velocity_horizontal; - override_max_velocity_vertical_ = transformed_path->override_max_velocity_vertical; - override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal; - override_max_acceleration_vertical_ = transformed_path->override_max_acceleration_vertical; - override_max_jerk_horizontal_ = transformed_path->override_max_jerk_horizontal; - override_max_jerk_vertical_ = transformed_path->override_max_jerk_horizontal; - stop_at_waypoints_ = transformed_path->stop_at_waypoints; - - std::vector waypoints; - - for (size_t i = 0; i < transformed_path->points.size(); i++) - { - - double x = transformed_path->points[i].position.x; - double y = transformed_path->points[i].position.y; - double z = transformed_path->points[i].position.z; - double heading = transformed_path->points[i].heading; - - Waypoint_t wp; - wp.coords = Eigen::Vector4d(x, y, z, heading); - wp.stop_at = stop_at_waypoints_; - - if (!checkNaN(wp)) - { - ROS_ERROR("[MrsTrajectoryGeneration]: NaN detected in waypoint #%d", int(i)); - return; - } + waypoints.push_back(wp); + } - waypoints.push_back(wp); - } + if (loop_) { + waypoints.push_back(waypoints[0]); + } - if (loop_) - { - waypoints.push_back(waypoints[0]); - } + bool success = false; + std::string message; + mrs_msgs::TrajectoryReference trajectory; - bool success = false; - std::string message; - mrs_msgs::TrajectoryReference trajectory; - - for (int i = 0; i < _n_attempts_; i++) - { - - // the last iteration and the fallback sampling is enabled - bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_; - - std::tie(success, message, trajectory) = optimize(waypoints, transformed_path->header, fallback_sampling, transformed_path->relax_heading); - - if (success) - { - break; - } else - { - if (i < _n_attempts_) - { - ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!"); - } else - { - ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory"); - } - } - } + for (int i = 0; i < _n_attempts_; i++) { - double total_time = (ros::Time::now() - start_time_total_).toSec(); + // the last iteration and the fallback sampling is enabled + bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_; - auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_); + std::tie(success, message, trajectory) = optimize(waypoints, transformed_path->header, fallback_sampling, transformed_path->relax_heading); - if (total_time > max_execution_time) - { - ROS_ERROR("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time, - total_time - max_execution_time); - } else - { - ROS_INFO("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time); + if (success) { + break; + } else { + if (i < _n_attempts_) { + ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!"); + } else { + ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory"); + } } + } + + double total_time = (ros::Time::now() - start_time_total_).toSec(); - trajectory.input_id = transformed_path->input_id; + auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_); - if (success) - { + if (total_time > max_execution_time) { + ROS_ERROR("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time, + total_time - max_execution_time); + } else { + ROS_INFO("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time); + } - bool published = trajectorySrv(trajectory); + trajectory.input_id = transformed_path->input_id; - if (published) - { + if (success) { - ROS_INFO("[MrsTrajectoryGeneration]: trajectory successfully published"); + bool published = trajectorySrv(trajectory); - } else - { + if (published) { - ROS_ERROR("[MrsTrajectoryGeneration]: could not publish the trajectory"); - } + ROS_INFO("[MrsTrajectoryGeneration]: trajectory successfully published"); - } else - { + } else { - ROS_ERROR("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, no publishing a result"); + ROS_ERROR("[MrsTrajectoryGeneration]: could not publish the trajectory"); } + + } else { + + ROS_ERROR("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, no publishing a result"); } +} - //} +//} - /* callbackPathSrv() //{ */ +/* callbackPathSrv() //{ */ - bool MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrv::Request& req, mrs_msgs::PathSrv::Response& res) - { +bool MrsTrajectoryGeneration::callbackPathSrv(mrs_msgs::PathSrv::Request& req, mrs_msgs::PathSrv::Response& res) { - if (!is_initialized_) - { - return false; - } + if (!is_initialized_) { + return false; + } - /* mrs_lib::ScopeTimer timer = mrs_lib::ScopeTimer("callback()"); */ + /* preconditions //{ */ - /* precondition //{ */ + if (!got_constraints_) { + std::stringstream ss; + ss << "missing constraints"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); - if (!got_constraints_) - { - std::stringstream ss; - ss << "missing constraints"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + res.message = ss.str(); + res.success = false; + return true; + } - res.message = ss.str(); - res.success = false; - return true; - } + if (!got_control_manager_diag_) { + std::stringstream ss; + ss << "missing control manager diagnostics"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); - if (!got_control_manager_diag_) - { - std::stringstream ss; - ss << "missing control manager diagnostics"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + res.message = ss.str(); + res.success = false; + return true; + } - res.message = ss.str(); - res.success = false; - return true; - } + if (!sh_uav_state_.hasMsg()) { + std::stringstream ss; + ss << "missing UAV state"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); - //} + res.message = ss.str(); + res.success = false; + return true; + } - { - std::scoped_lock lock(mutex_start_time_total_); + //} - start_time_total_ = ros::Time::now(); - } + { + std::scoped_lock lock(mutex_start_time_total_); - double path_time_offset = 0; + start_time_total_ = ros::Time::now(); + } - if (req.path.header.stamp != ros::Time(0)) - { - path_time_offset = (req.path.header.stamp - ros::Time::now()).toSec(); - } + double path_time_offset = 0; - if (path_time_offset > 1e-3) - { + if (req.path.header.stamp != ros::Time(0)) { + path_time_offset = (req.path.header.stamp - ros::Time::now()).toSec(); + } - std::scoped_lock lock(mutex_max_execution_time_); + if (path_time_offset > 1e-3) { - max_execution_time_ = std::min(FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset, params_.max_time); + std::scoped_lock lock(mutex_max_execution_time_); - ROS_INFO("[MrsTrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR, - path_time_offset); - } else - { + max_execution_time_ = std::min(FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset, params_.max_time); - std::scoped_lock lock(mutex_max_execution_time_, mutex_params_); + ROS_INFO("[MrsTrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR, + path_time_offset); + } else { - max_execution_time_ = params_.max_time; - } + std::scoped_lock lock(mutex_max_execution_time_, mutex_params_); - ROS_INFO("[MrsTrajectoryGeneration]: got path from service"); + max_execution_time_ = params_.max_time; + } - ph_original_path_.publish(req.path); + ROS_INFO("[MrsTrajectoryGeneration]: got path from service"); - if (req.path.points.empty()) - { - std::stringstream ss; - ss << "received an empty message"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + ph_original_path_.publish(req.path); - res.message = ss.str(); - res.success = false; - return true; - } + if (req.path.points.empty()) { + std::stringstream ss; + ss << "received an empty message"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); - auto transformed_path = transformPath(req.path, ""); + res.message = ss.str(); + res.success = false; + return true; + } - if (!transformed_path) - { - std::stringstream ss; - ss << "could not transform the path to the current control frame"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + auto transformed_path = transformPath(req.path, ""); - res.message = ss.str(); + if (!transformed_path) { + std::stringstream ss; + ss << "could not transform the path to the current control frame"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + + res.message = ss.str(); + res.success = false; + return true; + } + + fly_now_ = transformed_path->fly_now; + use_heading_ = transformed_path->use_heading; + frame_id_ = transformed_path->header.frame_id; + override_constraints_ = transformed_path->override_constraints; + loop_ = transformed_path->loop; + override_max_velocity_horizontal_ = transformed_path->override_max_velocity_horizontal; + override_max_velocity_vertical_ = transformed_path->override_max_velocity_vertical; + override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal; + override_max_acceleration_vertical_ = transformed_path->override_max_acceleration_vertical; + override_max_jerk_horizontal_ = transformed_path->override_max_jerk_horizontal; + override_max_jerk_vertical_ = transformed_path->override_max_jerk_horizontal; + stop_at_waypoints_ = transformed_path->stop_at_waypoints; + + std::vector waypoints; + + for (size_t i = 0; i < req.path.points.size(); i++) { + + double x = transformed_path->points[i].position.x; + double y = transformed_path->points[i].position.y; + double z = transformed_path->points[i].position.z; + double heading = transformed_path->points[i].heading; + + Waypoint_t wp; + wp.coords = Eigen::Vector4d(x, y, z, heading); + wp.stop_at = stop_at_waypoints_; + + if (!checkNaN(wp)) { + ROS_ERROR("[MrsTrajectoryGeneration]: NaN detected in waypoint #%d", int(i)); res.success = false; + res.message = "invalid path"; return true; } - fly_now_ = transformed_path->fly_now; - use_heading_ = transformed_path->use_heading; - frame_id_ = transformed_path->header.frame_id; - override_constraints_ = transformed_path->override_constraints; - loop_ = transformed_path->loop; - override_max_velocity_horizontal_ = transformed_path->override_max_velocity_horizontal; - override_max_velocity_vertical_ = transformed_path->override_max_velocity_vertical; - override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal; - override_max_acceleration_vertical_ = transformed_path->override_max_acceleration_vertical; - override_max_jerk_horizontal_ = transformed_path->override_max_jerk_horizontal; - override_max_jerk_vertical_ = transformed_path->override_max_jerk_horizontal; - stop_at_waypoints_ = transformed_path->stop_at_waypoints; - - std::vector waypoints; - - for (size_t i = 0; i < req.path.points.size(); i++) - { - - double x = transformed_path->points[i].position.x; - double y = transformed_path->points[i].position.y; - double z = transformed_path->points[i].position.z; - double heading = transformed_path->points[i].heading; - - Waypoint_t wp; - wp.coords = Eigen::Vector4d(x, y, z, heading); - wp.stop_at = stop_at_waypoints_; - - if (!checkNaN(wp)) - { - ROS_ERROR("[MrsTrajectoryGeneration]: NaN detected in waypoint #%d", int(i)); - res.success = false; - res.message = "invalid path"; - return true; - } + waypoints.push_back(wp); + } - waypoints.push_back(wp); - } + if (loop_) { + waypoints.push_back(waypoints[0]); + } - if (loop_) - { - waypoints.push_back(waypoints[0]); - } + bool success = false; + std::string message; + mrs_msgs::TrajectoryReference trajectory; - bool success = false; - std::string message; - mrs_msgs::TrajectoryReference trajectory; - - for (int i = 0; i < _n_attempts_; i++) - { - - // the last iteration and the fallback sampling is enabled - bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_; - - std::tie(success, message, trajectory) = optimize(waypoints, transformed_path->header, fallback_sampling, transformed_path->relax_heading); - - if (success) - { - break; - } else - { - if (i < _n_attempts_) - { - ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!"); - } else - { - ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory"); - } - } - } + for (int i = 0; i < _n_attempts_; i++) { - double total_time = (ros::Time::now() - start_time_total_).toSec(); + // the last iteration and the fallback sampling is enabled + bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_; - auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_); + std::tie(success, message, trajectory) = optimize(waypoints, transformed_path->header, fallback_sampling, transformed_path->relax_heading); - if (total_time > max_execution_time) - { - ROS_ERROR("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time, - total_time - max_execution_time); - } else - { - ROS_INFO("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time); + if (success) { + break; + } else { + if (i < _n_attempts_) { + ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!"); + } else { + ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory"); + } } + } - trajectory.input_id = transformed_path->input_id; + double total_time = (ros::Time::now() - start_time_total_).toSec(); - if (success) - { + auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_); - bool published = trajectorySrv(trajectory); + if (total_time > max_execution_time) { + ROS_ERROR("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time, + total_time - max_execution_time); + } else { + ROS_INFO("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time); + } - if (published) - { + trajectory.input_id = transformed_path->input_id; - res.success = success; - res.message = message; + if (success) { - } else - { + bool published = trajectorySrv(trajectory); - std::stringstream ss; - ss << "could not publish the trajectory"; + if (published) { - res.success = false; - res.message = ss.str(); + res.success = success; + res.message = message; - ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); - } + } else { - } else - { + std::stringstream ss; + ss << "could not publish the trajectory"; - ROS_ERROR("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, not publishing a result"); + res.success = false; + res.message = ss.str(); - res.success = success; - res.message = message; + ROS_ERROR_STREAM("[MrsTrajectoryGeneration]: " << ss.str()); } - return true; + } else { + + ROS_ERROR("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, not publishing a result"); + + res.success = success; + res.message = message; } - //} + return true; +} - /* callbackGetPathSrv() //{ */ +//} - bool MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrv::Request& req, mrs_msgs::GetPathSrv::Response& res) - { +/* callbackGetPathSrv() //{ */ - if (!is_initialized_) - { - return false; - } +bool MrsTrajectoryGeneration::callbackGetPathSrv(mrs_msgs::GetPathSrv::Request& req, mrs_msgs::GetPathSrv::Response& res) { - /* precondition //{ */ + if (!is_initialized_) { + return false; + } - if (!got_constraints_) - { - std::stringstream ss; - ss << "missing constraints"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + /* precondition //{ */ - res.message = ss.str(); - res.success = false; - return true; - } + if (!got_constraints_) { + std::stringstream ss; + ss << "missing constraints"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); - if (!got_control_manager_diag_) - { - std::stringstream ss; - ss << "missing control manager diagnostics"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + res.message = ss.str(); + res.success = false; + return true; + } - res.message = ss.str(); - res.success = false; - return true; - } + if (!got_control_manager_diag_) { + std::stringstream ss; + ss << "missing control manager diagnostics"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); - //} + res.message = ss.str(); + res.success = false; + return true; + } - { - std::scoped_lock lock(mutex_start_time_total_); + if (!sh_uav_state_.hasMsg()) { + std::stringstream ss; + ss << "missing UAV state"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); - start_time_total_ = ros::Time::now(); - } + res.message = ss.str(); + res.success = false; + return true; + } - double path_time_offset = 0; + //} - if (req.path.header.stamp != ros::Time(0)) - { - path_time_offset = (req.path.header.stamp - ros::Time::now()).toSec(); - } + { + std::scoped_lock lock(mutex_start_time_total_); - if (path_time_offset > 1e-3) - { + start_time_total_ = ros::Time::now(); + } - std::scoped_lock lock(mutex_max_execution_time_); + double path_time_offset = 0; - max_execution_time_ = FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset; + if (req.path.header.stamp != ros::Time(0)) { + path_time_offset = (req.path.header.stamp - ros::Time::now()).toSec(); + } - ROS_INFO("[MrsTrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR, - path_time_offset); - } else - { + if (path_time_offset > 1e-3) { - std::scoped_lock lock(mutex_max_execution_time_, mutex_params_); + std::scoped_lock lock(mutex_max_execution_time_); - max_execution_time_ = params_.max_time; - } + max_execution_time_ = FUTURIZATION_EXEC_TIME_FACTOR * path_time_offset; - ROS_INFO("[MrsTrajectoryGeneration]: got path from service"); + ROS_INFO("[MrsTrajectoryGeneration]: setting the max execution time to %.3f s = %.1f * %.3f", max_execution_time_, FUTURIZATION_EXEC_TIME_FACTOR, + path_time_offset); + } else { - ph_original_path_.publish(req.path); + std::scoped_lock lock(mutex_max_execution_time_, mutex_params_); - if (req.path.points.empty()) - { - std::stringstream ss; - ss << "received an empty message"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + max_execution_time_ = params_.max_time; + } - res.message = ss.str(); - res.success = false; - return true; - } + ROS_INFO("[MrsTrajectoryGeneration]: got path from service"); - auto transformed_path = transformPath(req.path, ""); + ph_original_path_.publish(req.path); - if (!transformed_path) - { - std::stringstream ss; - ss << "could not transform the path to the current control frame"; - ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + if (req.path.points.empty()) { + std::stringstream ss; + ss << "received an empty message"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); - res.message = ss.str(); + res.message = ss.str(); + res.success = false; + return true; + } + + auto transformed_path = transformPath(req.path, ""); + + if (!transformed_path) { + std::stringstream ss; + ss << "could not transform the path to the current control frame"; + ROS_ERROR_STREAM_THROTTLE(1.0, "[MrsTrajectoryGeneration]: " << ss.str()); + + res.message = ss.str(); + res.success = false; + return true; + } + + fly_now_ = transformed_path->fly_now; + use_heading_ = transformed_path->use_heading; + frame_id_ = transformed_path->header.frame_id; + override_constraints_ = transformed_path->override_constraints; + loop_ = transformed_path->loop; + override_max_velocity_horizontal_ = transformed_path->override_max_velocity_horizontal; + override_max_velocity_vertical_ = transformed_path->override_max_velocity_vertical; + override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal; + override_max_acceleration_vertical_ = transformed_path->override_max_acceleration_vertical; + override_max_jerk_horizontal_ = transformed_path->override_max_jerk_horizontal; + override_max_jerk_vertical_ = transformed_path->override_max_jerk_horizontal; + stop_at_waypoints_ = transformed_path->stop_at_waypoints; + + std::vector waypoints; + + for (size_t i = 0; i < transformed_path->points.size(); i++) { + + double x = transformed_path->points[i].position.x; + double y = transformed_path->points[i].position.y; + double z = transformed_path->points[i].position.z; + double heading = transformed_path->points[i].heading; + + Waypoint_t wp; + wp.coords = Eigen::Vector4d(x, y, z, heading); + wp.stop_at = stop_at_waypoints_; + + if (!checkNaN(wp)) { + ROS_ERROR("[MrsTrajectoryGeneration]: NaN detected in waypoint #%d", int(i)); res.success = false; + res.message = "invalid path"; return true; } - fly_now_ = transformed_path->fly_now; - use_heading_ = transformed_path->use_heading; - frame_id_ = transformed_path->header.frame_id; - override_constraints_ = transformed_path->override_constraints; - loop_ = transformed_path->loop; - override_max_velocity_horizontal_ = transformed_path->override_max_velocity_horizontal; - override_max_velocity_vertical_ = transformed_path->override_max_velocity_vertical; - override_max_acceleration_horizontal_ = transformed_path->override_max_acceleration_horizontal; - override_max_acceleration_vertical_ = transformed_path->override_max_acceleration_vertical; - override_max_jerk_horizontal_ = transformed_path->override_max_jerk_horizontal; - override_max_jerk_vertical_ = transformed_path->override_max_jerk_horizontal; - stop_at_waypoints_ = transformed_path->stop_at_waypoints; - - std::vector waypoints; - - for (size_t i = 0; i < transformed_path->points.size(); i++) - { - - double x = transformed_path->points[i].position.x; - double y = transformed_path->points[i].position.y; - double z = transformed_path->points[i].position.z; - double heading = transformed_path->points[i].heading; - - Waypoint_t wp; - wp.coords = Eigen::Vector4d(x, y, z, heading); - wp.stop_at = stop_at_waypoints_; - - if (!checkNaN(wp)) - { - ROS_ERROR("[MrsTrajectoryGeneration]: NaN detected in waypoint #%d", int(i)); - res.success = false; - res.message = "invalid path"; - return true; - } + waypoints.push_back(wp); + } - waypoints.push_back(wp); - } + if (loop_) { + waypoints.push_back(waypoints[0]); + } - if (loop_) - { - waypoints.push_back(waypoints[0]); - } + bool success = false; + std::string message; + mrs_msgs::TrajectoryReference trajectory; - bool success = false; - std::string message; - mrs_msgs::TrajectoryReference trajectory; - - for (int i = 0; i < _n_attempts_; i++) - { - - // the last iteration and the fallback sampling is enabled - bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_; - - std::tie(success, message, trajectory) = optimize(waypoints, transformed_path->header, fallback_sampling, transformed_path->relax_heading); - - if (success) - { - break; - } else - { - if (i < _n_attempts_) - { - ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!"); - } else - { - ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory"); - } - } - } + for (int i = 0; i < _n_attempts_; i++) { - double total_time = (ros::Time::now() - start_time_total_).toSec(); + // the last iteration and the fallback sampling is enabled + bool fallback_sampling = (_n_attempts_ > 1) && (i == (_n_attempts_ - 1)) && _fallback_sampling_enabled_; - auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_); + std::tie(success, message, trajectory) = optimize(waypoints, transformed_path->header, fallback_sampling, transformed_path->relax_heading); - if (total_time > max_execution_time) - { - ROS_ERROR("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time, - total_time - max_execution_time); - } else - { - ROS_INFO("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time); + if (success) { + break; + } else { + if (i < _n_attempts_) { + ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory, trying again with different initial conditions!"); + } else { + ROS_WARN("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory"); + } } + } - if (success) - { - - std::optional tf_traj_state = transformer_->getTransform("", req.path.header.frame_id, ros::Time::now()); + double total_time = (ros::Time::now() - start_time_total_).toSec(); - std::stringstream ss; + auto max_execution_time = mrs_lib::get_mutexed(mutex_max_execution_time_, max_execution_time_); - if (!tf_traj_state) - { - ss << "could not create TF transformer for the trajectory to the requested frame"; - ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str()); - res.success = false; - res.message = ss.str(); - } + if (total_time > max_execution_time) { + ROS_ERROR("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (exceeding maxtime %.3f s by %.3f s)", total_time, max_execution_time, + total_time - max_execution_time); + } else { + ROS_INFO("[MrsTrajectoryGeneration]: trajectory ready, took %.3f s in total (out of %.3f)", total_time, max_execution_time); + } - trajectory.header.frame_id = transformer_->frame_to(*tf_traj_state); + if (success) { - for (unsigned long i = 0; i < trajectory.points.size(); i++) - { + std::optional tf_traj_state = transformer_->getTransform("", req.path.header.frame_id, ros::Time::now()); - mrs_msgs::ReferenceStamped trajectory_point; - trajectory_point.header = trajectory.header; - trajectory_point.reference = trajectory.points[i]; + std::stringstream ss; - auto ret = transformer_->transform(trajectory_point, *tf_traj_state); + if (!tf_traj_state) { + ss << "could not create TF transformer for the trajectory to the requested frame"; + ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str()); + res.success = false; + res.message = ss.str(); + } - if (!ret) - { + trajectory.header.frame_id = transformer_->frame_to(*tf_traj_state); - ss << "trajectory cannnot be transformed to the requested frame"; - ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str()); - res.success = false; - res.message = ss.str(); + for (unsigned long i = 0; i < trajectory.points.size(); i++) { - } else - { + mrs_msgs::ReferenceStamped trajectory_point; + trajectory_point.header = trajectory.header; + trajectory_point.reference = trajectory.points[i]; - // transform the points in the trajectory to the current frame - trajectory.points[i] = ret.value().reference; - } - } + auto ret = transformer_->transform(trajectory_point, *tf_traj_state); - res.trajectory = trajectory; - res.success = success; - res.message = message; + if (!ret) { - } else - { + ss << "trajectory cannnot be transformed to the requested frame"; + ROS_WARN_STREAM_THROTTLE(1.0, "[ControlManager]: " << ss.str()); + res.success = false; + res.message = ss.str(); - ROS_ERROR("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory"); + } else { - res.success = success; - res.message = message; + // transform the points in the trajectory to the current frame + trajectory.points[i] = ret.value().reference; + } } - return true; - } + res.trajectory = trajectory; + res.success = success; + res.message = message; - //} + } else { - /* callbackConstraints() //{ */ + ROS_ERROR("[MrsTrajectoryGeneration]: failed to calculate a feasible trajectory"); - void MrsTrajectoryGeneration::callbackConstraints(const mrs_msgs::DynamicsConstraintsConstPtr& msg) - { + res.success = success; + res.message = message; + } - if (!is_initialized_) - { - return; - } + return true; +} - ROS_INFO_ONCE("[MrsTrajectoryGeneration]: got constraints"); +//} - mrs_lib::set_mutexed(mutex_constraints_, *msg, constraints_); +/* callbackConstraints() //{ */ - got_constraints_ = true; +void MrsTrajectoryGeneration::callbackConstraints(const mrs_msgs::DynamicsConstraintsConstPtr& msg) { + + if (!is_initialized_) { + return; } - //} + ROS_INFO_ONCE("[MrsTrajectoryGeneration]: got constraints"); - /* callbackTrackerCmd() //{ */ + mrs_lib::set_mutexed(mutex_constraints_, *msg, constraints_); - void MrsTrajectoryGeneration::callbackTrackerCmd(const mrs_msgs::TrackerCommand::ConstPtr msg) - { + got_constraints_ = true; +} - if (!is_initialized_) - { - return; - } +//} - ROS_INFO_ONCE("[MrsTrajectoryGeneration]: got tracker cmd"); +/* callbackUavState() //{ */ - transformer_->setDefaultFrame(msg->header.frame_id); +void MrsTrajectoryGeneration::callbackUavState(const mrs_msgs::UavState::ConstPtr msg) { + + if (!is_initialized_) { + return; } - //} + ROS_INFO_ONCE("[MrsTrajectoryGeneration]: getting uav state"); - /* callbackControlManagerDiag() //{ */ + transformer_->setDefaultFrame(msg->header.frame_id); +} - void MrsTrajectoryGeneration::callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnosticsConstPtr& msg) - { - - if (!is_initialized_) - { - return; - } +//} - ROS_INFO_ONCE("[MrsTrajectoryGeneration]: got control manager diagnostics"); +/* callbackControlManagerDiag() //{ */ - got_control_manager_diag_ = true; +void MrsTrajectoryGeneration::callbackControlManagerDiag(const mrs_msgs::ControlManagerDiagnosticsConstPtr& msg) { - mrs_lib::set_mutexed(mutex_control_manager_diag_, *msg, control_manager_diag_); + if (!is_initialized_) { + return; } - //} + ROS_INFO_ONCE("[MrsTrajectoryGeneration]: got control manager diagnostics"); - /* //{ callbackDrs() */ + got_control_manager_diag_ = true; - void MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig& params, [[maybe_unused]] uint32_t level) - { + mrs_lib::set_mutexed(mutex_control_manager_diag_, *msg, control_manager_diag_); +} - mrs_lib::set_mutexed(mutex_params_, params, params_); +//} - { - std::scoped_lock lock(mutex_max_execution_time_); +/* //{ callbackDrs() */ - max_execution_time_ = params.max_time; - } +void MrsTrajectoryGeneration::callbackDrs(mrs_uav_trajectory_generation::drsConfig& params, [[maybe_unused]] uint32_t level) { + + mrs_lib::set_mutexed(mutex_params_, params, params_); + + { + std::scoped_lock lock(mutex_max_execution_time_); - ROS_INFO("[MrsTrajectoryGeneration]: DRS updated"); + max_execution_time_ = params.max_time; } - //} + ROS_INFO("[MrsTrajectoryGeneration]: DRS updated"); +} + +//} } // namespace mrs_uav_trajectory_generation diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index e13ff2a..7f21b63 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -7,3 +7,7 @@ add_subdirectory(topic_fly_now) add_subdirectory(fallback_sampling) add_subdirectory(path_before_takeoff) + +add_subdirectory(get_path_before_takeoff) + +add_subdirectory(get_path_after_takeoff) diff --git a/test/get_path_after_takeoff/CMakeLists.txt b/test/get_path_after_takeoff/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/get_path_after_takeoff/CMakeLists.txt @@ -0,0 +1,16 @@ +get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) + +catkin_add_executable_with_gtest(test_${TEST_NAME} + test.cpp + ) + +target_link_libraries(test_${TEST_NAME} + ${catkin_LIBRARIES} + ) + +add_dependencies(test_${TEST_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + +add_rostest(${TEST_NAME}.test) diff --git a/test/get_path_after_takeoff/config/mrs_simulator.yaml b/test/get_path_after_takeoff/config/mrs_simulator.yaml new file mode 100644 index 0000000..2b5d218 --- /dev/null +++ b/test/get_path_after_takeoff/config/mrs_simulator.yaml @@ -0,0 +1,14 @@ +individual_takeoff_platform: + enabled: false + +uav_names: [ + "uav1", +] + +uav1: + type: "x500" + spawn: + x: 10.0 + y: 20.0 + z: 0.0 + heading: 1.2 diff --git a/test/get_path_after_takeoff/get_path_after_takeoff.test b/test/get_path_after_takeoff/get_path_after_takeoff.test new file mode 100644 index 0000000..3c68066 --- /dev/null +++ b/test/get_path_after_takeoff/get_path_after_takeoff.test @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/get_path_after_takeoff/test.cpp b/test/get_path_after_takeoff/test.cpp new file mode 100644 index 0000000..8b07865 --- /dev/null +++ b/test/get_path_after_takeoff/test.cpp @@ -0,0 +1,104 @@ +#include + +// include the generic test customized for this package +#include + +class Tester : public GetPathTest { + +public: + bool test(); +}; + +bool Tester::test() { + + std::vector points; + + points.push_back(Eigen::Vector4d(-5, -5, 5, 1)); + points.push_back(Eigen::Vector4d(-5, 5, 5, 2)); + points.push_back(Eigen::Vector4d(5, -5, 5, 3)); + points.push_back(Eigen::Vector4d(5, 5, 5, 4)); + + // | ---------------- prepare the path message ---------------- | + + mrs_msgs::Path path; + + path.fly_now = true; + path.use_heading = true; + + for (Eigen::Vector4d point : points) { + + mrs_msgs::Reference reference; + reference.position.x = point[0]; + reference.position.y = point[1]; + reference.position.z = point[2]; + reference.heading = point[3]; + + path.points.push_back(reference); + } + + // | ------------------------- takeoff ------------------------ | + + { + auto [success, message] = takeoff(); + + if (!success) { + ROS_ERROR("[%s]: takeoff failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | -------------------- call the service -------------------- | + + std::optional trajectory; + + { + std::string message; + + std::tie(trajectory, message) = getPathSrv(path); + + if (!trajectory) { + ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | ------------------ check the trajectory ------------------ | + + { + bool trajectory_is_fine = checkTrajectory(*trajectory, path, true); + + if (!trajectory_is_fine) { + ROS_ERROR("[%s]: trajectory check failed", ros::this_node::getName().c_str()); + return false; + } else { + return true; + } + } + + ROS_ERROR("[%s]: reached the end of the test without assertion", ros::this_node::getName().c_str()); + + return false; +} + + +TEST(TESTSuite, test) { + + Tester tester; + + bool result = tester.test(); + + if (result) { + GTEST_SUCCEED(); + } else { + GTEST_FAIL(); + } +} + +int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { + + ros::init(argc, argv, "test"); + + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/test/get_path_before_takeoff/CMakeLists.txt b/test/get_path_before_takeoff/CMakeLists.txt new file mode 100644 index 0000000..bf8f985 --- /dev/null +++ b/test/get_path_before_takeoff/CMakeLists.txt @@ -0,0 +1,16 @@ +get_filename_component(TEST_NAME "${CMAKE_CURRENT_SOURCE_DIR}" NAME) + +catkin_add_executable_with_gtest(test_${TEST_NAME} + test.cpp + ) + +target_link_libraries(test_${TEST_NAME} + ${catkin_LIBRARIES} + ) + +add_dependencies(test_${TEST_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} + ) + +add_rostest(${TEST_NAME}.test) diff --git a/test/get_path_before_takeoff/config/mrs_simulator.yaml b/test/get_path_before_takeoff/config/mrs_simulator.yaml new file mode 100644 index 0000000..2b5d218 --- /dev/null +++ b/test/get_path_before_takeoff/config/mrs_simulator.yaml @@ -0,0 +1,14 @@ +individual_takeoff_platform: + enabled: false + +uav_names: [ + "uav1", +] + +uav1: + type: "x500" + spawn: + x: 10.0 + y: 20.0 + z: 0.0 + heading: 1.2 diff --git a/test/get_path_before_takeoff/get_path_before_takeoff.test b/test/get_path_before_takeoff/get_path_before_takeoff.test new file mode 100644 index 0000000..3c68066 --- /dev/null +++ b/test/get_path_before_takeoff/get_path_before_takeoff.test @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/get_path_before_takeoff/test.cpp b/test/get_path_before_takeoff/test.cpp new file mode 100644 index 0000000..10f206d --- /dev/null +++ b/test/get_path_before_takeoff/test.cpp @@ -0,0 +1,111 @@ +#include + +// include the generic test customized for this package +#include + +class Tester : public GetPathTest { + +public: + bool test(); +}; + +bool Tester::test() { + + std::vector points; + + points.push_back(Eigen::Vector4d(-5, -5, 5, 1)); + points.push_back(Eigen::Vector4d(-5, 5, 5, 2)); + points.push_back(Eigen::Vector4d(5, -5, 5, 3)); + points.push_back(Eigen::Vector4d(5, 5, 5, 4)); + + // | ---------------- prepare the path message ---------------- | + + mrs_msgs::Path path; + + path.fly_now = true; + path.use_heading = true; + + for (Eigen::Vector4d point : points) { + + mrs_msgs::Reference reference; + reference.position.x = point[0]; + reference.position.y = point[1]; + reference.position.z = point[2]; + reference.heading = point[3]; + + path.points.push_back(reference); + } + + // | ---------------- wait for ready to takeoff --------------- | + + while (true) { + + if (!ros::ok()) { + return false; + } + + ROS_INFO_THROTTLE(1.0, "[%s]: waiting for the MRS UAV System", name_.c_str()); + + if (mrsSystemReady()) { + ROS_INFO("[%s]: MRS UAV System is ready", name_.c_str()); + break; + } + + sleep(0.01); + } + + // | -------------------- call the service -------------------- | + + std::optional trajectory; + + { + std::string message; + + std::tie(trajectory, message) = getPathSrv(path); + + if (!trajectory) { + ROS_ERROR("[%s]: goto failed with message: '%s'", ros::this_node::getName().c_str(), message.c_str()); + return false; + } + } + + // | ------------------ check the trajectory ------------------ | + + { + bool trajectory_is_fine = checkTrajectory(*trajectory, path, false); + + if (!trajectory_is_fine) { + ROS_ERROR("[%s]: trajectory check failed", ros::this_node::getName().c_str()); + return false; + } else { + return true; + } + } + + ROS_ERROR("[%s]: reached the end of the test without assertion", ros::this_node::getName().c_str()); + + return false; +} + + +TEST(TESTSuite, test) { + + Tester tester; + + bool result = tester.test(); + + if (result) { + GTEST_SUCCEED(); + } else { + GTEST_FAIL(); + } +} + +int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) { + + ros::init(argc, argv, "test"); + + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/test/include/get_path_test.h b/test/include/get_path_test.h new file mode 100644 index 0000000..8a736b3 --- /dev/null +++ b/test/include/get_path_test.h @@ -0,0 +1,63 @@ +#ifndef GET_PATH_TEST_H +#define GET_PATH_TEST_H + +#include + +#include + +using sradians = mrs_lib::geometry::sradians; + +#define POS_TOLERANCE 0.5 +#define HDG_TOLERANCE 0.2 + +class GetPathTest : public mrs_uav_testing::TestGeneric { + +public: + bool checkTrajectory(const mrs_msgs::TrajectoryReference& trajectory, const mrs_msgs::Path& path, bool starting_from_current_pos); +}; + +bool GetPathTest::checkTrajectory(const mrs_msgs::TrajectoryReference& trajectory, const mrs_msgs::Path& path, bool starting_from_current_pos) { + + if (starting_from_current_pos) { + + auto pos = this->getTrackerCmd()->position; + auto hdg = this->getTrackerCmd()->heading; + + if (std::hypot(pos.x - trajectory.points[0].position.x, pos.y - trajectory.points[0].position.y, pos.z - trajectory.points[0].position.z) > POS_TOLERANCE) { + ROS_ERROR("[%s]: trajectories differ at the translation of the initial condition", ros::this_node::getName().c_str()); + return false; + } + + if (path.use_heading && abs(sradians::diff(trajectory.points[0].heading, hdg)) > HDG_TOLERANCE) { + ROS_ERROR("[%s]: trajectories differ at the heading of the initial condition", ros::this_node::getName().c_str()); + return false; + } + } + + size_t waypoint_idx = 0; + + for (size_t i = 0; i < trajectory.points.size(); i++) { + + double points_dist = std::hypot(path.points[waypoint_idx].position.x - trajectory.points[i].position.x, + path.points[waypoint_idx].position.y - trajectory.points[i].position.y, + path.points[waypoint_idx].position.z - trajectory.points[i].position.z); + + double hdg_dist = 0; + + if (path.use_heading) { + hdg_dist = abs(sradians::diff(path.points[waypoint_idx].heading, trajectory.points[i].heading)); + } + + if (points_dist < POS_TOLERANCE && hdg_dist < HDG_TOLERANCE) { + waypoint_idx++; + } + } + + if (waypoint_idx == path.points.size()) { + return true; + } + + return false; +} + +#endif // GET_PATH_TEST_H