From cc34db2ed5db9821f7a781c52a694b083f3bbaf4 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Mon, 22 Jul 2019 18:34:11 +0200 Subject: [PATCH 01/15] Plan dynamically feasible trajectories --- local_planner/cfg/local_planner_node.cfg | 2 +- .../local_planner/candidate_direction.h | 8 ++++++ .../include/local_planner/local_planner.h | 1 + .../include/local_planner/star_planner.h | 5 +++- .../include/local_planner/tree_node.h | 7 ++--- local_planner/src/nodes/local_planner.cpp | 15 +++++++--- local_planner/src/nodes/star_planner.cpp | 28 ++++++++++++++----- local_planner/src/nodes/tree_node.cpp | 17 ++++++----- 8 files changed, 59 insertions(+), 24 deletions(-) diff --git a/local_planner/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg index 033ba6834..5f93a2802 100755 --- a/local_planner/cfg/local_planner_node.cfg +++ b/local_planner/cfg/local_planner_node.cfg @@ -27,6 +27,6 @@ gen.add("smoothing_margin_degrees_", double_t, 0, "smoothing radius for obstacle # star_planner gen.add("children_per_node_", int_t, 0, "Branching factor of the search tree", 8, 0, 100) gen.add("n_expanded_nodes_", int_t, 0, "Number of nodes expanded in complete tree", 40, 0, 200) -gen.add("tree_node_distance_", double_t, 0, "Distance between nodes", 2, 0, 20) +gen.add("tree_node_distance_", double_t, 0, "Distance in seconds between nodes", 0.5, 0.01, 2) exit(gen.generate(PACKAGE, "avoidance", "LocalPlannerNode")) diff --git a/local_planner/include/local_planner/candidate_direction.h b/local_planner/include/local_planner/candidate_direction.h index 06e58b592..55f697fe5 100644 --- a/local_planner/include/local_planner/candidate_direction.h +++ b/local_planner/include/local_planner/candidate_direction.h @@ -1,4 +1,7 @@ #pragma once +#include +#include +#include #include "avoidance/common.h" namespace avoidance { @@ -15,5 +18,10 @@ struct candidateDirection { bool operator>(const candidateDirection& y) const { return cost > y.cost; } PolarPoint toPolar(float r) const { return PolarPoint(elevation_angle, azimuth_angle, r); } + Eigen::Vector3f toEigen() const { + return Eigen::Vector3f(std::cos(elevation_angle * DEG_TO_RAD) * std::sin(azimuth_angle * DEG_TO_RAD), + std::cos(elevation_angle * DEG_TO_RAD) * std::cos(azimuth_angle * DEG_TO_RAD), + std::sin(elevation_angle * DEG_TO_RAD)); + } }; } diff --git a/local_planner/include/local_planner/local_planner.h b/local_planner/include/local_planner/local_planner.h index 7b0c7b764..fe5078ea4 100644 --- a/local_planner/include/local_planner/local_planner.h +++ b/local_planner/include/local_planner/local_planner.h @@ -7,6 +7,7 @@ #include "candidate_direction.h" #include "cost_parameters.h" #include "planner_functions.h" +#include "trajectory_simulator.h" #include #include diff --git a/local_planner/include/local_planner/star_planner.h b/local_planner/include/local_planner/star_planner.h index d5fb4ba68..3a63cad48 100644 --- a/local_planner/include/local_planner/star_planner.h +++ b/local_planner/include/local_planner/star_planner.h @@ -3,6 +3,7 @@ #include "avoidance/histogram.h" #include "cost_parameters.h" +#include "trajectory_simulator.h" #include @@ -33,6 +34,7 @@ class StarPlanner { Eigen::Vector3f position_ = Eigen::Vector3f(NAN, NAN, NAN); Eigen::Vector3f velocity_ = Eigen::Vector3f(NAN, NAN, NAN); costParameters cost_params_; + simulation_limits lims_; protected: /** @@ -53,8 +55,9 @@ class StarPlanner { /** * @brief setter method for costMatrix paramters * @param[in] cost_params, parameters for the histogram cost function + * @param[in] simulation limits defining maximum acceleration, velocity, and jerk **/ - void setParams(costParameters cost_params); + void setParams(const costParameters& cost_params, const simulation_limits& limits); /** * @brief setter method for star_planner pointcloud diff --git a/local_planner/include/local_planner/tree_node.h b/local_planner/include/local_planner/tree_node.h index 4f71a42d0..cb82c03f5 100644 --- a/local_planner/include/local_planner/tree_node.h +++ b/local_planner/include/local_planner/tree_node.h @@ -3,21 +3,20 @@ #include #include +#include "trajectory_simulator.h" namespace avoidance { class TreeNode { - Eigen::Vector3f position_; - Eigen::Vector3f velocity_; - public: float total_cost_; float heuristic_; int origin_; bool closed_; + simulation_state state; TreeNode(); - TreeNode(int from, const Eigen::Vector3f& pos, const Eigen::Vector3f& vel); + TreeNode(int from, const simulation_state& start_state); ~TreeNode() = default; /** diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index 0d56369da..6f5c53939 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -121,10 +121,17 @@ void LocalPlanner::determineStrategy() { create2DObstacleRepresentation(px4_.param_mpc_col_prev_d > 0.f); if (!polar_histogram_.isEmpty()) { - getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, cost_matrix_, - cost_image_data_); - - star_planner_->setParams(cost_params_); + getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, + 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_); // build search tree diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index ec1ce3811..313222aca 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -20,7 +20,10 @@ void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerN tree_heuristic_weight_ = static_cast(config.tree_heuristic_weight_); } -void StarPlanner::setParams(costParameters cost_params) { cost_params_ = cost_params; } +void StarPlanner::setParams(const costParameters& cost_params, const simulation_limits& limits) { + cost_params_ = cost_params; + lims_ = limits; +} void StarPlanner::setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel) { position_ = pos; @@ -49,7 +52,12 @@ void StarPlanner::buildLookAheadTree() { closed_set_.clear(); // insert first node - tree_.push_back(TreeNode(0, position_, velocity_)); + simulation_state start_state; + start_state.position = position_; + 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)); tree_.back().setCosts(treeHeuristicFunction(0), treeHeuristicFunction(0)); int origin = 0; @@ -77,14 +85,20 @@ void StarPlanner::buildLookAheadTree() { // insert new nodes int children = 0; for (candidateDirection candidate : candidate_vector) { - PolarPoint candidate_polar = candidate.toPolar(tree_node_distance_); - Eigen::Vector3f node_location = polarHistogramToCartesian(candidate_polar, origin_position); - Eigen::Vector3f node_velocity = node_location - origin_position; // todo: simulate! + simulation_state state = tree_[origin].state; + TrajectorySimulator sim(lims_, state, 0.05f); // todo: parameterize simulation step size [s] + std::vector trajectory = sim.generate_trajectory(candidate.toEigen(), tree_node_distance_); + + // 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++) { - float dist = (tree_[i].getPosition() - node_location).norm(); + float dist = (tree_[i].getPosition() - trajectory.back().position).norm(); if (dist < 0.2f) { close_nodes++; break; @@ -92,7 +106,7 @@ void StarPlanner::buildLookAheadTree() { } if (children < children_per_node_ && close_nodes == 0) { - tree_.push_back(TreeNode(origin, node_location, node_velocity)); + tree_.push_back(TreeNode(origin, trajectory.back())); float h = treeHeuristicFunction(tree_.size() - 1); tree_.back().heuristic_ = h; tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + candidate.cost + h; diff --git a/local_planner/src/nodes/tree_node.cpp b/local_planner/src/nodes/tree_node.cpp index 7e1577804..2a13e590f 100644 --- a/local_planner/src/nodes/tree_node.cpp +++ b/local_planner/src/nodes/tree_node.cpp @@ -3,14 +3,17 @@ namespace avoidance { TreeNode::TreeNode() : total_cost_{0.0f}, heuristic_{0.0f}, origin_{0}, closed_{false} { - position_ = Eigen::Vector3f::Zero(); - velocity_ = Eigen::Vector3f::Zero(); + state.position = Eigen::Vector3f::Zero(); + state.velocity = Eigen::Vector3f::Zero(); + state.acceleration = Eigen::Vector3f::Zero(); } -TreeNode::TreeNode(int from, const Eigen::Vector3f& pos, const Eigen::Vector3f& vel) +TreeNode::TreeNode(int from, const simulation_state& start_state) : total_cost_{0.0f}, heuristic_{0.0f}, origin_{from}, closed_{false} { - position_ = pos; - velocity_ = vel; + state.position = start_state.position; + state.velocity = start_state.velocity; + state.acceleration = start_state.acceleration; + state.time = start_state.time; } void TreeNode::setCosts(float h, float c) { @@ -18,6 +21,6 @@ void TreeNode::setCosts(float h, float c) { total_cost_ = c; } -Eigen::Vector3f TreeNode::getPosition() const { return position_; } -Eigen::Vector3f TreeNode::getVelocity() const { return velocity_; } +Eigen::Vector3f TreeNode::getPosition() const { return state.position; } +Eigen::Vector3f TreeNode::getVelocity() const { return state.velocity; } } From 6aa7e6d90fbda9e3e2f269b08c60f8afde3ddd01 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Mon, 29 Jul 2019 09:14:25 +0200 Subject: [PATCH 02/15] Fix style and tests --- local_planner/src/nodes/star_planner.cpp | 2 +- local_planner/test/test_star_planner.cpp | 10 ++++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 313222aca..5e487f5da 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -86,7 +86,7 @@ void StarPlanner::buildLookAheadTree() { int children = 0; for (candidateDirection candidate : candidate_vector) { simulation_state state = tree_[origin].state; - TrajectorySimulator sim(lims_, state, 0.05f); // todo: parameterize simulation step size [s] + TrajectorySimulator sim(lims_, state, 0.05f); // todo: parameterize simulation step size [s] std::vector trajectory = sim.generate_trajectory(candidate.toEigen(), tree_node_distance_); // Ignore node if it brings us farther away from the goal diff --git a/local_planner/test/test_star_planner.cpp b/local_planner/test/test_star_planner.cpp index dbc001638..f342cfdb8 100644 --- a/local_planner/test/test_star_planner.cpp +++ b/local_planner/test/test_star_planner.cpp @@ -50,8 +50,14 @@ class StarPlannerTests : public ::testing::Test { } } costParameters cost_params; - - star_planner.setParams(cost_params); + simulation_limits lims; + lims.max_z_velocity = 3.f; + lims.min_z_velocity = -1.f; + lims.max_xy_velocity_norm = 3.f; + lims.max_acceleration_norm = 5.f; + lims.max_jerk_norm = 20.f; + + star_planner.setParams(cost_params, lims); star_planner.setPointcloud(cloud); star_planner.setPose(position, velocity); star_planner.setGoal(goal); From 6945d7ceaca7715a34ec37e0be8ec59cf9d5b641 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Tue, 30 Jul 2019 10:50:34 +0200 Subject: [PATCH 03/15] Plan setpoints and positions separately This commit changes the star planner to distinguish between setpoints sent to the position controller and the actual positions we would then end up in. --- local_planner/cfg/local_planner_node.cfg | 2 +- local_planner/cfg/vehicle.yaml | 45 +++++++++---------- .../include/local_planner/avoidance_output.h | 11 ++--- .../include/local_planner/local_planner.h | 7 +-- .../local_planner_visualization.h | 4 +- .../include/local_planner/planner_functions.h | 7 +-- .../include/local_planner/star_planner.h | 14 +++--- .../local_planner/trajectory_simulator.h | 10 +++++ .../include/local_planner/tree_node.h | 13 ++++-- local_planner/src/nodes/local_planner.cpp | 9 ++-- .../src/nodes/local_planner_visualization.cpp | 16 +++---- local_planner/src/nodes/planner_functions.cpp | 31 +++++++------ local_planner/src/nodes/star_planner.cpp | 28 ++++-------- local_planner/src/nodes/tree_node.cpp | 16 ++----- .../src/nodes/waypoint_generator.cpp | 8 ++-- local_planner/test/test_local_planner.cpp | 27 +++++++---- local_planner/test/test_planner_functions.cpp | 13 +++--- local_planner/test/test_star_planner.cpp | 2 +- .../test/test_waypoint_generator.cpp | 2 +- 19 files changed, 136 insertions(+), 129 deletions(-) diff --git a/local_planner/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg index 5f93a2802..c5261a499 100755 --- a/local_planner/cfg/local_planner_node.cfg +++ b/local_planner/cfg/local_planner_node.cfg @@ -27,6 +27,6 @@ gen.add("smoothing_margin_degrees_", double_t, 0, "smoothing radius for obstacle # star_planner gen.add("children_per_node_", int_t, 0, "Branching factor of the search tree", 8, 0, 100) gen.add("n_expanded_nodes_", int_t, 0, "Number of nodes expanded in complete tree", 40, 0, 200) -gen.add("tree_node_distance_", double_t, 0, "Distance in seconds between nodes", 0.5, 0.01, 2) +gen.add("tree_node_duration_", double_t, 0, "Distance in seconds between nodes", 0.5, 0.01, 2) exit(gen.generate(PACKAGE, "avoidance", "LocalPlannerNode")) diff --git a/local_planner/cfg/vehicle.yaml b/local_planner/cfg/vehicle.yaml index eef20600f..840928400 100644 --- a/local_planner/cfg/vehicle.yaml +++ b/local_planner/cfg/vehicle.yaml @@ -1,54 +1,53 @@ -# This is a sample config file for a real vehicle !!python/object/new:dynamic_reconfigure.encoding.Config dictitems: - adapt_cost_params_: true - box_radius_: 12.0 - children_per_node_: 50 - goal_cost_param_: 10.0 + children_per_node_: 8 goal_z_param: 3.0 groups: !!python/object/new:dynamic_reconfigure.encoding.Config dictitems: - adapt_cost_params_: true - box_radius_: 12.0 - children_per_node_: 50 - goal_cost_param_: 10.0 + children_per_node_: 8 goal_z_param: 3.0 groups: !!python/object/new:dynamic_reconfigure.encoding.Config state: [] - heading_cost_param_: 0.5 id: 0 max_point_age_s_: 20.0 - min_num_points_per_cell_: 25 # Tune to your sensor requirements! + min_num_points_per_cell_: 1 min_sensor_range_: 0.2 - n_expanded_nodes_: 10 + n_expanded_nodes_: 40 name: Default - no_progress_slope_: -0.0007 + obstacle_cost_param_: 8.5 parameters: !!python/object/new:dynamic_reconfigure.encoding.Config state: [] parent: 0 - smooth_cost_param_: 1.5 + pitch_cost_param_: 25.0 + sensor_range_: 15.0 smoothing_margin_degrees_: 40.0 smoothing_speed_xy_: 10.0 smoothing_speed_z_: 3.0 state: true timeout_critical_: 0.5 + timeout_startup_: 5.0 timeout_termination_: 15.0 - tree_discount_factor_: 0.8 - tree_node_distance_: 1.0 + tree_heuristic_weight_: 35.0 + tree_node_duration_: 0.5 type: '' + velocity_cost_param_: 6000.0 + yaw_cost_param_: 3.0 state: [] - heading_cost_param_: 0.5 max_point_age_s_: 20.0 - min_num_points_per_cell_: 25 # Tune to your sensor requirements! + min_num_points_per_cell_: 1 min_sensor_range_: 0.2 - n_expanded_nodes_: 10 - no_progress_slope_: -0.0007 - smooth_cost_param_: 1.5 + n_expanded_nodes_: 40 + obstacle_cost_param_: 8.5 + pitch_cost_param_: 25.0 + max_sensor_range_: 15.0 smoothing_margin_degrees_: 40.0 smoothing_speed_xy_: 10.0 smoothing_speed_z_: 3.0 timeout_critical_: 0.5 + timeout_startup_: 5.0 timeout_termination_: 15.0 - tree_discount_factor_: 0.8 - tree_node_distance_: 1.0 + tree_heuristic_weight_: 35.0 + tree_node_duration_: 0.5 + velocity_cost_param_: 6000.0 + yaw_cost_param_: 3.0 state: [] diff --git a/local_planner/include/local_planner/avoidance_output.h b/local_planner/include/local_planner/avoidance_output.h index f9309bcfb..b057863cc 100644 --- a/local_planner/include/local_planner/avoidance_output.h +++ b/local_planner/include/local_planner/avoidance_output.h @@ -11,16 +11,13 @@ enum waypoint_choice { hover, tryPath, direct, reachHeight }; struct avoidanceOutput { waypoint_choice waypoint_type = hover; - bool obstacle_ahead; // true is there is an obstacle ahead of the vehicle - float cruise_velocity; // mission cruise velocity + bool obstacle_ahead; // true is there is an obstacle ahead of the vehicle + float cruise_velocity; // mission cruise velocity + float tree_node_duration; ros::Time last_path_time; // finish built time for the VFH+* tree Eigen::Vector3f take_off_pose; // last vehicle position when not armed - std::vector path_node_positions; // array of tree nodes - // position, each node - // is the minimum cost - // node for each tree - // depth level + std::vector path_node_setpoints; // array of setpoints }; } diff --git a/local_planner/include/local_planner/local_planner.h b/local_planner/include/local_planner/local_planner.h index fe5078ea4..2a1e3b4cb 100644 --- a/local_planner/include/local_planner/local_planner.h +++ b/local_planner/include/local_planner/local_planner.h @@ -45,6 +45,7 @@ class LocalPlanner { float max_point_age_s_ = 10; float yaw_fcu_frame_deg_ = 0.0f; float pitch_fcu_frame_deg_ = 0.0f; + float tree_node_duration_ = 0.5f; std::vector fov_fcu_frame_; @@ -175,11 +176,11 @@ class LocalPlanner { /** * @brief getter method to visualize the tree in rviz * @param[in] tree, the whole tree built during planning (vector of nodes) - * @param[in] closed_set, velocity message coming from the FCU - * @param[in] path_node_positions, velocity message coming from the FCU + * @param[in] closed_set + * @param[in] path_node_setpoints **/ void getTree(std::vector& tree, std::vector& closed_set, - std::vector& path_node_positions) const; + std::vector& path_node_setpoints) const; /** * @brief getter method for obstacle distance information * @param obstacle_distance, obstacle distance message to fill diff --git a/local_planner/include/local_planner/local_planner_visualization.h b/local_planner/include/local_planner/local_planner_visualization.h index dfdc58085..d8012dd43 100644 --- a/local_planner/include/local_planner/local_planner_visualization.h +++ b/local_planner/include/local_planner/local_planner_visualization.h @@ -36,11 +36,11 @@ class LocalPlannerVisualization { * chosen * @params[in] tree, the complete calculated search tree * @params[in] closed_set, the closed set (all expanded nodes) - * @params[in] path_node_positions, the positions of all nodes belonging to + * @params[in] path_node_setpoints, the setpoints of all nodes belonging to * the chosen best path **/ void publishTree(const std::vector& tree, const std::vector& closed_set, - const std::vector& path_node_positions) const; + const std::vector& path_node_setpoints) const; /** * @brief Visualization of the goal position diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h index a3ca8cd8f..c3e7a48a4 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -143,11 +143,12 @@ void printHistogram(const Histogram& histogram); * @brief Returns a setpoint that lies on the given path * @param[in] vector of nodes defining the path, with the last node of the path at index 0 * @param[in] ros time of path generation -* @param[in] velocity, scalar value for the norm of the current vehicle velocity +* @param[in] tree_node_duration, scalar value for length in time each path segment is to be used * @param[out] setpoint on the tree toward which the drone should fly * @returns boolean indicating whether the tree was valid **/ -bool getSetpointFromPath(const std::vector& path, const ros::Time& path_generation_time, - float velocity, Eigen::Vector3f& setpoint); +bool interpolateBetweenSetpoints(const std::vector& setpoint_array, + const ros::Time& path_generation_time, float tree_node_duration, + Eigen::Vector3f& setpoint); } #endif // LOCAL_PLANNER_FUNCTIONS_H diff --git a/local_planner/include/local_planner/star_planner.h b/local_planner/include/local_planner/star_planner.h index 3a63cad48..af292a8c9 100644 --- a/local_planner/include/local_planner/star_planner.h +++ b/local_planner/include/local_planner/star_planner.h @@ -21,12 +21,12 @@ namespace avoidance { class TreeNode; class StarPlanner { - int children_per_node_ = 1; - int n_expanded_nodes_ = 5; - float tree_node_distance_ = 1.0f; - float max_path_length_ = 4.f; - float smoothing_margin_degrees_ = 30.f; - float tree_heuristic_weight_ = 10.0f; + int children_per_node_ = 8; + int n_expanded_nodes_ = 40; + float tree_node_duration_ = 0.5f; + float max_path_length_ = 15.f; + float smoothing_margin_degrees_ = 40.f; + float tree_heuristic_weight_ = 35.0f; pcl::PointCloud cloud_; @@ -45,7 +45,7 @@ class StarPlanner { float treeHeuristicFunction(int node_number) const; public: - std::vector path_node_positions_; + std::vector path_node_setpoints_; std::vector closed_set_; std::vector tree_; diff --git a/local_planner/include/local_planner/trajectory_simulator.h b/local_planner/include/local_planner/trajectory_simulator.h index 138c8118b..49856225b 100644 --- a/local_planner/include/local_planner/trajectory_simulator.h +++ b/local_planner/include/local_planner/trajectory_simulator.h @@ -6,6 +6,9 @@ namespace avoidance { struct simulation_state { + simulation_state(float t, const Eigen::Vector3f& p, const Eigen::Vector3f& v, const Eigen::Vector3f& a) + : 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(); @@ -13,6 +16,13 @@ struct simulation_state { }; struct simulation_limits { + simulation_limits(float max_z, float min_z, float max_xy_n, float max_acc_n, float max_jerk_n) + : max_z_velocity(max_z), + min_z_velocity(min_z), + max_xy_velocity_norm(max_xy_n), + max_acceleration_norm(max_acc_n), + max_jerk_norm(max_jerk_n){}; + simulation_limits(){}; float max_z_velocity = NAN; float min_z_velocity = NAN; float max_xy_velocity_norm = NAN; diff --git a/local_planner/include/local_planner/tree_node.h b/local_planner/include/local_planner/tree_node.h index cb82c03f5..1a40249c8 100644 --- a/local_planner/include/local_planner/tree_node.h +++ b/local_planner/include/local_planner/tree_node.h @@ -13,10 +13,11 @@ class TreeNode { float heuristic_; int origin_; bool closed_; - simulation_state state; + 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(); - TreeNode(int from, const simulation_state& start_state); + TreeNode() = delete; + TreeNode(int from, const simulation_state& start_state, const Eigen::Vector3f& sp); ~TreeNode() = default; /** @@ -27,11 +28,15 @@ class TreeNode { void setCosts(float h, float c); /** - * @brief getter method for tree node position + * @defgroup getterFunctions + * @brief getter methods for tree node position, velocity and setpoint * @returns node position in 3D cartesian coordinates + * @{ **/ Eigen::Vector3f getPosition() const; Eigen::Vector3f getVelocity() const; + Eigen::Vector3f getSetpoint() const; + /** @} */ // end of doxygen group getterFunctions }; } diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index 6f5c53939..b8bbdbb9a 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -37,6 +37,7 @@ void LocalPlanner::dynamicReconfigureSetParams(avoidance::LocalPlannerNodeConfig children_per_node_ = config.children_per_node_; n_expanded_nodes_ = config.n_expanded_nodes_; smoothing_margin_degrees_ = static_cast(config.smoothing_margin_degrees_); + tree_node_duration_ = static_cast(config.tree_node_duration_); if (getGoal().z() != config.goal_z_param) { auto goal = getGoal(); @@ -196,10 +197,10 @@ void LocalPlanner::setDefaultPx4Parameters() { } void LocalPlanner::getTree(std::vector& tree, std::vector& closed_set, - std::vector& path_node_positions) const { + std::vector& path_node_setpoints) const { tree = star_planner_->tree_; closed_set = star_planner_->closed_set_; - path_node_positions = star_planner_->path_node_positions_; + path_node_setpoints = star_planner_->path_node_setpoints_; } void LocalPlanner::getObstacleDistanceData(sensor_msgs::LaserScan& obstacle_distance) { @@ -227,8 +228,8 @@ avoidanceOutput LocalPlanner::getAvoidanceOutput() const { out.cruise_velocity = max_speed; out.last_path_time = last_path_time_; - - out.path_node_positions = star_planner_->path_node_positions_; + out.tree_node_duration = tree_node_duration_; + out.path_node_setpoints = star_planner_->path_node_setpoints_; return out; } } diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp index 37ff83d38..d9f831832 100644 --- a/local_planner/src/nodes/local_planner_visualization.cpp +++ b/local_planner/src/nodes/local_planner_visualization.cpp @@ -44,9 +44,9 @@ void LocalPlannerVisualization::visualizePlannerData(const LocalPlanner& planner // visualize tree calculation std::vector tree; std::vector closed_set; - std::vector path_node_positions; - planner.getTree(tree, closed_set, path_node_positions); - publishTree(tree, closed_set, path_node_positions); + std::vector path_node_setpoints; + planner.getTree(tree, closed_set, path_node_setpoints); + publishTree(tree, closed_set, path_node_setpoints); // visualize goal publishGoal(toPoint(planner.getGoal())); @@ -190,7 +190,7 @@ void LocalPlannerVisualization::publishOfftrackPoints(Eigen::Vector3f& closest_p } void LocalPlannerVisualization::publishTree(const std::vector& tree, const std::vector& closed_set, - const std::vector& path_node_positions) const { + const std::vector& path_node_setpoints) const { visualization_msgs::Marker tree_marker; tree_marker.header.frame_id = "local_origin"; tree_marker.header.stamp = ros::Time::now(); @@ -227,10 +227,10 @@ void LocalPlannerVisualization::publishTree(const std::vector& tree, c tree_marker.points.push_back(p2); } - path_marker.points.reserve(path_node_positions.size() * 2); - for (size_t i = 1; i < path_node_positions.size(); i++) { - path_marker.points.push_back(toPoint(path_node_positions[i - 1])); - path_marker.points.push_back(toPoint(path_node_positions[i])); + 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])); } complete_tree_pub_.publish(tree_marker); diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index 93212f950..26d175320 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -323,33 +323,36 @@ std::pair costFunction(const PolarPoint& candidate_polar, float ob return std::pair(distance_cost, velocity_cost + yaw_cost + pitch_cost); } -bool getSetpointFromPath(const std::vector& path, const ros::Time& path_generation_time, - float velocity, Eigen::Vector3f& setpoint) { - int i = path.size(); +bool interpolateBetweenSetpoints(const std::vector& setpoint_array, + const ros::Time& path_generation_time, float tree_node_duration, + Eigen::Vector3f& setpoint) { + int i = setpoint_array.size(); // path contains nothing meaningful if (i < 2) { + ROS_WARN("Path contains fewer than two nodes, this is not a path!"); return false; } // path only has one segment: return end of that segment as setpoint if (i == 2) { - setpoint = path[0]; + ROS_INFO("Path contains only two nodes, using second node as setpoint!"); + setpoint = setpoint_array[0]; return true; } // step through the path until the point where we should be if we had traveled perfectly with velocity along it - Eigen::Vector3f path_segment = path[i - 3] - path[i - 2]; - float distance_left = (ros::Time::now() - path_generation_time).toSec() * velocity; - setpoint = path[i - 2] + (distance_left / path_segment.norm()) * path_segment; - - for (i = path.size() - 3; i > 0 && distance_left > path_segment.norm(); --i) { - distance_left -= path_segment.norm(); - path_segment = path[i - 1] - path[i]; - setpoint = path[i] + (distance_left / path_segment.norm()) * path_segment; + const int seg = i - 2 - std::floor((ros::Time::now() - path_generation_time).toSec() / tree_node_duration); + + // path expired + if (seg < 1) { + ROS_WARN("Path has expired!"); + return false; } - // If we excited because we're past the last node of the path, the path is no longer valid! - return distance_left < path_segment.norm(); + Eigen::Vector3f path_segment = setpoint_array[seg - 1] - setpoint_array[seg]; + float distance_left = fmod((ros::Time::now() - path_generation_time).toSec(), tree_node_duration); + setpoint = setpoint_array[seg] + (distance_left / tree_node_duration) * path_segment; + return true; } void printHistogram(Histogram& histogram) { diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 5e487f5da..f93693275 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -14,7 +14,7 @@ StarPlanner::StarPlanner() {} void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerNodeConfig& config, uint32_t level) { children_per_node_ = config.children_per_node_; n_expanded_nodes_ = config.n_expanded_nodes_; - tree_node_distance_ = static_cast(config.tree_node_distance_); + tree_node_duration_ = static_cast(config.tree_node_duration_); max_path_length_ = static_cast(config.max_sensor_range_); smoothing_margin_degrees_ = static_cast(config.smoothing_margin_degrees_); tree_heuristic_weight_ = static_cast(config.tree_heuristic_weight_); @@ -57,7 +57,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)); + tree_.push_back(TreeNode(0, start_state, Eigen::Vector3f::Zero())); tree_.back().setCosts(treeHeuristicFunction(0), treeHeuristicFunction(0)); int origin = 0; @@ -87,7 +87,7 @@ void StarPlanner::buildLookAheadTree() { for (candidateDirection candidate : candidate_vector) { simulation_state state = tree_[origin].state; TrajectorySimulator sim(lims_, state, 0.05f); // todo: parameterize simulation step size [s] - std::vector trajectory = sim.generate_trajectory(candidate.toEigen(), tree_node_distance_); + 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" @@ -106,7 +106,7 @@ void StarPlanner::buildLookAheadTree() { } if (children < children_per_node_ && close_nodes == 0) { - tree_.push_back(TreeNode(origin, trajectory.back())); + tree_.push_back(TreeNode(origin, trajectory.back(), candidate.toEigen())); float h = treeHeuristicFunction(tree_.size() - 1); tree_.back().heuristic_ = h; tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + candidate.cost + h; @@ -135,24 +135,14 @@ void StarPlanner::buildLookAheadTree() { cost_image_data.clear(); candidate_vector.clear(); } - // smoothing between trees + + // Get setpoints into member vector int tree_end = origin; - path_node_positions_.clear(); + path_node_setpoints_.clear(); while (tree_end > 0) { - path_node_positions_.push_back(tree_[tree_end].getPosition()); + path_node_setpoints_.push_back(tree_[tree_end].getSetpoint()); tree_end = tree_[tree_end].origin_; } - path_node_positions_.push_back(tree_[0].getPosition()); - - ROS_INFO("\033[0;35m[SP]Tree (%lu nodes, %lu path nodes, %lu expanded) calculated in %2.2fms.\033[0m", tree_.size(), - path_node_positions_.size(), closed_set_.size(), - static_cast((std::clock() - start_time) / static_cast(CLOCKS_PER_SEC / 1000))); - -#ifndef DISABLE_SIMULATION // For large trees, this could be very slow! - for (int j = 0; j < path_node_positions_.size(); j++) { - ROS_DEBUG("\033[0;35m[SP] node %i : [ %f, %f, %f]\033[0m", j, path_node_positions_[j].x(), - path_node_positions_[j].y(), path_node_positions_[j].z()); - } -#endif + path_node_setpoints_.push_back(tree_[0].getSetpoint()); } } diff --git a/local_planner/src/nodes/tree_node.cpp b/local_planner/src/nodes/tree_node.cpp index 2a13e590f..329579854 100644 --- a/local_planner/src/nodes/tree_node.cpp +++ b/local_planner/src/nodes/tree_node.cpp @@ -2,19 +2,8 @@ namespace avoidance { -TreeNode::TreeNode() : total_cost_{0.0f}, heuristic_{0.0f}, origin_{0}, closed_{false} { - state.position = Eigen::Vector3f::Zero(); - state.velocity = Eigen::Vector3f::Zero(); - state.acceleration = Eigen::Vector3f::Zero(); -} - -TreeNode::TreeNode(int from, const simulation_state& start_state) - : total_cost_{0.0f}, heuristic_{0.0f}, origin_{from}, closed_{false} { - state.position = start_state.position; - state.velocity = start_state.velocity; - state.acceleration = start_state.acceleration; - state.time = start_state.time; -} +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) {} void TreeNode::setCosts(float h, float c) { heuristic_ = h; @@ -23,4 +12,5 @@ void TreeNode::setCosts(float h, float c) { Eigen::Vector3f TreeNode::getPosition() const { return state.position; } Eigen::Vector3f TreeNode::getVelocity() const { return state.velocity; } +Eigen::Vector3f TreeNode::getSetpoint() const { return setpoint; } } diff --git a/local_planner/src/nodes/waypoint_generator.cpp b/local_planner/src/nodes/waypoint_generator.cpp index c945fd32e..96ccc0fb5 100644 --- a/local_planner/src/nodes/waypoint_generator.cpp +++ b/local_planner/src/nodes/waypoint_generator.cpp @@ -34,10 +34,10 @@ void WaypointGenerator::calculateWaypoint() { } case tryPath: { - Eigen::Vector3f setpoint = position_; - if (getSetpointFromPath(planner_info_.path_node_positions, planner_info_.last_path_time, - planner_info_.cruise_velocity, setpoint)) { - output_.goto_position = position_ + (setpoint - position_).normalized(); + Eigen::Vector3f setpoint = Eigen::Vector3f::Zero(); + if (interpolateBetweenSetpoints(planner_info_.path_node_setpoints, planner_info_.last_path_time, + planner_info_.tree_node_duration, setpoint)) { + output_.goto_position = position_ + setpoint; ROS_DEBUG("[WG] Using calculated tree\n"); } else { ROS_DEBUG("[WG] No valid tree, going straight"); diff --git a/local_planner/test/test_local_planner.cpp b/local_planner/test/test_local_planner.cpp index afa5143f3..5b23ad5ca 100644 --- a/local_planner/test/test_local_planner.cpp +++ b/local_planner/test/test_local_planner.cpp @@ -122,14 +122,23 @@ TEST_F(LocalPlannerTests, all_obstacles) { avoidanceOutput output = planner.getAvoidanceOutput(); EXPECT_TRUE(output.obstacle_ahead); - ASSERT_GE(output.path_node_positions.size(), 2); + ASSERT_GE(output.path_node_setpoints.size(), 2); float node_max_y = 0.f; float node_min_y = 0.f; - for (auto it = output.path_node_positions.rbegin(); it != output.path_node_positions.rend(); ++it) { + simulation_state state(0.f, Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero(), Eigen::Vector3f::Zero()); + simulation_limits lims(3.f, -1.f, 3.f, 5.f, 20.f); + for (auto it = output.path_node_setpoints.rbegin() + 1; it != output.path_node_setpoints.rend(); ++it) { auto node = *it; - if (node.x() > distance) break; - if (node.y() > node_max_y) node_max_y = node.y(); - if (node.y() < node_min_y) node_min_y = node.y(); + TrajectorySimulator sim(lims, state, 0.05f); + + std::vector trajectory = sim.generate_trajectory(node, output.tree_node_duration); + for (auto& p : trajectory) { + std::cout << p.position << std::endl; + if (p.position.x() > distance) break; + if (p.position.y() > node_max_y) node_max_y = p.position.y(); + if (p.position.y() < node_min_y) node_min_y = p.position.y(); + } + state = trajectory.back(); } bool steer_clear = node_max_y > max_y || node_min_y < min_y; @@ -183,9 +192,9 @@ TEST_F(LocalPlannerTests, obstacles_right) { avoidanceOutput output = planner.getAvoidanceOutput(); EXPECT_TRUE(output.obstacle_ahead); - ASSERT_GE(output.path_node_positions.size(), 2); + ASSERT_GE(output.path_node_setpoints.size(), 2); float node_max_y = 0.f; - for (auto it = output.path_node_positions.rbegin(); it != output.path_node_positions.rend(); ++it) { + for (auto it = output.path_node_setpoints.rbegin(); it != output.path_node_setpoints.rend(); ++it) { auto node = *it; if (node.x() > distance) break; if (node.y() > node_max_y) node_max_y = node.y(); @@ -240,9 +249,9 @@ TEST_F(LocalPlannerTests, obstacles_left) { avoidanceOutput output = planner.getAvoidanceOutput(); EXPECT_TRUE(output.obstacle_ahead); - ASSERT_GE(output.path_node_positions.size(), 2); + ASSERT_GE(output.path_node_setpoints.size(), 2); float node_min_y = 0.f; - for (auto it = output.path_node_positions.rbegin(); it != output.path_node_positions.rend(); ++it) { + for (auto it = output.path_node_setpoints.rbegin(); it != output.path_node_setpoints.rend(); ++it) { auto node = *it; if (node.x() > distance) break; if (node.y() < node_min_y) node_min_y = node.y(); diff --git a/local_planner/test/test_planner_functions.cpp b/local_planner/test/test_planner_functions.cpp index 2b665546c..eaa9bd341 100644 --- a/local_planner/test/test_planner_functions.cpp +++ b/local_planner/test/test_planner_functions.cpp @@ -128,7 +128,7 @@ TEST(PlannerFunctionsTests, processPointcloud) { EXPECT_EQ(7, processed_cloud3.size()); // since memory point is inside FOV, it isn't remembered } -TEST(PlannerFunctions, getSetpointFromPath) { +TEST(PlannerFunctions, interpolateBetweenSetpoints) { // GIVEN: the node positions in a path and some possible vehicle positions float n1_x = 0.8f; float n2_x = 1.5f; @@ -139,7 +139,7 @@ TEST(PlannerFunctions, getSetpointFromPath) { Eigen::Vector3f n2(n2_x, n1.y() + sqrtf(1 - powf(n2_x - n1.x(), 2)), 2.5f); Eigen::Vector3f n3(n3_x, n2.y() + sqrtf(1 - powf(n3_x - n2.x(), 2)), 2.5f); Eigen::Vector3f n4(n4_x, n3.y() + sqrtf(1 - powf(n4_x - n3.x(), 2)), 2.5f); - const std::vector path_node_positions = {n4, n3, n2, n1, n0}; + const std::vector path_node_setpoints = {n4, n3, n2, n1, n0}; const std::vector empty_path = {}; ros::Time t1 = ros::Time::now(); ros::Time t2 = t1 - ros::Duration(0.1); @@ -150,10 +150,11 @@ TEST(PlannerFunctions, getSetpointFromPath) { Eigen::Vector3f sp1, sp2, sp3; // WHEN: we look for the best direction to fly towards - bool res = getSetpointFromPath(path_node_positions, t1, velocity, sp1); // very short time should still return node 1 - bool res1 = getSetpointFromPath(path_node_positions, t2, velocity, sp2); - bool res2 = getSetpointFromPath(path_node_positions, t3, velocity, sp3); // should be second node on path - bool res3 = getSetpointFromPath(empty_path, t1, velocity, sp1); + bool res = interpolateBetweenSetpoints(path_node_setpoints, t1, velocity, + sp1); // very short time should still return node 1 + bool res1 = interpolateBetweenSetpoints(path_node_setpoints, t2, velocity, sp2); + bool res2 = interpolateBetweenSetpoints(path_node_setpoints, t3, velocity, sp3); // should be second node on path + bool res3 = interpolateBetweenSetpoints(empty_path, t1, velocity, sp1); // THEN: we expect the setpoint in between node n1 and n2 for t1 and t2 between // node n2 and n3 for t3, and not to get an available path for the empty path diff --git a/local_planner/test/test_star_planner.cpp b/local_planner/test/test_star_planner.cpp index f342cfdb8..17a2e6825 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_distance_ = 1.0; + config.tree_node_duration_ = 1.0; star_planner.dynamicReconfigureSetStarParams(config, 1); position.x() = 1.2f; diff --git a/local_planner/test/test_waypoint_generator.cpp b/local_planner/test/test_waypoint_generator.cpp index 3ad7018cb..3af379cb1 100644 --- a/local_planner/test/test_waypoint_generator.cpp +++ b/local_planner/test/test_waypoint_generator.cpp @@ -45,7 +45,7 @@ class WaypointGeneratorTests : public ::testing::Test, public WaypointGenerator Eigen::Vector3f n3(n3_x, n2.y() + sqrtf(1 - powf(n3_x - n2.x(), 2)), 2.0f); Eigen::Vector3f n4(n4_x, n3.y() + sqrtf(1 - powf(n4_x - n3.x(), 2)), 2.0f); Eigen::Vector3f n5(n5_x, n4.y() + sqrtf(1 - powf(n5_x - n4.x(), 2)), 2.0f); - avoidance_output.path_node_positions = {n5, n4, n3, n2, n1, n0}; + avoidance_output.path_node_setpoints = {n5, n4, n3, n2, n1, n0}; position = Eigen::Vector3f(0.f, 0.f, 0.f); q = Eigen::Quaternionf(1.f, 0.f, 0.f, 0.f); From bfed37cc32a8b8d4bbb9e2f1dbb30b7f6f2b3357 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Tue, 30 Jul 2019 16:28:11 +0200 Subject: [PATCH 04/15] Stop A* at goal and fix setpoint visualization --- avoidance/include/avoidance/common.h | 2 +- avoidance/src/avoidance_node.cpp | 2 +- .../include/local_planner/star_planner.h | 5 +-- .../local_planner/trajectory_simulator.h | 13 +++---- local_planner/src/nodes/local_planner.cpp | 35 ++++++++++--------- .../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, 55 insertions(+), 40 deletions(-) diff --git a/avoidance/include/avoidance/common.h b/avoidance/include/avoidance/common.h index 8a274f54f..4b0ee6a22 100644 --- a/avoidance/include/avoidance/common.h +++ b/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 diff --git a/avoidance/src/avoidance_node.cpp b/avoidance/src/avoidance_node.cpp index f5c47c468..1fc9a9c66 100644 --- a/avoidance/src/avoidance_node.cpp +++ b/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); 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 b8bbdbb9a..2f1c4924b 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -122,22 +122,22 @@ void LocalPlanner::determineStrategy() { create2DObstacleRepresentation(px4_.param_mpc_col_prev_d > 0.f); if (!polar_histogram_.isEmpty()) { - getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, - 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_); - - // build search tree - star_planner_->buildLookAheadTree(); - last_path_time_ = ros::Time::now(); + getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, + 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_); + + // build search tree + star_planner_->buildLookAheadTree(); + last_path_time_ = ros::Time::now(); } } @@ -188,12 +188,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_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp index d9f831832..e93333189 100644 --- a/local_planner/src/nodes/local_planner_visualization.cpp +++ b/local_planner/src/nodes/local_planner_visualization.cpp @@ -227,10 +227,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 f93693275..1f0ad6271 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); From 5628fa20693fe493f46b1481aeed4a7c701c5e32 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Thu, 29 Aug 2019 10:19:58 +0200 Subject: [PATCH 05/15] fix todos on px4 parameters and ros parameters, lower obstacle distance cost such that it's possible to fly at lower altitudes --- avoidance/src/avoidance_node.cpp | 4 ++++ local_planner/cfg/local_planner_node.cfg | 3 ++- local_planner/cfg/vehicle.yaml | 2 +- local_planner/include/local_planner/star_planner.h | 1 + local_planner/src/nodes/local_planner.cpp | 1 - local_planner/src/nodes/star_planner.cpp | 4 ++-- 6 files changed, 10 insertions(+), 5 deletions(-) diff --git a/avoidance/src/avoidance_node.cpp b/avoidance/src/avoidance_node.cpp index 1fc9a9c66..a2d2e04cc 100644 --- a/avoidance/src/avoidance_node.cpp +++ b/avoidance/src/avoidance_node.cpp @@ -134,6 +134,10 @@ void AvoidanceNode::checkPx4Parameters() { request_param("MPC_COL_PREV_D", px4_.param_mpc_col_prev_d); request_param("MPC_LAND_SPEED", px4_.param_mpc_land_speed); request_param("NAV_ACC_RAD", px4_.param_nav_acc_rad); + request_param("MPC_Z_VEL_MAX_DN", px4_.param_mpc_z_vel_max_dn); + request_param("MPC_Z_VEL_MAX_UP", px4_.param_mpc_z_vel_max_up); + request_param("MPC_ACC_HOR", px4_.param_mpc_acc_hor); + request_param("MPC_JERK_MAX", px4_.param_mpc_jerk_max); if (!std::isfinite(px4_.param_mpc_xy_cruise) || !std::isfinite(px4_.param_mpc_col_prev_d) || !std::isfinite(px4_.param_mpc_land_speed) || !std::isfinite(px4_.param_nav_acc_rad)) { diff --git a/local_planner/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg index c5261a499..7c9323457 100755 --- a/local_planner/cfg/local_planner_node.cfg +++ b/local_planner/cfg/local_planner_node.cfg @@ -12,8 +12,9 @@ gen.add("min_sensor_range_", double_t, 0, "Discard points closer than that", 0.2 gen.add("pitch_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 25.0, 0, 30.0) gen.add("yaw_cost_param_", double_t, 0, "Cost function weight for constant heading", 3.0, 0, 20.0) gen.add("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 6000, 0.0, 50000.0) -gen.add("obstacle_cost_param_", double_t, 0,"Approximate distance from obstacles (m) when the obstacle distance term dominates the cost function", 8.5, 0.0, 30.0) +gen.add("obstacle_cost_param_", double_t, 0,"Approximate distance from obstacles (m) when the obstacle distance term dominates the cost function", 5.0, 0.0, 30.0) gen.add("tree_heuristic_weight_", double_t, 0, "Weight for the tree heuristic cost", 35.0, 0.0, 50.0) +gen.add("tree_step_size_s_", double_t, 0, "Tree time step size in seconds", 0.05, 0.01, 5.0) gen.add("goal_z_param", double_t, 0, "Height of the goal position", 3.5, -20.0, 20.0) gen.add("timeout_startup_", double_t, 0, "After this timeout the companion status is MAV_STATE_CRITICAL", 5, 0, 60) gen.add("timeout_critical_", double_t, 0, "After this timeout the companion status is MAV_STATE_CRITICAL", 0.5, 0, 10) diff --git a/local_planner/cfg/vehicle.yaml b/local_planner/cfg/vehicle.yaml index 840928400..93512c76c 100644 --- a/local_planner/cfg/vehicle.yaml +++ b/local_planner/cfg/vehicle.yaml @@ -37,7 +37,7 @@ dictitems: min_num_points_per_cell_: 1 min_sensor_range_: 0.2 n_expanded_nodes_: 40 - obstacle_cost_param_: 8.5 + obstacle_cost_param_: 5.0 pitch_cost_param_: 25.0 max_sensor_range_: 15.0 smoothing_margin_degrees_: 40.0 diff --git a/local_planner/include/local_planner/star_planner.h b/local_planner/include/local_planner/star_planner.h index 5368b28e0..63ca10df4 100644 --- a/local_planner/include/local_planner/star_planner.h +++ b/local_planner/include/local_planner/star_planner.h @@ -28,6 +28,7 @@ class StarPlanner { float smoothing_margin_degrees_ = 40.f; float tree_heuristic_weight_ = 35.f; float acceptance_radius_ = 2.f; + float tree_step_size_s_ = 0.05f; pcl::PointCloud cloud_; diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index 2f1c4924b..eda3ff73c 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -126,7 +126,6 @@ void LocalPlanner::determineStrategy() { 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; diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 1f0ad6271..0178965ea 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -18,6 +18,7 @@ void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerN max_path_length_ = static_cast(config.max_sensor_range_); smoothing_margin_degrees_ = static_cast(config.smoothing_margin_degrees_); tree_heuristic_weight_ = static_cast(config.tree_heuristic_weight_); + tree_step_size_s_ = static_cast(config.tree_step_size_s_); } void StarPlanner::setParams(const costParameters& cost_params, const simulation_limits& limits, float acc_rad) { @@ -41,7 +42,6 @@ float StarPlanner::treeHeuristicFunction(int node_number) const { void StarPlanner::buildLookAheadTree() { std::clock_t start_time = std::clock(); - Histogram histogram(ALPHA_RES); std::vector cost_image_data; std::vector candidate_vector; @@ -87,7 +87,7 @@ void StarPlanner::buildLookAheadTree() { int children = 0; for (candidateDirection candidate : candidate_vector) { simulation_state state = tree_[origin].state; - TrajectorySimulator sim(lims_, state, 0.05f); // todo: parameterize simulation step size [s] + TrajectorySimulator sim(lims_, state, tree_step_size_s_); std::vector trajectory = sim.generate_trajectory(candidate.toEigen(), tree_node_duration_); // check if another close node has been added From 2568a666df0008ecbe5772a623cedda35b7032a4 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Thu, 29 Aug 2019 11:04:30 +0200 Subject: [PATCH 06/15] clang format --- local_planner/src/nodes/local_planner.cpp | 30 +++++++++++------------ 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index eda3ff73c..62780fa90 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -122,21 +122,21 @@ void LocalPlanner::determineStrategy() { create2DObstacleRepresentation(px4_.param_mpc_col_prev_d > 0.f); if (!polar_histogram_.isEmpty()) { - getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, - cost_matrix_, cost_image_data_); - - simulation_limits lims; - 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_); - - // build search tree - star_planner_->buildLookAheadTree(); - last_path_time_ = ros::Time::now(); + getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, cost_matrix_, + cost_image_data_); + + simulation_limits lims; + 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_); + + // build search tree + star_planner_->buildLookAheadTree(); + last_path_time_ = ros::Time::now(); } } From 3bc50037f5535812be0f201a2d4189d5840ed77a Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Thu, 29 Aug 2019 11:45:41 +0200 Subject: [PATCH 07/15] fix tests --- local_planner/test/test_local_planner.cpp | 1 - local_planner/test/test_waypoint_generator.cpp | 1 + 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/local_planner/test/test_local_planner.cpp b/local_planner/test/test_local_planner.cpp index 5b23ad5ca..35718b129 100644 --- a/local_planner/test/test_local_planner.cpp +++ b/local_planner/test/test_local_planner.cpp @@ -133,7 +133,6 @@ TEST_F(LocalPlannerTests, all_obstacles) { std::vector trajectory = sim.generate_trajectory(node, output.tree_node_duration); for (auto& p : trajectory) { - std::cout << p.position << std::endl; if (p.position.x() > distance) break; if (p.position.y() > node_max_y) node_max_y = p.position.y(); if (p.position.y() < node_min_y) node_min_y = p.position.y(); diff --git a/local_planner/test/test_waypoint_generator.cpp b/local_planner/test/test_waypoint_generator.cpp index 3af379cb1..87c2eea10 100644 --- a/local_planner/test/test_waypoint_generator.cpp +++ b/local_planner/test/test_waypoint_generator.cpp @@ -31,6 +31,7 @@ class WaypointGeneratorTests : public ::testing::Test, public WaypointGenerator avoidance_output.obstacle_ahead = false; avoidance_output.cruise_velocity = 1.0; avoidance_output.last_path_time = ros::Time(0.28); + avoidance_output.tree_node_duration = 0.5f; PolarPoint p_pol = histogramIndexToPolar(15, 35, 6, 0.f); From 0feb8c34178ad023b4f79154d98149a2eb4f7732 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 3 Sep 2019 10:28:22 +0200 Subject: [PATCH 08/15] iterate trough the cost matrix while building the tree --- local_planner/cfg/local_planner_node.cfg | 4 +- .../local_planner/candidate_direction.h | 12 ++- .../local_planner_visualization.h | 4 + .../local_planner/trajectory_simulator.h | 1 + .../include/local_planner/tree_node.h | 5 +- .../src/nodes/local_planner_visualization.cpp | 89 +++++++++++++++++++ local_planner/src/nodes/star_planner.cpp | 57 +++++++----- local_planner/src/nodes/tree_node.cpp | 4 +- .../src/utils/trajectory_simulator.cpp | 6 ++ 9 files changed, 153 insertions(+), 29 deletions(-) diff --git a/local_planner/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg index 7c9323457..d8079ddc1 100755 --- a/local_planner/cfg/local_planner_node.cfg +++ b/local_planner/cfg/local_planner_node.cfg @@ -9,9 +9,9 @@ gen = ParameterGenerator() gen.add("max_sensor_range_", double_t, 0, "Data points farther away will be discarded", 15.0, 0, 40) gen.add("min_sensor_range_", double_t, 0, "Discard points closer than that", 0.2, 0, 10) -gen.add("pitch_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 25.0, 0, 30.0) +gen.add("pitch_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 50.0, 0, 100.0) gen.add("yaw_cost_param_", double_t, 0, "Cost function weight for constant heading", 3.0, 0, 20.0) -gen.add("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 6000, 0.0, 50000.0) +gen.add("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 3000, 0.0, 50000.0) gen.add("obstacle_cost_param_", double_t, 0,"Approximate distance from obstacles (m) when the obstacle distance term dominates the cost function", 5.0, 0.0, 30.0) gen.add("tree_heuristic_weight_", double_t, 0, "Weight for the tree heuristic cost", 35.0, 0.0, 50.0) gen.add("tree_step_size_s_", double_t, 0, "Tree time step size in seconds", 0.05, 0.01, 5.0) diff --git a/local_planner/include/local_planner/candidate_direction.h b/local_planner/include/local_planner/candidate_direction.h index 55f697fe5..392d0116b 100644 --- a/local_planner/include/local_planner/candidate_direction.h +++ b/local_planner/include/local_planner/candidate_direction.h @@ -3,6 +3,8 @@ #include #include #include "avoidance/common.h" +#include "local_planner/tree_node.h" + namespace avoidance { @@ -10,8 +12,16 @@ struct candidateDirection { float cost; float elevation_angle; float azimuth_angle; + TreeNode tree_node; - candidateDirection(float c, float e, float z) : cost(c), elevation_angle(e), azimuth_angle(z){}; + candidateDirection(float c, float e, float z) : cost(c), elevation_angle(e), azimuth_angle(z) { + simulation_state start_state; + start_state.position = Eigen::Vector3f(0.0f, 0.0f, 0.0f);; + 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); + }; bool operator<(const candidateDirection& y) const { return cost < y.cost; } diff --git a/local_planner/include/local_planner/local_planner_visualization.h b/local_planner/include/local_planner/local_planner_visualization.h index d8012dd43..19ec95230 100644 --- a/local_planner/include/local_planner/local_planner_visualization.h +++ b/local_planner/include/local_planner/local_planner_visualization.h @@ -113,6 +113,7 @@ class LocalPlannerVisualization { void publishFOV(const std::vector& fov, float max_range) const; void publishRangeScan(const sensor_msgs::LaserScan& scan, const geometry_msgs::PoseStamped& newest_pose) const; + std::tuple HSVtoRGB(std::tuple hsv) const; private: ros::Publisher local_pointcloud_pub_; @@ -135,6 +136,9 @@ class LocalPlannerVisualization { ros::Publisher deg60_point_pub_; ros::Publisher fov_pub_; ros::Publisher range_scan_pub_; + ros::Publisher tree_cost_pub_; + + int path_length_ = 0; }; diff --git a/local_planner/include/local_planner/trajectory_simulator.h b/local_planner/include/local_planner/trajectory_simulator.h index a6a3e64fb..2239f803f 100644 --- a/local_planner/include/local_planner/trajectory_simulator.h +++ b/local_planner/include/local_planner/trajectory_simulator.h @@ -36,6 +36,7 @@ class TrajectorySimulator { TrajectorySimulator(const simulation_limits& config, const simulation_state& start, float step_time = 0.1f); std::vector generate_trajectory(const Eigen::Vector3f& goal_direction, float simulation_duration); + simulation_state generate_trajectory_endpoint(const Eigen::Vector3f& goal_direction, float simulation_duration); protected: const simulation_limits config_; diff --git a/local_planner/include/local_planner/tree_node.h b/local_planner/include/local_planner/tree_node.h index 1a40249c8..2fe5f3495 100644 --- a/local_planner/include/local_planner/tree_node.h +++ b/local_planner/include/local_planner/tree_node.h @@ -10,14 +10,15 @@ 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() = delete; - TreeNode(int from, const simulation_state& start_state, const Eigen::Vector3f& sp); + TreeNode() = default; + TreeNode(int from, const simulation_state& start_state, const Eigen::Vector3f& sp, const float cost); ~TreeNode() = default; /** diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp index e93333189..57c017a0b 100644 --- a/local_planner/src/nodes/local_planner_visualization.cpp +++ b/local_planner/src/nodes/local_planner_visualization.cpp @@ -31,6 +31,7 @@ void LocalPlannerVisualization::initializePublishers(ros::NodeHandle& nh) { deg60_point_pub_ = nh.advertise("/deg60_point", 1); fov_pub_ = nh.advertise("/fov", 4); range_scan_pub_ = nh.advertise("/range_scan", 1); + tree_cost_pub_ = nh.advertise("/tree_cost", 1); } void LocalPlannerVisualization::visualizePlannerData(const LocalPlanner& planner, @@ -242,10 +243,98 @@ void LocalPlannerVisualization::publishTree(const std::vector& tree, c } } + visualization_msgs::MarkerArray marker_array; + visualization_msgs::Marker marker; + marker.header.frame_id = "local_origin"; + marker.header.stamp = ros::Time::now(); + marker.id = 0; + marker.type = visualization_msgs::Marker::SPHERE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.2; + marker.scale.y = 0.2; + marker.scale.z = 0.2; + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 0.0; + + float variance_max_value = -1.f; + float variance_min_value = FLT_MAX; + 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_); + } + 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 heuristic = tree[i].heuristic_; + + float h = ((range_max - range_min) * (cost - variance_min_value) / + (variance_max_value - variance_min_value)) + + range_min; + float red, green, blue; + float max_aa = 1.f; + std::tuple test(h, 1.f, 1.f); + std::tie(marker.color.r, marker.color.g, marker.color.b) = HSVtoRGB(test); + marker_array.markers.push_back(marker); + marker.id += 1; + } + + tree_cost_pub_.publish(marker_array); + complete_tree_pub_.publish(tree_marker); tree_path_pub_.publish(path_marker); } +std::tuple LocalPlannerVisualization::HSVtoRGB(std::tuple hsv) const { + std::tuple rgb; + float fC = std::get<2>(hsv) * std::get<1>(hsv); // fV * fS; // Chroma + float fHPrime = fmod(std::get<0>(hsv) / 60.0, 6); + float fX = fC * (1 - fabs(fmod(fHPrime, 2) - 1)); + float fM = std::get<2>(hsv) - fC; + + if (0 <= fHPrime && fHPrime < 1) { + std::get<0>(rgb) = fC; + std::get<1>(rgb) = fX; + std::get<2>(rgb) = 0; + } else if (1 <= fHPrime && fHPrime < 2) { + std::get<0>(rgb) = fX; + std::get<1>(rgb) = fC; + std::get<2>(rgb) = 0; + } else if (2 <= fHPrime && fHPrime < 3) { + std::get<0>(rgb) = 0; + std::get<1>(rgb) = fC; + std::get<2>(rgb) = fX; + } else if (3 <= fHPrime && fHPrime < 4) { + std::get<0>(rgb) = 0; + std::get<1>(rgb) = fX; + std::get<2>(rgb) = fC; + } else if (4 <= fHPrime && fHPrime < 5) { + std::get<0>(rgb) = fX; + std::get<1>(rgb) = 0; + std::get<2>(rgb) = fC; + } else if (5 <= fHPrime && fHPrime < 6) { + std::get<0>(rgb) = fC; + std::get<1>(rgb) = 0; + std::get<2>(rgb) = fX; + } else { + std::get<0>(rgb) = 0; + std::get<1>(rgb) = 0; + std::get<2>(rgb) = 0; + } + + std::get<0>(rgb) += fM; + std::get<1>(rgb) += fM; + std::get<2>(rgb) += fM; + return rgb; +} void LocalPlannerVisualization::publishGoal(const geometry_msgs::Point& goal) const { visualization_msgs::MarkerArray marker_goal; visualization_msgs::Marker m; diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 0178965ea..9f68b3022 100644 --- 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())); + tree_.push_back(TreeNode(0, start_state, Eigen::Vector3f::Zero(), 0.f)); tree_.back().setCosts(treeHeuristicFunction(0), treeHeuristicFunction(0)); int origin = 0; @@ -77,39 +77,52 @@ void StarPlanner::buildLookAheadTree() { candidate_vector.clear(); getCostMatrix(histogram, goal_, origin_position, origin_velocity, cost_params_, smoothing_margin_degrees_, cost_matrix, cost_image_data); - getBestCandidatesFromCostMatrix(cost_matrix, children_per_node_, candidate_vector); - - // add candidates as nodes - if (candidate_vector.empty()) { - tree_[origin].total_cost_ = HUGE_VAL; - } else { - // insert new nodes - int children = 0; - for (candidateDirection candidate : candidate_vector) { + + std::priority_queue, std::less> queue; + for (int row_index = 0; row_index < cost_matrix.rows(); row_index++) { + for (int col_index = 0; col_index < cost_matrix.cols(); col_index++) { + PolarPoint p_pol = histogramIndexToPolar(row_index, col_index, ALPHA_RES, 1.0); + float cost = cost_matrix(row_index, col_index); + candidateDirection candidate(cost, p_pol.e, p_pol.z); simulation_state state = tree_[origin].state; TrajectorySimulator sim(lims_, state, tree_step_size_s_); - std::vector trajectory = sim.generate_trajectory(candidate.toEigen(), tree_node_duration_); - - // check if another close node has been added + simulation_state trajectory_endpoint = sim.generate_trajectory_endpoint(candidate.toEigen(), tree_node_duration_); int close_nodes = 0; - for (size_t i = 0; i < tree_.size(); i++) { - float dist = (tree_[i].getPosition() - trajectory.back().position).norm(); + std::priority_queue, std::less> queue_tmp = queue; + while (!queue_tmp.empty()) { + float dist = (queue_tmp.top().tree_node.getPosition() - trajectory_endpoint.position).norm(); + queue_tmp.pop(); if (dist < 0.2f) { close_nodes++; break; } } - if (children < children_per_node_ && close_nodes == 0) { - tree_.push_back(TreeNode(origin, trajectory.back(), candidate.toEigen())); - float h = treeHeuristicFunction(tree_.size() - 1); - tree_.back().heuristic_ = h; - tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + candidate.cost + h; - children++; + if (queue.size() < children_per_node_) { + candidate.tree_node = TreeNode(origin, trajectory_endpoint, candidate.toEigen(), candidate.cost); + queue.push(candidate); + } else if (candidate < queue.top() && close_nodes == 0) { + candidate.tree_node = TreeNode(origin, trajectory_endpoint, candidate.toEigen(), candidate.cost); + queue.push(candidate); + queue.pop(); } } } + int children = 0; + while (!queue.empty()) { + if (children < children_per_node_) { + tree_.push_back(queue.top().tree_node); + float h = treeHeuristicFunction(tree_.size() - 1); + tree_.back().heuristic_ = h; + tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + queue.top().cost + h; + queue.pop(); + children++; + } else { + break; + } + } + closed_set_.push_back(origin); tree_[origin].closed_ = true; @@ -120,7 +133,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_.push_back(TreeNode(i, simulation_state(0.f, goal_), goal_ - tree_[i].getPosition(), tree_[i].cost_)); 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 index 329579854..cf552fc1b 100644 --- a/local_planner/src/nodes/tree_node.cpp +++ b/local_planner/src/nodes/tree_node.cpp @@ -2,8 +2,8 @@ namespace avoidance { -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) {} +TreeNode::TreeNode(int from, const simulation_state& start_state, const Eigen::Vector3f& sp, const float cost) + : total_cost_{0.0f}, heuristic_{0.0f}, origin_{from}, closed_{false}, state(start_state), setpoint(sp), cost_{cost} {} void TreeNode::setCosts(float h, float c) { heuristic_ = h; diff --git a/local_planner/src/utils/trajectory_simulator.cpp b/local_planner/src/utils/trajectory_simulator.cpp index 46ce756ee..d6c836ae8 100644 --- a/local_planner/src/utils/trajectory_simulator.cpp +++ b/local_planner/src/utils/trajectory_simulator.cpp @@ -20,6 +20,12 @@ TrajectorySimulator::TrajectorySimulator(const avoidance::simulation_limits& con const avoidance::simulation_state& start, float step_time) : config_(config), start_(start), step_time_(step_time) {} +simulation_state TrajectorySimulator::generate_trajectory_endpoint(const Eigen::Vector3f& goal_direction, + float simulation_duration) { + std::vector result = generate_trajectory(goal_direction, simulation_duration); + return result.back(); +} + std::vector TrajectorySimulator::generate_trajectory(const Eigen::Vector3f& goal_direction, float simulation_duration) { int num_steps = static_cast(std::ceil(simulation_duration / step_time_)); From da00272a3f8573f60fd631562c40b32f7c73a75e Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Tue, 3 Sep 2019 18:21:01 +0200 Subject: [PATCH 09/15] clang-format --- .../include/local_planner/candidate_direction.h | 5 ++--- .../include/local_planner/local_planner_visualization.h | 2 -- local_planner/src/nodes/local_planner_visualization.cpp | 9 ++++----- local_planner/src/nodes/star_planner.cpp | 6 ++++-- local_planner/src/nodes/tree_node.cpp | 8 +++++++- 5 files changed, 17 insertions(+), 13 deletions(-) diff --git a/local_planner/include/local_planner/candidate_direction.h b/local_planner/include/local_planner/candidate_direction.h index 392d0116b..2296dabfa 100644 --- a/local_planner/include/local_planner/candidate_direction.h +++ b/local_planner/include/local_planner/candidate_direction.h @@ -5,7 +5,6 @@ #include "avoidance/common.h" #include "local_planner/tree_node.h" - namespace avoidance { struct candidateDirection { @@ -16,8 +15,8 @@ struct candidateDirection { candidateDirection(float c, float e, float z) : cost(c), elevation_angle(e), azimuth_angle(z) { simulation_state start_state; - start_state.position = Eigen::Vector3f(0.0f, 0.0f, 0.0f);; - start_state.velocity = Eigen::Vector3f(0.0f, 0.0f, 0.0f);; + start_state.position = Eigen::Vector3f(0.0f, 0.0f, 0.0f); + 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); diff --git a/local_planner/include/local_planner/local_planner_visualization.h b/local_planner/include/local_planner/local_planner_visualization.h index 19ec95230..23c268bb8 100644 --- a/local_planner/include/local_planner/local_planner_visualization.h +++ b/local_planner/include/local_planner/local_planner_visualization.h @@ -138,8 +138,6 @@ class LocalPlannerVisualization { ros::Publisher range_scan_pub_; ros::Publisher tree_cost_pub_; - - int path_length_ = 0; }; } diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp index 57c017a0b..3e1681a09 100644 --- a/local_planner/src/nodes/local_planner_visualization.cpp +++ b/local_planner/src/nodes/local_planner_visualization.cpp @@ -252,8 +252,8 @@ void LocalPlannerVisualization::publishTree(const std::vector& tree, c marker.action = visualization_msgs::Marker::ADD; marker.pose.orientation.w = 1.0; marker.scale.x = 0.2; - marker.scale.y = 0.2; - marker.scale.z = 0.2; + marker.scale.y = 0.2; + marker.scale.z = 0.2; marker.color.a = 1.0; marker.color.r = 0.0; marker.color.g = 0.0; @@ -276,9 +276,8 @@ void LocalPlannerVisualization::publishTree(const std::vector& tree, c float cost = tree[node_nr].cost_; float heuristic = tree[i].heuristic_; - float h = ((range_max - range_min) * (cost - variance_min_value) / - (variance_max_value - variance_min_value)) + - range_min; + float h = + ((range_max - range_min) * (cost - variance_min_value) / (variance_max_value - variance_min_value)) + range_min; float red, green, blue; float max_aa = 1.f; std::tuple test(h, 1.f, 1.f); diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 9f68b3022..d3f1fe139 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -86,9 +86,11 @@ void StarPlanner::buildLookAheadTree() { candidateDirection candidate(cost, p_pol.e, p_pol.z); simulation_state state = tree_[origin].state; TrajectorySimulator sim(lims_, state, tree_step_size_s_); - simulation_state trajectory_endpoint = sim.generate_trajectory_endpoint(candidate.toEigen(), tree_node_duration_); + simulation_state trajectory_endpoint = + sim.generate_trajectory_endpoint(candidate.toEigen(), tree_node_duration_); int close_nodes = 0; - std::priority_queue, std::less> queue_tmp = queue; + std::priority_queue, std::less> + queue_tmp = queue; while (!queue_tmp.empty()) { float dist = (queue_tmp.top().tree_node.getPosition() - trajectory_endpoint.position).norm(); queue_tmp.pop(); diff --git a/local_planner/src/nodes/tree_node.cpp b/local_planner/src/nodes/tree_node.cpp index cf552fc1b..d8d3b06df 100644 --- a/local_planner/src/nodes/tree_node.cpp +++ b/local_planner/src/nodes/tree_node.cpp @@ -3,7 +3,13 @@ namespace avoidance { TreeNode::TreeNode(int from, const simulation_state& start_state, const Eigen::Vector3f& sp, const float cost) - : total_cost_{0.0f}, heuristic_{0.0f}, origin_{from}, closed_{false}, state(start_state), setpoint(sp), cost_{cost} {} + : total_cost_{0.0f}, + heuristic_{0.0f}, + origin_{from}, + closed_{false}, + state(start_state), + setpoint(sp), + cost_{cost} {} void TreeNode::setCosts(float h, float c) { heuristic_ = h; From e378edcfde7f6adbc29d261c2395e42b46b49431 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Tue, 3 Sep 2019 18:21:58 +0200 Subject: [PATCH 10/15] Get rid of dynamic allocation when doing simulation and you only need the endpoint --- .../local_planner/trajectory_simulator.h | 4 ++++ .../src/utils/trajectory_simulator.cpp | 22 ++++++++++++++----- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/local_planner/include/local_planner/trajectory_simulator.h b/local_planner/include/local_planner/trajectory_simulator.h index 2239f803f..b72c23ccf 100644 --- a/local_planner/include/local_planner/trajectory_simulator.h +++ b/local_planner/include/local_planner/trajectory_simulator.h @@ -49,6 +49,10 @@ class TrajectorySimulator { static Eigen::Vector3f jerk_for_velocity_setpoint(float P_constant, float D_constant, float max_jerk_norm, const Eigen::Vector3f& desired_velocity, const simulation_state& state); + + private: + void generate_trajectory(const Eigen::Vector3f& goal_direction, int num_steps, + std::function path_handler); }; // templated helper function diff --git a/local_planner/src/utils/trajectory_simulator.cpp b/local_planner/src/utils/trajectory_simulator.cpp index d6c836ae8..e74b6f9e0 100644 --- a/local_planner/src/utils/trajectory_simulator.cpp +++ b/local_planner/src/utils/trajectory_simulator.cpp @@ -21,9 +21,13 @@ TrajectorySimulator::TrajectorySimulator(const avoidance::simulation_limits& con : config_(config), start_(start), step_time_(step_time) {} simulation_state TrajectorySimulator::generate_trajectory_endpoint(const Eigen::Vector3f& goal_direction, - float simulation_duration) { - std::vector result = generate_trajectory(goal_direction, simulation_duration); - return result.back(); + float simulation_duration) { + int num_steps = static_cast(std::ceil(simulation_duration / step_time_)); + simulation_state last_state; + + generate_trajectory(goal_direction, num_steps, [&last_state](simulation_state state) { last_state = state; }); + + return last_state; } std::vector TrajectorySimulator::generate_trajectory(const Eigen::Vector3f& goal_direction, @@ -32,6 +36,14 @@ std::vector TrajectorySimulator::generate_trajectory(const Eig std::vector timepoints; timepoints.reserve(num_steps); + generate_trajectory(goal_direction, num_steps, + [&timepoints](simulation_state state) { timepoints.push_back(state); }); + + return timepoints; +} + +void TrajectorySimulator::generate_trajectory(const Eigen::Vector3f& goal_direction, int num_steps, + std::function path_handler) { const Eigen::Vector3f unit_goal = goal_direction.normalized(); const Eigen::Vector3f desired_velocity = xy_norm_z_clamp(unit_goal * std::hypot(config_.max_xy_velocity_norm, @@ -67,10 +79,8 @@ std::vector TrajectorySimulator::generate_trajectory(const Eig // update the state based on motion equations with the final jerk run_state = simulate_step_constant_jerk(run_state, jerk, single_step_time); - timepoints.push_back(run_state); + path_handler(run_state); } - - return timepoints; } simulation_state TrajectorySimulator::simulate_step_constant_jerk(const simulation_state& state, From ffa0f2e63cb63d27fff70b92e615d2b6197844f2 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Wed, 4 Sep 2019 09:02:20 +0200 Subject: [PATCH 11/15] Slightly cleaner trajectory simulator reusage --- .../local_planner/trajectory_simulator.h | 4 ++-- .../src/utils/trajectory_simulator.cpp | 24 ++++++++----------- 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/local_planner/include/local_planner/trajectory_simulator.h b/local_planner/include/local_planner/trajectory_simulator.h index b72c23ccf..ecded1e62 100644 --- a/local_planner/include/local_planner/trajectory_simulator.h +++ b/local_planner/include/local_planner/trajectory_simulator.h @@ -51,8 +51,8 @@ class TrajectorySimulator { const simulation_state& state); private: - void generate_trajectory(const Eigen::Vector3f& goal_direction, int num_steps, - std::function path_handler); + simulation_state generate_trajectory(const Eigen::Vector3f& goal_direction, int num_steps, + std::vector* timepoints); }; // templated helper function diff --git a/local_planner/src/utils/trajectory_simulator.cpp b/local_planner/src/utils/trajectory_simulator.cpp index e74b6f9e0..e094c64bd 100644 --- a/local_planner/src/utils/trajectory_simulator.cpp +++ b/local_planner/src/utils/trajectory_simulator.cpp @@ -23,11 +23,7 @@ TrajectorySimulator::TrajectorySimulator(const avoidance::simulation_limits& con simulation_state TrajectorySimulator::generate_trajectory_endpoint(const Eigen::Vector3f& goal_direction, float simulation_duration) { int num_steps = static_cast(std::ceil(simulation_duration / step_time_)); - simulation_state last_state; - - generate_trajectory(goal_direction, num_steps, [&last_state](simulation_state state) { last_state = state; }); - - return last_state; + return generate_trajectory(goal_direction, num_steps, nullptr); } std::vector TrajectorySimulator::generate_trajectory(const Eigen::Vector3f& goal_direction, @@ -36,22 +32,20 @@ std::vector TrajectorySimulator::generate_trajectory(const Eig std::vector timepoints; timepoints.reserve(num_steps); - generate_trajectory(goal_direction, num_steps, - [&timepoints](simulation_state state) { timepoints.push_back(state); }); + generate_trajectory(goal_direction, num_steps, &timepoints); return timepoints; } -void TrajectorySimulator::generate_trajectory(const Eigen::Vector3f& goal_direction, int num_steps, - std::function path_handler) { +simulation_state TrajectorySimulator::generate_trajectory(const Eigen::Vector3f& goal_direction, int num_steps, + std::vector* timepoints) { const Eigen::Vector3f unit_goal = goal_direction.normalized(); const Eigen::Vector3f desired_velocity = xy_norm_z_clamp(unit_goal * std::hypot(config_.max_xy_velocity_norm, unit_goal.z() > 0 ? config_.max_z_velocity : config_.min_z_velocity), config_.max_xy_velocity_norm, config_.min_z_velocity, config_.max_z_velocity); - // calculate P and D constants such that they hit the jerk limit when - // doing accel from 0 + // calculate P and D constants such that they hit the jerk limit when doing accel from 0 float max_accel_norm = std::min(2 * std::sqrt(config_.max_jerk_norm), config_.max_acceleration_norm); float P_constant = (std::sqrt(sqr(max_accel_norm) + config_.max_jerk_norm * desired_velocity.norm()) - max_accel_norm) / @@ -64,8 +58,7 @@ void TrajectorySimulator::generate_trajectory(const Eigen::Vector3f& goal_direct const Eigen::Vector3f damped_jerk = jerk_for_velocity_setpoint(P_constant, D_constant, config_.max_jerk_norm, desired_velocity, run_state); - // limit time step to not exceed the maximum acceleration, but clamp - // jerk to 0 if at maximum acceleration already + // limit time step to not exceed the maximum acceleration, but clamp jerk to 0 if at maximum acceleration already const Eigen::Vector3f requested_accel = run_state.acceleration + single_step_time * damped_jerk; Eigen::Vector3f jerk = damped_jerk; if (requested_accel.squaredNorm() > sqr(max_accel_norm)) { @@ -79,8 +72,11 @@ void TrajectorySimulator::generate_trajectory(const Eigen::Vector3f& goal_direct // update the state based on motion equations with the final jerk run_state = simulate_step_constant_jerk(run_state, jerk, single_step_time); - path_handler(run_state); + if (timepoints != nullptr) { + timepoints->push_back(run_state); + } } + return run_state; } simulation_state TrajectorySimulator::simulate_step_constant_jerk(const simulation_state& state, From baa697895765b079cbdef091cb875744249a3de7 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Wed, 4 Sep 2019 09:13:07 +0200 Subject: [PATCH 12/15] avoid copying the priority queue to iterate over it --- local_planner/include/local_planner/star_planner.h | 14 ++++++++++++++ local_planner/src/nodes/star_planner.cpp | 9 +++------ 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/local_planner/include/local_planner/star_planner.h b/local_planner/include/local_planner/star_planner.h index 63ca10df4..05af19bf5 100644 --- a/local_planner/include/local_planner/star_planner.h +++ b/local_planner/include/local_planner/star_planner.h @@ -15,9 +15,23 @@ #include #include +#include #include namespace avoidance { + +template , typename Compare = std::less > +class iterable_priority_queue : public std::priority_queue { + public: + typedef typename Container::iterator iterator; + typedef typename Container::const_iterator const_iterator; + + iterator begin() { return this->c.begin(); } + iterator end() { return this->c.end(); } + const_iterator begin() const { return this->c.begin(); } + const_iterator end() const { return this->c.end(); } +}; + class TreeNode; class StarPlanner { diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index d3f1fe139..02fa32bb1 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -78,7 +78,7 @@ void StarPlanner::buildLookAheadTree() { getCostMatrix(histogram, goal_, origin_position, origin_velocity, cost_params_, smoothing_margin_degrees_, cost_matrix, cost_image_data); - std::priority_queue, std::less> queue; + iterable_priority_queue, std::less> queue; for (int row_index = 0; row_index < cost_matrix.rows(); row_index++) { for (int col_index = 0; col_index < cost_matrix.cols(); col_index++) { PolarPoint p_pol = histogramIndexToPolar(row_index, col_index, ALPHA_RES, 1.0); @@ -89,11 +89,8 @@ void StarPlanner::buildLookAheadTree() { simulation_state trajectory_endpoint = sim.generate_trajectory_endpoint(candidate.toEigen(), tree_node_duration_); int close_nodes = 0; - std::priority_queue, std::less> - queue_tmp = queue; - while (!queue_tmp.empty()) { - float dist = (queue_tmp.top().tree_node.getPosition() - trajectory_endpoint.position).norm(); - queue_tmp.pop(); + for (auto it = queue.begin(); it != queue.end(); it++) { + float dist = ((*it).tree_node.getPosition() - trajectory_endpoint.position).norm(); if (dist < 0.2f) { close_nodes++; break; From 8ae40773b05678802ee0507afe31a993bfbdd069 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Wed, 4 Sep 2019 09:29:20 +0200 Subject: [PATCH 13/15] remove cost of candidate from treenode --- local_planner/include/local_planner/candidate_direction.h | 2 +- local_planner/include/local_planner/tree_node.h | 3 +-- local_planner/src/nodes/local_planner_visualization.cpp | 8 +++----- local_planner/src/nodes/star_planner.cpp | 8 ++++---- local_planner/src/nodes/tree_node.cpp | 5 ++--- 5 files changed, 11 insertions(+), 15 deletions(-) diff --git a/local_planner/include/local_planner/candidate_direction.h b/local_planner/include/local_planner/candidate_direction.h index 2296dabfa..85e70d967 100644 --- 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 index 2fe5f3495..78e8225bb 100644 --- a/local_planner/include/local_planner/tree_node.h +++ b/local_planner/include/local_planner/tree_node.h @@ -10,7 +10,6 @@ namespace avoidance { class TreeNode { public: float total_cost_; - float cost_; float heuristic_; int origin_; bool closed_; @@ -18,7 +17,7 @@ class TreeNode { 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 index 3e1681a09..47942fddc 100644 --- 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& 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 index 02fa32bb1..4a0437d37 100644 --- 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 index d8d3b06df..234804335 100644 --- 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; From a8b294303505403371f7109d32b1ec4da765afe6 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Wed, 4 Sep 2019 10:27:17 +0200 Subject: [PATCH 14/15] set the trajectory speed limit based on the distance to the goal --- .../include/local_planner/local_planner.h | 22 ++++++++++ local_planner/src/nodes/local_planner.cpp | 40 ++++++++++++------- local_planner/src/nodes/tree_node.cpp | 7 +--- 3 files changed, 49 insertions(+), 20 deletions(-) diff --git a/local_planner/include/local_planner/local_planner.h b/local_planner/include/local_planner/local_planner.h index 2a1e3b4cb..0cf1bdc49 100644 --- a/local_planner/include/local_planner/local_planner.h +++ b/local_planner/include/local_planner/local_planner.h @@ -88,6 +88,28 @@ class LocalPlanner { **/ void generateHistogramImage(Histogram& histogram); + /** + * @brief compute the maximum speed allowed based on sensor range and vehicle tuning + * @param jerk, vehicle maximum jerk + * @param accel, vehicle maximum horizontal acceleration + * @param braking_distance, maximum sensor range + * @returns maximum speed + **/ + float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance) const; + + /** + * @brief compute if the cruise speed requested by paramters or mission item is feasible based on vehicle dynamics + *and sesnor range + * @param jerk, vehicle maximum jerk + * @param accel, vehicle maximum horizontal acceleration + * @param braking_distance, maximum sensor range + * @param mpc_xy_cruise, desired speed set from parameter + * @param mission_item_speed, desired speed set from mission item + * @returns maximum speed + **/ + float getMaxSpeed(const float jerk, const float accel, const float braking_distance, const float mpc_xy_cruise, + const float mission_item_speed) const; + public: std::vector histogram_image_data_; std::vector cost_image_data_; diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index 62780fa90..9b48a2f20 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -128,7 +128,11 @@ void LocalPlanner::determineStrategy() { simulation_limits lims; 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_xy_velocity_norm = + std::min(getMaxSpeed(px4_.param_mpc_jerk_max, px4_.param_mpc_acc_hor, max_sensor_range_, + px4_.param_mpc_xy_cruise, mission_item_speed_), + getMaxSpeed(px4_.param_mpc_jerk_max, px4_.param_mpc_acc_hor, (goal_ - position_).norm(), + px4_.param_mpc_xy_cruise, mission_item_speed_)); 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); @@ -212,19 +216,8 @@ avoidanceOutput LocalPlanner::getAvoidanceOutput() const { out.waypoint_type = waypoint_type_; out.obstacle_ahead = !polar_histogram_.isEmpty(); - // calculate maximum speed given the sensor range and vehicle parameters - // quadratic solve of 0 = u^2 + 2as, with s = u * |a/j| + r - // u = initial velocity, a = max acceleration - // s = stopping distance under constant acceleration - // j = maximum jerk, r = maximum range sensor distance - float accel_ramp_time = px4_.param_mpc_acc_hor / px4_.param_mpc_jerk_max; - float a = 1; - float b = 2 * px4_.param_mpc_acc_hor * accel_ramp_time; - float c = 2 * -px4_.param_mpc_acc_hor * max_sensor_range_; - float limited_speed = (-b + std::sqrt(b * b - 4 * a * c)) / (2 * a); - - float speed = std::isfinite(mission_item_speed_) ? mission_item_speed_ : px4_.param_mpc_xy_cruise; - float max_speed = std::min(speed, limited_speed); + float max_speed = getMaxSpeed(px4_.param_mpc_jerk_max, px4_.param_mpc_acc_hor, max_sensor_range_, + px4_.param_mpc_xy_cruise, mission_item_speed_); out.cruise_velocity = max_speed; out.last_path_time = last_path_time_; @@ -232,4 +225,23 @@ avoidanceOutput LocalPlanner::getAvoidanceOutput() const { out.path_node_setpoints = star_planner_->path_node_setpoints_; return out; } + +float LocalPlanner::computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, + const float braking_distance) const { + // Calculate maximum speed given the sensor range and vehicle parameters + // We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from + // opposite max acceleration) + // Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk) + float b = 4.f * accel * accel / jerk; + float c = -2.f * accel * braking_distance; + + return 0.5f * (-b + sqrtf(b * b - 4.f * c)); +} + +float LocalPlanner::getMaxSpeed(const float jerk, const float accel, const float braking_distance, + const float mpc_xy_cruise, const float mission_item_speed) const { + float limited_speed = computeMaxSpeedFromBrakingDistance(jerk, accel, braking_distance); + float speed = std::isfinite(mission_item_speed) ? mission_item_speed : mpc_xy_cruise; + return std::min(speed, limited_speed); +} } diff --git a/local_planner/src/nodes/tree_node.cpp b/local_planner/src/nodes/tree_node.cpp index 234804335..329579854 100644 --- a/local_planner/src/nodes/tree_node.cpp +++ b/local_planner/src/nodes/tree_node.cpp @@ -3,12 +3,7 @@ namespace avoidance { 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) {} + : total_cost_{0.0f}, heuristic_{0.0f}, origin_{from}, closed_{false}, state(start_state), setpoint(sp) {} void TreeNode::setCosts(float h, float c) { heuristic_ = h; From 347e290af7f11a553f70bfd9f7951fcf60b896c8 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Wed, 4 Sep 2019 11:29:48 +0200 Subject: [PATCH 15/15] use maximum speed computation when simulating the look ahead trajectories --- avoidance/include/avoidance/common.h | 21 ++++++++++++++++++ avoidance/src/common.cpp | 18 +++++++++++++++ .../include/local_planner/local_planner.h | 22 ------------------- local_planner/src/nodes/local_planner.cpp | 19 ---------------- local_planner/src/nodes/star_planner.cpp | 9 ++++++-- 5 files changed, 46 insertions(+), 43 deletions(-) diff --git a/avoidance/include/avoidance/common.h b/avoidance/include/avoidance/common.h index 4b0ee6a22..2a1e191ef 100644 --- a/avoidance/include/avoidance/common.h +++ b/avoidance/include/avoidance/common.h @@ -352,6 +352,27 @@ pcl::PointCloud removeNaNAndGetMaxima(pcl::PointCloud& maxima); +/** +* @brief compute the maximum speed allowed based on sensor range and vehicle tuning +* @param jerk, vehicle maximum jerk +* @param accel, vehicle maximum horizontal acceleration +* @param braking_distance, maximum sensor range +* @returns maximum speed +**/ +float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance); + +/** +* @brief compute if the cruise speed requested by paramters or mission item is feasible based on vehicle dynamics +*and sesnor range +* @param jerk, vehicle maximum jerk +* @param accel, vehicle maximum horizontal acceleration +* @param braking_distance, maximum sensor range +* @param mpc_xy_cruise, desired speed set from parameter +* @param mission_item_speed, desired speed set from mission item +* @returns maximum speed +**/ +float getMaxSpeed(const float jerk, const float accel, const float braking_distance, const float mpc_xy_cruise, + const float mission_item_speed); inline Eigen::Vector3f toEigen(const geometry_msgs::Point& p) { Eigen::Vector3f ev3(p.x, p.y, p.z); diff --git a/avoidance/src/common.cpp b/avoidance/src/common.cpp index fdf4e6ab5..b9c49408a 100644 --- a/avoidance/src/common.cpp +++ b/avoidance/src/common.cpp @@ -418,4 +418,22 @@ void updateFOVFromMaxima(FOV& fov, const pcl::PointCloud& maxima) } } +float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance) { + // Calculate maximum speed given the sensor range and vehicle parameters + // We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from + // opposite max acceleration) + // Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk) + float b = 4.f * accel * accel / jerk; + float c = -2.f * accel * braking_distance; + + return 0.5f * (-b + sqrtf(b * b - 4.f * c)); +} + +float getMaxSpeed(const float jerk, const float accel, const float braking_distance, const float mpc_xy_cruise, + const float mission_item_speed) { + float limited_speed = computeMaxSpeedFromBrakingDistance(jerk, accel, braking_distance); + float speed = std::isfinite(mission_item_speed) ? mission_item_speed : mpc_xy_cruise; + return std::min(speed, limited_speed); +} + } // namespace avoidance diff --git a/local_planner/include/local_planner/local_planner.h b/local_planner/include/local_planner/local_planner.h index 0cf1bdc49..2a1e3b4cb 100644 --- a/local_planner/include/local_planner/local_planner.h +++ b/local_planner/include/local_planner/local_planner.h @@ -88,28 +88,6 @@ class LocalPlanner { **/ void generateHistogramImage(Histogram& histogram); - /** - * @brief compute the maximum speed allowed based on sensor range and vehicle tuning - * @param jerk, vehicle maximum jerk - * @param accel, vehicle maximum horizontal acceleration - * @param braking_distance, maximum sensor range - * @returns maximum speed - **/ - float computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, const float braking_distance) const; - - /** - * @brief compute if the cruise speed requested by paramters or mission item is feasible based on vehicle dynamics - *and sesnor range - * @param jerk, vehicle maximum jerk - * @param accel, vehicle maximum horizontal acceleration - * @param braking_distance, maximum sensor range - * @param mpc_xy_cruise, desired speed set from parameter - * @param mission_item_speed, desired speed set from mission item - * @returns maximum speed - **/ - float getMaxSpeed(const float jerk, const float accel, const float braking_distance, const float mpc_xy_cruise, - const float mission_item_speed) const; - public: std::vector histogram_image_data_; std::vector cost_image_data_; diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index 9b48a2f20..5539093af 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -225,23 +225,4 @@ avoidanceOutput LocalPlanner::getAvoidanceOutput() const { out.path_node_setpoints = star_planner_->path_node_setpoints_; return out; } - -float LocalPlanner::computeMaxSpeedFromBrakingDistance(const float jerk, const float accel, - const float braking_distance) const { - // Calculate maximum speed given the sensor range and vehicle parameters - // We assume a constant acceleration profile with a delay of 2*accel/jerk (time to reach the desired acceleration from - // opposite max acceleration) - // Equation to solve: 0 = vel^2 - 2*acc*(x - vel*2*acc/jerk) - float b = 4.f * accel * accel / jerk; - float c = -2.f * accel * braking_distance; - - return 0.5f * (-b + sqrtf(b * b - 4.f * c)); -} - -float LocalPlanner::getMaxSpeed(const float jerk, const float accel, const float braking_distance, - const float mpc_xy_cruise, const float mission_item_speed) const { - float limited_speed = computeMaxSpeedFromBrakingDistance(jerk, accel, braking_distance); - float speed = std::isfinite(mission_item_speed) ? mission_item_speed : mpc_xy_cruise; - return std::min(speed, limited_speed); -} } diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 4a0437d37..ff6170d01 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -78,14 +78,19 @@ void StarPlanner::buildLookAheadTree() { getCostMatrix(histogram, goal_, origin_position, origin_velocity, cost_params_, smoothing_margin_degrees_, cost_matrix, cost_image_data); + simulation_limits limits = lims_; + simulation_state state = tree_[origin].state; + limits.max_xy_velocity_norm = + std::min(computeMaxSpeedFromBrakingDistance(lims_.max_jerk_norm, lims_.max_acceleration_norm, + (state.position - goal_).head<2>().norm()), + lims_.max_xy_velocity_norm); + TrajectorySimulator sim(limits, state, tree_step_size_s_); iterable_priority_queue, std::less> queue; for (int row_index = 0; row_index < cost_matrix.rows(); row_index++) { for (int col_index = 0; col_index < cost_matrix.cols(); col_index++) { PolarPoint p_pol = histogramIndexToPolar(row_index, col_index, ALPHA_RES, 1.0); float cost = cost_matrix(row_index, col_index); candidateDirection candidate(cost, p_pol.e, p_pol.z); - simulation_state state = tree_[origin].state; - TrajectorySimulator sim(lims_, state, tree_step_size_s_); simulation_state trajectory_endpoint = sim.generate_trajectory_endpoint(candidate.toEigen(), tree_node_duration_); int close_nodes = 0;