Merge pull request #51 from ethz-asl/fix/rewiring_and_yaw

Fix/rewiring and yaw

active_3d_planning_core/include/active_3d_planning_core/module/trajectory_evaluator/continuous_yaw_planning_evaluator.h

@@ -21,8 +21,6 @@ class ContinuousYawPlanningEvaluator : public YawPlanningEvaluator {
   // Override virtual functions
   bool computeGain(TrajectorySegment* traj_in) override;
 
-  bool updateSegment(TrajectorySegment* segment) override;
-
   void visualizeTrajectoryValue(VisualizationMarkers* markers,
                                 const TrajectorySegment& trajectory) override;
 
@@ -40,19 +38,14 @@ class ContinuousYawPlanningEvaluator : public YawPlanningEvaluator {
   bool p_visualize_followup_;  // true: also visualize the gain of the best
   // orientation
   int p_n_sections_fov_;   // Number of sections visible, i.e. fov/section_width
-  double p_update_range_;  // Update only gains within this distance (use 0.0 to
-  // check all)
-  double p_update_gain_;             // Update only gains within above this
-  bool p_update_sections_separate_;  // True: check for each section, false:
-  // check for whole segment
 
   // methods
   double sampleYaw(double original_yaw, int sample) override;
 
   void setTrajectoryYaw(TrajectorySegment* segment, double start_yaw,
                         double target_yaw) override;
 
-  void setBestYaw(TrajectorySegment* segment);
+  void setBestYaw(TrajectorySegment* segment) override;
 };
 
 }  // namespace trajectory_evaluator

active_3d_planning_core/include/active_3d_planning_core/module/trajectory_evaluator/yaw_planning_evaluator.h

@@ -48,12 +48,17 @@ class YawPlanningEvaluator : public TrajectoryEvaluator {
   int p_n_directions_;
   bool p_select_by_value_;  // false: only evaluate the gain, true: evaluate
   // gain+cost+value
+  double p_update_range_;  // Update only gains within this distance (use 0.0 to
+  // check all)
+  double p_update_gain_;  // Update only gains within above this
 
   // methods
   virtual double sampleYaw(double original_yaw, int sample) = 0;
 
   virtual void setTrajectoryYaw(TrajectorySegment* segment, double start_yaw,
                                 double target_yaw) = 0;
+
+  virtual void setBestYaw(TrajectorySegment* segment);
 };
 
 // Information struct that is assigned to segments

active_3d_planning_core/include/active_3d_planning_core/planner/online_planner.h

@@ -158,6 +158,9 @@ class OnlinePlanner : public PlannerI, public ModuleBase {
   void updateGeneratorStep(TrajectorySegment* target);
 
   void updateEvaluatorStep(TrajectorySegment* target);
+
+  // TEST verify that the entire tree is connected correctly and feasible.
+  void verifyTree(TrajectorySegment* next_segment = nullptr);
 };
 }  // namespace active_3d_planning
 

active_3d_planning_core/src/module/trajectory_evaluator/continuous_yaw_planning_evaluator.cpp

