added option to override heading using atan2

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?

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<bool, std::string, mrs_msgs::TrajectoryReference> 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);
   }