From 90b2e5b46b30fe1f27182ec5dc43ab1e42b4ae90 Mon Sep 17 00:00:00 2001 From: Tanja Baumann Date: Mon, 2 Sep 2019 11:53:57 +0200 Subject: [PATCH 01/28] Fix histogram compression (#475) * add new test for histogram compression * fix elevation cropping at compression --- .../include/local_planner/planner_functions.h | 2 +- local_planner/src/nodes/local_planner.cpp | 2 +- local_planner/src/nodes/planner_functions.cpp | 6 ++-- local_planner/test/test_planner_functions.cpp | 29 +++++++++++++++++++ 4 files changed, 34 insertions(+), 5 deletions(-) diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h index a3ca8cd8f..229026f2f 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -52,7 +52,7 @@ void generateNewHistogram(Histogram& polar_histogram, const pcl::PointCloud 0) { // check if inside vertical range PolarPoint obstacle = histogramIndexToPolar(e, z, ALPHA_RES, input_hist.get_dist(e, z)); - float ceil_angle = DEG_TO_RAD * (std::abs(obstacle.e) + ALPHA_RES / 2); - float height_difference = std::abs(input_hist.get_dist(e, z) * std::sin(ceil_angle)); + Eigen::Vector3f obstacle_cartesian = polarHistogramToCartesian(obstacle, position); + float height_difference = std::abs(position.z() - obstacle_cartesian.z()); if (height_difference < vertical_cap && (input_hist.get_dist(e, z) < new_hist.get_dist(0, z) || new_hist.get_dist(0, z) == 0.f)) new_hist.set_dist(0, z, input_hist.get_dist(e, z)); diff --git a/local_planner/test/test_planner_functions.cpp b/local_planner/test/test_planner_functions.cpp index 2b665546c..6ace81a05 100644 --- a/local_planner/test/test_planner_functions.cpp +++ b/local_planner/test/test_planner_functions.cpp @@ -128,6 +128,35 @@ TEST(PlannerFunctionsTests, processPointcloud) { EXPECT_EQ(7, processed_cloud3.size()); // since memory point is inside FOV, it isn't remembered } +TEST(PlannerFunctions, compressHistogramElevation) { + // GIVEN: a position and a pointcloud with data + const Eigen::Vector3f position(0.f, 0.f, 5.f); + const Eigen::Vector3f data_point(14.f, 0.f, 5.f); + pcl::PointCloud cloud; + + for (int j = 0; j < 1000; j++) { + cloud.push_back(toXYZI(data_point, 0)); + } + + Histogram histogram = Histogram(ALPHA_RES); + Histogram compressed_histogram = Histogram(ALPHA_RES); + + // WHEN: we build a histogram + generateNewHistogram(histogram, cloud, position); + compressHistogramElevation(compressed_histogram, histogram, position); + + PolarPoint data_pol = cartesianToPolarHistogram(data_point, position); + Eigen::Vector2i indices = polarToHistogramIndex(data_pol, ALPHA_RES); + + for (int z = 0; z < GRID_LENGTH_Z; z++) { + if (z == indices.x()) { + EXPECT_FLOAT_EQ(compressed_histogram.get_dist(0, z), 14.f); + } else { + EXPECT_FLOAT_EQ(compressed_histogram.get_dist(0, z), 0.f); + } + } +} + TEST(PlannerFunctions, getSetpointFromPath) { // GIVEN: the node positions in a path and some possible vehicle positions float n1_x = 0.8f; From 1e186529525c4e9df1adc9c6976a2ded5b27c4dc Mon Sep 17 00:00:00 2001 From: baumanta Date: Tue, 3 Sep 2019 10:03:24 +0200 Subject: [PATCH 02/28] don't send range invalid range message --- local_planner/src/nodes/local_planner_node.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/local_planner/src/nodes/local_planner_node.cpp b/local_planner/src/nodes/local_planner_node.cpp index 7ef516a72..b0ace8e9b 100644 --- a/local_planner/src/nodes/local_planner_node.cpp +++ b/local_planner/src/nodes/local_planner_node.cpp @@ -428,7 +428,11 @@ void LocalPlannerNode::publishLaserScan() const { if (!(local_planner_->px4_.param_mpc_col_prev_d < 0)) { sensor_msgs::LaserScan distance_data_to_fcu; local_planner_->getObstacleDistanceData(distance_data_to_fcu); - mavros_obstacle_distance_pub_.publish(distance_data_to_fcu); + + // only send message if planner had a chance to fill it with valid data + if (distance_data_to_fcu.angle_increment > 0.f) { + mavros_obstacle_distance_pub_.publish(distance_data_to_fcu); + } } } From 9fcb0ac01f7d63b6657a45239fb74d92e646eb7a Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Tue, 3 Sep 2019 15:21:59 +0200 Subject: [PATCH 03/28] Port FSM to Local Planner (#472) * use fsm in local planner waypoint generator * add direct state to local planner fsm * adapt local planner waypoint generator tests to the state machine * remove unused code * clean up state machine in local planner * modify cost function to get back on the prev to current waypoint line after the obstacle is avoided * init state machine in loiter state and uncomment code * fix tests for new cost function and state machine initialization * fix uninitialized variables and remove unused variables * SLPState -> PlannerState for local planner state machine * improve cost function to go back to the line previous-current goal only if there aren't any obstacle in the direction of the goal. This prevents indecision when avoiding wide obstacles --- .../include/local_planner/avoidance_output.h | 4 - .../include/local_planner/local_planner.h | 11 +- .../local_planner_visualization.h | 3 +- .../include/local_planner/planner_functions.h | 9 +- .../include/local_planner/star_planner.h | 9 + .../local_planner/waypoint_generator.h | 50 ++- local_planner/src/nodes/local_planner.cpp | 23 +- .../src/nodes/local_planner_node.cpp | 3 + .../src/nodes/local_planner_visualization.cpp | 10 +- local_planner/src/nodes/planner_functions.cpp | 44 ++- local_planner/src/nodes/star_planner.cpp | 6 +- .../src/nodes/waypoint_generator.cpp | 325 +++++++++++------- .../src/nodes/waypoint_generator.cpp.dot | 12 + local_planner/test/test_local_planner.cpp | 4 - local_planner/test/test_planner_functions.cpp | 19 +- local_planner/test/test_star_planner.cpp | 1 + .../test/test_waypoint_generator.cpp | 52 +-- tools/check_state_machine_diagrams.sh | 1 + 18 files changed, 367 insertions(+), 219 deletions(-) create mode 100644 local_planner/src/nodes/waypoint_generator.cpp.dot diff --git a/local_planner/include/local_planner/avoidance_output.h b/local_planner/include/local_planner/avoidance_output.h index f9309bcfb..372faa866 100644 --- a/local_planner/include/local_planner/avoidance_output.h +++ b/local_planner/include/local_planner/avoidance_output.h @@ -10,13 +10,9 @@ namespace avoidance { 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 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 diff --git a/local_planner/include/local_planner/local_planner.h b/local_planner/include/local_planner/local_planner.h index 7b0c7b764..21eda61ae 100644 --- a/local_planner/include/local_planner/local_planner.h +++ b/local_planner/include/local_planner/local_planner.h @@ -47,7 +47,6 @@ class LocalPlanner { std::vector fov_fcu_frame_; - waypoint_choice waypoint_type_ = hover; ros::Time last_path_time_; ros::Time last_pointcloud_process_time_; @@ -61,6 +60,8 @@ class LocalPlanner { Eigen::Vector3f position_ = Eigen::Vector3f::Zero(); Eigen::Vector3f velocity_ = Eigen::Vector3f::Zero(); Eigen::Vector3f goal_ = Eigen::Vector3f::Zero(); + Eigen::Vector3f prev_goal_ = Eigen::Vector3f::Zero(); + Eigen::Vector3f closest_pt_ = Eigen::Vector3f::Zero(); Histogram polar_histogram_ = Histogram(ALPHA_RES); Histogram to_fcu_histogram_ = Histogram(ALPHA_RES); @@ -118,10 +119,16 @@ class LocalPlanner { /** * @brief setter method for mission goal - * @param[in] mgs, goal message coming from the FCU + * @param[in] goal, goal message coming from the FCU **/ void setGoal(const Eigen::Vector3f& goal); + /** + * @brief setter method for previous mission goal + * @param[in] prev_goal, previous goal message coming from the FCU + **/ + void setPreviousGoal(const Eigen::Vector3f& prev_goal); + /** * @brief setter method for field of view * @param[in] index of the camera diff --git a/local_planner/include/local_planner/local_planner_visualization.h b/local_planner/include/local_planner/local_planner_visualization.h index dfdc58085..b3d3a071c 100644 --- a/local_planner/include/local_planner/local_planner_visualization.h +++ b/local_planner/include/local_planner/local_planner_visualization.h @@ -2,6 +2,7 @@ #define LOCAL_PLANNER_VISUALIZATION_H #include "local_planner/local_planner.h" +#include "local_planner/waypoint_generator.h" #include #include @@ -98,7 +99,7 @@ class LocalPlannerVisualization { * visualization * @params[in] newest_pos, location of the drone at the current timestep **/ - void publishCurrentSetpoint(const geometry_msgs::Twist& wp, const waypoint_choice& waypoint_type, + void publishCurrentSetpoint(const geometry_msgs::Twist& wp, const PlannerState& waypoint_type, const geometry_msgs::Point& newest_pos) const; /** diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h index 229026f2f..93be83ebb 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -63,11 +63,15 @@ void compressHistogramElevation(Histogram& new_hist, const Histogram& input_hist * @param[in] last_sent_waypoint, last position waypoint * @param[in] cost_params, weight for the cost function * @param[in] parameter how far an obstacle is spread in the cost matrix +* @param[in] closest_pt, vehicle position projection on the line previous-current goal +* @param[in] max_sensor_range, maximum distance at which the sensor detects objects +* @param[in] min_sensor_range, minimum distance at which the sensor detects objects * @param[out] cost_matrix * @param[out] image of the cost matrix for visualization **/ void getCostMatrix(const Histogram& histogram, const Eigen::Vector3f& goal, const Eigen::Vector3f& position, const Eigen::Vector3f& velocity, const costParameters& cost_params, float smoothing_margin_degrees, + const Eigen::Vector3f& closest_pt, const float max_sensor_range, const float min_sensor_range, Eigen::MatrixXf& cost_matrix, std::vector& image_data); /** @@ -103,11 +107,14 @@ void getBestCandidatesFromCostMatrix(const Eigen::MatrixXf& matrix, unsigned int * @param[in] position, current vehicle position * @param[in] velocity, current vehicle velocity * @param[in] cost_params, weights for goal oriented vs smooth behaviour +* @param[in] closest_pt, vehicle position projection on the line previous-current goal +* @param[in] is_obstacle_facing_goal, true if there is an obstacle in the goal direction * @returns a pair with the first value representing the distance cost, and the second the sum of all other costs **/ std::pair costFunction(const PolarPoint& candidate_polar, float obstacle_distance, const Eigen::Vector3f& goal, const Eigen::Vector3f& position, - const Eigen::Vector3f& velocity, const costParameters& cost_params); + const Eigen::Vector3f& velocity, const costParameters& cost_params, + const Eigen::Vector3f& closest_pt, const bool is_obstacle_facing_goal); /** * @brief max-median filtes the cost matrix diff --git a/local_planner/include/local_planner/star_planner.h b/local_planner/include/local_planner/star_planner.h index d5fb4ba68..b68f07909 100644 --- a/local_planner/include/local_planner/star_planner.h +++ b/local_planner/include/local_planner/star_planner.h @@ -26,12 +26,15 @@ class StarPlanner { float max_path_length_ = 4.f; float smoothing_margin_degrees_ = 30.f; float tree_heuristic_weight_ = 10.0f; + float max_sensor_range_ = 15.f; + float min_sensor_range_ = 0.2f; pcl::PointCloud cloud_; Eigen::Vector3f goal_ = Eigen::Vector3f(NAN, NAN, NAN); Eigen::Vector3f position_ = Eigen::Vector3f(NAN, NAN, NAN); Eigen::Vector3f velocity_ = Eigen::Vector3f(NAN, NAN, NAN); + Eigen::Vector3f closest_pt_ = Eigen::Vector3f(NAN, NAN, NAN); costParameters cost_params_; protected: @@ -68,6 +71,12 @@ class StarPlanner { **/ void setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel); + /** + * @brief setter method for vehicle position projection on the line between the current and previous goal + * @param[in] closest_pt, projection point + **/ + void setClosestPointOnLine(const Eigen::Vector3f& closest_pt); + /** * @brief setter method for current goal * @param[in] goal, current goal position diff --git a/local_planner/include/local_planner/waypoint_generator.h b/local_planner/include/local_planner/waypoint_generator.h index e89742e15..b3cba19d7 100644 --- a/local_planner/include/local_planner/waypoint_generator.h +++ b/local_planner/include/local_planner/waypoint_generator.h @@ -1,6 +1,7 @@ #ifndef WAYPOINT_GENERATOR_H #define WAYPOINT_GENERATOR_H +#include #include "avoidance/common.h" #include "avoidance_output.h" @@ -13,8 +14,11 @@ namespace avoidance { +enum class PlannerState { TRY_PATH, ALTITUDE_CHANGE, LOITER, DIRECT }; +std::string toString(PlannerState state); // for logging + struct waypointResult { - waypoint_choice waypoint_type = hover; + avoidance::PlannerState waypoint_type; Eigen::Vector3f position_wp; Eigen::Quaternionf orientation_wp; Eigen::Vector3f linear_velocity_wp; @@ -24,11 +28,10 @@ struct waypointResult { Eigen::Vector3f smoothed_goto_position; // what is sent to the drone }; -class WaypointGenerator { +class WaypointGenerator : public usm::StateMachine { private: avoidanceOutput planner_info_; waypointResult output_; - waypoint_choice last_wp_type_ = hover; Eigen::Vector3f smoothed_goto_location_ = Eigen::Vector3f(NAN, NAN, NAN); Eigen::Vector3f smoothed_goto_location_velocity_ = Eigen::Vector3f::Zero(); @@ -40,6 +43,8 @@ class WaypointGenerator { Eigen::Vector3f tmp_goal_ = Eigen::Vector3f(NAN, NAN, NAN); Eigen::Vector3f desired_vel_ = Eigen::Vector3f(NAN, NAN, NAN); Eigen::Vector3f change_altitude_pos_ = Eigen::Vector3f(NAN, NAN, NAN); + Eigen::Vector3f hover_position_ = Eigen::Vector3f(NAN, NAN, NAN); + float curr_yaw_rad_ = NAN; float curr_pitch_deg_ = NAN; ros::Time last_time_{99999.}; @@ -51,8 +56,9 @@ class WaypointGenerator { bool is_airborne_ = false; bool is_land_waypoint_{false}; bool is_takeoff_waypoint_{false}; - bool reach_altitude_{false}; + bool reach_altitude_offboard_{false}; bool auto_land_{false}; + bool loiter_{false}; float setpoint_yaw_rad_ = 0.0f; float setpoint_yaw_velocity_ = 0.0f; float heading_at_goal_rad_ = NAN; @@ -60,30 +66,40 @@ class WaypointGenerator { float speed_ = 1.0f; std::vector fov_fcu_frame_; - Eigen::Vector3f hover_position_; - NavigationState nav_state_ = NavigationState::none; ros::Time velocity_time_; + // state + bool trigger_reset_ = false; + bool state_changed_ = false; + PlannerState prev_slp_state_ = PlannerState::TRY_PATH; + usm::Transition runTryPath(); + usm::Transition runAltitudeChange(); + usm::Transition runLoiter(); + usm::Transition runDirect(); + + /** + * @brief iterate the statemachine + */ + usm::Transition runCurrentState() override final; + + /** + * @brief the setup of the statemachine + */ + PlannerState chooseNextState(PlannerState currentState, usm::Transition transition) override final; + /** * @brief computes position and velocity waypoints based on the input * waypoint_choice **/ void calculateWaypoint(); - /** - * @brief computes waypoints when there isn't any obstacle - **/ - void goStraight(); + /** * @brief transform a position waypoint into a velocity waypoint **/ void transformPositionToVelocityWaypoint(); - /** - * @brief checks if the goal altitude has been reached. If not, it computes - * waypoints to climb to the goal altitude - **/ - void reachGoalAltitudeFirst(); + /** * @brief smooths waypoints with a critically damped PD controller * @param[in] dt, time elapsed between two cycles @@ -106,7 +122,7 @@ class WaypointGenerator { **/ void getPathMsg(); - void changeAltitude(); + bool isAltitudeChange(); public: /** @@ -173,7 +189,7 @@ class WaypointGenerator { **/ void getOfftrackPointsForVisualization(Eigen::Vector3f& closest_pt, Eigen::Vector3f& deg60_pt); - WaypointGenerator() = default; + WaypointGenerator(); virtual ~WaypointGenerator() = default; }; } diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index e0277bad7..125e0ccdb 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -51,9 +51,11 @@ void LocalPlanner::dynamicReconfigureSetParams(avoidance::LocalPlannerNodeConfig void LocalPlanner::setGoal(const Eigen::Vector3f& goal) { goal_ = goal; + ROS_INFO("===== Set Goal ======: [%f, %f, %f].", goal_.x(), goal_.y(), goal_.z()); applyGoal(); } +void LocalPlanner::setPreviousGoal(const Eigen::Vector3f& prev_goal) { prev_goal_ = prev_goal; } void LocalPlanner::setFOV(int i, const FOV& fov) { if (i < fov_fcu_frame_.size()) { @@ -116,16 +118,27 @@ void LocalPlanner::determineStrategy() { cost_image_data_.clear(); cost_image_data_.resize(3 * GRID_LENGTH_E * GRID_LENGTH_Z, 0); - waypoint_type_ = tryPath; - create2DObstacleRepresentation(px4_.param_mpc_col_prev_d > 0.f); + // calculate the vehicle projected position on the line between the previous and current goal + Eigen::Vector2f u_prev_to_goal = (goal_ - prev_goal_).head<2>().normalized(); + Eigen::Vector2f prev_to_pos = (position_ - prev_goal_).head<2>(); + closest_pt_.head<2>() = prev_goal_.head<2>() + (u_prev_to_goal * u_prev_to_goal.dot(prev_to_pos)); + closest_pt_.z() = goal_.z(); + + // if the vehicle is less than the cruise speed away from the line, set the projection point to the goal such that + // the cost function doesn't pull the vehicle towards the line + if ((position_ - closest_pt_).head<2>().norm() < px4_.param_mpc_xy_cruise) { + closest_pt_ = goal_; + } + if (!polar_histogram_.isEmpty()) { - getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, cost_matrix_, - cost_image_data_); + getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, closest_pt_, + max_sensor_range_, min_sensor_range_, cost_matrix_, cost_image_data_); star_planner_->setParams(cost_params_); star_planner_->setPointcloud(final_cloud_); + star_planner_->setClosestPointOnLine(closest_pt_); // build search tree star_planner_->buildLookAheadTree(); @@ -201,8 +214,6 @@ void LocalPlanner::getObstacleDistanceData(sensor_msgs::LaserScan& obstacle_dist avoidanceOutput LocalPlanner::getAvoidanceOutput() const { avoidanceOutput out; - 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 diff --git a/local_planner/src/nodes/local_planner_node.cpp b/local_planner/src/nodes/local_planner_node.cpp index b0ace8e9b..30e9d1f4d 100644 --- a/local_planner/src/nodes/local_planner_node.cpp +++ b/local_planner/src/nodes/local_planner_node.cpp @@ -180,6 +180,7 @@ void LocalPlannerNode::updatePlannerInfo() { // update goal if (new_goal_) { local_planner_->setGoal(toEigen(goal_msg_.pose.position)); + local_planner_->setPreviousGoal(toEigen(prev_goal_.pose.position)); new_goal_ = false; } @@ -312,6 +313,7 @@ void LocalPlannerNode::clickedPointCallback(const geometry_msgs::PointStamped& m void LocalPlannerNode::clickedGoalCallback(const geometry_msgs::PoseStamped& msg) { new_goal_ = true; + prev_goal_ = goal_msg_; goal_msg_ = msg; /* Selecting the goal from Rviz sets x and y. Get the z coordinate set in * the launch file */ @@ -320,6 +322,7 @@ void LocalPlannerNode::clickedGoalCallback(const geometry_msgs::PoseStamped& msg void LocalPlannerNode::updateGoalCallback(const visualization_msgs::MarkerArray& msg) { if (accept_goal_input_topic_ && msg.markers.size() > 0) { + prev_goal_ = goal_msg_; goal_msg_.pose = msg.markers[0].pose; new_goal_ = true; } diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp index 37ff83d38..1741449c4 100644 --- a/local_planner/src/nodes/local_planner_visualization.cpp +++ b/local_planner/src/nodes/local_planner_visualization.cpp @@ -447,7 +447,7 @@ void LocalPlannerVisualization::publishPaths(const geometry_msgs::Point& last_po } void LocalPlannerVisualization::publishCurrentSetpoint(const geometry_msgs::Twist& wp, - const waypoint_choice& waypoint_type, + const PlannerState& waypoint_type, const geometry_msgs::Point& newest_pos) const { visualization_msgs::Marker setpoint; setpoint.header.frame_id = "local_origin"; @@ -468,25 +468,25 @@ void LocalPlannerVisualization::publishCurrentSetpoint(const geometry_msgs::Twis setpoint.color.a = 1.0; switch (waypoint_type) { - case hover: { + case PlannerState::LOITER: { setpoint.color.r = 1.0; setpoint.color.g = 1.0; setpoint.color.b = 0.0; break; } - case tryPath: { + case PlannerState::TRY_PATH: { setpoint.color.r = 0.0; setpoint.color.g = 1.0; setpoint.color.b = 0.0; break; } - case direct: { + case PlannerState::DIRECT: { setpoint.color.r = 0.0; setpoint.color.g = 0.0; setpoint.color.b = 1.0; break; } - case reachHeight: { + case PlannerState::ALTITUDE_CHANGE: { setpoint.color.r = 1.0; setpoint.color.g = 0.0; setpoint.color.b = 1.0; diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index e38b303a6..2066ef89c 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -127,6 +127,7 @@ void compressHistogramElevation(Histogram& new_hist, const Histogram& input_hist void getCostMatrix(const Histogram& histogram, const Eigen::Vector3f& goal, const Eigen::Vector3f& position, const Eigen::Vector3f& velocity, const costParameters& cost_params, float smoothing_margin_degrees, + const Eigen::Vector3f& closest_pt, const float max_sensor_range, const float min_sensor_range, Eigen::MatrixXf& cost_matrix, std::vector& image_data) { Eigen::MatrixXf distance_matrix(GRID_LENGTH_E, GRID_LENGTH_Z); distance_matrix.fill(NAN); @@ -135,6 +136,27 @@ void getCostMatrix(const Histogram& histogram, const Eigen::Vector3f& goal, cons cost_matrix.resize(GRID_LENGTH_E, GRID_LENGTH_Z); cost_matrix.fill(NAN); + // look if there are any obstacles in the goal direcion +-33deg azimuth, +-15deg elevation + PolarPoint goal_polar = cartesianToPolarHistogram(goal, position); + Eigen::Vector2i goal_index = polarToHistogramIndex(goal_polar, ALPHA_RES); + bool is_obstacle_facing_goal = false; + for (int j = -2; j <= 2; j++) { // elevation 5*ALPHA_RES = 30deg + for (int i = -5; i <= 5; i++) { // azimuth 11*ALPHA_RES = 66deg + PolarPoint tmp; + tmp.z = goal_polar.z; + tmp.e = goal_polar.e; + Eigen::Vector2i tmp_index = goal_index; + + tmp.z += (float)i * ALPHA_RES; + tmp.e += (float)j * ALPHA_RES; + tmp_index = polarToHistogramIndex(tmp, ALPHA_RES); + if (histogram.get_dist(tmp_index.y(), tmp_index.x()) > min_sensor_range && + histogram.get_dist(tmp_index.y(), tmp_index.x()) < max_sensor_range) { + is_obstacle_facing_goal = (is_obstacle_facing_goal || true); + } + } + } + // fill in cost matrix for (int e_index = 0; e_index < GRID_LENGTH_E; e_index++) { // determine how many bins at this elevation angle would be equivalent to @@ -145,8 +167,8 @@ void getCostMatrix(const Histogram& histogram, const Eigen::Vector3f& goal, cons for (int z_index = 0; z_index < GRID_LENGTH_Z; z_index += step_size) { float obstacle_distance = histogram.get_dist(e_index, z_index); PolarPoint p_pol = histogramIndexToPolar(e_index, z_index, ALPHA_RES, 1.0f); // unit vector of current direction - - std::pair costs = costFunction(p_pol, obstacle_distance, goal, position, velocity, cost_params); + std::pair costs = costFunction(p_pol, obstacle_distance, goal, position, velocity, cost_params, + closest_pt, is_obstacle_facing_goal); cost_matrix(e_index, z_index) = costs.second; distance_matrix(e_index, z_index) = costs.first; } @@ -305,7 +327,8 @@ void padPolarMatrix(const Eigen::MatrixXf& matrix, unsigned int n_lines_padding, // cost function for every histogram cell std::pair costFunction(const PolarPoint& candidate_polar, float obstacle_distance, const Eigen::Vector3f& goal, const Eigen::Vector3f& position, - const Eigen::Vector3f& velocity, const costParameters& cost_params) { + const Eigen::Vector3f& velocity, const costParameters& cost_params, + const Eigen::Vector3f& closest_pt, const bool is_obstacle_facing_goal) { // Compute polar direction to goal and cartesian representation of current direction to evaluate const PolarPoint facing_goal = cartesianToPolarHistogram(goal, position); const Eigen::Vector3f candidate_velocity_cartesian = @@ -313,14 +336,25 @@ std::pair costFunction(const PolarPoint& candidate_polar, float ob const float angle_diff = angleDifference(candidate_polar.z, facing_goal.z); + const PolarPoint facing_line = cartesianToPolarHistogram(closest_pt, position); + const float angle_diff_to_line = angleDifference(candidate_polar.z, facing_line.z); + const float velocity_cost = cost_params.velocity_cost_param * (velocity.norm() - candidate_velocity_cartesian.normalized().dot(velocity)); - const float yaw_cost = cost_params.yaw_cost_param * angle_diff * angle_diff; + + float weight = 0.f; // yaw cost partition between back to line previous-current goal and goal + if (!is_obstacle_facing_goal) { + weight = 0.5f; + } + + const float yaw_cost = (1.f - weight) * cost_params.yaw_cost_param * angle_diff * angle_diff; + const float yaw_to_line_cost = weight * cost_params.yaw_cost_param * angle_diff_to_line * angle_diff_to_line; const float pitch_cost = cost_params.pitch_cost_param * (candidate_polar.e - facing_goal.e) * (candidate_polar.e - facing_goal.e); const float d = cost_params.obstacle_cost_param - obstacle_distance; const float distance_cost = obstacle_distance > 0 ? 5000.0f * (1 + d / sqrt(1 + d * d)) : 0.0f; - return std::pair(distance_cost, velocity_cost + yaw_cost + pitch_cost); + + return std::pair(distance_cost, velocity_cost + yaw_cost + yaw_to_line_cost + pitch_cost); } bool getSetpointFromPath(const std::vector& path, const ros::Time& path_generation_time, diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index ec1ce3811..e5e3050e7 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -18,6 +18,8 @@ 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_); + max_sensor_range_ = static_cast(config.max_sensor_range_); + min_sensor_range_ = static_cast(config.min_sensor_range_); } void StarPlanner::setParams(costParameters cost_params) { cost_params_ = cost_params; } @@ -31,6 +33,8 @@ void StarPlanner::setGoal(const Eigen::Vector3f& goal) { goal_ = goal; } void StarPlanner::setPointcloud(const pcl::PointCloud& cloud) { cloud_ = cloud; } +void StarPlanner::setClosestPointOnLine(const Eigen::Vector3f& closest_pt) { closest_pt_ = closest_pt; } + float StarPlanner::treeHeuristicFunction(int node_number) const { return (goal_ - tree_[node_number].getPosition()).norm() * tree_heuristic_weight_; } @@ -67,7 +71,7 @@ void StarPlanner::buildLookAheadTree() { cost_image_data.clear(); candidate_vector.clear(); getCostMatrix(histogram, goal_, origin_position, origin_velocity, cost_params_, smoothing_margin_degrees_, - cost_matrix, cost_image_data); + closest_pt_, max_sensor_range_, min_sensor_range_, cost_matrix, cost_image_data); getBestCandidatesFromCostMatrix(cost_matrix, children_per_node_, candidate_vector); // add candidates as nodes diff --git a/local_planner/src/nodes/waypoint_generator.cpp b/local_planner/src/nodes/waypoint_generator.cpp index c945fd32e..98d404a33 100644 --- a/local_planner/src/nodes/waypoint_generator.cpp +++ b/local_planner/src/nodes/waypoint_generator.cpp @@ -9,85 +9,203 @@ namespace avoidance { +WaypointGenerator::WaypointGenerator() : usm::StateMachine(PlannerState::LOITER) {} ros::Time WaypointGenerator::getSystemTime() { return ros::Time::now(); } -void WaypointGenerator::calculateWaypoint() { - ROS_DEBUG("\033[1;32m[WG] Generate Waypoint, current position: [%f, %f, %f].\033[0m", position_.x(), position_.y(), - position_.z()); - output_.waypoint_type = planner_info_.waypoint_type; - output_.linear_velocity_wp = Eigen::Vector3f(NAN, NAN, NAN); +using avoidance::PlannerState; +std::string toString(PlannerState state) { + std::string state_str = "unknown"; + switch (state) { + case PlannerState::TRY_PATH: + state_str = "TRY_PATH"; + break; + case PlannerState::ALTITUDE_CHANGE: + state_str = "ALTITUDE CHANGE"; + break; + case PlannerState::LOITER: + state_str = "LOITER"; + break; + case PlannerState::DIRECT: + state_str = "DIRECT"; + break; + } + return state_str; +} - // Timing - last_time_ = current_time_; - current_time_ = getSystemTime(); +PlannerState WaypointGenerator::chooseNextState(PlannerState currentState, usm::Transition transition) { + prev_slp_state_ = currentState; + state_changed_ = true; + + // clang-format off + USM_TABLE( + currentState, PlannerState::LOITER, + USM_STATE(transition, PlannerState::TRY_PATH, USM_MAP(usm::Transition::NEXT1, PlannerState::ALTITUDE_CHANGE); + USM_MAP(usm::Transition::NEXT2, PlannerState::DIRECT); + USM_MAP(usm::Transition::NEXT3, PlannerState::LOITER)); + USM_STATE(transition, PlannerState::LOITER, USM_MAP(usm::Transition::NEXT1, PlannerState::TRY_PATH)); + USM_STATE(transition, PlannerState::DIRECT, USM_MAP(usm::Transition::NEXT1, PlannerState::TRY_PATH); + USM_MAP(usm::Transition::NEXT2, PlannerState::ALTITUDE_CHANGE); + USM_MAP(usm::Transition::NEXT3, PlannerState::LOITER)); + USM_STATE(transition, PlannerState::ALTITUDE_CHANGE, USM_MAP(usm::Transition::NEXT1, PlannerState::TRY_PATH); + USM_MAP(usm::Transition::NEXT2, PlannerState::LOITER))); + // clang-format on +} - switch (planner_info_.waypoint_type) { - case hover: { - if (last_wp_type_ != hover) { - hover_position_ = position_; - } - output_.goto_position = hover_position_; - ROS_DEBUG("[WG] Hover at: [%f, %f, %f].", output_.goto_position.x(), output_.goto_position.y(), - output_.goto_position.z()); - getPathMsg(); +usm::Transition WaypointGenerator::runCurrentState() { + if (trigger_reset_) { + trigger_reset_ = false; + return usm::Transition::ERROR; + } + + usm::Transition t; + switch (getState()) { + case PlannerState::TRY_PATH: + t = runTryPath(); break; - } - 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(); - ROS_DEBUG("[WG] Using calculated tree\n"); - } else { - ROS_DEBUG("[WG] No valid tree, going straight"); - output_.waypoint_type = direct; - - // calculate the vehicle position on the line between the previous and - // current goal - Eigen::Vector2f u_prev_to_goal = (goal_ - prev_goal_).head<2>().normalized(); - Eigen::Vector2f prev_to_pos = (position_ - prev_goal_).head<2>(); - Eigen::Vector2f pos_2f = position_.head<2>(); - closest_pt_ = prev_goal_.head<2>() + (u_prev_to_goal * u_prev_to_goal.dot(prev_to_pos)); - - // if the vehicle is more than the cruise velocity away from the line - // previous to current goal, set temporary goal on the line entering - // at 60 degrees - if ((pos_2f - closest_pt_).norm() > planner_info_.cruise_velocity) { - float len = (pos_2f - closest_pt_).norm() * std::cos(DEG_TO_RAD * 60.0f) / std::sin(DEG_TO_RAD * 60.0f); - tmp_goal_.x() = closest_pt_.x() + len * u_prev_to_goal.x(); - tmp_goal_.y() = closest_pt_.y() + len * u_prev_to_goal.y(); - tmp_goal_.z() = goal_.z(); - - Eigen::Vector3f dir = (tmp_goal_ - position_).normalized(); - output_.goto_position = position_ + dir; - } else { - goStraight(); - } - } - getPathMsg(); + case PlannerState::ALTITUDE_CHANGE: + t = runAltitudeChange(); break; - } - case direct: { - ROS_DEBUG("[WG] No obstacle ahead, going straight"); - goStraight(); - getPathMsg(); + case PlannerState::LOITER: + t = runLoiter(); break; + + case PlannerState::DIRECT: + t = runDirect(); + break; + } + state_changed_ = false; + return t; +} + +usm::Transition WaypointGenerator::runTryPath() { + Eigen::Vector3f setpoint = position_; + const bool tree_available = getSetpointFromPath(planner_info_.path_node_positions, planner_info_.last_path_time, + planner_info_.cruise_velocity, setpoint); + output_.goto_position = position_ + (setpoint - position_).normalized(); + getPathMsg(); + + if (isAltitudeChange()) { + return usm::Transition::NEXT1; // ALTITUDE_CHANGE + } else if (tree_available) { + ROS_DEBUG("[WG] Using calculated tree\n"); + return usm::Transition::REPEAT; + } else if (loiter_) { + return usm::Transition::NEXT3; // LOITER + } else { + return usm::Transition::NEXT2; // DIRECT + } +} + +usm::Transition WaypointGenerator::runAltitudeChange() { + if (state_changed_) { + yaw_reach_height_rad_ = curr_yaw_rad_; + change_altitude_pos_ = position_; + } + if (nav_state_ == NavigationState::offboard) { + // goto_position is a unit vector pointing straight up/down from current + // location + output_.goto_position = position_; + goal_.x() = position_.x(); // Needed so adaptSpeed can clamp to goal + goal_.y() = position_.y(); + + // Only move the setpoint if drone is in the air + if (is_airborne_) { + // Ascend/Descend to goal altitude + if (position_.z() <= goal_.z()) { + output_.goto_position.z() += 1.0f; + } else { + output_.goto_position.z() -= 1.0f; + } } + } else { + if (velocity_.normXY() > 0.5f) { + // First decelerate + output_.linear_velocity_wp.topRows<2>() = Eigen::Vector2f::Zero(); + output_.linear_velocity_wp.z() = NAN; + output_.position_wp.topRows<2>() = Eigen::Vector2f(NAN, NAN); + output_.position_wp.z() = position_.z(); + output_.goto_position = position_; + output_.adapted_goto_position = position_; + output_.smoothed_goto_position = position_; + change_altitude_pos_ = position_; + } else { + // Change altitude + output_.goto_position = goal_; - case reachHeight: { - ROS_DEBUG("[WG] Reaching height first"); - if (last_wp_type_ != reachHeight) { - yaw_reach_height_rad_ = curr_yaw_rad_; - change_altitude_pos_ = position_; + if (auto_land_) { + output_.goto_position.topRows<2>() = change_altitude_pos_.topRows<2>(); + output_.adapted_goto_position = change_altitude_pos_; + output_.smoothed_goto_position = change_altitude_pos_; + output_.adapted_goto_position.z() = goal_.z(); + output_.smoothed_goto_position.z() = goal_.z(); } - reachGoalAltitudeFirst(); - getPathMsg(); - break; + output_.linear_velocity_wp = desired_vel_; } } - last_wp_type_ = planner_info_.waypoint_type; + getPathMsg(); + + if (isAltitudeChange()) { + return usm::Transition::REPEAT; + } else if (loiter_) { + return usm::Transition::NEXT2; // LOITER + } else { + return usm::Transition::NEXT1; // TRY_PATH + } +} +usm::Transition WaypointGenerator::runLoiter() { + if (state_changed_ || hover_position_.array().hasNaN()) { + hover_position_ = position_; + } + output_.goto_position = hover_position_; + ROS_DEBUG("[WG] Hover at: [%f, %f, %f].", output_.goto_position.x(), output_.goto_position.y(), + output_.goto_position.z()); + getPathMsg(); + + if (loiter_) { + return usm::Transition::REPEAT; + } else { + return usm::Transition::NEXT1; + } +} + +usm::Transition WaypointGenerator::runDirect() { + Eigen::Vector3f dir = (goal_ - position_).normalized(); + output_.goto_position = position_ + dir; + + ROS_DEBUG("[WG] Going straight to selected waypoint: [%f, %f, %f].", output_.goto_position.x(), + output_.goto_position.y(), output_.goto_position.z()); + + getPathMsg(); + Eigen::Vector3f setpoint; + if (getSetpointFromPath(planner_info_.path_node_positions, planner_info_.last_path_time, + planner_info_.cruise_velocity, setpoint)) { + return usm::Transition::NEXT1; // TRY_PATH + } else if (isAltitudeChange()) { + return usm::Transition::NEXT2; // ALTITUDE_CHANGE + } else if (loiter_) { + return usm::Transition::NEXT3; // LOITER + } else { + return usm::Transition::REPEAT; + } +} + +void WaypointGenerator::calculateWaypoint() { + ROS_DEBUG("\033[1;32m[WG] Generate Waypoint, current position: [%f, %f, %f].\033[0m", position_.x(), position_.y(), + position_.z()); + output_.linear_velocity_wp = Eigen::Vector3f(NAN, NAN, NAN); + + // Timing + last_time_ = current_time_; + current_time_ = getSystemTime(); + + iterateOnce(); + output_.waypoint_type = getState(); + if (getState() != prev_slp_state_) { + std::string state_str = toString(getState()); + ROS_DEBUG("\033[1;36m [WGN] Update to %s state \n \033[0m", state_str.c_str()); + } } void WaypointGenerator::setFOV(int i, const FOV& fov) { @@ -113,10 +231,8 @@ void WaypointGenerator::updateState(const Eigen::Vector3f& act_pose, const Eigen is_land_waypoint_ = is_land_waypoint; is_takeoff_waypoint_ = is_takeoff_waypoint; desired_vel_ = desired_vel; + loiter_ = stay; - if (stay) { - planner_info_.waypoint_type = hover; - } is_airborne_ = is_airborne; // Initialize the smoothing point to current location, if it is undefined or @@ -126,7 +242,7 @@ void WaypointGenerator::updateState(const Eigen::Vector3f& act_pose, const Eigen smoothed_goto_location_velocity_ = Eigen::Vector3f::Zero(); setpoint_yaw_rad_ = curr_yaw_rad_; setpoint_yaw_velocity_ = 0.f; - reach_altitude_ = false; + reach_altitude_offboard_ = false; } // If we're changing altitude by Firmware setpoints, keep reinitializing the smoothing @@ -136,15 +252,6 @@ void WaypointGenerator::updateState(const Eigen::Vector3f& act_pose, const Eigen } } -// if there isn't any obstacle in front of the UAV, increase cruising speed -void WaypointGenerator::goStraight() { - Eigen::Vector3f dir = (goal_ - position_).normalized(); - output_.goto_position = position_ + dir; - - ROS_DEBUG("[WG] Going straight to selected waypoint: [%f, %f, %f].", output_.goto_position.x(), - output_.goto_position.y(), output_.goto_position.z()); -} - void WaypointGenerator::transformPositionToVelocityWaypoint() { output_.linear_velocity_wp = output_.position_wp - position_; output_.angular_velocity_wp.x() = 0.0f; @@ -152,51 +259,6 @@ void WaypointGenerator::transformPositionToVelocityWaypoint() { output_.angular_velocity_wp.z() = getAngularVelocity(setpoint_yaw_rad_, curr_yaw_rad_); } -// when taking off, first publish waypoints to reach the goal altitude -void WaypointGenerator::reachGoalAltitudeFirst() { - if (nav_state_ == NavigationState::offboard) { - // goto_position is a unit vector pointing straight up/down from current - // location - output_.goto_position = position_; - goal_.x() = position_.x(); // Needed so adaptSpeed can clamp to goal - goal_.y() = position_.y(); - - // Only move the setpoint if drone is in the air - if (is_airborne_) { - // Ascend/Descend to goal altitude - if (position_.z() <= goal_.z()) { - output_.goto_position.z() += 1.0f; - } else { - output_.goto_position.z() -= 1.0f; - } - } - } else { - if (velocity_.normXY() > 0.5f) { - // First decelerate - output_.linear_velocity_wp.topRows<2>() = Eigen::Vector2f::Zero(); - output_.linear_velocity_wp.z() = NAN; - output_.position_wp.topRows<2>() = Eigen::Vector2f(NAN, NAN); - output_.position_wp.z() = position_.z(); - output_.goto_position = position_; - output_.adapted_goto_position = position_; - output_.smoothed_goto_position = position_; - change_altitude_pos_ = position_; - } else { - // Change altitude - output_.goto_position = goal_; - - if (auto_land_) { - output_.goto_position.topRows<2>() = change_altitude_pos_.topRows<2>(); - output_.adapted_goto_position = change_altitude_pos_; - output_.smoothed_goto_position = change_altitude_pos_; - output_.adapted_goto_position.z() = goal_.z(); - output_.smoothed_goto_position.z() = goal_.z(); - } - output_.linear_velocity_wp = desired_vel_; - } - } -} - void WaypointGenerator::smoothWaypoint(float dt) { // If the smoothing speed is set to zero, dont smooth, aka use adapted // waypoint directly @@ -242,7 +304,7 @@ void WaypointGenerator::nextSmoothYaw(float dt) { float desired_setpoint_yaw_rad = (position_ - output_.goto_position).normXY() > 0.1f ? nextYaw(position_, output_.goto_position) : curr_yaw_rad_; - if (planner_info_.waypoint_type == reachHeight) { + if (getState() == PlannerState::ALTITUDE_CHANGE) { desired_setpoint_yaw_rad = yaw_reach_height_rad_; } @@ -283,7 +345,7 @@ void WaypointGenerator::adaptSpeed() { setpoint_yaw_rad_ = heading_at_goal_rad_; } else { // Scale the speed by a factor that is 0 if the waypoint is outside the FOV - if (output_.waypoint_type != reachHeight) { + if (getState() != PlannerState::ALTITUDE_CHANGE) { PolarPoint p_pol_fcu = cartesianToPolarFCU(output_.goto_position, position_); p_pol_fcu.e -= curr_pitch_deg_; p_pol_fcu.z -= RAD_TO_DEG * curr_yaw_rad_; @@ -328,12 +390,11 @@ void WaypointGenerator::getPathMsg() { } waypointResult WaypointGenerator::getWaypoints() { - changeAltitude(); calculateWaypoint(); return output_; } -void WaypointGenerator::changeAltitude() { +bool WaypointGenerator::isAltitudeChange() { bool rtl_descend = false; bool rtl_climb = false; if (position_.z() > (goal_.z() - 0.8f)) { @@ -347,7 +408,7 @@ void WaypointGenerator::changeAltitude() { rtl_climb = false; } - const bool offboard_goal_altitude_not_reached = nav_state_ == NavigationState::offboard && !reach_altitude_; + const bool offboard_goal_altitude_not_reached = nav_state_ == NavigationState::offboard && !reach_altitude_offboard_; const bool auto_takeoff = nav_state_ == NavigationState::auto_takeoff || (nav_state_ == NavigationState::mission && is_takeoff_waypoint_) || (nav_state_ == NavigationState::auto_rtl && rtl_climb); @@ -356,17 +417,19 @@ void WaypointGenerator::changeAltitude() { (nav_state_ == NavigationState::auto_rtl && rtl_descend); const bool need_to_change_altitude = offboard_goal_altitude_not_reached || auto_takeoff || auto_land_; if (need_to_change_altitude) { - planner_info_.waypoint_type = reachHeight; + return true; if (nav_state_ == NavigationState::offboard) { if (position_.z() > goal_.z()) { - reach_altitude_ = true; - planner_info_.waypoint_type = direct; + reach_altitude_offboard_ = true; + return false; } } ROS_INFO("\033[1;35m[OA] Reach height first \033[0m"); } + + return false; } void WaypointGenerator::setPlannerInfo(const avoidanceOutput& input) { planner_info_ = input; } diff --git a/local_planner/src/nodes/waypoint_generator.cpp.dot b/local_planner/src/nodes/waypoint_generator.cpp.dot new file mode 100644 index 000000000..d3c8faf76 --- /dev/null +++ b/local_planner/src/nodes/waypoint_generator.cpp.dot @@ -0,0 +1,12 @@ +digraph { + "TRY_PATH" -> "ALTITUDE_CHANGE" [label="NEXT1", style="solid", weight=1] + "TRY_PATH" -> "DIRECT" [label="NEXT2", style="solid", weight=1] + "TRY_PATH" -> "LOITER" [label="NEXT3\nERROR", style="solid", weight=1] + "LOITER" -> "TRY_PATH" [label="NEXT1", style="solid", weight=1] + "LOITER" -> "LOITER" [label="ERROR", style="dotted", weight=0.1] + "DIRECT" -> "TRY_PATH" [label="NEXT1", style="solid", weight=1] + "DIRECT" -> "ALTITUDE_CHANGE" [label="NEXT2", style="solid", weight=1] + "DIRECT" -> "LOITER" [label="NEXT3\nERROR", style="solid", weight=1] + "ALTITUDE_CHANGE" -> "TRY_PATH" [label="NEXT1", style="solid", weight=1] + "ALTITUDE_CHANGE" -> "LOITER" [label="NEXT2\nERROR", style="solid", weight=1] +} \ No newline at end of file diff --git a/local_planner/test/test_local_planner.cpp b/local_planner/test/test_local_planner.cpp index afa5143f3..c526f187d 100644 --- a/local_planner/test/test_local_planner.cpp +++ b/local_planner/test/test_local_planner.cpp @@ -56,7 +56,6 @@ TEST_F(LocalPlannerTests, no_obstacles) { // THEN: it shouldn't find any obstacles avoidanceOutput output = planner.getAvoidanceOutput(); - EXPECT_FALSE(output.obstacle_ahead); // AND: the scan should contain NANs outside the FOV and out of range data inside sensor_msgs::LaserScan scan; @@ -121,7 +120,6 @@ TEST_F(LocalPlannerTests, all_obstacles) { // THEN: it should detect the obstacle and go left avoidanceOutput output = planner.getAvoidanceOutput(); - EXPECT_TRUE(output.obstacle_ahead); ASSERT_GE(output.path_node_positions.size(), 2); float node_max_y = 0.f; float node_min_y = 0.f; @@ -182,7 +180,6 @@ TEST_F(LocalPlannerTests, obstacles_right) { // THEN: it should modify the path to the left avoidanceOutput output = planner.getAvoidanceOutput(); - EXPECT_TRUE(output.obstacle_ahead); ASSERT_GE(output.path_node_positions.size(), 2); float node_max_y = 0.f; for (auto it = output.path_node_positions.rbegin(); it != output.path_node_positions.rend(); ++it) { @@ -239,7 +236,6 @@ TEST_F(LocalPlannerTests, obstacles_left) { // THEN: it should modify the path to the right avoidanceOutput output = planner.getAvoidanceOutput(); - EXPECT_TRUE(output.obstacle_ahead); ASSERT_GE(output.path_node_positions.size(), 2); float node_min_y = 0.f; for (auto it = output.path_node_positions.rbegin(); it != output.path_node_positions.rend(); ++it) { diff --git a/local_planner/test/test_planner_functions.cpp b/local_planner/test/test_planner_functions.cpp index 6ace81a05..644b413c1 100644 --- a/local_planner/test/test_planner_functions.cpp +++ b/local_planner/test/test_planner_functions.cpp @@ -415,10 +415,13 @@ TEST(PlannerFunctions, getCostMatrixNoObstacles) { Eigen::MatrixXf cost_matrix; Histogram histogram = Histogram(ALPHA_RES); float smoothing_radius = 30.f; + const float max_sensor_range = 15.f; + const float min_sensor_range = 0.2f; // WHEN: we calculate the cost matrix from the input data std::vector cost_image_data; - getCostMatrix(histogram, goal, position, velocity, cost_params, smoothing_radius, cost_matrix, cost_image_data); + getCostMatrix(histogram, goal, position, velocity, cost_params, smoothing_radius, goal, max_sensor_range, + min_sensor_range, cost_matrix, cost_image_data); // THEN: The minimum cost should be in the direction of the goal PolarPoint best_pol = cartesianToPolarHistogram(goal, position); @@ -496,8 +499,8 @@ TEST(PlannerFunctions, CostfunctionGoalCost) { // WHEN: we calculate the cost of one cell for the same scenario but with two // different goals - cost_1 = costFunction(candidate_1, obstacle_distance, goal_1, position, velocity, cost_params); - cost_2 = costFunction(candidate_1, obstacle_distance, goal_2, position, velocity, cost_params); + cost_1 = costFunction(candidate_1, obstacle_distance, goal_1, position, velocity, cost_params, goal_1, true); + cost_2 = costFunction(candidate_1, obstacle_distance, goal_2, position, velocity, cost_params, goal_2, true); // THEN: The cost in the case where the goal is in the cell direction should // be lower @@ -523,9 +526,9 @@ TEST(PlannerFunctions, CostfunctionDistanceCost) { // WHEN: we calculate the cost of one cell for the same scenario but with two // different obstacle distance - cost_1 = costFunction(candidate_1, distance_1, goal, position, velocity, cost_params); - cost_2 = costFunction(candidate_1, distance_2, goal, position, velocity, cost_params); - cost_3 = costFunction(candidate_1, distance_3, goal, position, velocity, cost_params); + cost_1 = costFunction(candidate_1, distance_1, goal, position, velocity, cost_params, goal, true); + cost_2 = costFunction(candidate_1, distance_2, goal, position, velocity, cost_params, goal, true); + cost_3 = costFunction(candidate_1, distance_3, goal, position, velocity, cost_params, goal, true); // THEN: The distance cost for no obstacle should be zero and the distance // cost for the closer obstacle should be bigger @@ -553,8 +556,8 @@ TEST(PlannerFunctions, CostfunctionVelocityCost) { // WHEN: we calculate the cost of one cell for the same scenario but with two // different initial velocities - cost_1 = costFunction(candidate_1, obstacle_distance, goal, position, velocity_1, cost_params); - cost_2 = costFunction(candidate_1, obstacle_distance, goal, position, velocity_2, cost_params); + cost_1 = costFunction(candidate_1, obstacle_distance, goal, position, velocity_1, cost_params, goal, true); + cost_2 = costFunction(candidate_1, obstacle_distance, goal, position, velocity_2, cost_params, goal, true); // THEN: The cost in the case where the initial heading is closer to the // candidate should be lower diff --git a/local_planner/test/test_star_planner.cpp b/local_planner/test/test_star_planner.cpp index dbc001638..d4554b2a9 100644 --- a/local_planner/test/test_star_planner.cpp +++ b/local_planner/test/test_star_planner.cpp @@ -55,6 +55,7 @@ class StarPlannerTests : public ::testing::Test { star_planner.setPointcloud(cloud); star_planner.setPose(position, velocity); star_planner.setGoal(goal); + star_planner.setClosestPointOnLine(goal); } void TearDown() override {} }; diff --git a/local_planner/test/test_waypoint_generator.cpp b/local_planner/test/test_waypoint_generator.cpp index 3ad7018cb..3891fb214 100644 --- a/local_planner/test/test_waypoint_generator.cpp +++ b/local_planner/test/test_waypoint_generator.cpp @@ -27,8 +27,6 @@ class WaypointGeneratorTests : public ::testing::Test, public WaypointGenerator void SetUp() override { ros::Time::init(); - avoidance_output.waypoint_type = direct; - avoidance_output.obstacle_ahead = false; avoidance_output.cruise_velocity = 1.0; avoidance_output.last_path_time = ros::Time(0.28); @@ -69,7 +67,8 @@ class WaypointGeneratorTests : public ::testing::Test, public WaypointGenerator TEST_F(WaypointGeneratorTests, reachAltitudeTest) { // GIVEN: a waypoint of type goFast and the vehicle has not yet reached the // goal altiude - avoidance_output.waypoint_type = reachHeight; + ASSERT_EQ(PlannerState::LOITER, getState()); + goal << 0.f, 0.f, 5.f; setPlannerInfo(avoidance_output); double time_sec = 0.0; @@ -83,35 +82,9 @@ TEST_F(WaypointGeneratorTests, reachAltitudeTest) { updateState(position, q, goal, prev_goal, velocity, stay, is_airborne, nav_state, is_land_waypoint, is_takeoff_waypoint, desired_velocity); waypointResult result = getWaypoints(); + result = getWaypoints(); - // THEN: we expect the goto position to point straight up - EXPECT_NEAR(goal.x(), result.goto_position.x(), 0.1); - EXPECT_NEAR(goal.y(), result.goto_position.y(), 0.1); - EXPECT_LT(position.z(), result.goto_position.z()); - - // THEN: we expect the adapted goto position to be between goal and drone in z - EXPECT_GT(result.adapted_goto_position.z(), position.z()); - EXPECT_LT(result.adapted_goto_position.z(), goal.z()); - - // THEN: we expect the adapted goto position to be close to the drone location - // in xy - EXPECT_NEAR(position.x(), result.adapted_goto_position.x(), 0.1); - EXPECT_NEAR(position.y(), result.adapted_goto_position.y(), 0.1); - - // THEN: we expect the smoothed goto position to be the same as the drone - // location - // (first iteration of smoothing) - EXPECT_NEAR(position.x(), result.smoothed_goto_position.x(), 0.1); - EXPECT_NEAR(position.y(), result.smoothed_goto_position.y(), 0.1); - EXPECT_NEAR(position.z(), result.smoothed_goto_position.z(), 0.1); - - // THEN: we expect the smoothed goto position to be the position waypoint, - // since smoothing was enabled - EXPECT_EQ(result.smoothed_goto_position.x(), result.position_wp.x()); - EXPECT_EQ(result.smoothed_goto_position.y(), result.position_wp.y()); - EXPECT_EQ(result.smoothed_goto_position.z(), result.position_wp.z()); - - ASSERT_TRUE(std::isfinite(result.linear_velocity_wp.z())); + ASSERT_EQ(PlannerState::ALTITUDE_CHANGE, getState()); // WHEN: we generate subsequent waypoints for (size_t i = 0; i < 10; i++) { @@ -122,6 +95,8 @@ TEST_F(WaypointGeneratorTests, reachAltitudeTest) { is_takeoff_waypoint, desired_velocity); waypointResult result = getWaypoints(); + ASSERT_EQ(PlannerState::ALTITUDE_CHANGE, getState()); + // THEN: we expect the goto location to point straight up EXPECT_NEAR(position.x(), result.goto_position.x(), 0.1); EXPECT_NEAR(position.y(), result.goto_position.y(), 0.1); @@ -163,7 +138,6 @@ TEST_F(WaypointGeneratorTests, reachAltitudeTest) { TEST_F(WaypointGeneratorTests, goStraightTest) { // GIVEN: a waypoint of type goStraight - avoidance_output.waypoint_type = direct; is_takeoff_waypoint = false; desired_velocity.z() = NAN; setPlannerInfo(avoidance_output); @@ -173,6 +147,11 @@ TEST_F(WaypointGeneratorTests, goStraightTest) { float pos_sp_to_goal_prev = 1000.0f; double time_sec = time.toSec(); + updateState(position, q, goal, prev_goal, velocity, stay, is_airborne, nav_state, is_land_waypoint, + is_takeoff_waypoint, desired_velocity); + + waypointResult result = getWaypoints(); + // WHEN: we generate waypoints for (size_t i = 0; i < 10; i++) { time_sec += 0.03; @@ -181,6 +160,9 @@ TEST_F(WaypointGeneratorTests, goStraightTest) { is_takeoff_waypoint, desired_velocity); waypointResult result = getWaypoints(); + + ASSERT_EQ(PlannerState::DIRECT, getState()); + float goto_to_goal = (goal - result.goto_position).norm(); float adapted_to_goal = (goal - result.adapted_goto_position).norm(); float pos_sp_to_goal = (goal - result.position_wp).norm(); @@ -208,17 +190,19 @@ TEST_F(WaypointGeneratorTests, goStraightTest) { TEST_F(WaypointGeneratorTests, hoverTest) { // GIVEN: a waypoint of type hover + ASSERT_EQ(PlannerState::LOITER, getState()); // first run one the waypoint generator such that smoothed_goto_location_ gets // initialize setPlannerInfo(avoidance_output); double time_sec = 0.0; time = ros::Time(time_sec); + stay = true; updateState(position, q, goal, prev_goal, velocity, stay, is_airborne, nav_state, is_land_waypoint, is_takeoff_waypoint, desired_velocity); waypointResult result = getWaypoints(); + ASSERT_EQ(PlannerState::LOITER, getState()); - avoidance_output.waypoint_type = hover; setPlannerInfo(avoidance_output); time_sec += 0.033; time = ros::Time(time_sec); @@ -227,6 +211,7 @@ TEST_F(WaypointGeneratorTests, hoverTest) { // WHEN: we generate waypoints result = getWaypoints(); + ASSERT_EQ(PlannerState::LOITER, getState()); // THEN: we expect the position waypoint to be the same as the current vehicle // position @@ -251,7 +236,6 @@ TEST_F(WaypointGeneratorTests, hoverTest) { TEST_F(WaypointGeneratorTests, trypathTest) { // GIVEN: a waypoint of type tryPath - avoidance_output.waypoint_type = tryPath; setPlannerInfo(avoidance_output); float goto_to_goal_prev = 1000.0f; diff --git a/tools/check_state_machine_diagrams.sh b/tools/check_state_machine_diagrams.sh index d0eb92fa1..29cd98464 100755 --- a/tools/check_state_machine_diagrams.sh +++ b/tools/check_state_machine_diagrams.sh @@ -3,6 +3,7 @@ # Generate the existing state machine diagrams again python generate_flow_diagram.py ../avoidance/test/test_usm.cpp python generate_flow_diagram.py ../safe_landing_planner/src/nodes/waypoint_generator.cpp +python generate_flow_diagram.py ../local_planner/src/nodes/waypoint_generator.cpp # Print the diff with the remote branch (empty if no diff) git --no-pager diff -U0 --color From 000650c8e0ddf97ea3adb95374c0db47a3c04960 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Mon, 22 Jul 2019 18:34:11 +0200 Subject: [PATCH 04/28] 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 | 9 +++++- local_planner/src/nodes/star_planner.cpp | 28 ++++++++++++++----- local_planner/src/nodes/tree_node.cpp | 17 ++++++----- 8 files changed, 56 insertions(+), 21 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 21eda61ae..b39c6f39b 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 b68f07909..3f69eab60 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 @@ -36,6 +37,7 @@ class StarPlanner { Eigen::Vector3f velocity_ = Eigen::Vector3f(NAN, NAN, NAN); Eigen::Vector3f closest_pt_ = Eigen::Vector3f(NAN, NAN, NAN); costParameters cost_params_; + simulation_limits lims_; protected: /** @@ -56,8 +58,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 125e0ccdb..1475a57b2 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -136,7 +136,14 @@ void LocalPlanner::determineStrategy() { getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, closest_pt_, max_sensor_range_, min_sensor_range_, cost_matrix_, cost_image_data_); - star_planner_->setParams(cost_params_); + simulation_limits lims; + setDefaultPx4Parameters(); // TODO: remove but make sure they're set! + lims.max_z_velocity = px4_.param_mpc_z_vel_max_up; + lims.min_z_velocity = -1.0f * px4_.param_mpc_vel_max_dn; + lims.max_xy_velocity_norm = px4_.param_mpc_xy_cruise; + lims.max_acceleration_norm = px4_.param_mpc_acc_hor; + lims.max_jerk_norm = px4_.param_mpc_jerk_max; + star_planner_->setParams(cost_params_, lims); star_planner_->setPointcloud(final_cloud_); star_planner_->setClosestPointOnLine(closest_pt_); diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index e5e3050e7..ade7e1fcb 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -22,7 +22,10 @@ void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerN min_sensor_range_ = static_cast(config.min_sensor_range_); } -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; @@ -53,7 +56,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; @@ -81,14 +89,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; @@ -96,7 +110,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 65bcc6c808e98f0be6234316d58a57af44c9fd37 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Mon, 29 Jul 2019 09:14:25 +0200 Subject: [PATCH 05/28] 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 ade7e1fcb..9b7e74d1d 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -90,7 +90,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 d4554b2a9..a20e205df 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 c04e945b1e4e01c03e7de31dd48737f645e70b73 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Tue, 30 Jul 2019 10:50:34 +0200 Subject: [PATCH 06/28] 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 | 10 ++--- .../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 | 14 +++--- 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, 138 insertions(+), 132 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 372faa866..360491f8a 100644 --- a/local_planner/include/local_planner/avoidance_output.h +++ b/local_planner/include/local_planner/avoidance_output.h @@ -10,13 +10,9 @@ namespace avoidance { enum waypoint_choice { hover, tryPath, direct, reachHeight }; struct avoidanceOutput { - float cruise_velocity; // mission cruise velocity + float cruise_velocity; // mission cruise velocity + float tree_node_duration; ros::Time last_path_time; // finish built time for the VFH+* tree - - 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 b39c6f39b..34328f1c9 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_; @@ -182,11 +183,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 b3d3a071c..901730fed 100644 --- a/local_planner/include/local_planner/local_planner_visualization.h +++ b/local_planner/include/local_planner/local_planner_visualization.h @@ -37,11 +37,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 93be83ebb..d57c7fa91 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -150,11 +150,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 3f69eab60..29afca6c2 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; float max_sensor_range_ = 15.f; float min_sensor_range_ = 0.2f; @@ -48,7 +48,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 1475a57b2..e247c47dd 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(); @@ -209,10 +210,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) { @@ -238,8 +239,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 1741449c4..ba7aa6445 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 2066ef89c..bd77bd65b 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -357,33 +357,36 @@ std::pair costFunction(const PolarPoint& candidate_polar, float ob return std::pair(distance_cost, velocity_cost + yaw_cost + yaw_to_line_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 9b7e74d1d..67ed71e39 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_); @@ -61,7 +61,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; @@ -91,7 +91,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" @@ -110,7 +110,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; @@ -139,24 +139,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 98d404a33..66c90e3b5 100644 --- a/local_planner/src/nodes/waypoint_generator.cpp +++ b/local_planner/src/nodes/waypoint_generator.cpp @@ -80,10 +80,10 @@ usm::Transition WaypointGenerator::runCurrentState() { } usm::Transition WaypointGenerator::runTryPath() { - Eigen::Vector3f setpoint = position_; - const bool tree_available = 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(); + const bool tree_available = interpolateBetweenSetpoints(planner_info_.path_node_setpoints, planner_info_.last_path_time, + planner_info_.tree_node_duration, setpoint); + output_.goto_position = position_ + setpoint; getPathMsg(); if (isAltitudeChange()) { @@ -178,9 +178,9 @@ usm::Transition WaypointGenerator::runDirect() { output_.goto_position.y(), output_.goto_position.z()); getPathMsg(); - Eigen::Vector3f setpoint; - if (getSetpointFromPath(planner_info_.path_node_positions, planner_info_.last_path_time, - planner_info_.cruise_velocity, setpoint)) { + Eigen::Vector3f setpoint = Eigen::Vector3f::Zero(); + if (interpolateBetweenSetpoints(planner_info_.path_node_setpoints, planner_info_.last_path_time, + planner_info_.tree_node_duration, setpoint)) { return usm::Transition::NEXT1; // TRY_PATH } else if (isAltitudeChange()) { return usm::Transition::NEXT2; // ALTITUDE_CHANGE diff --git a/local_planner/test/test_local_planner.cpp b/local_planner/test/test_local_planner.cpp index c526f187d..eecb22a91 100644 --- a/local_planner/test/test_local_planner.cpp +++ b/local_planner/test/test_local_planner.cpp @@ -120,14 +120,23 @@ TEST_F(LocalPlannerTests, all_obstacles) { // THEN: it should detect the obstacle and go left avoidanceOutput output = planner.getAvoidanceOutput(); - 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; @@ -180,9 +189,9 @@ TEST_F(LocalPlannerTests, obstacles_right) { // THEN: it should modify the path to the left avoidanceOutput output = planner.getAvoidanceOutput(); - 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(); @@ -236,9 +245,9 @@ TEST_F(LocalPlannerTests, obstacles_left) { // THEN: it should modify the path to the right avoidanceOutput output = planner.getAvoidanceOutput(); - 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 644b413c1..f85ff3de5 100644 --- a/local_planner/test/test_planner_functions.cpp +++ b/local_planner/test/test_planner_functions.cpp @@ -157,7 +157,7 @@ TEST(PlannerFunctions, compressHistogramElevation) { } } -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; @@ -168,7 +168,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); @@ -179,10 +179,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 a20e205df..2c588061b 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 3891fb214..72517a2b0 100644 --- a/local_planner/test/test_waypoint_generator.cpp +++ b/local_planner/test/test_waypoint_generator.cpp @@ -43,7 +43,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 07589db8d80f713d5c40e358955506d6222e2d91 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Tue, 30 Jul 2019 16:28:11 +0200 Subject: [PATCH 07/28] 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 | 37 ++++++++++--------- .../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, 56 insertions(+), 41 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 29afca6c2..836ee9432 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; float max_sensor_range_ = 15.f; float min_sensor_range_ = 0.2f; @@ -60,7 +61,7 @@ class StarPlanner { * @param[in] cost_params, parameters for the histogram cost function * @param[in] simulation limits defining maximum acceleration, velocity, and jerk **/ - void setParams(const costParameters& cost_params, const simulation_limits& limits); + void setParams(const costParameters& cost_params, const simulation_limits& limits, float acc_rad); /** * @brief setter method for star_planner pointcloud 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 e247c47dd..7eb6eecf3 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -134,23 +134,23 @@ void LocalPlanner::determineStrategy() { } if (!polar_histogram_.isEmpty()) { - getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, closest_pt_, - max_sensor_range_, min_sensor_range_, cost_matrix_, cost_image_data_); - - simulation_limits lims; - setDefaultPx4Parameters(); // TODO: remove but make sure they're set! - lims.max_z_velocity = px4_.param_mpc_z_vel_max_up; - lims.min_z_velocity = -1.0f * px4_.param_mpc_vel_max_dn; - lims.max_xy_velocity_norm = px4_.param_mpc_xy_cruise; - lims.max_acceleration_norm = px4_.param_mpc_acc_hor; - lims.max_jerk_norm = px4_.param_mpc_jerk_max; - star_planner_->setParams(cost_params_, lims); - star_planner_->setPointcloud(final_cloud_); - star_planner_->setClosestPointOnLine(closest_pt_); - - // build search tree - star_planner_->buildLookAheadTree(); - last_path_time_ = ros::Time::now(); + getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, closest_pt_, + max_sensor_range_, min_sensor_range_, cost_matrix_, cost_image_data_); + + simulation_limits lims; + setDefaultPx4Parameters(); // TODO: remove but make sure they're set! + lims.max_z_velocity = px4_.param_mpc_z_vel_max_up; + lims.min_z_velocity = -1.0f * px4_.param_mpc_z_vel_max_dn; + lims.max_xy_velocity_norm = px4_.param_mpc_xy_cruise; + lims.max_acceleration_norm = px4_.param_mpc_acc_hor; + lims.max_jerk_norm = px4_.param_mpc_jerk_max; + star_planner_->setParams(cost_params_, lims, px4_.param_nav_acc_rad); + star_planner_->setPointcloud(final_cloud_); + star_planner_->setClosestPointOnLine(closest_pt_); + + // build search tree + star_planner_->buildLookAheadTree(); + last_path_time_ = ros::Time::now(); } } @@ -201,12 +201,13 @@ void LocalPlanner::setDefaultPx4Parameters() { px4_.param_acc_up_max = 10.f; px4_.param_mpc_z_vel_max_up = 3.f; px4_.param_mpc_acc_down_max = 10.f; - px4_.param_mpc_vel_max_dn = 1.f; + px4_.param_mpc_z_vel_max_dn = 1.f; px4_.param_mpc_acc_hor = 5.f; px4_.param_mpc_xy_cruise = 3.f; px4_.param_mpc_tko_speed = 1.f; px4_.param_mpc_land_speed = 0.7f; px4_.param_mpc_col_prev_d = 4.f; + px4_.param_nav_acc_rad = 2.f; } void LocalPlanner::getTree(std::vector& 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 ba7aa6445..63b061c13 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 67ed71e39..9208b99c9 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -22,9 +22,10 @@ void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerN min_sensor_range_ = static_cast(config.min_sensor_range_); } -void StarPlanner::setParams(const costParameters& cost_params, const simulation_limits& limits) { +void StarPlanner::setParams(const costParameters& cost_params, const simulation_limits& limits, float acc_rad) { cost_params_ = cost_params; lims_ = limits; + acceptance_radius_ = acc_rad; } void StarPlanner::setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel) { @@ -93,12 +94,6 @@ void StarPlanner::buildLookAheadTree() { TrajectorySimulator sim(lims_, state, 0.05f); // todo: parameterize simulation step size [s] std::vector trajectory = sim.generate_trajectory(candidate.toEigen(), tree_node_duration_); - // Ignore node if it brings us farther away from the goal - // todo: this breaks being able to get out of concave obstacles! But it helps to not "overplan" - if ((trajectory.back().position - goal_).norm() > (trajectory.front().position - goal_).norm()) { - continue; - } - // check if another close node has been added int close_nodes = 0; for (size_t i = 0; i < tree_.size(); i++) { @@ -127,6 +122,14 @@ void StarPlanner::buildLookAheadTree() { is_expanded_node = false; for (size_t i = 0; i < tree_.size(); i++) { if (!(tree_[i].closed_)) { + // If we reach the acceptance radius, add goal as last node and exit + if (i > 1 && (tree_[i].getPosition() - goal_).norm() < acceptance_radius_) { + tree_.push_back(TreeNode(i, simulation_state(0.f, goal_), goal_ - tree_[i].getPosition())); + closed_set_.push_back(i); + closed_set_.push_back(tree_.size() - 1); + break; + } + float node_distance = (tree_[i].getPosition() - position_).norm(); if (tree_[i].total_cost_ < minimal_cost && node_distance < max_path_length_) { minimal_cost = tree_[i].total_cost_; diff --git a/local_planner/test/test_star_planner.cpp b/local_planner/test/test_star_planner.cpp index 2c588061b..f1a8fad36 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 32af0e4d3a0866b5f3d4ad89e78c92a1d70a2be9 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Tue, 3 Sep 2019 18:21:58 +0200 Subject: [PATCH 08/28] Get rid of dynamic allocation when doing simulation and you only need the endpoint --- .../local_planner/trajectory_simulator.h | 5 +++++ .../src/utils/trajectory_simulator.cpp | 22 ++++++++++++++++--- 2 files changed, 24 insertions(+), 3 deletions(-) diff --git a/local_planner/include/local_planner/trajectory_simulator.h b/local_planner/include/local_planner/trajectory_simulator.h index a6a3e64fb..b72c23ccf 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_; @@ -48,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 46ce756ee..e74b6f9e0 100644 --- a/local_planner/src/utils/trajectory_simulator.cpp +++ b/local_planner/src/utils/trajectory_simulator.cpp @@ -20,12 +20,30 @@ 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) { + 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, float simulation_duration) { int num_steps = static_cast(std::ceil(simulation_duration / step_time_)); 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, @@ -61,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 ac9903fcb0a89c19afcf36c9bf2ea430c119cce4 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Wed, 4 Sep 2019 09:02:20 +0200 Subject: [PATCH 09/28] 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 2327540a23c11ce405093ae064ba9f8fbede98ce Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Sun, 15 Sep 2019 11:00:22 +0200 Subject: [PATCH 10/28] - add escape directions to the best candidate directio - penalize for big angular changes between parent-child nodes in the tree - penalize for big angular changes at the tree root between iteration --- avoidance/include/avoidance/common.h | 22 +++++ avoidance/sim/worlds/boxes1.world | 6 +- avoidance/sim/worlds/boxes1.yaml | 4 +- avoidance/src/common.cpp | 18 +++++ local_planner/cfg/local_planner_node.cfg | 7 +- .../include/local_planner/planner_functions.h | 3 +- .../include/local_planner/star_planner.h | 3 +- .../launch/local_planner_depth-camera.launch | 2 +- local_planner/resource/local_planner.rviz | 80 ++++++++++--------- local_planner/src/nodes/local_planner.cpp | 24 ++---- local_planner/src/nodes/planner_functions.cpp | 54 +++++++++++-- local_planner/src/nodes/star_planner.cpp | 46 +++++++++-- 12 files changed, 191 insertions(+), 78 deletions(-) diff --git a/avoidance/include/avoidance/common.h b/avoidance/include/avoidance/common.h index 4b0ee6a22..f1a1e2284 100644 --- a/avoidance/include/avoidance/common.h +++ b/avoidance/include/avoidance/common.h @@ -353,6 +353,28 @@ 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); return ev3; diff --git a/avoidance/sim/worlds/boxes1.world b/avoidance/sim/worlds/boxes1.world index 1d86a0bea..56d124b6a 100644 --- a/avoidance/sim/worlds/boxes1.world +++ b/avoidance/sim/worlds/boxes1.world @@ -85,18 +85,18 @@ true - 15 0 2.5 0 0 0 + 15 0 8 0 0 0 - 1 10 5 + 1 15 25 - 1 10 5 + 1 15 25 diff --git a/avoidance/sim/worlds/boxes1.yaml b/avoidance/sim/worlds/boxes1.yaml index 1ce03007f..6969691f0 100644 --- a/avoidance/sim/worlds/boxes1.yaml +++ b/avoidance/sim/worlds/boxes1.yaml @@ -2,6 +2,6 @@ name: "box" frame_id: "local_origin" mesh_resource: "" - position: [15, 0.0, 2.5] + position: [15, 0.0, 8] orientation: [0.0, 0.0, 0.0, 1.0] - scale: [1.0, 10.0, 5.0] + scale: [1.0, 15.0, 25.0] 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/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg index c5261a499..8260e2f9b 100755 --- a/local_planner/cfg/local_planner_node.cfg +++ b/local_planner/cfg/local_planner_node.cfg @@ -11,9 +11,10 @@ gen.add("max_sensor_range_", double_t, 0, "Data points farther away will b 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("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("tree_heuristic_weight_", double_t, 0, "Weight for the tree heuristic cost", 35.0, 0.0, 50.0) +gen.add("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 1000, 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("distance_weigth_cost_param_", double_t, 0,"Approximate distance from obstacles (m) when the obstacle distance term dominates the cost function", 500.0, 0.0, 5000.0) +gen.add("tree_heuristic_weight_", double_t, 0, "Weight for the tree heuristic cost", 1000.0, 0.0, 5000.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/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h index d57c7fa91..9fb3ec4f4 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -98,7 +98,7 @@ void generateCostImage(const Eigen::MatrixXf& cost_matrix, const Eigen::MatrixXf * the least to the most expensive **/ void getBestCandidatesFromCostMatrix(const Eigen::MatrixXf& matrix, unsigned int number_of_candidates, - std::vector& candidate_vector); + std::vector& candidate_vector, const Eigen::Vector3f prev_init_dir, const Eigen::Vector3f pos); /** * @brief computes the cost of each direction in the polar histogram @@ -157,5 +157,6 @@ void printHistogram(const Histogram& histogram); bool interpolateBetweenSetpoints(const std::vector& setpoint_array, const ros::Time& path_generation_time, float tree_node_duration, Eigen::Vector3f& setpoint); +float costChangeInTreeDirection(Eigen::Vector2f &prev_direction, Eigen::Vector2f &curr_direction, float &init_angle); } #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 836ee9432..ebb72e88a 100644 --- a/local_planner/include/local_planner/star_planner.h +++ b/local_planner/include/local_planner/star_planner.h @@ -39,7 +39,8 @@ class StarPlanner { Eigen::Vector3f closest_pt_ = Eigen::Vector3f(NAN, NAN, NAN); costParameters cost_params_; simulation_limits lims_; - + Eigen::Vector3f starting_direction_ = Eigen::Vector3f(NAN, NAN, NAN); + float init_angle_ = 0.f; protected: /** * @brief computes the heuristic for a node diff --git a/local_planner/launch/local_planner_depth-camera.launch b/local_planner/launch/local_planner_depth-camera.launch index 9434cffe6..8c7b3a8ed 100644 --- a/local_planner/launch/local_planner_depth-camera.launch +++ b/local_planner/launch/local_planner_depth-camera.launch @@ -1,6 +1,6 @@ - + diff --git a/local_planner/resource/local_planner.rviz b/local_planner/resource/local_planner.rviz index e9fe52e3f..ebf4e03df 100644 --- a/local_planner/resource/local_planner.rviz +++ b/local_planner/resource/local_planner.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 571 + Tree Height: 382 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -13,7 +13,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 @@ -24,6 +24,8 @@ Panels: Name: Time SyncMode: 0 SyncSource: Local pointcloud +Preferences: + PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: @@ -51,7 +53,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -91,14 +93,14 @@ Visualization Manager: Enabled: false Length: 1 Name: fcu - Radius: 0.100000001 + Radius: 0.10000000149011612 Reference Frame: fcu Value: false - Class: rviz/Axes Enabled: true Length: 1 Name: local_origin - Radius: 0.100000001 + Radius: 0.10000000149011612 Reference Frame: local_origin Value: true - Alpha: 1 @@ -124,7 +126,7 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.100000001 + Size (m): 0.10000000149011612 Style: Flat Squares Topic: /local_pointcloud Unreliable: false @@ -144,21 +146,21 @@ Visualization Manager: Unreliable: false Value: true - Class: rviz/Marker - Enabled: false + Enabled: true Marker Topic: /complete_tree Name: Tree Namespaces: - {} + "": true Queue Size: 100 - Value: false + Value: true - Class: rviz/Marker - Enabled: false + Enabled: true Marker Topic: /tree_path Name: TreePath Namespaces: - {} + "": true Queue Size: 100 - Value: false + Value: true - Class: rviz/Marker Enabled: false Marker Topic: /take_off_pose @@ -254,30 +256,30 @@ Visualization Manager: Views: Current: Class: rviz/ThirdPersonFollower - Distance: 19.7187691 + Distance: 19.718769073486328 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.0648926124 - Y: -0.0846519172 + X: 0.06489261239767075 + Y: -0.08465191721916199 Z: 0 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.72479701 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7247970104217529 Target Frame: fcu Value: ThirdPersonFollower (rviz) - Yaw: 3.09110641 + Yaw: 3.091106414794922 Saved: - Class: rviz/ThirdPersonFollower Distance: 10 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false @@ -286,43 +288,43 @@ Visualization Manager: Y: 0 Z: 0 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: ThirdPersonFollower - Near Clip Distance: 0.00999999978 - Pitch: 0.220398411 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.220398411154747 Target Frame: camera_link Value: ThirdPersonFollower (rviz) - Yaw: 4.69858599 + Yaw: 4.6985859870910645 - Class: rviz/Orbit - Distance: 49.4510117 + Distance: 49.451011657714844 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 5.36913252 - Y: -1.03582704 - Z: -0.00413707877 + X: 5.3691325187683105 + Y: -1.0358270406723022 + Z: -0.004137078765779734 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Orbit - Near Clip Distance: 0.00999999978 - Pitch: 1.56979632 + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 Target Frame: world Value: Orbit (rviz) - Yaw: 3.12039614 + Yaw: 3.120396137237549 Window Geometry: Displays: collapsed: false - Height: 1416 + Height: 1052 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a000004f2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000a00000004fd00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000027c000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002aa000002700000001600fffffffb0000000a0049006d00610067006500000002ce000000c000000000000000000000000100000132000004f2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000004f2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bf0000004afc0100000002fb0000000800540069006d00650100000000000009bf0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000084f000004f200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000372fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000005fb0000039500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001bb000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001fe000001b10000001600fffffffb0000000a0049006d00610067006500000002ce000000c000000000000000000000000100000132000004f2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000004f2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000004afc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006100000037200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -331,6 +333,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2495 - X: 65 - Y: 24 + Width: 1920 + X: 1920 + Y: 0 diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index 7eb6eecf3..aedd14479 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -141,7 +141,11 @@ void LocalPlanner::determineStrategy() { 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_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); @@ -223,22 +227,8 @@ void LocalPlanner::getObstacleDistanceData(sensor_msgs::LaserScan& obstacle_dist avoidanceOutput LocalPlanner::getAvoidanceOutput() const { avoidanceOutput out; - - // 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); - - out.cruise_velocity = max_speed; + out.cruise_velocity = getMaxSpeed(px4_.param_mpc_jerk_max, px4_.param_mpc_acc_hor, max_sensor_range_, + px4_.param_mpc_xy_cruise, mission_item_speed_); out.last_path_time = last_path_time_; out.tree_node_duration = tree_node_duration_; out.path_node_setpoints = star_planner_->path_node_setpoints_; diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index bd77bd65b..6de301634 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -229,7 +229,8 @@ int colorImageIndex(int e_ind, int z_ind, int color) { } void getBestCandidatesFromCostMatrix(const Eigen::MatrixXf& matrix, unsigned int number_of_candidates, - std::vector& candidate_vector) { + std::vector& candidate_vector, const Eigen::Vector3f prev_init_dir, + const Eigen::Vector3f pos) { std::priority_queue, std::less> queue; for (int row_index = 0; row_index < matrix.rows(); row_index++) { @@ -237,7 +238,13 @@ void getBestCandidatesFromCostMatrix(const Eigen::MatrixXf& matrix, unsigned int PolarPoint p_pol = histogramIndexToPolar(row_index, col_index, ALPHA_RES, 1.0); float cost = matrix(row_index, col_index); candidateDirection candidate(cost, p_pol.e, p_pol.z); - + if (!prev_init_dir.array().hasNaN()) { + Eigen::Vector2f candidate_dir = candidate.toEigen().head<2>(); + Eigen::Vector2f prev_init_dir_2f = prev_init_dir.head<2>(); + float angle = 0.f; + float add = costChangeInTreeDirection(prev_init_dir_2f, candidate_dir, angle); + candidate.cost += add; + } if (queue.size() < number_of_candidates) { queue.push(candidate); } else if (candidate < queue.top()) { @@ -246,6 +253,25 @@ void getBestCandidatesFromCostMatrix(const Eigen::MatrixXf& matrix, unsigned int } } } + + candidateDirection best_direction = queue.top(); + for (int i = 0; i < 5; i++) { + candidateDirection candidate(0.f, best_direction.elevation_angle, best_direction.azimuth_angle + (i + 1) * 60.f); + PolarPoint candidate_polar = PolarPoint(candidate.elevation_angle, candidate.azimuth_angle, 1.f); + wrapPolar(candidate_polar); + Eigen::Vector2i histogram_index = polarToHistogramIndex(candidate_polar, ALPHA_RES); + candidate.cost = matrix(histogram_index.y(), histogram_index.x()); + + if (!prev_init_dir.array().hasNaN()) { + Eigen::Vector2f candidate_dir = candidate.toEigen().head<2>(); + Eigen::Vector2f prev_init_dir_2f = prev_init_dir.head<2>(); + float angle = 0.f; + float add = costChangeInTreeDirection(prev_init_dir_2f, candidate_dir, angle); + candidate.cost += add; + } + queue.push(candidate); + } + // copy queue to vector and change order such that lowest cost is at the // front candidate_vector.clear(); @@ -257,6 +283,24 @@ void getBestCandidatesFromCostMatrix(const Eigen::MatrixXf& matrix, unsigned int std::reverse(candidate_vector.begin(), candidate_vector.end()); } +float costChangeInTreeDirection(Eigen::Vector2f &prev_direction, Eigen::Vector2f &curr_direction, float &init_angle) { + init_angle = atan2(curr_direction.y(), curr_direction.x()) - atan2(prev_direction.y(), prev_direction.x()); + if (init_angle > M_PI_F) { + init_angle -= 2 * M_PI_F; + } else if (init_angle <= -M_PI_F) { + init_angle += 2 * M_PI_F; + } + + init_angle *= RAD_TO_DEG; + init_angle = std::abs(init_angle); + float add = init_angle > 10.f ? (5000.f / (1.f + std::exp((-init_angle + 10.f) / 20.f))) : 0.f; + if (init_angle > 20.f) { + add = 1000000.0f; + } + + return add; +} + void smoothPolarMatrix(Eigen::MatrixXf& matrix, unsigned int smoothing_radius) { // pad matrix by smoothing radius respecting all wrapping rules Eigen::MatrixXf matrix_padded; @@ -344,7 +388,7 @@ std::pair costFunction(const PolarPoint& candidate_polar, float ob float weight = 0.f; // yaw cost partition between back to line previous-current goal and goal if (!is_obstacle_facing_goal) { - weight = 0.5f; + weight = 0.25f; } const float yaw_cost = (1.f - weight) * cost_params.yaw_cost_param * angle_diff * angle_diff; @@ -352,9 +396,9 @@ std::pair costFunction(const PolarPoint& candidate_polar, float ob const float pitch_cost = cost_params.pitch_cost_param * (candidate_polar.e - facing_goal.e) * (candidate_polar.e - facing_goal.e); const float d = cost_params.obstacle_cost_param - obstacle_distance; - const float distance_cost = obstacle_distance > 0 ? 5000.0f * (1 + d / sqrt(1 + d * d)) : 0.0f; + const float distance_cost = obstacle_distance > 0.f ? 1000.0f * (1 + d / sqrt(1 + d * d)) : 0.0f; - return std::pair(distance_cost, velocity_cost + yaw_cost + yaw_to_line_cost + pitch_cost); + return std::pair(distance_cost, yaw_cost + yaw_to_line_cost + pitch_cost); } bool interpolateBetweenSetpoints(const std::vector& setpoint_array, diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 9208b99c9..75ea869e9 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -40,7 +40,8 @@ void StarPlanner::setPointcloud(const pcl::PointCloud& cloud) { void StarPlanner::setClosestPointOnLine(const Eigen::Vector3f& closest_pt) { closest_pt_ = closest_pt; } float StarPlanner::treeHeuristicFunction(int node_number) const { - return (goal_ - tree_[node_number].getPosition()).norm() * tree_heuristic_weight_; + // return (goal_ - tree_[node_number].getPosition()).norm() * tree_heuristic_weight_; + return ((goal_ - tree_[node_number].getPosition()).norm() / lims_.max_xy_velocity_norm + tree_[node_number].state.time) * tree_heuristic_weight_; } void StarPlanner::buildLookAheadTree() { @@ -81,7 +82,17 @@ void StarPlanner::buildLookAheadTree() { candidate_vector.clear(); getCostMatrix(histogram, goal_, origin_position, origin_velocity, cost_params_, smoothing_margin_degrees_, closest_pt_, max_sensor_range_, min_sensor_range_, cost_matrix, cost_image_data); - getBestCandidatesFromCostMatrix(cost_matrix, children_per_node_, candidate_vector); + if (n!=0) { + starting_direction_ = tree_[origin].getSetpoint(); + } + getBestCandidatesFromCostMatrix(cost_matrix, children_per_node_, candidate_vector, starting_direction_, position_); + + 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); // add candidates as nodes if (candidate_vector.empty()) { @@ -91,20 +102,21 @@ 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(limits, state, 0.05f); // todo: parameterize simulation step size [s] std::vector trajectory = sim.generate_trajectory(candidate.toEigen(), tree_node_duration_); // check if another close node has been added + float dist = 1.f; int close_nodes = 0; for (size_t i = 0; i < tree_.size(); i++) { - float dist = (tree_[i].getPosition() - trajectory.back().position).norm(); + dist = (tree_[i].getPosition() - trajectory.back().position).norm(); if (dist < 0.2f) { close_nodes++; break; } } - if (children < children_per_node_ && close_nodes == 0) { + 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; @@ -143,13 +155,35 @@ void StarPlanner::buildLookAheadTree() { candidate_vector.clear(); } + float min_cost_per_depth = FLT_MAX; + int chosen_children = 0; + for (size_t i = 0; i < tree_.size(); i++) { + if (tree_[i].closed_ == 0) { + size_t parent = tree_[i].origin_; + float total_cost = tree_[i].total_cost_; + int depth = 0; + while(tree_[parent].origin_ > 0) { + parent = tree_[parent].origin_; + depth++; + } + + if (min_cost_per_depth > (total_cost / depth)) { + min_cost_per_depth = (total_cost / depth); + chosen_children = (int)i; + } + } + } // Get setpoints into member vector - int tree_end = origin; + int tree_end = chosen_children; path_node_setpoints_.clear(); while (tree_end > 0) { path_node_setpoints_.push_back(tree_[tree_end].getSetpoint()); tree_end = tree_[tree_end].origin_; } + + path_node_setpoints_.push_back(tree_[0].getSetpoint()); + starting_direction_ = path_node_setpoints_[path_node_setpoints_.size() - 2]; + } } From 18d0dd2ca4deb7d6cae6545856c6212a9f2f8517 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Thu, 26 Sep 2019 17:35:24 +0200 Subject: [PATCH 11/28] tmp --- local_planner/src/nodes/planner_functions.cpp | 2 +- local_planner/src/nodes/star_planner.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index 6de301634..6d71b0374 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -295,7 +295,7 @@ float costChangeInTreeDirection(Eigen::Vector2f &prev_direction, Eigen::Vector2f init_angle = std::abs(init_angle); float add = init_angle > 10.f ? (5000.f / (1.f + std::exp((-init_angle + 10.f) / 20.f))) : 0.f; if (init_angle > 20.f) { - add = 1000000.0f; + add = 500000.0f; } return add; diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 75ea869e9..a2dd0d925 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -90,8 +90,9 @@ void StarPlanner::buildLookAheadTree() { 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, + std::min(std::min(computeMaxSpeedFromBrakingDistance(lims_.max_jerk_norm, lims_.max_acceleration_norm, (state.position - goal_).head<2>().norm()), + computeMaxSpeedFromBrakingDistance(lims_.max_jerk_norm, lims_.max_acceleration_norm, max_sensor_range_)), lims_.max_xy_velocity_norm); // add candidates as nodes From ae115b814004f694055ceaf3101c95778a64e61e Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Fri, 27 Sep 2019 10:53:20 +0200 Subject: [PATCH 12/28] minor cleanup --- local_planner/cfg/local_planner_node.cfg | 2 +- local_planner/include/local_planner/planner_functions.h | 2 +- local_planner/src/nodes/planner_functions.cpp | 5 ++--- local_planner/src/nodes/star_planner.cpp | 9 ++++----- 4 files changed, 8 insertions(+), 10 deletions(-) diff --git a/local_planner/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg index 8260e2f9b..1ebddf13f 100755 --- a/local_planner/cfg/local_planner_node.cfg +++ b/local_planner/cfg/local_planner_node.cfg @@ -9,7 +9,7 @@ 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", 35.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", 1000, 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) diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h index 9fb3ec4f4..f5c446bf1 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -98,7 +98,7 @@ void generateCostImage(const Eigen::MatrixXf& cost_matrix, const Eigen::MatrixXf * the least to the most expensive **/ void getBestCandidatesFromCostMatrix(const Eigen::MatrixXf& matrix, unsigned int number_of_candidates, - std::vector& candidate_vector, const Eigen::Vector3f prev_init_dir, const Eigen::Vector3f pos); + std::vector& candidate_vector, const Eigen::Vector3f prev_init_dir); /** * @brief computes the cost of each direction in the polar histogram diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index 6d71b0374..6fa8fd473 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -229,8 +229,7 @@ int colorImageIndex(int e_ind, int z_ind, int color) { } void getBestCandidatesFromCostMatrix(const Eigen::MatrixXf& matrix, unsigned int number_of_candidates, - std::vector& candidate_vector, const Eigen::Vector3f prev_init_dir, - const Eigen::Vector3f pos) { + std::vector& candidate_vector, const Eigen::Vector3f prev_init_dir) { std::priority_queue, std::less> queue; for (int row_index = 0; row_index < matrix.rows(); row_index++) { @@ -395,7 +394,7 @@ std::pair costFunction(const PolarPoint& candidate_polar, float ob const float yaw_to_line_cost = weight * cost_params.yaw_cost_param * angle_diff_to_line * angle_diff_to_line; const float pitch_cost = cost_params.pitch_cost_param * (candidate_polar.e - facing_goal.e) * (candidate_polar.e - facing_goal.e); - const float d = cost_params.obstacle_cost_param - obstacle_distance; + const float d = 2 + cost_params.obstacle_cost_param - obstacle_distance; const float distance_cost = obstacle_distance > 0.f ? 1000.0f * (1 + d / sqrt(1 + d * d)) : 0.0f; return std::pair(distance_cost, yaw_cost + yaw_to_line_cost + pitch_cost); diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index a2dd0d925..7b01a12a0 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -40,7 +40,6 @@ void StarPlanner::setPointcloud(const pcl::PointCloud& cloud) { void StarPlanner::setClosestPointOnLine(const Eigen::Vector3f& closest_pt) { closest_pt_ = closest_pt; } float StarPlanner::treeHeuristicFunction(int node_number) const { - // return (goal_ - tree_[node_number].getPosition()).norm() * tree_heuristic_weight_; return ((goal_ - tree_[node_number].getPosition()).norm() / lims_.max_xy_velocity_norm + tree_[node_number].state.time) * tree_heuristic_weight_; } @@ -70,8 +69,6 @@ void StarPlanner::buildLookAheadTree() { for (int n = 0; n < n_expanded_nodes_ && is_expanded_node; n++) { Eigen::Vector3f origin_position = tree_[origin].getPosition(); Eigen::Vector3f origin_velocity = tree_[origin].getVelocity(); - PolarPoint facing_goal = cartesianToPolarHistogram(goal_, origin_position); - float distance_to_goal = (goal_ - origin_position).norm(); histogram.setZero(); generateNewHistogram(histogram, cloud_, origin_position); @@ -85,7 +82,7 @@ void StarPlanner::buildLookAheadTree() { if (n!=0) { starting_direction_ = tree_[origin].getSetpoint(); } - getBestCandidatesFromCostMatrix(cost_matrix, children_per_node_, candidate_vector, starting_direction_, position_); + getBestCandidatesFromCostMatrix(cost_matrix, children_per_node_, candidate_vector, starting_direction_); simulation_limits limits = lims_; simulation_state state = tree_[origin].state; @@ -184,7 +181,9 @@ void StarPlanner::buildLookAheadTree() { path_node_setpoints_.push_back(tree_[0].getSetpoint()); - starting_direction_ = path_node_setpoints_[path_node_setpoints_.size() - 2]; + if ((path_node_setpoints_.size() - 2) >= 0) { + starting_direction_ = path_node_setpoints_[path_node_setpoints_.size() - 2]; + } } } From 84319ee263e7198e9e8d96a478f085ae94a3fb16 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Fri, 27 Sep 2019 10:53:49 +0200 Subject: [PATCH 13/28] clang format --- .../include/local_planner/avoidance_output.h | 2 +- .../include/local_planner/planner_functions.h | 5 ++- .../include/local_planner/star_planner.h | 1 + local_planner/src/nodes/local_planner.cpp | 44 +++++++++---------- local_planner/src/nodes/planner_functions.cpp | 5 ++- local_planner/src/nodes/star_planner.cpp | 21 ++++----- .../src/nodes/waypoint_generator.cpp | 4 +- 7 files changed, 43 insertions(+), 39 deletions(-) diff --git a/local_planner/include/local_planner/avoidance_output.h b/local_planner/include/local_planner/avoidance_output.h index 360491f8a..9d132b3d2 100644 --- a/local_planner/include/local_planner/avoidance_output.h +++ b/local_planner/include/local_planner/avoidance_output.h @@ -12,7 +12,7 @@ enum waypoint_choice { hover, tryPath, direct, reachHeight }; struct avoidanceOutput { float cruise_velocity; // mission cruise velocity float tree_node_duration; - ros::Time last_path_time; // finish built time for the VFH+* tree + ros::Time last_path_time; // finish built time for the VFH+* tree std::vector path_node_setpoints; // array of setpoints }; } diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h index f5c446bf1..4e963048f 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -98,7 +98,8 @@ void generateCostImage(const Eigen::MatrixXf& cost_matrix, const Eigen::MatrixXf * the least to the most expensive **/ void getBestCandidatesFromCostMatrix(const Eigen::MatrixXf& matrix, unsigned int number_of_candidates, - std::vector& candidate_vector, const Eigen::Vector3f prev_init_dir); + std::vector& candidate_vector, + const Eigen::Vector3f prev_init_dir); /** * @brief computes the cost of each direction in the polar histogram @@ -157,6 +158,6 @@ void printHistogram(const Histogram& histogram); bool interpolateBetweenSetpoints(const std::vector& setpoint_array, const ros::Time& path_generation_time, float tree_node_duration, Eigen::Vector3f& setpoint); -float costChangeInTreeDirection(Eigen::Vector2f &prev_direction, Eigen::Vector2f &curr_direction, float &init_angle); +float costChangeInTreeDirection(Eigen::Vector2f& prev_direction, Eigen::Vector2f& curr_direction, float& init_angle); } #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 ebb72e88a..ea4048f15 100644 --- a/local_planner/include/local_planner/star_planner.h +++ b/local_planner/include/local_planner/star_planner.h @@ -41,6 +41,7 @@ class StarPlanner { simulation_limits lims_; Eigen::Vector3f starting_direction_ = Eigen::Vector3f(NAN, NAN, NAN); float init_angle_ = 0.f; + protected: /** * @brief computes the heuristic for a node diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index aedd14479..7d9c75589 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -134,27 +134,27 @@ void LocalPlanner::determineStrategy() { } if (!polar_histogram_.isEmpty()) { - getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, closest_pt_, - max_sensor_range_, min_sensor_range_, cost_matrix_, cost_image_data_); - - simulation_limits lims; - setDefaultPx4Parameters(); // TODO: remove but make sure they're set! - lims.max_z_velocity = px4_.param_mpc_z_vel_max_up; - lims.min_z_velocity = -1.0f * px4_.param_mpc_z_vel_max_dn; - 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); - star_planner_->setPointcloud(final_cloud_); - star_planner_->setClosestPointOnLine(closest_pt_); - - // build search tree - star_planner_->buildLookAheadTree(); - last_path_time_ = ros::Time::now(); + getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, closest_pt_, + max_sensor_range_, min_sensor_range_, cost_matrix_, cost_image_data_); + + simulation_limits lims; + setDefaultPx4Parameters(); // TODO: remove but make sure they're set! + lims.max_z_velocity = px4_.param_mpc_z_vel_max_up; + lims.min_z_velocity = -1.0f * px4_.param_mpc_z_vel_max_dn; + lims.max_xy_velocity_norm = + 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); + star_planner_->setPointcloud(final_cloud_); + star_planner_->setClosestPointOnLine(closest_pt_); + + // build search tree + star_planner_->buildLookAheadTree(); + last_path_time_ = ros::Time::now(); } } @@ -228,7 +228,7 @@ void LocalPlanner::getObstacleDistanceData(sensor_msgs::LaserScan& obstacle_dist avoidanceOutput LocalPlanner::getAvoidanceOutput() const { avoidanceOutput out; out.cruise_velocity = getMaxSpeed(px4_.param_mpc_jerk_max, px4_.param_mpc_acc_hor, max_sensor_range_, - px4_.param_mpc_xy_cruise, mission_item_speed_); + px4_.param_mpc_xy_cruise, mission_item_speed_); out.last_path_time = last_path_time_; out.tree_node_duration = tree_node_duration_; out.path_node_setpoints = star_planner_->path_node_setpoints_; diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index 6fa8fd473..a5828edda 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -229,7 +229,8 @@ int colorImageIndex(int e_ind, int z_ind, int color) { } void getBestCandidatesFromCostMatrix(const Eigen::MatrixXf& matrix, unsigned int number_of_candidates, - std::vector& candidate_vector, const Eigen::Vector3f prev_init_dir) { + std::vector& candidate_vector, + const Eigen::Vector3f prev_init_dir) { std::priority_queue, std::less> queue; for (int row_index = 0; row_index < matrix.rows(); row_index++) { @@ -282,7 +283,7 @@ void getBestCandidatesFromCostMatrix(const Eigen::MatrixXf& matrix, unsigned int std::reverse(candidate_vector.begin(), candidate_vector.end()); } -float costChangeInTreeDirection(Eigen::Vector2f &prev_direction, Eigen::Vector2f &curr_direction, float &init_angle) { +float costChangeInTreeDirection(Eigen::Vector2f& prev_direction, Eigen::Vector2f& curr_direction, float& init_angle) { init_angle = atan2(curr_direction.y(), curr_direction.x()) - atan2(prev_direction.y(), prev_direction.x()); if (init_angle > M_PI_F) { init_angle -= 2 * M_PI_F; diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 7b01a12a0..46a6f342b 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -40,7 +40,9 @@ void StarPlanner::setPointcloud(const pcl::PointCloud& cloud) { void StarPlanner::setClosestPointOnLine(const Eigen::Vector3f& closest_pt) { closest_pt_ = closest_pt; } float StarPlanner::treeHeuristicFunction(int node_number) const { - return ((goal_ - tree_[node_number].getPosition()).norm() / lims_.max_xy_velocity_norm + tree_[node_number].state.time) * tree_heuristic_weight_; + return ((goal_ - tree_[node_number].getPosition()).norm() / lims_.max_xy_velocity_norm + + tree_[node_number].state.time) * + tree_heuristic_weight_; } void StarPlanner::buildLookAheadTree() { @@ -79,18 +81,19 @@ void StarPlanner::buildLookAheadTree() { candidate_vector.clear(); getCostMatrix(histogram, goal_, origin_position, origin_velocity, cost_params_, smoothing_margin_degrees_, closest_pt_, max_sensor_range_, min_sensor_range_, cost_matrix, cost_image_data); - if (n!=0) { + if (n != 0) { starting_direction_ = tree_[origin].getSetpoint(); } getBestCandidatesFromCostMatrix(cost_matrix, children_per_node_, candidate_vector, starting_direction_); simulation_limits limits = lims_; simulation_state state = tree_[origin].state; - limits.max_xy_velocity_norm = - std::min(std::min(computeMaxSpeedFromBrakingDistance(lims_.max_jerk_norm, lims_.max_acceleration_norm, - (state.position - goal_).head<2>().norm()), - computeMaxSpeedFromBrakingDistance(lims_.max_jerk_norm, lims_.max_acceleration_norm, max_sensor_range_)), - lims_.max_xy_velocity_norm); + limits.max_xy_velocity_norm = std::min( + std::min( + computeMaxSpeedFromBrakingDistance(lims_.max_jerk_norm, lims_.max_acceleration_norm, + (state.position - goal_).head<2>().norm()), + computeMaxSpeedFromBrakingDistance(lims_.max_jerk_norm, lims_.max_acceleration_norm, max_sensor_range_)), + lims_.max_xy_velocity_norm); // add candidates as nodes if (candidate_vector.empty()) { @@ -160,7 +163,7 @@ void StarPlanner::buildLookAheadTree() { size_t parent = tree_[i].origin_; float total_cost = tree_[i].total_cost_; int depth = 0; - while(tree_[parent].origin_ > 0) { + while (tree_[parent].origin_ > 0) { parent = tree_[parent].origin_; depth++; } @@ -179,11 +182,9 @@ void StarPlanner::buildLookAheadTree() { tree_end = tree_[tree_end].origin_; } - path_node_setpoints_.push_back(tree_[0].getSetpoint()); if ((path_node_setpoints_.size() - 2) >= 0) { starting_direction_ = path_node_setpoints_[path_node_setpoints_.size() - 2]; } - } } diff --git a/local_planner/src/nodes/waypoint_generator.cpp b/local_planner/src/nodes/waypoint_generator.cpp index 66c90e3b5..576f696a0 100644 --- a/local_planner/src/nodes/waypoint_generator.cpp +++ b/local_planner/src/nodes/waypoint_generator.cpp @@ -81,8 +81,8 @@ usm::Transition WaypointGenerator::runCurrentState() { usm::Transition WaypointGenerator::runTryPath() { Eigen::Vector3f setpoint = Eigen::Vector3f::Zero(); - const bool tree_available = interpolateBetweenSetpoints(planner_info_.path_node_setpoints, planner_info_.last_path_time, - planner_info_.tree_node_duration, setpoint); + const bool tree_available = interpolateBetweenSetpoints( + planner_info_.path_node_setpoints, planner_info_.last_path_time, planner_info_.tree_node_duration, setpoint); output_.goto_position = position_ + setpoint; getPathMsg(); From 6a760966c552f3ba5fbed8d24efb59e4c4146414 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Tue, 16 Jul 2019 15:00:20 +0200 Subject: [PATCH 14/28] Hack the histogram to get nearest distance in any direction from a point in that direction, rather than nearest distance in that direction --- avoidance/include/avoidance/common.h | 5 + avoidance/include/avoidance/kdtree.h | 479 ++++++++++++++++++ avoidance/src/common.cpp | 1 + .../include/local_planner/planner_functions.h | 6 + local_planner/src/nodes/planner_functions.cpp | 17 + local_planner/src/nodes/star_planner.cpp | 8 +- 6 files changed, 515 insertions(+), 1 deletion(-) create mode 100644 avoidance/include/avoidance/kdtree.h diff --git a/avoidance/include/avoidance/common.h b/avoidance/include/avoidance/common.h index f1a1e2284..389c1469b 100644 --- a/avoidance/include/avoidance/common.h +++ b/avoidance/include/avoidance/common.h @@ -395,6 +395,11 @@ inline Eigen::Vector3f toEigen(const pcl::PointXYZI& p) { return ev3; } +inline std::array toArray(const Eigen::Vector3f& ev3){ + std::array ret {ev3.x(), ev3.y(), ev3.z()}; + return ret; +} + inline Eigen::Quaternionf toEigen(const geometry_msgs::Quaternion& gmq) { Eigen::Quaternionf eqf; eqf.x() = gmq.x; diff --git a/avoidance/include/avoidance/kdtree.h b/avoidance/include/avoidance/kdtree.h new file mode 100644 index 000000000..dc259108b --- /dev/null +++ b/avoidance/include/avoidance/kdtree.h @@ -0,0 +1,479 @@ + +#pragma once + +/** + * KDTree.h by Julian Kent + * A C++11 KD-Tree with the following features: + * single file + * header only + * high performance K Nearest Neighbor and ball searches + * dynamic insertions + * simple API + * depends only on the STL + * templatable on your custom data type to store in the leaves. No need to keep a separate data structure! + * templatable on double, float etc + * templatable on L1, SquaredL2 or custom distance functor + * templated on number of dimensions for efficient inlining + * + * ------------------------------------------------------------------- + * + * This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not + * distributed with this file, you can obtain one at http://mozilla.org/MPL/2.0/. + * + * A high level explanation of MPLv2: You may use this in any software provided you give attribution. ou *must* make + * available any changes you make to the source code of this file to anybody you distribute your software to. + * + * Upstreaming features and bugfixes are highly appreciated via https://bitbucket.org/jkflying/KDTree.h + * + * For additional licensing rights, feature requests or questions, please contact Julian Kent + * + * ------------------------------------------------------------------- + * + * Example usage: + * + * // setup + * using tree_t = jk::tree::KDTree; + * using point_t = std::array; + * tree_t tree; + * tree.addPoint(point_t{{1, 2}}, "George"); + * tree.addPoint(point_t{{1, 3}}, "Harold"); + * tree.addPoint(point_t{{7, 7}}, "Melvin"); + * + * // KNN search + * point_t lazyMonsterLocation{{6, 6}}; // this monster will always try to eat the closest people + * const std::size_t monsterHeads = 2; // this monster can eat two people at once + * auto lazyMonsterVictims = tree.searchKnn(lazyMonsterLocation, monsterHeads); + * for (const auto& victim : lazyMonsterVictims) + * { + * std::cout << victim.payload << " closest to lazy monster, with distance " << sqrt(victim.distance) << "!" + * << std::endl; + * } + * + * // ball search + * point_t stationaryMonsterLocation{{8, 8}}; // this monster doesn't move, so can only eat people that are close + * const double neckLength = 6.0; // it can only reach within this range + * auto potentialVictims = tree.searchBall(stationaryMonsterLocation, neckLength * neckLength); // metric is SquaredL2 + * std::cout << "Stationary monster can reach any of " << potentialVictims.size() << " people!" << std::endl; + * + * // hybrid KNN/ball search + * auto actualVictims + * = tree.searchCapacityLimitedBall(stationaryMonsterLocation, neckLength * neckLength, monsterHeads); + * std::cout << "The stationary monster will try to eat "; + * for (const auto& victim : actualVictims) + * { + * std::cout << victim.payload << " and "; + * } + * std::cout << "nobody else." << std::endl; + * + * Output: + * + * Melvin closest to lazy monster, with distance 1.41421! + * Harold closest to lazy monster, with distance 5.83095! + * Stationary monster can reach any of 1 people! + * The stationary monster will try to eat Melvin and nobody else. + * + * ------------------------------------------------------------------- + * + * Tuning tips: + * + * If you need to add a lot of points before doing any queries, set the optional `autosplit` parameter to false, + * then call splitOutstanding(). This will reduce temporaries and result in a better balanced tree. + * + * Set the bucket size to be at least twice the K in a typical KNN query. If you have more dimensions, it is better to + * have a larger bucket size. 32 is a good starting point. If possible use powers of 2 for the bucket size. + * + * If you experience linear search performance, check that you don't have a bunch of duplicate point locations. This + * will result in the tree being unable to split the bucket the points are in, degrading search performance. + * + * The tree adapts to the parallel-to-axis dimensionality of the problem. Thus, if there is one dimension with a much + * larger scale than the others, most of the splitting will happen on this dimension. This is achieved by trying to + * keep the bounding boxes of the data in the buckets equal lengths in all axes. + * + * Random data performs worse than 'real world' data with structure. This is because real world data has tighter + * bounding boxes, meaning more branches of the tree can be eliminated sooner. + * + * On pure random data, more than 7 dimensions won't be much faster than linear. However, most data isn't actually + * random. The tree will adapt to any locally reduced dimensionality, which is found in most real world data. + * + * Hybrid ball/KNN searches are faster than either type on its own, because subtrees can be more aggresively eliminated. + */ + +#include +#include +#include +#include +#include +#include +#include + +namespace jk { +namespace tree { +struct L1 { + template + static Scalar distance(const std::array& location1, + const std::array& location2) { + auto abs = [](Scalar v) { return v >= 0 ? v : -v; }; + Scalar dist = 0; + for (std::size_t i = 0; i < Dimensions; i++) { + dist += abs(location1[i] - location2[i]); + } + return dist; + } +}; + +struct SquaredL2 { + template + static Scalar distance(const std::array& location1, + const std::array& location2) { + auto sqr = [](Scalar v) { return v * v; }; + Scalar dist = 0; + for (std::size_t i = 0; i < Dimensions; i++) { + dist += sqr(location1[i] - location2[i]); + } + return dist; + } +}; + +template +class KDTree { + private: + struct Node; + std::vector m_nodes; + std::set waitingForSplit; + + public: + using distance_t = Distance; + using scalar_t = Scalar; + using payload_t = Payload; + using point_t = std::array; + static const std::size_t dimensions = Dimensions; + static const std::size_t bucketSize = BucketSize; + using tree_t = KDTree; + + KDTree() { m_nodes.emplace_back(BucketSize); } // initialize the root node + + void addPoint(const point_t& location, const Payload& payload, bool autosplit = true) { + std::size_t addNode = 0; + + while (m_nodes[addNode].m_splitDimension != Dimensions) { + m_nodes[addNode].expandBounds(location); + if (location[m_nodes[addNode].m_splitDimension] < m_nodes[addNode].m_splitValue) { + addNode = m_nodes[addNode].m_children.first; + } else { + addNode = m_nodes[addNode].m_children.second; + } + } + m_nodes[addNode].add(LocationPayload{location, payload}); + + if (m_nodes[addNode].shouldSplit() && m_nodes[addNode].m_entries % BucketSize == 0) { + if (autosplit) { + split(addNode); + } else { + waitingForSplit.insert(addNode); + } + } + } + + void splitOutstanding() { + std::vector searchStack(waitingForSplit.begin(), waitingForSplit.end()); + waitingForSplit.clear(); + while (searchStack.size() > 0) { + std::size_t addNode = searchStack.back(); + searchStack.pop_back(); + if (m_nodes[addNode].m_splitDimension == Dimensions && m_nodes[addNode].shouldSplit() && split(addNode)) { + searchStack.push_back(m_nodes[addNode].m_children.first); + searchStack.push_back(m_nodes[addNode].m_children.second); + } + } + } + + struct DistancePayload { + Scalar distance; + Payload payload; + bool operator<(const DistancePayload& dp) const { return distance < dp.distance; } + }; + + std::vector searchKnn(const point_t& location, std::size_t maxPoints) const { + return searcher().search(location, std::numeric_limits::max(), maxPoints); + } + + std::vector searchBall(const point_t& location, Scalar maxRadius) const { + return searcher().search(location, maxRadius, std::numeric_limits::max()); + } + + std::vector searchCapacityLimitedBall(const point_t& location, Scalar maxRadius, + std::size_t maxPoints) const { + return searcher().search(location, maxRadius, maxPoints); + } + + DistancePayload search(const point_t& location) const { + DistancePayload result; + result.distance = std::numeric_limits::infinity(); + + if (m_nodes[0].m_entries > 0) { + std::vector searchStack; + searchStack.reserve(1 + std::size_t(1.5 * std::log2(1 + m_nodes[0].m_entries / BucketSize))); + searchStack.push_back(0); + + while (searchStack.size() > 0) { + std::size_t nodeIndex = searchStack.back(); + searchStack.pop_back(); + const Node& node = m_nodes[nodeIndex]; + if (result.distance > node.pointRectDist(location)) { + if (node.m_splitDimension == Dimensions) { + for (const auto& lp : node.m_locationPayloads) { + Scalar nodeDist = Distance::distance(location, lp.location); + if (nodeDist < result.distance) { + result = DistancePayload{nodeDist, lp.payload}; + } + } + } else { + node.queueChildren(location, searchStack); + } + } + } + } + return result; + } + + class Searcher { + public: + Searcher(const tree_t& tree) : m_tree(tree) {} + Searcher(const Searcher& searcher) : m_tree(searcher.m_tree) {} + + // NB! this method is not const. Do not call this on same instance from different threads simultaneously. + const std::vector& search(const point_t& location, Scalar maxRadius, std::size_t maxPoints) { + // clear any remainng search results + m_searchStack.clear(); + while (m_prioqueue.size() > 0) { + m_prioqueue.pop(); + } + m_results.clear(); + + // reserve capacities + m_searchStack.reserve(1 + std::size_t(1.5 * std::log2(1 + m_tree.m_nodes[0].m_entries / BucketSize))); + if (m_prioqueueCapacity < maxPoints && maxPoints < m_tree.m_nodes[0].m_entries) { + std::vector container; + container.reserve(maxPoints); + m_prioqueue = std::priority_queue>(std::less(), + std::move(container)); + m_prioqueueCapacity = maxPoints; + } + + m_tree.searchCapacityLimitedBall(location, maxRadius, maxPoints, m_searchStack, m_prioqueue, m_results); + + m_prioqueueCapacity = std::max(m_prioqueueCapacity, m_results.size()); + return m_results; + } + + private: + const tree_t& m_tree; + + std::vector m_searchStack; + std::priority_queue> m_prioqueue; + std::size_t m_prioqueueCapacity = 0; + std::vector m_results; + }; + + // NB! returned class has no const methods. Get one instance per thread! + Searcher searcher() const { return Searcher(*this); } + + private: + struct LocationPayload { + point_t location; + Payload payload; + }; + std::vector m_bucketRecycle; + + void searchCapacityLimitedBall(const point_t& location, Scalar maxRadius, std::size_t maxPoints, + std::vector& searchStack, + std::priority_queue>& prioqueue, + std::vector& results) const { + std::size_t numSearchPoints = std::min(maxPoints, m_nodes[0].m_entries); + + if (numSearchPoints > 0) { + searchStack.push_back(0); + while (searchStack.size() > 0) { + std::size_t nodeIndex = searchStack.back(); + searchStack.pop_back(); + const Node& node = m_nodes[nodeIndex]; + Scalar minDist = node.pointRectDist(location); + if (maxRadius > minDist && (prioqueue.size() < numSearchPoints || prioqueue.top().distance > minDist)) { + if (node.m_splitDimension == Dimensions) { + node.searchCapacityLimitedBall(location, maxRadius, numSearchPoints, prioqueue); + } else { + node.queueChildren(location, searchStack); + } + } + } + + results.reserve(prioqueue.size()); + while (prioqueue.size() > 0) { + results.push_back(prioqueue.top()); + prioqueue.pop(); + } + std::reverse(results.begin(), results.end()); + } + } + + bool split(std::size_t index) { + if (m_nodes.capacity() < m_nodes.size() + 2) { + m_nodes.reserve((m_nodes.capacity() + 1) * 2); + } + Node& splitNode = m_nodes[index]; + splitNode.m_splitDimension = Dimensions; + Scalar width(0); + // select widest dimension + for (std::size_t i = 0; i < Dimensions; i++) { + auto diff = [](std::array vals) { return vals[1] - vals[0]; }; + Scalar dWidth = diff(splitNode.m_bounds[i]); + if (dWidth > width) { + splitNode.m_splitDimension = i; + width = dWidth; + } + } + if (splitNode.m_splitDimension == Dimensions) { + return false; + } + + std::vector splitDimVals; + splitDimVals.reserve(splitNode.m_entries); + for (const auto& lp : splitNode.m_locationPayloads) { + splitDimVals.push_back(lp.location[splitNode.m_splitDimension]); + } + std::nth_element(splitDimVals.begin(), splitDimVals.begin() + splitDimVals.size() / 2 + 1, splitDimVals.end()); + std::nth_element(splitDimVals.begin(), splitDimVals.begin() + splitDimVals.size() / 2, + splitDimVals.begin() + splitDimVals.size() / 2 + 1); + splitNode.m_splitValue = + (splitDimVals[splitDimVals.size() / 2] + splitDimVals[splitDimVals.size() / 2 + 1]) / Scalar(2); + + splitNode.m_children = std::make_pair(m_nodes.size(), m_nodes.size() + 1); + std::size_t entries = splitNode.m_entries; + m_nodes.emplace_back(m_bucketRecycle, entries); + Node& leftNode = m_nodes.back(); + m_nodes.emplace_back(entries); + Node& rightNode = m_nodes.back(); + + for (const auto& lp : splitNode.m_locationPayloads) { + if (lp.location[splitNode.m_splitDimension] < splitNode.m_splitValue) { + leftNode.add(lp); + } else { + rightNode.add(lp); + } + } + + if (leftNode.m_entries == 0 || rightNode.m_entries == 0) { + splitNode.m_splitValue = 0; + splitNode.m_splitDimension = Dimensions; + splitNode.m_children = std::pair(0, 0); + std::swap(leftNode.m_locationPayloads, m_bucketRecycle); + m_nodes.pop_back(); + m_nodes.pop_back(); + return false; + } else { + splitNode.m_locationPayloads.clear(); + if (splitNode.m_locationPayloads.capacity() == BucketSize) { + std::swap(splitNode.m_locationPayloads, m_bucketRecycle); + } else { + std::vector empty; + std::swap(splitNode.m_locationPayloads, empty); + } + return true; + } + } + + struct Node { + Node(std::size_t capacity) { init(capacity); } + + Node(std::vector& recycle, std::size_t capacity) { + std::swap(m_locationPayloads, recycle); + init(capacity); + } + + void init(std::size_t capacity) { + m_bounds.fill({{std::numeric_limits::max(), std::numeric_limits::lowest()}}); + m_locationPayloads.reserve(std::max(BucketSize, capacity)); + } + + void expandBounds(const point_t& location) { + for (std::size_t i = 0; i < Dimensions; i++) { + if (m_bounds[i][0] > location[i]) { + m_bounds[i][0] = location[i]; + } + if (m_bounds[i][1] < location[i]) { + m_bounds[i][1] = location[i]; + } + } + m_entries++; + } + + void add(const LocationPayload& lp) { + expandBounds(lp.location); + m_locationPayloads.push_back(lp); + } + + bool shouldSplit() const { return m_entries >= BucketSize; } + + void searchCapacityLimitedBall(const point_t& location, Scalar maxRadius, std::size_t K, + std::priority_queue& results) const { + std::size_t i = 0; + + // this fills up the queue if it isn't full yet + for (; results.size() < K && i < m_entries; i++) { + const auto& lp = m_locationPayloads[i]; + Scalar distance = Distance::distance(location, lp.location); + if (distance < maxRadius) { + results.emplace(DistancePayload{distance, lp.payload}); + } + } + + // this adds new things to the queue once it is full + for (; i < m_entries; i++) { + const auto& lp = m_locationPayloads[i]; + Scalar distance = Distance::distance(location, lp.location); + if (distance < maxRadius && distance < results.top().distance) { + results.pop(); + results.emplace(DistancePayload{distance, lp.payload}); + } + } + } + + void queueChildren(const point_t& location, std::vector& searchStack) const { + if (location[m_splitDimension] < m_splitValue) { + searchStack.push_back(m_children.second); + searchStack.push_back(m_children.first); // left is popped first + } else { + searchStack.push_back(m_children.first); + searchStack.push_back(m_children.second); // right is popped first + } + } + + Scalar pointRectDist(const point_t& location) const { + point_t closestBoundsPoint; + + for (std::size_t i = 0; i < Dimensions; i++) { + if (m_bounds[i][0] > location[i]) { + closestBoundsPoint[i] = m_bounds[i][0]; + } else if (m_bounds[i][1] < location[i]) { + closestBoundsPoint[i] = m_bounds[i][1]; + } else { + closestBoundsPoint[i] = location[i]; + } + } + return Distance::distance(closestBoundsPoint, location); + } + + std::size_t m_entries = 0; /// size of the tree, or subtree + + std::size_t m_splitDimension = Dimensions; /// split dimension of this node + Scalar m_splitValue = 0; /// split value of this node + + std::array, Dimensions> m_bounds; /// bounding box of this node + + std::pair m_children; /// subtrees of this node (if not a leaf) + std::vector m_locationPayloads; /// data held in this node (if a leaf) + }; +}; +} +} diff --git a/avoidance/src/common.cpp b/avoidance/src/common.cpp index b9c49408a..208c8dbf0 100644 --- a/avoidance/src/common.cpp +++ b/avoidance/src/common.cpp @@ -278,6 +278,7 @@ double getAngularVelocity(float desired_yaw, float curr_yaw) { return 0.5 * static_cast(vel); } + void transformToTrajectory(mavros_msgs::Trajectory& obst_avoid, geometry_msgs::PoseStamped pose, geometry_msgs::Twist vel) { obst_avoid.header.stamp = ros::Time::now(); diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h index 4e963048f..d317c2152 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -3,6 +3,7 @@ #include "avoidance/common.h" #include "avoidance/histogram.h" +#include "avoidance/kdtree.h" #include "candidate_direction.h" #include "cost_parameters.h" @@ -16,6 +17,8 @@ namespace avoidance { +using kdtree_t = jk::tree::KDTree; + /** * @brief crops and subsamples the incomming data, then combines it with * the data from the last timestep @@ -46,6 +49,9 @@ void processPointcloud(pcl::PointCloud& final_cloud, void generateNewHistogram(Histogram& polar_histogram, const pcl::PointCloud& cropped_cloud, const Eigen::Vector3f& position); +void generateHistogramHACK(Histogram& polar_histogram, const kdtree_t& cropped_cloud, const Eigen::Vector3f& position, + float distance); + /** * @brief compresses the histogram such that for each azimuth the minimum * distance at the elevation inside the FOV is saved diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index a5828edda..9c10f9c5f 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -75,6 +75,23 @@ void processPointcloud(pcl::PointCloud& final_cloud, final_cloud.width = final_cloud.points.size(); } +void generateHistogramHACK(Histogram& polar_histogram, const kdtree_t& cropped_cloud, const Eigen::Vector3f& position, + float distance) { + // for each bin put a point in that direction and get the distance to the nearest obstacle + for (int e = 0; e < GRID_LENGTH_E; e++) { + for (int z = 0; z < GRID_LENGTH_Z; z++) { + Eigen::Vector3f eval = polarHistogramToCartesian(histogramIndexToPolar(e, z, ALPHA_RES, distance), position); + std::array point{eval.x(), eval.y(), eval.z()}; + auto nearest = cropped_cloud.search(point); + if (!std::isinf(nearest.distance)) { + polar_histogram.set_dist(e, z, nearest.distance); + } else { + polar_histogram.set_dist(e, z, 0); + } + } + } +} + // Generate new histogram from pointcloud void generateNewHistogram(Histogram& polar_histogram, const pcl::PointCloud& cropped_cloud, const Eigen::Vector3f& position) { diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 46a6f342b..6fb183e5a 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -67,13 +67,19 @@ void StarPlanner::buildLookAheadTree() { tree_.push_back(TreeNode(0, start_state, Eigen::Vector3f::Zero())); tree_.back().setCosts(treeHeuristicFunction(0), treeHeuristicFunction(0)); + kdtree_t tree_cloud; + for (auto point : cloud_) { + tree_cloud.addPoint(toArray(toEigen(point)), point.intensity, false); + } + tree_cloud.splitOutstanding(); + int origin = 0; for (int n = 0; n < n_expanded_nodes_ && is_expanded_node; n++) { Eigen::Vector3f origin_position = tree_[origin].getPosition(); Eigen::Vector3f origin_velocity = tree_[origin].getVelocity(); histogram.setZero(); - generateNewHistogram(histogram, cloud_, origin_position); + generateHistogramHACK(histogram, tree_cloud, origin_position, 2.5f);// hack: project 2.5m into each histogram cell // calculate candidates cost_matrix.fill(0.f); From 7a99f0ae83bd8164d26546d830c56c9a55833cb0 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Fri, 27 Sep 2019 17:03:38 +0200 Subject: [PATCH 15/28] KDTree: add getAllPoints() and size() --- avoidance/include/avoidance/kdtree.h | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/avoidance/include/avoidance/kdtree.h b/avoidance/include/avoidance/kdtree.h index dc259108b..0ba3558ad 100644 --- a/avoidance/include/avoidance/kdtree.h +++ b/avoidance/include/avoidance/kdtree.h @@ -175,6 +175,33 @@ class KDTree { } } + std::vector> getAllPoints() const { + std::vector> result; + result.reserve(size()); + if (size() > 0) { + std::vector searchStack; + searchStack.reserve(1 + std::size_t(1.5 * std::log2(1 + size() / BucketSize))); + searchStack.push_back(0); + + while (searchStack.size() > 0) { + std::size_t nodeIndex = searchStack.back(); + searchStack.pop_back(); + const Node& node = m_nodes[nodeIndex]; + if (node.m_splitDimension == Dimensions) { + for (const auto& lp : node.m_locationPayloads) { + result.emplace_back(lp.location, lp.payload); + } + } else { + searchStack.push_back(node.m_children.first); + searchStack.push_back(node.m_children.second); // right is popped first + } + } + } + return result; + } + + std::size_t size() const { return m_nodes[0].m_entries; } + void splitOutstanding() { std::vector searchStack(waitingForSplit.begin(), waitingForSplit.end()); waitingForSplit.clear(); From d14300bb690377c89d2aa49b82ee6b6693bc515e Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Fri, 27 Sep 2019 17:15:41 +0200 Subject: [PATCH 16/28] KDTree: replace PCL cloud in planner --- .../include/local_planner/local_planner.h | 5 +- .../include/local_planner/planner_functions.h | 11 ++- .../include/local_planner/star_planner.h | 7 +- local_planner/src/nodes/local_planner.cpp | 12 ++- .../src/nodes/local_planner_visualization.cpp | 2 +- local_planner/src/nodes/planner_functions.cpp | 80 ++++++------------- local_planner/src/nodes/star_planner.cpp | 10 +-- 7 files changed, 50 insertions(+), 77 deletions(-) diff --git a/local_planner/include/local_planner/local_planner.h b/local_planner/include/local_planner/local_planner.h index 34328f1c9..baed96fa3 100644 --- a/local_planner/include/local_planner/local_planner.h +++ b/local_planner/include/local_planner/local_planner.h @@ -27,6 +27,7 @@ #include #include #include +#include "avoidance/kdtree.h" namespace avoidance { @@ -57,7 +58,7 @@ class LocalPlanner { std::unique_ptr star_planner_; costParameters cost_params_; - pcl::PointCloud final_cloud_; + kdtree_t final_cloud_; Eigen::Vector3f position_ = Eigen::Vector3f::Zero(); Eigen::Vector3f velocity_ = Eigen::Vector3f::Zero(); @@ -178,7 +179,7 @@ class LocalPlanner { * @brief getter method to visualize the pointcloud used for planning * @returns reference to pointcloud **/ - const pcl::PointCloud& getPointcloud() const; + pcl::PointCloud getPointcloud() const; /** * @brief getter method to visualize the tree in rviz diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h index d317c2152..3c073ae79 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -17,7 +17,7 @@ namespace avoidance { -using kdtree_t = jk::tree::KDTree; +using kdtree_t = jk::tree::KDTree; /** * @brief crops and subsamples the incomming data, then combines it with @@ -33,11 +33,10 @@ using kdtree_t = jk::tree::KDTree; * be kept, less points are discarded as noise (careful: 0 is not * a valid input here) **/ -void processPointcloud(pcl::PointCloud& final_cloud, - const std::vector>& complete_cloud, const std::vector& fov, - float yaw_fcu_frame_deg, float pitch_fcu_frame_deg, const Eigen::Vector3f& position, - float min_sensor_range, float max_sensor_range, float max_age, float elapsed_s, - int min_num_points_per_cell); +void processPointcloud(kdtree_t& final_cloud, const std::vector>& complete_cloud, + const std::vector& fov, float yaw_fcu_frame_deg, float pitch_fcu_frame_deg, + const Eigen::Vector3f& position, float min_sensor_range, float max_sensor_range, float max_age, + float elapsed_s, int min_num_points_per_cell); /** * @brief calculates a histogram from the current frame pointcloud around diff --git a/local_planner/include/local_planner/star_planner.h b/local_planner/include/local_planner/star_planner.h index ea4048f15..f3bfcffc3 100644 --- a/local_planner/include/local_planner/star_planner.h +++ b/local_planner/include/local_planner/star_planner.h @@ -12,11 +12,14 @@ #include +#include #include #include #include +using kdtree_t = jk::tree::KDTree; + namespace avoidance { class TreeNode; @@ -31,7 +34,7 @@ class StarPlanner { float max_sensor_range_ = 15.f; float min_sensor_range_ = 0.2f; - pcl::PointCloud cloud_; + kdtree_t cloud_; Eigen::Vector3f goal_ = Eigen::Vector3f(NAN, NAN, NAN); Eigen::Vector3f position_ = Eigen::Vector3f(NAN, NAN, NAN); @@ -69,7 +72,7 @@ class StarPlanner { * @brief setter method for star_planner pointcloud * @param[in] cloud, processed data already cropped and combined with history **/ - void setPointcloud(const pcl::PointCloud& cloud); + void setPointcloud(const kdtree_t& cloud); /** * @brief setter method for vehicle position diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index 7d9c75589..f32cce693 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -88,7 +88,7 @@ void LocalPlanner::create2DObstacleRepresentation(const bool send_to_fcu) { // or if it is required by the FCU Histogram new_histogram = Histogram(ALPHA_RES); to_fcu_histogram_.setZero(); - generateNewHistogram(new_histogram, final_cloud_, position_); + generateHistogramHACK(new_histogram, final_cloud_, position_, 2.5f); // HACK: hard-code 2.5m outward projection if (send_to_fcu) { compressHistogramElevation(to_fcu_histogram_, new_histogram, position_); @@ -196,7 +196,15 @@ void LocalPlanner::updateObstacleDistanceMsg() { Eigen::Vector3f LocalPlanner::getPosition() const { return position_; } -const pcl::PointCloud& LocalPlanner::getPointcloud() const { return final_cloud_; } +pcl::PointCloud LocalPlanner::getPointcloud() const { + pcl::PointCloud ret; + std::vector, float>> local_cloud = final_cloud_.getAllPoints(); + for (const auto& p : local_cloud) { + ret.push_back(toXYZI(p.first[0], p.first[1], p.first[2], p.second)); + } + ret.header.frame_id = "/local_origin"; + return ret; +} void LocalPlanner::setDefaultPx4Parameters() { px4_.param_mpc_auto_mode = 1; diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp index 63b061c13..6cdca0c93 100644 --- a/local_planner/src/nodes/local_planner_visualization.cpp +++ b/local_planner/src/nodes/local_planner_visualization.cpp @@ -11,7 +11,7 @@ namespace avoidance { // initialize subscribers for local planner visualization topics void LocalPlannerVisualization::initializePublishers(ros::NodeHandle& nh) { - local_pointcloud_pub_ = nh.advertise>("/local_pointcloud", 1); + local_pointcloud_pub_ = nh.advertise>("/local_pointcloud", 1); pointcloud_size_pub_ = nh.advertise("/pointcloud_size", 1); bounding_box_pub_ = nh.advertise("/bounding_box", 1); ground_measurement_pub_ = nh.advertise("/ground_measurement", 1); diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index 9c10f9c5f..f7253d47a 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -9,70 +9,38 @@ namespace avoidance { // trim the point cloud so that only one valid point per histogram cell is around -void processPointcloud(pcl::PointCloud& final_cloud, - const std::vector>& complete_cloud, const std::vector& fov, - float yaw_fcu_frame_deg, float pitch_fcu_frame_deg, const Eigen::Vector3f& position, - float min_sensor_range, float max_sensor_range, float max_age, float elapsed_s, - int min_num_points_per_cell) { - const int SCALE_FACTOR = 3; - pcl::PointCloud old_cloud; - std::swap(final_cloud, old_cloud); - final_cloud.points.clear(); - final_cloud.width = 0; - final_cloud.points.reserve((SCALE_FACTOR * GRID_LENGTH_Z) * (SCALE_FACTOR * GRID_LENGTH_E)); - - // counter to keep track of how many points lie in a given cell - Eigen::MatrixXi histogram_points_counter(180 / (ALPHA_RES / SCALE_FACTOR), 360 / (ALPHA_RES / SCALE_FACTOR)); - histogram_points_counter.fill(0); - - auto sqr = [](float f) { return f * f; }; - +void processPointcloud(kdtree_t& final_cloud, const std::vector>& complete_cloud, + const std::vector& fov, float yaw_fcu_frame_deg, float pitch_fcu_frame_deg, + const Eigen::Vector3f& position, float min_sensor_range, float max_sensor_range, float max_age, + float elapsed_s, int min_num_points_per_cell) { + const float MIN_SPATIAL_SAMPLING_M = 0.5f; + std::vector, float>> memory = final_cloud.getAllPoints(); + kdtree_t new_cloud; + std::swap(final_cloud, new_cloud); for (const auto& cloud : complete_cloud) { for (const pcl::PointXYZ& xyz : cloud) { - // Check if the point is invalid - if (!std::isnan(xyz.x) && !std::isnan(xyz.y) && !std::isnan(xyz.z)) { - float distanceSq = (position - toEigen(xyz)).squaredNorm(); - if (sqr(min_sensor_range) < distanceSq && distanceSq < sqr(max_sensor_range)) { - // subsampling the cloud - PolarPoint p_pol = cartesianToPolarHistogram(toEigen(xyz), position); - Eigen::Vector2i p_ind = polarToHistogramIndex(p_pol, ALPHA_RES / SCALE_FACTOR); - histogram_points_counter(p_ind.y(), p_ind.x())++; - if (histogram_points_counter(p_ind.y(), p_ind.x()) == min_num_points_per_cell) { - final_cloud.points.push_back(toXYZI(toEigen(xyz), 0.0f)); - } - } + auto nearest = final_cloud.search(toArray(toEigen(xyz))); + // if it's too far away, forget point + if (std::isinf(nearest.distance) || nearest.distance > MIN_SPATIAL_SAMPLING_M) { + final_cloud.addPoint(toArray(toEigen(xyz)), 0.0f, true); } } } - // combine with old cloud - for (const pcl::PointXYZI& xyzi : old_cloud) { - float distanceSq = (position - toEigen(xyzi)).squaredNorm(); - if (distanceSq < sqr(max_sensor_range)) { - // adding older points if not expired and space is free according to new cloud - PolarPoint p_pol = cartesianToPolarHistogram(toEigen(xyzi), position); - PolarPoint p_pol_fcu = cartesianToPolarFCU(toEigen(xyzi), position); - p_pol_fcu.e -= pitch_fcu_frame_deg; - p_pol_fcu.z -= yaw_fcu_frame_deg; - wrapPolar(p_pol_fcu); - Eigen::Vector2i p_ind = polarToHistogramIndex(p_pol, ALPHA_RES / SCALE_FACTOR); - - // only remember point if it's in a cell not previously populated by complete_cloud, as well as outside FOV and - // 'young' enough - if (histogram_points_counter(p_ind.y(), p_ind.x()) < min_num_points_per_cell && xyzi.intensity < max_age && - !pointInsideFOV(fov, p_pol_fcu)) { - final_cloud.points.push_back(toXYZI(toEigen(xyzi), xyzi.intensity + elapsed_s)); - - // to indicate that this cell now has a point - histogram_points_counter(p_ind.y(), p_ind.x()) = min_num_points_per_cell; - } + for (const auto& p : memory) { + auto nearest = final_cloud.search(p.first); + // if it's too far away, forget point + Eigen::Vector3f point(p.first[0], p.first[1], p.first[2]); + PolarPoint p_pol_fcu = cartesianToPolarFCU(point, position); + p_pol_fcu.e -= pitch_fcu_frame_deg; + p_pol_fcu.z -= yaw_fcu_frame_deg; + wrapPolar(p_pol_fcu); + + if (!std::isinf(nearest.distance) && nearest.distance > MIN_SPATIAL_SAMPLING_M && + (point - position).norm() < max_sensor_range && p.second < max_age && !pointInsideFOV(fov, p_pol_fcu)) { + final_cloud.addPoint(p.first, p.second + elapsed_s, true); } } - - final_cloud.header.stamp = complete_cloud[0].header.stamp; - final_cloud.header.frame_id = complete_cloud[0].header.frame_id; - final_cloud.height = 1; - final_cloud.width = final_cloud.points.size(); } void generateHistogramHACK(Histogram& polar_histogram, const kdtree_t& cropped_cloud, const Eigen::Vector3f& position, diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 6fb183e5a..a65041c62 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -35,7 +35,7 @@ void StarPlanner::setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel void StarPlanner::setGoal(const Eigen::Vector3f& goal) { goal_ = goal; } -void StarPlanner::setPointcloud(const pcl::PointCloud& cloud) { cloud_ = cloud; } +void StarPlanner::setPointcloud(const kdtree_t& cloud) { cloud_ = cloud; } void StarPlanner::setClosestPointOnLine(const Eigen::Vector3f& closest_pt) { closest_pt_ = closest_pt; } @@ -67,19 +67,13 @@ void StarPlanner::buildLookAheadTree() { tree_.push_back(TreeNode(0, start_state, Eigen::Vector3f::Zero())); tree_.back().setCosts(treeHeuristicFunction(0), treeHeuristicFunction(0)); - kdtree_t tree_cloud; - for (auto point : cloud_) { - tree_cloud.addPoint(toArray(toEigen(point)), point.intensity, false); - } - tree_cloud.splitOutstanding(); - int origin = 0; for (int n = 0; n < n_expanded_nodes_ && is_expanded_node; n++) { Eigen::Vector3f origin_position = tree_[origin].getPosition(); Eigen::Vector3f origin_velocity = tree_[origin].getVelocity(); histogram.setZero(); - generateHistogramHACK(histogram, tree_cloud, origin_position, 2.5f);// hack: project 2.5m into each histogram cell + generateHistogramHACK(histogram, cloud_, origin_position, 2.5f); // hack: project 2.5m into each histogram cell // calculate candidates cost_matrix.fill(0.f); From 717f4dd24735e390dcd1e86a6ec41de22fea287f Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Fri, 27 Sep 2019 17:16:13 +0200 Subject: [PATCH 17/28] Fix style --- avoidance/include/avoidance/common.h | 6 +++--- avoidance/src/common.cpp | 1 - 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/avoidance/include/avoidance/common.h b/avoidance/include/avoidance/common.h index 389c1469b..2df766b7e 100644 --- a/avoidance/include/avoidance/common.h +++ b/avoidance/include/avoidance/common.h @@ -395,9 +395,9 @@ inline Eigen::Vector3f toEigen(const pcl::PointXYZI& p) { return ev3; } -inline std::array toArray(const Eigen::Vector3f& ev3){ - std::array ret {ev3.x(), ev3.y(), ev3.z()}; - return ret; +inline std::array toArray(const Eigen::Vector3f& ev3) { + std::array ret{ev3.x(), ev3.y(), ev3.z()}; + return ret; } inline Eigen::Quaternionf toEigen(const geometry_msgs::Quaternion& gmq) { diff --git a/avoidance/src/common.cpp b/avoidance/src/common.cpp index 208c8dbf0..b9c49408a 100644 --- a/avoidance/src/common.cpp +++ b/avoidance/src/common.cpp @@ -278,7 +278,6 @@ double getAngularVelocity(float desired_yaw, float curr_yaw) { return 0.5 * static_cast(vel); } - void transformToTrajectory(mavros_msgs::Trajectory& obst_avoid, geometry_msgs::PoseStamped pose, geometry_msgs::Twist vel) { obst_avoid.header.stamp = ros::Time::now(); From e8a020ae4bb116bf74cac37519c3ac6558753850 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Mon, 30 Sep 2019 10:49:32 +0200 Subject: [PATCH 18/28] star_planner: remove histogram --- .../include/local_planner/planner_functions.h | 3 ++ local_planner/src/nodes/planner_functions.cpp | 12 ++++++ local_planner/src/nodes/star_planner.cpp | 41 ++++++------------- 3 files changed, 28 insertions(+), 28 deletions(-) diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h index 3c073ae79..5b9a36e8f 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -6,6 +6,7 @@ #include "avoidance/kdtree.h" #include "candidate_direction.h" #include "cost_parameters.h" +#include "tree_node.h" #include @@ -122,6 +123,8 @@ std::pair costFunction(const PolarPoint& candidate_polar, float ob const Eigen::Vector3f& velocity, const costParameters& cost_params, const Eigen::Vector3f& closest_pt, const bool is_obstacle_facing_goal); +float simpleCost(const TreeNode& node, const Eigen::Vector3f& goal, const costParameters& cost_params,const kdtree_t& cloud); + /** * @brief max-median filtes the cost matrix * @param matrix, cost matrix diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index f7253d47a..355c4c327 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -353,6 +353,18 @@ void padPolarMatrix(const Eigen::MatrixXf& matrix, unsigned int n_lines_padding, matrix_padded.block(0, n_lines_padding, matrix_padded.rows(), n_lines_padding); } +float simpleCost(const TreeNode& node, const Eigen::Vector3f& goal, const costParameters& cost_params, const kdtree_t& cloud){ + const Eigen::Vector3f node_position = node.getPosition(); + const std::array node_point{node_position.x(), node_position.y(), node_position.z()}; + const auto nearest = cloud.search(node_point); + const PolarPoint facing_goal = cartesianToPolarHistogram(goal, node_position); + const float angle_diff = angleDifference(cartesianToPolarHistogram(node.getSetpoint(), node_position).z, facing_goal.z); + + const float smoothness_cost = cost_params.velocity_cost_param * node.getVelocity().dot(node.getSetpoint()); + const float distance_cost = cost_params.obstacle_cost_param * nearest.distance; + const float yaw_cost = cost_params.yaw_cost_param * angle_diff * angle_diff; +} + // cost function for every histogram cell std::pair costFunction(const PolarPoint& candidate_polar, float obstacle_distance, const Eigen::Vector3f& goal, const Eigen::Vector3f& position, diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index a65041c62..dc7b0a09f 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -47,13 +47,17 @@ 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; - Eigen::MatrixXf cost_matrix; - bool is_expanded_node = true; + // Simple 6-way unit direction setpoints allowed only. + // TODO: If compute allowws, make this more fine-grained + const std::vector candidates{Eigen::Vector3f{1.0f, 0.0f, 0.0f}, + Eigen::Vector3f{0.5f, 0.5f, 0.0f}, + Eigen::Vector3f{0.5f, -0.5f, 0.0f}, + Eigen::Vector3f{0.0f, 1.0f, 0.0f}, + Eigen::Vector3f{0.0f, 0.0f, 1.0f}, + Eigen::Vector3f{-1.0f, 0.0f, 0.0f}, + Eigen::Vector3f{0.0f, -1.0f, 0.0f}, + Eigen::Vector3f{0.0f, 0.0f, -1.0f}}; tree_.clear(); closed_set_.clear(); @@ -72,20 +76,6 @@ void StarPlanner::buildLookAheadTree() { Eigen::Vector3f origin_position = tree_[origin].getPosition(); Eigen::Vector3f origin_velocity = tree_[origin].getVelocity(); - histogram.setZero(); - generateHistogramHACK(histogram, cloud_, origin_position, 2.5f); // hack: project 2.5m into each histogram cell - - // calculate candidates - cost_matrix.fill(0.f); - cost_image_data.clear(); - candidate_vector.clear(); - getCostMatrix(histogram, goal_, origin_position, origin_velocity, cost_params_, smoothing_margin_degrees_, - closest_pt_, max_sensor_range_, min_sensor_range_, cost_matrix, cost_image_data); - if (n != 0) { - starting_direction_ = tree_[origin].getSetpoint(); - } - getBestCandidatesFromCostMatrix(cost_matrix, children_per_node_, candidate_vector, starting_direction_); - simulation_limits limits = lims_; simulation_state state = tree_[origin].state; limits.max_xy_velocity_norm = std::min( @@ -96,15 +86,12 @@ void StarPlanner::buildLookAheadTree() { lims_.max_xy_velocity_norm); // 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) { + for (const auto& candidate : candidates) { simulation_state state = tree_[origin].state; TrajectorySimulator sim(limits, state, 0.05f); // todo: parameterize simulation step size [s] - std::vector trajectory = sim.generate_trajectory(candidate.toEigen(), tree_node_duration_); + std::vector trajectory = sim.generate_trajectory(candidate, tree_node_duration_); // check if another close node has been added float dist = 1.f; @@ -121,7 +108,7 @@ void StarPlanner::buildLookAheadTree() { 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; + tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + simpleCost(tree_.back(), goal_, cost_params_, cloud_) + h; children++; } } @@ -152,8 +139,6 @@ void StarPlanner::buildLookAheadTree() { } } - cost_image_data.clear(); - candidate_vector.clear(); } float min_cost_per_depth = FLT_MAX; From 5c71530a740a1aadbe8e7d9cbfcf50a687d82343 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Mon, 30 Sep 2019 10:50:29 +0200 Subject: [PATCH 19/28] star_planner: plan to goal --- local_planner/src/nodes/star_planner.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index dc7b0a09f..d4e99b1c3 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -47,7 +47,6 @@ float StarPlanner::treeHeuristicFunction(int node_number) const { void StarPlanner::buildLookAheadTree() { std::clock_t start_time = std::clock(); - bool is_expanded_node = true; // Simple 6-way unit direction setpoints allowed only. // TODO: If compute allowws, make this more fine-grained const std::vector candidates{Eigen::Vector3f{1.0f, 0.0f, 0.0f}, @@ -58,6 +57,7 @@ void StarPlanner::buildLookAheadTree() { Eigen::Vector3f{-1.0f, 0.0f, 0.0f}, Eigen::Vector3f{0.0f, -1.0f, 0.0f}, Eigen::Vector3f{0.0f, 0.0f, -1.0f}}; + bool has_reached_goal = false; tree_.clear(); closed_set_.clear(); @@ -72,7 +72,7 @@ void StarPlanner::buildLookAheadTree() { tree_.back().setCosts(treeHeuristicFunction(0), treeHeuristicFunction(0)); int origin = 0; - for (int n = 0; n < n_expanded_nodes_ && is_expanded_node; n++) { + while (!has_reached_goal) { Eigen::Vector3f origin_position = tree_[origin].getPosition(); Eigen::Vector3f origin_velocity = tree_[origin].getVelocity(); @@ -104,22 +104,22 @@ void StarPlanner::buildLookAheadTree() { } } - if (children < (children_per_node_) && close_nodes == 0) { - tree_.push_back(TreeNode(origin, trajectory.back(), candidate.toEigen())); + if (close_nodes == 0) { + tree_.push_back(TreeNode(origin, trajectory.back(), candidate)); float h = treeHeuristicFunction(tree_.size() - 1); tree_.back().heuristic_ = h; tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + simpleCost(tree_.back(), goal_, cost_params_, cloud_) + h; children++; } } - } + closed_set_.push_back(origin); tree_[origin].closed_ = true; // find best node to continue float minimal_cost = HUGE_VAL; - is_expanded_node = false; + has_reached_goal = 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 @@ -127,18 +127,21 @@ void StarPlanner::buildLookAheadTree() { 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); + has_reached_goal = true; break; } - float node_distance = (tree_[i].getPosition() - position_).norm(); - if (tree_[i].total_cost_ < minimal_cost && node_distance < max_path_length_) { + if (tree_[i].total_cost_ < minimal_cost ) { minimal_cost = tree_[i].total_cost_; origin = i; - is_expanded_node = true; } } } + // if there is only one node in the tree, we already expanded it. + if (tree_.size() == 1){ + has_reached_goal = true; + } } float min_cost_per_depth = FLT_MAX; From a48638662cca5aebcd5aca26af0851e3a14dde0a Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Mon, 30 Sep 2019 10:50:52 +0200 Subject: [PATCH 20/28] Fix style --- .../include/local_planner/planner_functions.h | 3 +- local_planner/src/nodes/planner_functions.cpp | 8 ++- local_planner/src/nodes/star_planner.cpp | 64 +++++++++---------- 3 files changed, 37 insertions(+), 38 deletions(-) diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h index 5b9a36e8f..08698be88 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -123,7 +123,8 @@ std::pair costFunction(const PolarPoint& candidate_polar, float ob const Eigen::Vector3f& velocity, const costParameters& cost_params, const Eigen::Vector3f& closest_pt, const bool is_obstacle_facing_goal); -float simpleCost(const TreeNode& node, const Eigen::Vector3f& goal, const costParameters& cost_params,const kdtree_t& cloud); +float simpleCost(const TreeNode& node, const Eigen::Vector3f& goal, const costParameters& cost_params, + const kdtree_t& cloud); /** * @brief max-median filtes the cost matrix diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index 355c4c327..2e46130b0 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -353,16 +353,18 @@ void padPolarMatrix(const Eigen::MatrixXf& matrix, unsigned int n_lines_padding, matrix_padded.block(0, n_lines_padding, matrix_padded.rows(), n_lines_padding); } -float simpleCost(const TreeNode& node, const Eigen::Vector3f& goal, const costParameters& cost_params, const kdtree_t& cloud){ +float simpleCost(const TreeNode& node, const Eigen::Vector3f& goal, const costParameters& cost_params, + const kdtree_t& cloud) { const Eigen::Vector3f node_position = node.getPosition(); const std::array node_point{node_position.x(), node_position.y(), node_position.z()}; const auto nearest = cloud.search(node_point); const PolarPoint facing_goal = cartesianToPolarHistogram(goal, node_position); - const float angle_diff = angleDifference(cartesianToPolarHistogram(node.getSetpoint(), node_position).z, facing_goal.z); + const float angle_diff = + angleDifference(cartesianToPolarHistogram(node.getSetpoint(), node_position).z, facing_goal.z); const float smoothness_cost = cost_params.velocity_cost_param * node.getVelocity().dot(node.getSetpoint()); const float distance_cost = cost_params.obstacle_cost_param * nearest.distance; - const float yaw_cost = cost_params.yaw_cost_param * angle_diff * angle_diff; + const float yaw_cost = cost_params.yaw_cost_param * angle_diff * angle_diff; } // cost function for every histogram cell diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index d4e99b1c3..25ded61f6 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -49,14 +49,10 @@ void StarPlanner::buildLookAheadTree() { std::clock_t start_time = std::clock(); // Simple 6-way unit direction setpoints allowed only. // TODO: If compute allowws, make this more fine-grained - const std::vector candidates{Eigen::Vector3f{1.0f, 0.0f, 0.0f}, - Eigen::Vector3f{0.5f, 0.5f, 0.0f}, - Eigen::Vector3f{0.5f, -0.5f, 0.0f}, - Eigen::Vector3f{0.0f, 1.0f, 0.0f}, - Eigen::Vector3f{0.0f, 0.0f, 1.0f}, - Eigen::Vector3f{-1.0f, 0.0f, 0.0f}, - Eigen::Vector3f{0.0f, -1.0f, 0.0f}, - Eigen::Vector3f{0.0f, 0.0f, -1.0f}}; + const std::vector candidates{Eigen::Vector3f{1.0f, 0.0f, 0.0f}, Eigen::Vector3f{0.5f, 0.5f, 0.0f}, + Eigen::Vector3f{0.5f, -0.5f, 0.0f}, Eigen::Vector3f{0.0f, 1.0f, 0.0f}, + Eigen::Vector3f{0.0f, 0.0f, 1.0f}, Eigen::Vector3f{-1.0f, 0.0f, 0.0f}, + Eigen::Vector3f{0.0f, -1.0f, 0.0f}, Eigen::Vector3f{0.0f, 0.0f, -1.0f}}; bool has_reached_goal = false; tree_.clear(); @@ -86,33 +82,33 @@ void StarPlanner::buildLookAheadTree() { lims_.max_xy_velocity_norm); // add candidates as nodes - // insert new nodes - int children = 0; - for (const auto& candidate : candidates) { - simulation_state state = tree_[origin].state; - TrajectorySimulator sim(limits, state, 0.05f); // todo: parameterize simulation step size [s] - std::vector trajectory = sim.generate_trajectory(candidate, tree_node_duration_); - - // check if another close node has been added - float dist = 1.f; - int close_nodes = 0; - for (size_t i = 0; i < tree_.size(); i++) { - dist = (tree_[i].getPosition() - trajectory.back().position).norm(); - if (dist < 0.2f) { - close_nodes++; - break; - } - } - - if (close_nodes == 0) { - tree_.push_back(TreeNode(origin, trajectory.back(), candidate)); - float h = treeHeuristicFunction(tree_.size() - 1); - tree_.back().heuristic_ = h; - tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + simpleCost(tree_.back(), goal_, cost_params_, cloud_) + h; - children++; + // insert new nodes + int children = 0; + for (const auto& candidate : candidates) { + simulation_state state = tree_[origin].state; + TrajectorySimulator sim(limits, state, 0.05f); // todo: parameterize simulation step size [s] + std::vector trajectory = sim.generate_trajectory(candidate, tree_node_duration_); + + // check if another close node has been added + float dist = 1.f; + int close_nodes = 0; + for (size_t i = 0; i < tree_.size(); i++) { + dist = (tree_[i].getPosition() - trajectory.back().position).norm(); + if (dist < 0.2f) { + close_nodes++; + break; } } + if (close_nodes == 0) { + tree_.push_back(TreeNode(origin, trajectory.back(), candidate)); + float h = treeHeuristicFunction(tree_.size() - 1); + tree_.back().heuristic_ = h; + tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + + simpleCost(tree_.back(), goal_, cost_params_, cloud_) + h; + children++; + } + } closed_set_.push_back(origin); tree_[origin].closed_ = true; @@ -131,7 +127,7 @@ void StarPlanner::buildLookAheadTree() { break; } - if (tree_[i].total_cost_ < minimal_cost ) { + if (tree_[i].total_cost_ < minimal_cost) { minimal_cost = tree_[i].total_cost_; origin = i; } @@ -139,7 +135,7 @@ void StarPlanner::buildLookAheadTree() { } // if there is only one node in the tree, we already expanded it. - if (tree_.size() == 1){ + if (tree_.size() == 1) { has_reached_goal = true; } } From 6dc0c5076dd77a7288aab92d83d2e40bfce627a3 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Mon, 30 Sep 2019 14:48:58 +0200 Subject: [PATCH 21/28] star_planner: use orientation to body-align setpoints --- .../include/local_planner/star_planner.h | 3 ++- local_planner/src/nodes/local_planner.cpp | 2 +- local_planner/src/nodes/star_planner.cpp | 15 +++++++++------ 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/local_planner/include/local_planner/star_planner.h b/local_planner/include/local_planner/star_planner.h index f3bfcffc3..ecb500afe 100644 --- a/local_planner/include/local_planner/star_planner.h +++ b/local_planner/include/local_planner/star_planner.h @@ -39,6 +39,7 @@ class StarPlanner { Eigen::Vector3f goal_ = Eigen::Vector3f(NAN, NAN, NAN); Eigen::Vector3f position_ = Eigen::Vector3f(NAN, NAN, NAN); Eigen::Vector3f velocity_ = Eigen::Vector3f(NAN, NAN, NAN); + Eigen::Quaternionf q_ = Eigen::Quaternionf(NAN, NAN, NAN, NAN); Eigen::Vector3f closest_pt_ = Eigen::Vector3f(NAN, NAN, NAN); costParameters cost_params_; simulation_limits lims_; @@ -78,7 +79,7 @@ class StarPlanner { * @brief setter method for vehicle position * @param[in] vehicle current position **/ - void setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel); + void setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel, const Eigen::Quaternionf& q); /** * @brief setter method for vehicle position projection on the line between the current and previous goal diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index f32cce693..4c49c190e 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -18,7 +18,7 @@ void LocalPlanner::setState(const Eigen::Vector3f& pos, const Eigen::Vector3f& v velocity_ = vel; yaw_fcu_frame_deg_ = getYawFromQuaternion(q); pitch_fcu_frame_deg_ = getPitchFromQuaternion(q); - star_planner_->setPose(position_, velocity_); + star_planner_->setPose(position_, velocity_, q); } // set parameters changed by dynamic rconfigure diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 25ded61f6..73ec5d85a 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -28,9 +28,10 @@ void StarPlanner::setParams(const costParameters& cost_params, const simulation_ acceptance_radius_ = acc_rad; } -void StarPlanner::setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel) { +void StarPlanner::setPose(const Eigen::Vector3f& pos, const Eigen::Vector3f& vel, const Eigen::Quaternionf& q) { position_ = pos; velocity_ = vel; + q_ = q; } void StarPlanner::setGoal(const Eigen::Vector3f& goal) { goal_ = goal; } @@ -49,10 +50,12 @@ void StarPlanner::buildLookAheadTree() { std::clock_t start_time = std::clock(); // Simple 6-way unit direction setpoints allowed only. // TODO: If compute allowws, make this more fine-grained - const std::vector candidates{Eigen::Vector3f{1.0f, 0.0f, 0.0f}, Eigen::Vector3f{0.5f, 0.5f, 0.0f}, - Eigen::Vector3f{0.5f, -0.5f, 0.0f}, Eigen::Vector3f{0.0f, 1.0f, 0.0f}, + // These are in a shitty local-aligned but body-centered frame + const std::vector candidates{Eigen::Vector3f{1.0f, 0.0f, 0.0f}, Eigen::Vector3f{0.0f, 1.0f, 0.0f}, Eigen::Vector3f{0.0f, 0.0f, 1.0f}, Eigen::Vector3f{-1.0f, 0.0f, 0.0f}, - Eigen::Vector3f{0.0f, -1.0f, 0.0f}, Eigen::Vector3f{0.0f, 0.0f, -1.0f}}; + Eigen::Vector3f{0.0f, -1.0f, 0.0f}, Eigen::Vector3f{0.0f, 0.0f, -1.0f}, + Eigen::Vector3f{0.707f, 0.707f, 0.0f}, Eigen::Vector3f{0.707f, -0.707f, 0.0f}, + Eigen::Vector3f{-0.707f, 0.707f, 0.0f}, Eigen::Vector3f{-0.707f, -0.707f, 0.0f}}; bool has_reached_goal = false; tree_.clear(); @@ -87,7 +90,7 @@ void StarPlanner::buildLookAheadTree() { for (const auto& candidate : candidates) { simulation_state state = tree_[origin].state; TrajectorySimulator sim(limits, state, 0.05f); // todo: parameterize simulation step size [s] - std::vector trajectory = sim.generate_trajectory(candidate, tree_node_duration_); + std::vector trajectory = sim.generate_trajectory(q_*candidate, tree_node_duration_); // check if another close node has been added float dist = 1.f; @@ -101,7 +104,7 @@ void StarPlanner::buildLookAheadTree() { } if (close_nodes == 0) { - tree_.push_back(TreeNode(origin, trajectory.back(), candidate)); + tree_.push_back(TreeNode(origin, trajectory.back(), q_*candidate)); float h = treeHeuristicFunction(tree_.size() - 1); tree_.back().heuristic_ = h; tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + From 38a4521a733d80ab73244ecc1ca71fb2e98881ae Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Mon, 30 Sep 2019 14:49:54 +0200 Subject: [PATCH 22/28] star_planner: fix distance cost function --- local_planner/src/nodes/planner_functions.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index 2e46130b0..8d050d627 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -363,8 +363,11 @@ float simpleCost(const TreeNode& node, const Eigen::Vector3f& goal, const costPa angleDifference(cartesianToPolarHistogram(node.getSetpoint(), node_position).z, facing_goal.z); const float smoothness_cost = cost_params.velocity_cost_param * node.getVelocity().dot(node.getSetpoint()); - const float distance_cost = cost_params.obstacle_cost_param * nearest.distance; + const float d = 2 + cost_params.obstacle_cost_param - nearest.distance; + const float distance_cost = nearest.distance > 0.f ? 1000.0f * (1 + d / sqrt(1 + d * d)) : 0.0f; const float yaw_cost = cost_params.yaw_cost_param * angle_diff * angle_diff; + + return distance_cost; } // cost function for every histogram cell From 8766701a2c7462d9d5a19d9d28defe0bad80100e Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Mon, 30 Sep 2019 14:51:00 +0200 Subject: [PATCH 23/28] star_planner: exit search if outside sensor horizon --- local_planner/src/nodes/star_planner.cpp | 44 +++++++----------------- 1 file changed, 13 insertions(+), 31 deletions(-) diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 73ec5d85a..a41d4117f 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -87,6 +87,16 @@ void StarPlanner::buildLookAheadTree() { // add candidates as nodes // insert new nodes int children = 0; + // If we reach the acceptance radius or the sensor horizon, add goal as last node and exit + if (origin > 1 && ((tree_[origin].getPosition() - goal_).norm() < acceptance_radius_) || + (tree_[origin].getPosition() - position_).norm() >= 2.0f * max_sensor_range_) { + tree_.push_back(TreeNode(origin, simulation_state(0.f, goal_), goal_ - tree_[origin].getPosition())); + closed_set_.push_back(origin); + closed_set_.push_back(tree_.size() - 1); + has_reached_goal = true; + break; + } + for (const auto& candidate : candidates) { simulation_state state = tree_[origin].state; TrajectorySimulator sim(limits, state, 0.05f); // todo: parameterize simulation step size [s] @@ -117,19 +127,9 @@ void StarPlanner::buildLookAheadTree() { tree_[origin].closed_ = true; // find best node to continue - float minimal_cost = HUGE_VAL; - has_reached_goal = false; + float minimal_cost = FLT_MAX; 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); - has_reached_goal = true; - break; - } - if (tree_[i].total_cost_ < minimal_cost) { minimal_cost = tree_[i].total_cost_; origin = i; @@ -138,31 +138,13 @@ void StarPlanner::buildLookAheadTree() { } // if there is only one node in the tree, we already expanded it. - if (tree_.size() == 1) { + if (tree_.size() <= 1) { has_reached_goal = true; } } - float min_cost_per_depth = FLT_MAX; - int chosen_children = 0; - for (size_t i = 0; i < tree_.size(); i++) { - if (tree_[i].closed_ == 0) { - size_t parent = tree_[i].origin_; - float total_cost = tree_[i].total_cost_; - int depth = 0; - while (tree_[parent].origin_ > 0) { - parent = tree_[parent].origin_; - depth++; - } - - if (min_cost_per_depth > (total_cost / depth)) { - min_cost_per_depth = (total_cost / depth); - chosen_children = (int)i; - } - } - } // Get setpoints into member vector - int tree_end = chosen_children; + int tree_end = origin; path_node_setpoints_.clear(); while (tree_end > 0) { path_node_setpoints_.push_back(tree_[tree_end].getSetpoint()); From 740fe1b1b116ea984ff0b40f1438d6d6a65035a8 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Tue, 1 Oct 2019 08:31:52 +0200 Subject: [PATCH 24/28] star_planner: move heuristic back to distance-based from time --- local_planner/src/nodes/star_planner.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index a41d4117f..3bef58720 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -41,9 +41,7 @@ void StarPlanner::setPointcloud(const kdtree_t& cloud) { cloud_ = cloud; } void StarPlanner::setClosestPointOnLine(const Eigen::Vector3f& closest_pt) { closest_pt_ = closest_pt; } float StarPlanner::treeHeuristicFunction(int node_number) const { - return ((goal_ - tree_[node_number].getPosition()).norm() / lims_.max_xy_velocity_norm + - tree_[node_number].state.time) * - tree_heuristic_weight_; + return (goal_ - tree_[node_number].getPosition()).norm() * tree_heuristic_weight_; } void StarPlanner::buildLookAheadTree() { From 7cb69c9351e46aaa9265e6b08810e81723cefeb9 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Tue, 1 Oct 2019 08:36:09 +0200 Subject: [PATCH 25/28] star_planner: only add nodes far enough from current nodes --- local_planner/src/nodes/planner_functions.cpp | 2 +- local_planner/src/nodes/star_planner.cpp | 48 +++++++++---------- 2 files changed, 24 insertions(+), 26 deletions(-) diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index 8d050d627..21eadfe67 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -367,7 +367,7 @@ float simpleCost(const TreeNode& node, const Eigen::Vector3f& goal, const costPa const float distance_cost = nearest.distance > 0.f ? 1000.0f * (1 + d / sqrt(1 + d * d)) : 0.0f; const float yaw_cost = cost_params.yaw_cost_param * angle_diff * angle_diff; - return distance_cost; + return distance_cost; // TODO: use other costs as well. Couldn't get them to do anything meaningful yet } // cost function for every histogram cell diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 3bef58720..7ea62b848 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -49,11 +49,12 @@ void StarPlanner::buildLookAheadTree() { // Simple 6-way unit direction setpoints allowed only. // TODO: If compute allowws, make this more fine-grained // These are in a shitty local-aligned but body-centered frame - const std::vector candidates{Eigen::Vector3f{1.0f, 0.0f, 0.0f}, Eigen::Vector3f{0.0f, 1.0f, 0.0f}, - Eigen::Vector3f{0.0f, 0.0f, 1.0f}, Eigen::Vector3f{-1.0f, 0.0f, 0.0f}, - Eigen::Vector3f{0.0f, -1.0f, 0.0f}, Eigen::Vector3f{0.0f, 0.0f, -1.0f}, - Eigen::Vector3f{0.707f, 0.707f, 0.0f}, Eigen::Vector3f{0.707f, -0.707f, 0.0f}, - Eigen::Vector3f{-0.707f, 0.707f, 0.0f}, Eigen::Vector3f{-0.707f, -0.707f, 0.0f}}; + const std::vector candidates{ + Eigen::Vector3f{1.0f, 0.0f, 0.0f}, Eigen::Vector3f{0.0f, 1.0f, 0.0f}, + Eigen::Vector3f{0.0f, 0.0f, 1.0f}, Eigen::Vector3f{-1.0f, 0.0f, 0.0f}, + Eigen::Vector3f{0.0f, -1.0f, 0.0f}, Eigen::Vector3f{0.0f, 0.0f, -1.0f}, + Eigen::Vector3f{0.707f, 0.707f, 0.0f}, Eigen::Vector3f{0.707f, -0.707f, 0.0f}, + Eigen::Vector3f{-0.707f, 0.707f, 0.0f}, Eigen::Vector3f{-0.707f, -0.707f, 0.0f}}; bool has_reached_goal = false; tree_.clear(); @@ -82,42 +83,39 @@ void StarPlanner::buildLookAheadTree() { computeMaxSpeedFromBrakingDistance(lims_.max_jerk_norm, lims_.max_acceleration_norm, max_sensor_range_)), lims_.max_xy_velocity_norm); - // add candidates as nodes - // insert new nodes - int children = 0; // If we reach the acceptance radius or the sensor horizon, add goal as last node and exit - if (origin > 1 && ((tree_[origin].getPosition() - goal_).norm() < acceptance_radius_) || - (tree_[origin].getPosition() - position_).norm() >= 2.0f * max_sensor_range_) { - tree_.push_back(TreeNode(origin, simulation_state(0.f, goal_), goal_ - tree_[origin].getPosition())); + if ((tree_[origin].getPosition() - goal_).norm() < acceptance_radius_ || + (tree_[origin].getPosition() - position_).norm() >= max_sensor_range_) { + tree_.push_back(TreeNode(origin + 1, simulation_state(0.f, goal_), goal_ - tree_[origin].getPosition())); + tree_.back().total_cost_ = tree_[origin].total_cost_; + tree_.back().heuristic_ = 0.f; closed_set_.push_back(origin); closed_set_.push_back(tree_.size() - 1); has_reached_goal = true; break; } + // Expand this node: add all candidates for (const auto& candidate : candidates) { simulation_state state = tree_[origin].state; - TrajectorySimulator sim(limits, state, 0.05f); // todo: parameterize simulation step size [s] - std::vector trajectory = sim.generate_trajectory(q_*candidate, tree_node_duration_); - - // check if another close node has been added - float dist = 1.f; - int close_nodes = 0; - for (size_t i = 0; i < tree_.size(); i++) { - dist = (tree_[i].getPosition() - trajectory.back().position).norm(); - if (dist < 0.2f) { - close_nodes++; + TrajectorySimulator sim(limits, state, 0.1f); // todo: parameterize simulation step size [s] + std::vector trajectory = sim.generate_trajectory(q_ * candidate, tree_node_duration_); + + // Only add the candidate as a node if it is significantly far away from any current node + const float minimal_distance = 0.2f; // must be at least 1m away + bool is_useful_node = true; + for (const auto& n : tree_) { + if ((n.getPosition() - trajectory.back().position).norm() < minimal_distance) { + is_useful_node = false; break; } } - - if (close_nodes == 0) { - tree_.push_back(TreeNode(origin, trajectory.back(), q_*candidate)); + if (is_useful_node) { + tree_.push_back(TreeNode(origin, trajectory.back(), q_ * candidate)); float h = treeHeuristicFunction(tree_.size() - 1); tree_.back().heuristic_ = h; tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + simpleCost(tree_.back(), goal_, cost_params_, cloud_) + h; - children++; } } From ca41c59f666b158780f26a6856b74f76d8b5fd18 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Tue, 1 Oct 2019 08:45:33 +0200 Subject: [PATCH 26/28] star_planner: rename origin to current_node --- local_planner/src/nodes/star_planner.cpp | 33 ++++++++++++------------ 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 7ea62b848..e1e713334 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -69,13 +69,13 @@ void StarPlanner::buildLookAheadTree() { tree_.push_back(TreeNode(0, start_state, Eigen::Vector3f::Zero())); tree_.back().setCosts(treeHeuristicFunction(0), treeHeuristicFunction(0)); - int origin = 0; + int current_node = 0; while (!has_reached_goal) { - Eigen::Vector3f origin_position = tree_[origin].getPosition(); - Eigen::Vector3f origin_velocity = tree_[origin].getVelocity(); + Eigen::Vector3f current_node_position = tree_[current_node].getPosition(); + Eigen::Vector3f current_node_velocity = tree_[current_node].getVelocity(); simulation_limits limits = lims_; - simulation_state state = tree_[origin].state; + simulation_state state = tree_[current_node].state; limits.max_xy_velocity_norm = std::min( std::min( computeMaxSpeedFromBrakingDistance(lims_.max_jerk_norm, lims_.max_acceleration_norm, @@ -84,12 +84,13 @@ void StarPlanner::buildLookAheadTree() { lims_.max_xy_velocity_norm); // If we reach the acceptance radius or the sensor horizon, add goal as last node and exit - if ((tree_[origin].getPosition() - goal_).norm() < acceptance_radius_ || - (tree_[origin].getPosition() - position_).norm() >= max_sensor_range_) { - tree_.push_back(TreeNode(origin + 1, simulation_state(0.f, goal_), goal_ - tree_[origin].getPosition())); - tree_.back().total_cost_ = tree_[origin].total_cost_; + if ((tree_[current_node].getPosition() - goal_).norm() < acceptance_radius_ || + (tree_[current_node].getPosition() - position_).norm() >= max_sensor_range_) { + tree_.push_back( + TreeNode(current_node + 1, simulation_state(0.f, goal_), goal_ - tree_[current_node].getPosition())); + tree_.back().total_cost_ = tree_[current_node].total_cost_; tree_.back().heuristic_ = 0.f; - closed_set_.push_back(origin); + closed_set_.push_back(current_node); closed_set_.push_back(tree_.size() - 1); has_reached_goal = true; break; @@ -97,7 +98,7 @@ void StarPlanner::buildLookAheadTree() { // Expand this node: add all candidates for (const auto& candidate : candidates) { - simulation_state state = tree_[origin].state; + simulation_state state = tree_[current_node].state; TrajectorySimulator sim(limits, state, 0.1f); // todo: parameterize simulation step size [s] std::vector trajectory = sim.generate_trajectory(q_ * candidate, tree_node_duration_); @@ -111,16 +112,16 @@ void StarPlanner::buildLookAheadTree() { } } if (is_useful_node) { - tree_.push_back(TreeNode(origin, trajectory.back(), q_ * candidate)); + tree_.push_back(TreeNode(current_node, trajectory.back(), q_ * candidate)); float h = treeHeuristicFunction(tree_.size() - 1); tree_.back().heuristic_ = h; - tree_.back().total_cost_ = tree_[origin].total_cost_ - tree_[origin].heuristic_ + + tree_.back().total_cost_ = tree_[current_node].total_cost_ - tree_[current_node].heuristic_ + simpleCost(tree_.back(), goal_, cost_params_, cloud_) + h; } } - closed_set_.push_back(origin); - tree_[origin].closed_ = true; + closed_set_.push_back(current_node); + tree_[current_node].closed_ = true; // find best node to continue float minimal_cost = FLT_MAX; @@ -128,7 +129,7 @@ void StarPlanner::buildLookAheadTree() { if (!(tree_[i].closed_)) { if (tree_[i].total_cost_ < minimal_cost) { minimal_cost = tree_[i].total_cost_; - origin = i; + current_node = i; } } } @@ -140,7 +141,7 @@ void StarPlanner::buildLookAheadTree() { } // Get setpoints into member vector - int tree_end = origin; + int tree_end = current_node; path_node_setpoints_.clear(); while (tree_end > 0) { path_node_setpoints_.push_back(tree_[tree_end].getSetpoint()); From 10965dc584e3a0a2a5c84c548b060dfd0a1d0b21 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Tue, 1 Oct 2019 08:45:55 +0200 Subject: [PATCH 27/28] tree_node: rename origin to parent --- local_planner/include/local_planner/tree_node.h | 4 ++-- local_planner/src/nodes/local_planner_visualization.cpp | 2 +- local_planner/src/nodes/star_planner.cpp | 2 +- local_planner/src/nodes/tree_node.cpp | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/local_planner/include/local_planner/tree_node.h b/local_planner/include/local_planner/tree_node.h index 1a40249c8..78378f25d 100644 --- a/local_planner/include/local_planner/tree_node.h +++ b/local_planner/include/local_planner/tree_node.h @@ -11,13 +11,13 @@ class TreeNode { public: float total_cost_; float heuristic_; - int origin_; + int parent_; 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(int parent, 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 6cdca0c93..46a8b9aa9 100644 --- a/local_planner/src/nodes/local_planner_visualization.cpp +++ b/local_planner/src/nodes/local_planner_visualization.cpp @@ -221,7 +221,7 @@ void LocalPlannerVisualization::publishTree(const std::vector& tree, c for (size_t i = 0; i < closed_set.size(); i++) { int node_nr = closed_set[i]; geometry_msgs::Point p1 = toPoint(tree[node_nr].getPosition()); - int origin = tree[node_nr].origin_; + int origin = tree[node_nr].parent_; geometry_msgs::Point p2 = toPoint(tree[origin].getPosition()); tree_marker.points.push_back(p1); tree_marker.points.push_back(p2); diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index e1e713334..d599c6ca5 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -145,7 +145,7 @@ void StarPlanner::buildLookAheadTree() { path_node_setpoints_.clear(); while (tree_end > 0) { path_node_setpoints_.push_back(tree_[tree_end].getSetpoint()); - tree_end = tree_[tree_end].origin_; + tree_end = tree_[tree_end].parent_; } 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 329579854..3ef2ba4b3 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 parent, const simulation_state& start_state, const Eigen::Vector3f& sp) + : total_cost_{0.0f}, heuristic_{0.0f}, parent_{parent}, closed_{false}, state(start_state), setpoint(sp) {} void TreeNode::setCosts(float h, float c) { heuristic_ = h; From f87509fae84693a4e788a1c621b78174d2a37284 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Thu, 24 Oct 2019 16:15:57 +0200 Subject: [PATCH 28/28] Use endpoints instead of whole trajectory --- local_planner/src/nodes/star_planner.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index d599c6ca5..c96ea35fc 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -71,9 +71,6 @@ void StarPlanner::buildLookAheadTree() { int current_node = 0; while (!has_reached_goal) { - Eigen::Vector3f current_node_position = tree_[current_node].getPosition(); - Eigen::Vector3f current_node_velocity = tree_[current_node].getVelocity(); - simulation_limits limits = lims_; simulation_state state = tree_[current_node].state; limits.max_xy_velocity_norm = std::min( @@ -100,19 +97,19 @@ void StarPlanner::buildLookAheadTree() { for (const auto& candidate : candidates) { simulation_state state = tree_[current_node].state; TrajectorySimulator sim(limits, state, 0.1f); // todo: parameterize simulation step size [s] - std::vector trajectory = sim.generate_trajectory(q_ * candidate, tree_node_duration_); + simulation_state trajectory_end = sim.generate_trajectory_endpoint(q_ * candidate, tree_node_duration_); // Only add the candidate as a node if it is significantly far away from any current node const float minimal_distance = 0.2f; // must be at least 1m away bool is_useful_node = true; for (const auto& n : tree_) { - if ((n.getPosition() - trajectory.back().position).norm() < minimal_distance) { + if ((n.getPosition() - trajectory_end.position).norm() < minimal_distance) { is_useful_node = false; break; } } if (is_useful_node) { - tree_.push_back(TreeNode(current_node, trajectory.back(), q_ * candidate)); + tree_.push_back(TreeNode(current_node, trajectory_end, q_ * candidate)); float h = treeHeuristicFunction(tree_.size() - 1); tree_.back().heuristic_ = h; tree_.back().total_cost_ = tree_[current_node].total_cost_ - tree_[current_node].heuristic_ +