@@ -27,11 +27,6 @@ void ContinuousYawPlanningEvaluator::setupFromParamMap(
     Module::ParamMap* param_map) {
   setParam<bool>(param_map, "visualize_followup", &p_visualize_followup_, true);
   setParam<int>(param_map, "n_sections_fov", &p_n_sections_fov_, 1);
-  setParam<double>(param_map, "update_range", &p_update_range_,
-                   -1.0);  // default is no updates
-  setParam<double>(param_map, "update_gain", &p_update_gain_, 0.0);
-  setParam<bool>(param_map, "update_sections_separate",
-                 &p_update_sections_separate_, false);
 
   // setup parent
   YawPlanningEvaluator::setupFromParamMap(param_map);
@@ -46,29 +41,6 @@ bool ContinuousYawPlanningEvaluator::computeGain(TrajectorySegment* traj_in) {
   return true;
 }
 
-bool ContinuousYawPlanningEvaluator::updateSegment(TrajectorySegment* segment) {
-  if (segment->parent && segment->info) {
-    double dist =
-        (planner_.getCurrentPosition() - segment->trajectory.back().position_W)
-            .norm();
-    if (p_update_range_ == 0.0 || p_update_range_ > dist) {
-      bool update_all =
-          (~p_update_sections_separate_) && (segment->gain > p_update_gain_);
-      YawPlanningInfo* info =
-          reinterpret_cast<YawPlanningInfo*>(segment->info.get());
-      for (int i = 0; i < info->orientations.size(); ++i) {
-        if (update_all || info->orientations[i].gain > p_update_gain_) {
-          // all conditions met: update gain of segment
-          following_evaluator_->computeGain(&(info->orientations[i]));
-        }
-      }
-    }
-    // Update trajectory
-    setBestYaw(segment);
-  }
-  return following_evaluator_->updateSegment(segment);
-}
-
 void ContinuousYawPlanningEvaluator::setBestYaw(TrajectorySegment* segment) {
   // compute gain from section overlap
   YawPlanningInfo* info =
@@ -96,11 +68,11 @@ void ContinuousYawPlanningEvaluator::setBestYaw(TrajectorySegment* segment) {
     }
   }
   double best_yaw;
-  if (max_gain > min_gain || segment->trajectory.size() > 1) {
+  if (max_gain > min_gain) {
     // got a best yaw
     best_yaw = defaults::angleScaled(
         info->orientations[max_index].trajectory.back().getYaw() +
-        static_cast<double>(p_n_sections_fov_) / p_n_directions_ * M_PI);
+        static_cast<double>(p_n_sections_fov_ - 1) / p_n_directions_ * M_PI);
   } else {
     // indifferent, use direction of travel
     Eigen::Vector3d direction = segment->trajectory.back().position_W -
@@ -182,11 +154,7 @@ void ContinuousYawPlanningEvaluator::visualizeTrajectoryValue(
 
     // Color according to relative value (blue when indifferent, grey for values
     // below update range)
-    if (info->orientations[i].gain <= p_update_sections_separate_) {
-      marker.color.r = 0.5;
-      marker.color.g = 0.5;
-      marker.color.b = 0.5;
-    } else if (max_value != min_value) {
+    if (max_value != min_value) {
       double frac =
           (info->orientations[i].gain - min_value) / (max_value - min_value);
       if (p_select_by_value_) {

active_3d_planning_core/src/module/trajectory_evaluator/yaw_planning_evaluator.cpp

@@ -1,5 +1,6 @@
 #include "active_3d_planning_core/module/trajectory_evaluator/yaw_planning_evaluator.h"
 
+#include <limits>
 #include <string>
 #include <vector>
 
@@ -16,6 +17,9 @@ YawPlanningEvaluator::YawPlanningEvaluator(PlannerI& planner)
 void YawPlanningEvaluator::setupFromParamMap(Module::ParamMap* param_map) {
   setParam<int>(param_map, "n_directions", &p_n_directions_, 4);
   setParam<bool>(param_map, "select_by_value", &p_select_by_value_, false);
+  setParam<double>(param_map, "update_range", &p_update_range_,
+                   -1.0);  // default is no updates
+  setParam<double>(param_map, "update_gain", &p_update_gain_, 0.0);
 
   // Register link for yaw planning udpaters
   planner_.getFactory().registerLinkableModule("YawPlanningEvaluator", this);
@@ -45,51 +49,63 @@ bool YawPlanningEvaluator::computeGain(TrajectorySegment* traj_in) {
   // Init
   double start_yaw = traj_in->trajectory.front().getYaw();
   double original_yaw = traj_in->trajectory.back().getYaw();
-  YawPlanningInfo* info =
-      new YawPlanningInfo();  // already create the info struct and directly
-  // populate it //where is this getting deleted??
+  traj_in->info = std::make_unique<YawPlanningInfo>();
+  YawPlanningInfo* info = static_cast<YawPlanningInfo*>(traj_in->info.get());
   info->active_orientation = 0;
 
-  // Get value for initial position
-  info->orientations.push_back(traj_in->shallowCopy());
-  setTrajectoryYaw(&(info->orientations.back()), start_yaw, original_yaw);
-  following_evaluator_->computeGain(&(info->orientations.back()));
-  double current_value = info->orientations.back().gain;
-  if (p_select_by_value_) {
-    following_evaluator_->computeCost(&(info->orientations.back()));
-    following_evaluator_->computeValue(&(info->orientations.back()));
-    current_value = info->orientations.back().value;
-  }
-  double best_value = current_value;
-
   // Sample all directions
-  for (int i = 1; i < p_n_directions_; ++i) {
+  for (int i = 0; i < p_n_directions_; ++i) {
     info->orientations.push_back(traj_in->shallowCopy());
     setTrajectoryYaw(&(info->orientations.back()), start_yaw,
                      sampleYaw(original_yaw, i));
     following_evaluator_->computeGain(&(info->orientations.back()));
     if (p_select_by_value_) {
       following_evaluator_->computeCost(&(info->orientations.back()));
       following_evaluator_->computeValue(&(info->orientations.back()));
-      current_value = info->orientations.back().value;
-    } else {
-      current_value = info->orientations.back().gain;
-    }
-    if (current_value > best_value) {
-      best_value = current_value;
-      info->active_orientation = i;
     }
   }
 
   // Apply best segment to the original one, store rest in info
-  traj_in->trajectory = info->orientations[info->active_orientation].trajectory;
-  traj_in->gain = info->orientations[info->active_orientation].gain;
+  setBestYaw(traj_in);
+  return true;
+}
+
+void YawPlanningEvaluator::setBestYaw(TrajectorySegment* segment) {
+  // Select the best yaw out of all current orientations.
+  YawPlanningInfo* info = static_cast<YawPlanningInfo*>(segment->info.get());
+  double min_gain = std::numeric_limits<double>::max();
+  double max_gain = std::numeric_limits<double>::lowest();
+  int max_index = 0;
+  for (int i = 0; i < info->orientations.size(); ++i) {
+    double gain = p_select_by_value_ ? gain = info->orientations[i].value
+                                     : gain = info->orientations[i].gain;
+    if (gain > max_gain) {
+      max_gain = gain;
+      max_index = i;
+    } else if (gain < min_gain) {
+      min_gain = gain;
+    }
+  }
+  double best_yaw;
+  if (max_gain > min_gain) {
+    // got a best yaw
+    best_yaw = defaults::angleScaled(
+        info->orientations[max_index].trajectory.back().getYaw());
+  } else {
+    // indifferent, use direction of travel
+    Eigen::Vector3d direction = segment->trajectory.back().position_W -
+                                segment->trajectory.front().position_W;
+    best_yaw = defaults::angleScaled(std::atan2(direction[1], direction[0]));
+  }
+  setTrajectoryYaw(segment, segment->trajectory.front().getYaw(), best_yaw);
+  info->active_orientation = max_index;
+
+  // Set the gain, and optionally cost and value.
+  segment->gain = info->orientations[max_index].gain;
   if (p_select_by_value_) {
-    traj_in->cost = info->orientations[info->active_orientation].cost;
-    traj_in->value = info->orientations[info->active_orientation].value;
+    segment->cost = info->orientations[max_index].cost;
+    segment->value = info->orientations[max_index].value;
   }
-  traj_in->info.reset(info);
-  return true;
 }
 
 bool YawPlanningEvaluator::computeCost(TrajectorySegment* traj_in) {
@@ -105,6 +121,41 @@ int YawPlanningEvaluator::selectNextBest(TrajectorySegment* traj_in) {
 }
 
 bool YawPlanningEvaluator::updateSegment(TrajectorySegment* segment) {
+  // Double check whether the trajectory has changed.
+  YawPlanningInfo* info =
+      reinterpret_cast<YawPlanningInfo*>(segment->info.get());
+  if (info) {
+    for (size_t i = 0; i < info->orientations.size(); ++i) {
+      if (info->orientations[i].parent != segment->parent ||
+          info->orientations[i].trajectory.front().position_W !=
+              segment->trajectory.front().position_W) {
+        double original_yaw = info->orientations[i].trajectory.back().getYaw();
+        info->orientations[i].parent = segment->parent;
+        info->orientations[i].trajectory = segment->trajectory;
+        setTrajectoryYaw(&(info->orientations[i]),
+                         segment->trajectory.front().getYaw(), original_yaw);
+      }
+    }
+  }
+
+  // Update the gains if required.
+  if (segment->parent && info) {
+    double dist =
+        (planner_.getCurrentPosition() - segment->trajectory.back().position_W)
+            .norm();
+    if (p_update_range_ == 0.0 || p_update_range_ > dist) {
+      YawPlanningInfo* info =
+          reinterpret_cast<YawPlanningInfo*>(segment->info.get());
+      for (int i = 0; i < info->orientations.size(); ++i) {
+        if (info->orientations[i].gain > p_update_gain_) {
+          // all conditions met: update gain of segment
+          following_evaluator_->computeGain(&(info->orientations[i]));
+        }
+      }
+    }
+    // Update trajectory
+    setBestYaw(segment);
+  }
   return following_evaluator_->updateSegment(segment);
 }
 

active_3d_planning_core/src/module/trajectory_generator/generator_updater/recheck_collision.cpp

@@ -20,9 +20,9 @@ RecheckCollision::RecheckCollision(PlannerI& planner)
 void RecheckCollision::setupFromParamMap(Module::ParamMap* param_map) {}
 
 bool RecheckCollision::updateSegment(TrajectorySegment* segment) {
-  for (int i = 0; i < segment->trajectory.size(); ++i) {
+  for (const EigenTrajectoryPoint& point : segment->trajectory) {
     if (!(planner_.getTrajectoryGenerator().checkTraversable(
-            segment->trajectory[i].position_W))) {
+            point.position_W))) {
       return false;
     }
   }

active_3d_planning_core/src/module/trajectory_generator/rrt.cpp

@@ -258,7 +258,7 @@ bool RRT::connectPoses(const EigenTrajectoryPoint& start,
     }
   }
   // Build trajectory
-  for (int i = 0; i < n_points; ++i) {
+  for (int i = 0; i <= n_points; ++i) {
     EigenTrajectoryPoint trajectory_point;
     trajectory_point.position_W =
         start_pos +

active_3d_planning_core/src/module/trajectory_generator/rrt_star.cpp

@@ -344,10 +344,10 @@ bool RRTStar::rewireToBestParent(
   TrajectorySegment* initial_parent = segment->parent;
 
   // Find best segment
-  for (int i = 0; i < candidates.size(); ++i) {
+  for (TrajectorySegment* candidate : candidates) {
     segment->trajectory.clear();
-    segment->parent = candidates[i];
-    if (connectPoses(candidates[i]->trajectory.back(), goal_point,
+    segment->parent = candidate;
+    if (connectPoses(candidate->trajectory.back(), goal_point,
                      &(segment->trajectory))) {
       // Feasible connection: evaluate the trajectory
       planner_.getTrajectoryEvaluator().computeCost(segment);
@@ -367,6 +367,7 @@ bool RRTStar::rewireToBestParent(
     segment->parent = best_segment.parent;
     segment->trajectory = best_segment.trajectory;
     segment->cost = best_segment.cost;
+    segment->value = best_segment.value;
     planner_.getTrajectoryEvaluator().computeValue(segment);
     if (segment->parent == initial_parent) {
       // Back to old parent