removed all instances of constants

tritonuas · Jun 11, 2024 · ab79080 · ab79080
1 parent 46c3784
commit ab79080
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 44 deletions.
diff --git a/include/utilities/constants.hpp b/include/utilities/constants.hpp
@@ -14,10 +14,6 @@ const size_t MAX_CV_PIPELINES = 5;
 const double TWO_PI = 2 * M_PI;
 const double HALF_PI = M_PI / 2;
 
-// FROM OBC PYTHON
-const double TURNING_RADIUS = 30.0;
-const double POINT_SEPARATION = 10.0;
-
 // RRT CONSTANTS
 const int ITERATIONS_PER_WAYPOINT = 256; // number of times RRT is ran per waypoint
 const double SEARCH_RADIUS =

diff --git a/src/pathing/environment.cpp b/src/pathing/environment.cpp
@@ -74,43 +74,43 @@ bool Environment::isPathInBounds(const std::vector<XYZCoord>& path) const {
  return true;
 }
 
-bool Environment::isPathInBoundsAdv(const std::vector<XYZCoord>& path,
- const RRTOption& option) const {
- if (!option.has_straight) {
- return isPathInBounds(path);
- }
-
- // finds the last point on the first curve, and the first point on the second curve
- // does this using the option, using arclength and the point separation
- const int first_curve_end =
- std::abs(option.dubins_path.beta_0) * TURNING_RADIUS / POINT_SEPARATION + 1;
- const int second_curve_start =
- path.size() - std::abs(option.dubins_path.beta_2) * TURNING_RADIUS / POINT_SEPARATION;
-
- // sanity check
- if (first_curve_end >= second_curve_start) {
- return isPathInBounds(path);
- }
-
- if (!isLineInBounds(path[first_curve_end], path[second_curve_start])) {
- return false;
- }
-
- // checks the points manually in the curve
- for (int i = 0; i <= first_curve_end; i++) {
- if (!isPointInBounds(path[i])) {
- return false;
- }
- }
-
- for (int i = second_curve_start; i < path.size(); i++) {
- if (!isPointInBounds(path[i])) {
- return false;
- }
- }
-
- return true;
-}
+// bool Environment::isPathInBoundsAdv(const std::vector<XYZCoord>& path,
+//  const RRTOption& option) const {
+//  if (!option.has_straight) {
+//  return isPathInBounds(path);
+//  }
+
+//  // finds the last point on the first curve, and the first point on the second curve
+//  // does this using the option, using arclength and the point separation
+//  const int first_curve_end =
+//  std::abs(option.dubins_path.beta_0) * TURNING_RADIUS / POINT_SEPARATION + 1;
+//  const int second_curve_start =
+//  path.size() - std::abs(option.dubins_path.beta_2) * TURNING_RADIUS / POINT_SEPARATION;
+
+//  // sanity check
+//  if (first_curve_end >= second_curve_start) {
+//  return isPathInBounds(path);
+//  }
+
+//  if (!isLineInBounds(path[first_curve_end], path[second_curve_start])) {
+//  return false;
+//  }
+
+//  // checks the points manually in the curve
+//  for (int i = 0; i <= first_curve_end; i++) {
+//  if (!isPointInBounds(path[i])) {
+//  return false;
+//  }
+//  }
+
+//  for (int i = second_curve_start; i < path.size(); i++) {
+//  if (!isPointInBounds(path[i])) {
+//  return false;
+//  }
+//  }
+
+//  return true;
+// }
 
 const XYZCoord& Environment::getGoal() const { return goals[goals_found]; }
 

diff --git a/src/pathing/static.cpp b/src/pathing/static.cpp
@@ -37,7 +37,8 @@ RRT::RRT(RRTPoint start, std::vector<XYZCoord> goals, double search_radius, Envi
  : iterations_per_waypoint(config.pathing.rrt.iterations_per_waypoint),
  search_radius(search_radius),
  rewire_radius(config.pathing.rrt.rewire_radius),
- tree(start, airspace, Dubins(TURNING_RADIUS, POINT_SEPARATION)),
+ tree(start, airspace,
+ Dubins(config.pathing.dubins.turning_radius, config.pathing.dubins.point_separation)),
  config(config.pathing.rrt) {
  if (angles.size() != 0) {
  this->angles = angles;

diff --git a/src/pathing/tree.cpp b/src/pathing/tree.cpp
@@ -95,8 +95,8 @@ RRTTree::RRTTree(RRTPoint root_point, Environment airspace, Dubins dubins)
 RRTTree::~RRTTree() { delete root; }
 
 bool RRTTree::validatePath(const std::vector<XYZCoord>& path, const RRTOption& option) const {
- // return airspace.isPathInBounds(path);
- return airspace.isPathInBoundsAdv(path, option);
+ return airspace.isPathInBounds(path);
+ // return airspace.isPathInBoundsAdv(path, option);
 }
 
 RRTNode* RRTTree::generateNode(RRTNode* anchor_node, const RRTPoint& new_point,