remove cost of candidate from treenode

PX4 · Sep 4, 2019 · 8ae4077 · 8ae4077
1 parent baa6978
commit 8ae4077
Show file tree

Hide file tree

Showing 5 changed files with 11 additions and 15 deletions.
diff --git a/local_planner/include/local_planner/candidate_direction.h b/local_planner/include/local_planner/candidate_direction.h
@@ -19,7 +19,7 @@ struct candidateDirection {
     start_state.velocity = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
     start_state.acceleration = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
     start_state.time = ros::Time::now().toSec();
-    tree_node = TreeNode(0, start_state, Eigen::Vector3f::Zero(), 0.f);
+    tree_node = TreeNode(0, start_state, Eigen::Vector3f::Zero());
   };
 
   bool operator<(const candidateDirection& y) const { return cost < y.cost; }

diff --git a/local_planner/include/local_planner/tree_node.h b/local_planner/include/local_planner/tree_node.h
@@ -10,15 +10,14 @@ namespace avoidance {
 class TreeNode {
  public:
   float total_cost_;
-  float cost_;
   float heuristic_;
   int origin_;
   bool closed_;
   simulation_state state;    // State containing position, velocity and time of the drone
   Eigen::Vector3f setpoint;  // Setpoint required to send to PX4 in order to get to this state
 
   TreeNode() = default;
-  TreeNode(int from, const simulation_state& start_state, const Eigen::Vector3f& sp, const float cost);
+  TreeNode(int from, const simulation_state& start_state, const Eigen::Vector3f& sp);
   ~TreeNode() = default;
 
   /**

diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp
@@ -263,17 +263,15 @@ void LocalPlannerVisualization::publishTree(const std::vector<TreeNode>& tree, c
   float range_max = 120.f;
   float range_min = 0.f;
 
-  // std::cout << tree.size() << std::endl;
   for (size_t i = 0; i < closed_set.size(); i++) {
     int node_nr = closed_set[i];
-
-    variance_max_value = std::max(variance_max_value, tree[node_nr].cost_);
-    variance_min_value = std::min(variance_min_value, tree[node_nr].cost_);
+    variance_max_value = std::max(variance_max_value, tree[node_nr].total_cost_);
+    variance_min_value = std::min(variance_min_value, tree[node_nr].total_cost_);
   }
   for (size_t i = 0; i < closed_set.size(); i++) {
     int node_nr = closed_set[i];
     marker.pose.position = toPoint(tree[node_nr].getPosition());
-    float cost = tree[node_nr].cost_;
+    float cost = tree[node_nr].total_cost_;
     float heuristic = tree[i].heuristic_;
 
     float h =

diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp
@@ -58,7 +58,7 @@ void StarPlanner::buildLookAheadTree() {
   start_state.velocity = velocity_;
   start_state.acceleration = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
   start_state.time = ros::Time::now().toSec();
-  tree_.push_back(TreeNode(0, start_state, Eigen::Vector3f::Zero(), 0.f));
+  tree_.push_back(TreeNode(0, start_state, Eigen::Vector3f::Zero()));
   tree_.back().setCosts(treeHeuristicFunction(0), treeHeuristicFunction(0));
 
   int origin = 0;
@@ -98,10 +98,10 @@ void StarPlanner::buildLookAheadTree() {
         }
 
         if (queue.size() < children_per_node_) {
-          candidate.tree_node = TreeNode(origin, trajectory_endpoint, candidate.toEigen(), candidate.cost);
+          candidate.tree_node = TreeNode(origin, trajectory_endpoint, candidate.toEigen());
           queue.push(candidate);
         } else if (candidate < queue.top() && close_nodes == 0) {
-          candidate.tree_node = TreeNode(origin, trajectory_endpoint, candidate.toEigen(), candidate.cost);
+          candidate.tree_node = TreeNode(origin, trajectory_endpoint, candidate.toEigen());
           queue.push(candidate);
           queue.pop();
         }
@@ -132,7 +132,7 @@ void StarPlanner::buildLookAheadTree() {
       if (!(tree_[i].closed_)) {
         // If we reach the acceptance radius, add goal as last node and exit
         if (i > 1 && (tree_[i].getPosition() - goal_).norm() < acceptance_radius_) {
-          tree_.push_back(TreeNode(i, simulation_state(0.f, goal_), goal_ - tree_[i].getPosition(), tree_[i].cost_));
+          tree_.push_back(TreeNode(i, simulation_state(0.f, goal_), goal_ - tree_[i].getPosition()));
           closed_set_.push_back(i);
           closed_set_.push_back(tree_.size() - 1);
           break;

diff --git a/local_planner/src/nodes/tree_node.cpp b/local_planner/src/nodes/tree_node.cpp
@@ -2,14 +2,13 @@
 
 namespace avoidance {
 
-TreeNode::TreeNode(int from, const simulation_state& start_state, const Eigen::Vector3f& sp, const float cost)
+TreeNode::TreeNode(int from, const simulation_state& start_state, const Eigen::Vector3f& sp)
     : total_cost_{0.0f},
       heuristic_{0.0f},
       origin_{from},
       closed_{false},
       state(start_state),
-      setpoint(sp),
-      cost_{cost} {}
+      setpoint(sp) {}
 
 void TreeNode::setCosts(float h, float c) {
   heuristic_ = h;