Stop A* at goal and fix setpoint visualization

avoidance/include/avoidance/common.h

@@ -85,7 +85,7 @@ 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

avoidance/src/avoidance_node.cpp

@@ -113,7 +113,7 @@ void AvoidanceNode::px4ParamsCallback(const mavros_msgs::Param& msg) {
   parse_param_f("MPC_LAND_SPEED", px4_.param_mpc_land_speed) ||
   parse_param_f("MPC_TKO_SPEED", px4_.param_mpc_tko_speed) ||
   parse_param_f("MPC_XY_CRUISE", px4_.param_mpc_xy_cruise) ||
-  parse_param_f("MPC_Z_VEL_MAX_DN", px4_.param_mpc_vel_max_dn) ||
+  parse_param_f("MPC_Z_VEL_MAX_DN", px4_.param_mpc_z_vel_max_dn) ||
   parse_param_f("MPC_Z_VEL_MAX_UP", px4_.param_mpc_z_vel_max_up) ||
   parse_param_f("MPC_COL_PREV_D", px4_.param_mpc_col_prev_d) ||
   parse_param_f("NAV_ACC_RAD", px4_.param_nav_acc_rad);

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;
   float max_sensor_range_ = 15.f;
   float min_sensor_range_ = 0.2f;
 
@@ -60,7 +61,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

@@ -134,23 +134,23 @@ void LocalPlanner::determineStrategy() {
   }
 
   if (!polar_histogram_.isEmpty()) {
-    getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, closest_pt_,
-                  max_sensor_range_, min_sensor_range_, cost_matrix_, cost_image_data_);
-
-    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.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_->setPointcloud(final_cloud_);
-    star_planner_->setClosestPointOnLine(closest_pt_);
-
-    // build search tree
-    star_planner_->buildLookAheadTree();
-    last_path_time_ = ros::Time::now();
+      getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, closest_pt_,
+                    max_sensor_range_, min_sensor_range_, cost_matrix_, cost_image_data_);
+
+      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_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, px4_.param_nav_acc_rad);
+      star_planner_->setPointcloud(final_cloud_);
+      star_planner_->setClosestPointOnLine(closest_pt_);
+
+      // build search tree
+      star_planner_->buildLookAheadTree();
+      last_path_time_ = ros::Time::now();
   }
 }
 
@@ -201,12 +201,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_visualization.cpp

@@ -227,10 +227,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

@@ -22,9 +22,10 @@ void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerN
   min_sensor_range_ = static_cast<float>(config.min_sensor_range_);
 }
 
-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) {
@@ -93,12 +94,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++) {
@@ -127,6 +122,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);