From dc7e0d56a1961275adb2b0f0a83b134aad0d7089 Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Thu, 20 Jun 2024 14:07:10 +0200 Subject: [PATCH] added option to override heading using atan2 --- config/public/trajectory_generation.yaml | 3 +++ src/mrs_trajectory_generation.cpp | 16 +++++++++++++--- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/config/public/trajectory_generation.yaml b/config/public/trajectory_generation.yaml index 84143c6..3daf7cd 100644 --- a/config/public/trajectory_generation.yaml +++ b/config/public/trajectory_generation.yaml @@ -38,6 +38,9 @@ mrs_uav_trajectory_generation: # how many times to try the plannin + sampling n_attempts: 3 # [-] + # when true, the resulting trajectory will have a heading always pointing tangentially along the trajectory + override_heading_atan2: false + # if the generation and sampling fails completely and the result is infeasible # should we at least provide a naive constant-velocity solution which will # move the drone and therefore avoid potential deadlocks? diff --git a/src/mrs_trajectory_generation.cpp b/src/mrs_trajectory_generation.cpp index 85f0799..63f80ee 100644 --- a/src/mrs_trajectory_generation.cpp +++ b/src/mrs_trajectory_generation.cpp @@ -112,6 +112,8 @@ class MrsTrajectoryGeneration : public nodelet::Nodelet { double _path_straightener_max_deviation_; double _path_straightener_max_hdg_deviation_; + bool _override_heading_atan2_; + // | -------- variable parameters (come with the path) -------- | std::string frame_id_; @@ -331,6 +333,8 @@ void MrsTrajectoryGeneration::onInit() { 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 + "override_heading_atan2", _override_heading_atan2_); + param_loader.loadParam(yaml_prefix + "min_waypoint_distance", _min_waypoint_distance_); // | --------------------- tf transformer --------------------- | @@ -783,7 +787,8 @@ std::tuple MrsTrajectoryGenera reference.header = current_prediction.header; - reference.reference.heading = current_prediction.heading.at(i); + reference.reference.heading = current_prediction.heading.at(i); + reference.reference.position = current_prediction.position.at(i); auto res = transformer_->transformSingle(reference, waypoints_header.frame_id); @@ -1497,11 +1502,16 @@ mrs_msgs::TrajectoryReference MrsTrajectoryGeneration::getTrajectoryReference(co for (size_t it = 0; it < trajectory.size(); it++) { mrs_msgs::Reference point; - point.heading = 0; + point.position.x = trajectory.at(it).position_W(0); point.position.y = trajectory.at(it).position_W(1); point.position.z = trajectory.at(it).position_W(2); - point.heading = trajectory.at(it).getYaw(); + + if (_override_heading_atan2_ && it < (trajectory.size() - 1)) { + point.heading = atan2(trajectory.at(it+1).position_W(1) - point.position.y, trajectory.at(it+1).position_W(0) - point.position.x); + } else { + point.heading = trajectory.at(it).getYaw(); + } msg.points.push_back(point); }