Stop A* at goal and fix setpoint visualization

local_planner/include/local_planner/local_planner.h

@@ -51,12 +51,13 @@ struct ModelParameters {
   float param_acc_up_max = NAN;   // Maximum vertical acceleration in velocity controlled modes upward
   float param_mpc_z_vel_max_up = NAN;   // Maximum vertical ascent velocity
   float param_mpc_acc_down_max = NAN; // Maximum vertical acceleration in velocity controlled modes down
-  float param_mpc_vel_max_dn = NAN; // Maximum vertical descent velocity
+  float param_mpc_z_vel_max_dn = NAN; // Maximum vertical descent velocity
   float param_mpc_acc_hor = NAN;  // Maximum horizontal acceleration for auto mode and
                       // maximum deceleration for manual mode
   float param_mpc_xy_cruise = NAN;   // Desired horizontal velocity in mission
   float param_mpc_tko_speed = NAN; // Takeoff climb rate
   float param_mpc_land_speed = NAN;   // Landing descend rate
+  float param_nav_acc_rad = NAN; // Navigator acceptance radius to check off mission items
 
   // TODO: add estimator limitations for max speed and height
 

local_planner/include/local_planner/star_planner.h

@@ -26,7 +26,8 @@ class StarPlanner {
   float tree_node_duration_ = 0.5f;
   float max_path_length_ = 15.f;
   float smoothing_margin_degrees_ = 40.f;
-  float tree_heuristic_weight_ = 35.0f;
+  float tree_heuristic_weight_ = 35.f;
+  float acceptance_radius_ = 2.f;
 
   pcl::PointCloud<pcl::PointXYZI> cloud_;
 
@@ -57,7 +58,7 @@ class StarPlanner {
   * @param[in] cost_params, parameters for the histogram cost function
   * @param[in] simulation limits defining maximum acceleration, velocity, and jerk
   **/
-  void setParams(const costParameters& cost_params, const simulation_limits& limits);
+  void setParams(const costParameters& cost_params, const simulation_limits& limits, float acc_rad);
 
   /**
   * @brief     setter method for star_planner pointcloud

local_planner/include/local_planner/trajectory_simulator.h

@@ -6,13 +6,14 @@
 namespace avoidance {
 
 struct simulation_state {
-  simulation_state(float t, const Eigen::Vector3f& p, const Eigen::Vector3f& v, const Eigen::Vector3f& a)
+  simulation_state(float t = NAN, const Eigen::Vector3f& p = NAN * Eigen::Vector3f::Zero(),
+                   const Eigen::Vector3f& v = NAN * Eigen::Vector3f::Zero(),
+                   const Eigen::Vector3f& a = NAN * Eigen::Vector3f::Zero())
       : time(t), position(p), velocity(v), acceleration(a){};
-  simulation_state(){};
-  float time = NAN;
-  Eigen::Vector3f position = NAN * Eigen::Vector3f::Ones();
-  Eigen::Vector3f velocity = NAN * Eigen::Vector3f::Ones();
-  Eigen::Vector3f acceleration = NAN * Eigen::Vector3f::Ones();
+  float time;
+  Eigen::Vector3f position;
+  Eigen::Vector3f velocity;
+  Eigen::Vector3f acceleration;
 };
 
 struct simulation_limits {

local_planner/src/nodes/local_planner.cpp

@@ -154,11 +154,11 @@ void LocalPlanner::determineStrategy() {
       simulation_limits lims;
       setDefaultPx4Parameters();  // TODO: remove but make sure they're set!
       lims.max_z_velocity = px4_.param_mpc_z_vel_max_up;
-      lims.min_z_velocity = -1.0f * px4_.param_mpc_vel_max_dn;
+      lims.min_z_velocity = -1.0f * px4_.param_mpc_z_vel_max_dn;
       lims.max_xy_velocity_norm = px4_.param_mpc_xy_cruise;
       lims.max_acceleration_norm = px4_.param_mpc_acc_hor;
       lims.max_jerk_norm = px4_.param_mpc_jerk_max;
-      star_planner_->setParams(cost_params_, lims);
+      star_planner_->setParams(cost_params_, lims, px4_.param_nav_acc_rad);
       star_planner_->setPointcloud(final_cloud_);
 
       // build search tree
@@ -211,12 +211,13 @@ void LocalPlanner::setDefaultPx4Parameters() {
   px4_.param_acc_up_max = 10.f;
   px4_.param_mpc_z_vel_max_up = 3.f;
   px4_.param_mpc_acc_down_max = 10.f;
-  px4_.param_mpc_vel_max_dn = 1.f;
+  px4_.param_mpc_z_vel_max_dn = 1.f;
   px4_.param_mpc_acc_hor = 5.f;
   px4_.param_mpc_xy_cruise = 3.f;
   px4_.param_mpc_tko_speed = 1.f;
   px4_.param_mpc_land_speed = 0.7f;
   px4_.param_mpc_col_prev_d = 4.f;
+  px4_.param_nav_acc_rad = 2.f;
 }
 
 void LocalPlanner::getTree(std::vector<TreeNode>& tree, std::vector<int>& closed_set,

local_planner/src/nodes/local_planner_node.cpp

@@ -386,9 +386,10 @@ void LocalPlannerNode::px4ParamsCallback(const mavros_msgs::Param& msg) {
   parse_param_f("MPC_LAND_SPEED", local_planner_->px4_.param_mpc_land_speed) ||
   parse_param_f("MPC_TKO_SPEED", local_planner_->px4_.param_mpc_tko_speed) ||
   parse_param_f("MPC_XY_CRUISE", local_planner_->px4_.param_mpc_xy_cruise) ||
-  parse_param_f("MPC_Z_VEL_MAX_DN", local_planner_->px4_.param_mpc_vel_max_dn) ||
+  parse_param_f("MPC_Z_VEL_MAX_DN", local_planner_->px4_.param_mpc_z_vel_max_dn) ||
   parse_param_f("MPC_Z_VEL_MAX_UP", local_planner_->px4_.param_mpc_z_vel_max_up) ||
-  parse_param_f("MPC_COL_PREV_D", local_planner_->px4_.param_mpc_col_prev_d);
+  parse_param_f("MPC_COL_PREV_D", local_planner_->px4_.param_mpc_col_prev_d) ||
+  parse_param_f("NAV_ACC_RAD", local_planner_->px4_.param_nav_acc_rad);
   // clang-format on
 }
 
@@ -404,9 +405,21 @@ void LocalPlannerNode::checkPx4Parameters() {
   while (!should_exit_) {
     request_param("MPC_XY_CRUISE", local_planner_->px4_.param_mpc_xy_cruise);
     request_param("MPC_COL_PREV_D", local_planner_->px4_.param_mpc_col_prev_d);
+    request_param("NAV_ACC_RAD", local_planner_->px4_.param_nav_acc_rad);
+    request_param("MPC_Z_VEL_MAX_UP", local_planner_->px4_.param_mpc_z_vel_max_up);
+    request_param("MPC_Z_VEL_MAX_DN", local_planner_->px4_.param_mpc_z_vel_max_dn);
+    request_param("MPC_XY_CRUISE", local_planner_->px4_.param_mpc_xy_cruise);
+    request_param("MPC_ACC_HOR", local_planner_->px4_.param_mpc_acc_hor);
+    request_param("MPC_JERK_MAX", local_planner_->px4_.param_mpc_jerk_max);
 
     if (!std::isfinite(local_planner_->px4_.param_mpc_xy_cruise) ||
-        !std::isfinite(local_planner_->px4_.param_mpc_col_prev_d)) {
+        !std::isfinite(local_planner_->px4_.param_mpc_col_prev_d) ||
+        !std::isfinite(local_planner_->px4_.param_nav_acc_rad) ||
+        !std::isfinite(local_planner_->px4_.param_mpc_z_vel_max_dn) ||
+        !std::isfinite(local_planner_->px4_.param_mpc_z_vel_max_up) ||
+        !std::isfinite(local_planner_->px4_.param_mpc_xy_cruise) ||
+        !std::isfinite(local_planner_->px4_.param_mpc_acc_hor) ||
+        !std::isfinite(local_planner_->px4_.param_mpc_jerk_max)) {
       std::this_thread::sleep_for(std::chrono::seconds(5));
     } else {
       std::this_thread::sleep_for(std::chrono::seconds(30));

local_planner/src/nodes/local_planner_visualization.cpp

@@ -175,10 +175,19 @@ void LocalPlannerVisualization::publishTree(const std::vector<TreeNode>& tree, c
     tree_marker.points.push_back(p2);
   }
 
-  path_marker.points.reserve(path_node_setpoints.size() * 2);
-  for (size_t i = 1; i < path_node_setpoints.size(); i++) {
-    path_marker.points.push_back(toPoint(path_node_setpoints[i - 1]));
-    path_marker.points.push_back(toPoint(path_node_setpoints[i]));
+  // Visualizing the setpoints is a hack: they are actually in the body frame. This is an approximate visualization
+  // that just accumulates them to illustrate them as a path
+  if (path_node_setpoints.size() > 0) {
+    path_marker.points.reserve(path_node_setpoints.size() * 2);
+    Eigen::Vector3f p1;
+    Eigen::Vector3f p2 = tree[closed_set.front()].getPosition();
+    for (int i = path_node_setpoints.size() - 1; i >= 1; --i) {
+      float scale = (tree[closed_set[i]].getPosition() - tree[closed_set[i - 1]].getPosition()).norm();
+      p1 = p2;
+      p2 = p1 + scale * path_node_setpoints[i];
+      path_marker.points.push_back(toPoint(p1));
+      path_marker.points.push_back(toPoint(p2));
+    }
   }
 
   complete_tree_pub_.publish(tree_marker);

local_planner/src/nodes/star_planner.cpp

@@ -20,9 +20,10 @@ void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerN
   tree_heuristic_weight_ = static_cast<float>(config.tree_heuristic_weight_);
 }
 
-void StarPlanner::setParams(const costParameters& cost_params, const simulation_limits& limits) {
+void StarPlanner::setParams(const costParameters& cost_params, const simulation_limits& limits, float acc_rad) {
   cost_params_ = cost_params;
   lims_ = limits;
+  acceptance_radius_ = acc_rad;
 }
 
 void StarPlanner::setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel) {
@@ -89,12 +90,6 @@ void StarPlanner::buildLookAheadTree() {
         TrajectorySimulator sim(lims_, state, 0.05f);  // todo: parameterize simulation step size [s]
         std::vector<simulation_state> trajectory = sim.generate_trajectory(candidate.toEigen(), tree_node_duration_);
 
-        // Ignore node if it brings us farther away from the goal
-        // todo: this breaks being able to get out of concave obstacles! But it helps to not "overplan"
-        if ((trajectory.back().position - goal_).norm() > (trajectory.front().position - goal_).norm()) {
-          continue;
-        }
-
         // check if another close node has been added
         int close_nodes = 0;
         for (size_t i = 0; i < tree_.size(); i++) {
@@ -123,6 +118,14 @@ void StarPlanner::buildLookAheadTree() {
     is_expanded_node = false;
     for (size_t i = 0; i < tree_.size(); i++) {
       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()));
+          closed_set_.push_back(i);
+          closed_set_.push_back(tree_.size() - 1);
+          break;
+        }
+
         float node_distance = (tree_[i].getPosition() - position_).norm();
         if (tree_[i].total_cost_ < minimal_cost && node_distance < max_path_length_) {
           minimal_cost = tree_[i].total_cost_;

local_planner/test/test_star_planner.cpp

@@ -28,7 +28,7 @@ class StarPlannerTests : public ::testing::Test {
     avoidance::LocalPlannerNodeConfig config = avoidance::LocalPlannerNodeConfig::__getDefault__();
     config.children_per_node_ = 2;
     config.n_expanded_nodes_ = 10;
-    config.tree_node_duration_ = 1.0;
+    config.tree_node_duration_ = 0.5f;
     star_planner.dynamicReconfigureSetStarParams(config, 1);
 
     position.x() = 1.2f;
@@ -57,7 +57,7 @@ class StarPlannerTests : public ::testing::Test {
     lims.max_acceleration_norm = 5.f;
     lims.max_jerk_norm = 20.f;
 
-    star_planner.setParams(cost_params, lims);
+    star_planner.setParams(cost_params, lims, 2.0f);
     star_planner.setPointcloud(cloud);
     star_planner.setPose(position, velocity);
     star_planner.setGoal(goal);