From f76efa3516a71a661948a9693c628fb728644baf Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Tue, 30 Jul 2019 16:28:11 +0200 Subject: [PATCH] Stop A* at goal and fix setpoint visualization --- .../include/local_planner/local_planner.h | 3 ++- .../include/local_planner/star_planner.h | 5 +++-- .../local_planner/trajectory_simulator.h | 13 +++++++------ local_planner/src/nodes/local_planner.cpp | 7 ++++--- .../src/nodes/local_planner_node.cpp | 19 ++++++++++++++++--- .../src/nodes/local_planner_visualization.cpp | 17 +++++++++++++---- local_planner/src/nodes/star_planner.cpp | 17 ++++++++++------- local_planner/test/test_star_planner.cpp | 4 ++-- 8 files changed, 57 insertions(+), 28 deletions(-) diff --git a/local_planner/include/local_planner/local_planner.h b/local_planner/include/local_planner/local_planner.h index 33b79651b..a6c01ac43 100644 --- a/local_planner/include/local_planner/local_planner.h +++ b/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 diff --git a/local_planner/include/local_planner/star_planner.h b/local_planner/include/local_planner/star_planner.h index af292a8c9..5368b28e0 100644 --- a/local_planner/include/local_planner/star_planner.h +++ b/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 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 diff --git a/local_planner/include/local_planner/trajectory_simulator.h b/local_planner/include/local_planner/trajectory_simulator.h index 49856225b..a6a3e64fb 100644 --- a/local_planner/include/local_planner/trajectory_simulator.h +++ b/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 { diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index 869a8c89c..1dd856e7c 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/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& tree, std::vector& closed_set, diff --git a/local_planner/src/nodes/local_planner_node.cpp b/local_planner/src/nodes/local_planner_node.cpp index 0899d3bb6..6d7ada49b 100644 --- a/local_planner/src/nodes/local_planner_node.cpp +++ b/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)); diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp index 3f9cf6ccf..f11bc6a3e 100644 --- a/local_planner/src/nodes/local_planner_visualization.cpp +++ b/local_planner/src/nodes/local_planner_visualization.cpp @@ -175,10 +175,19 @@ void LocalPlannerVisualization::publishTree(const std::vector& 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); diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 7c50e3b47..5c4c1e00a 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -20,9 +20,10 @@ void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerN tree_heuristic_weight_ = static_cast(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 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_; diff --git a/local_planner/test/test_star_planner.cpp b/local_planner/test/test_star_planner.cpp index 17a2e6825..0c57610fd 100644 --- a/local_planner/test/test_star_planner.cpp +++ b/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);