diff --git a/local_planner/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg index 2b3751fab..c269d30d6 100755 --- a/local_planner/cfg/local_planner_node.cfg +++ b/local_planner/cfg/local_planner_node.cfg @@ -10,8 +10,8 @@ gen = ParameterGenerator() gen.add("sensor_range_", double_t, 0, "Data points farther away will be discarded", 15.0, 0, 20) gen.add("pitch_cost_param_", double_t, 0, "Cost function weight for goal oriented behavior", 20.0, 0, 30.0) gen.add("yaw_cost_param_", double_t, 0, "Cost function weight for constant heading", 2.0, 0, 20.0) -gen.add("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 1200, 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("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 500, 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", 4.5, 0.0, 30.0) gen.add("goal_z_param", double_t, 0, "Height of the goal position", 3.5, -20.0, 20.0) gen.add("min_realsense_dist_", double_t, 0, "Discard points closer than that", 0.2, 0, 10) gen.add("timeout_startup_", double_t, 0, "After this timeout the companion status is MAV_STATE_CRITICAL", 5, 0, 60) @@ -28,6 +28,6 @@ gen.add("use_vel_setpoints_", bool_t, 0, "Enable velocity setpoints (if false, p # 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 4d72d4ef5..968a68cff 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 @@ -120,7 +121,7 @@ class LocalPlanner { void generateHistogramImage(Histogram& histogram); public: - //Box histogram_box_; + // Box histogram_box_; std::vector histogram_image_data_; std::vector cost_image_data_; bool use_vel_setpoints_; @@ -131,8 +132,7 @@ class LocalPlanner { double timeout_critical_; double timeout_termination_; double starting_height_ = 0.0; - //float ground_distance_ = 2.0; - + // float ground_distance_ = 2.0; ModelParameters px4_; // PX4 Firmware paramters @@ -173,7 +173,7 @@ class LocalPlanner { float getHFOV(int i) { return i < fov_fcu_frame_.size() ? fov_fcu_frame_[i].h_fov_deg : 0.f; } float getVFOV(int i) { return i < fov_fcu_frame_.size() ? fov_fcu_frame_[i].v_fov_deg : 0.f; } const std::vector& getFOV() const { return fov_fcu_frame_; } - float getSensorRange() const {return sensor_range_;} + float getSensorRange() const { return sensor_range_; } /** * @brief getter method for current goal diff --git a/local_planner/include/local_planner/local_planner_visualization.h b/local_planner/include/local_planner/local_planner_visualization.h index a4b7b1be4..383484d8a 100644 --- a/local_planner/include/local_planner/local_planner_visualization.h +++ b/local_planner/include/local_planner/local_planner_visualization.h @@ -48,8 +48,6 @@ class LocalPlannerVisualization { **/ void publishGoal(const geometry_msgs::Point& goal) const; - - /** * @brief Visualization of the data used during takeoff * @params[in] take_off_pose, pose at which the vehicle was armed @@ -111,8 +109,6 @@ class LocalPlannerVisualization { void publishCurrentSetpoint(const geometry_msgs::Twist& wp, const waypoint_choice& waypoint_type, const geometry_msgs::Point& newest_pos) const; - - /** * @brief Visualization of the offtrack state * @params[in] closest_pt, vehicle position projection on the line previous to diff --git a/local_planner/include/local_planner/planner_functions.h b/local_planner/include/local_planner/planner_functions.h index 4f5ff214a..4b68a484c 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -31,10 +31,9 @@ namespace avoidance { * 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_realsense_dist, int max_age, float elapsed_s, - int min_num_points_per_cell); + 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_realsense_dist, int 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 0ff5ff61b..7787db22b 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 @@ -32,6 +33,7 @@ class StarPlanner { Eigen::Vector3f position_ = Eigen::Vector3f(NAN, NAN, NAN); Eigen::Vector3f velocity_ = Eigen::Vector3f(NAN, NAN, NAN); costParameters cost_params_; + simulation_limits lims_; protected: /** @@ -52,8 +54,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 f32e2f12a..948350f26 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -28,8 +28,8 @@ void LocalPlanner::setState(const Eigen::Vector3f& pos, const Eigen::Vector3f& v // set parameters changed by dynamic rconfigure void LocalPlanner::dynamicReconfigureSetParams(avoidance::LocalPlannerNodeConfig& config, uint32_t level) { -// histogram_box_.radius_ = static_cast(config.box_radius_); -sensor_range_ = static_cast(config.sensor_range_); + // histogram_box_.radius_ = static_cast(config.box_radius_); + sensor_range_ = static_cast(config.sensor_range_); cost_params_.pitch_cost_param = config.pitch_cost_param_; cost_params_.yaw_cost_param = config.yaw_cost_param_; cost_params_.velocity_cost_param = config.velocity_cost_param_; @@ -80,9 +80,9 @@ void LocalPlanner::runPlanner() { static_cast(original_cloud_vector_.size())); float elapsed_since_last_processing = static_cast((ros::Time::now() - last_pointcloud_process_time_).toSec()); - processPointcloud(final_cloud_, original_cloud_vector_, fov_fcu_frame_, yaw_fcu_frame_deg_, - pitch_fcu_frame_deg_, position_, min_realsense_dist_, max_point_age_s_, - elapsed_since_last_processing, min_num_points_per_cell_); + processPointcloud(final_cloud_, original_cloud_vector_, fov_fcu_frame_, yaw_fcu_frame_deg_, pitch_fcu_frame_deg_, + position_, min_realsense_dist_, max_point_age_s_, elapsed_since_last_processing, + min_num_points_per_cell_); last_pointcloud_process_time_ = ros::Time::now(); determineStrategy(); @@ -113,7 +113,7 @@ void LocalPlanner::generateHistogramImage(Histogram& histogram) { for (int e = GRID_LENGTH_E - 1; e >= 0; e--) { for (int z = 0; z < GRID_LENGTH_Z; z++) { float dist = histogram.get_dist(e, z); - float depth_val = dist > 0.01f ? 255.f - 255.f * dist / sensor_range_: 0.f; + float depth_val = dist > 0.01f ? 255.f - 255.f * dist / sensor_range_ : 0.f; histogram_image_data_.push_back((int)std::max(0.0f, std::min(255.f, depth_val))); } } @@ -150,7 +150,14 @@ void LocalPlanner::determineStrategy() { getCostMatrix(polar_histogram_, goal_, position_, velocity_, cost_params_, smoothing_margin_degrees_, 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_); // build search tree @@ -166,7 +173,7 @@ void LocalPlanner::updateObstacleDistanceMsg(Histogram hist) { msg.header.frame_id = "local_origin"; msg.angle_increment = static_cast(ALPHA_RES) * M_PI / 180.0; msg.range_min = min_realsense_dist_; - msg.range_max = sensor_range_;//histogram_box_.radius_; + msg.range_max = sensor_range_; // histogram_box_.radius_; msg.ranges.reserve(GRID_LENGTH_Z); for (int i = 0; i < GRID_LENGTH_Z; ++i) { @@ -175,7 +182,7 @@ void LocalPlanner::updateObstacleDistanceMsg(Histogram hist) { float dist = hist.get_dist(0, j); // special case: distance of 0 denotes 'no obstacle in sight' - msg.ranges.push_back(dist > min_realsense_dist_ ? dist : sensor_range_+ 1.0f); + msg.ranges.push_back(dist > min_realsense_dist_ ? dist : sensor_range_ + 1.0f); } distance_data_ = msg; diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp index 1233f7120..21768e34e 100644 --- a/local_planner/src/nodes/local_planner_visualization.cpp +++ b/local_planner/src/nodes/local_planner_visualization.cpp @@ -207,8 +207,6 @@ void LocalPlannerVisualization::publishGoal(const geometry_msgs::Point& goal) co marker_goal_pub_.publish(marker_goal); } - - void LocalPlannerVisualization::publishReachHeight(const Eigen::Vector3f& take_off_pose, float starting_height) const { visualization_msgs::Marker m; m.header.frame_id = "local_origin"; diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index b636f2752..7b97f72c5 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -10,10 +10,9 @@ 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_realsense_dist, int max_age, float elapsed_s, - int min_num_points_per_cell) { + 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_realsense_dist, int max_age, float elapsed_s, int min_num_points_per_cell) { pcl::PointCloud old_cloud; std::swap(final_cloud, old_cloud); final_cloud.points.clear(); @@ -30,16 +29,16 @@ void processPointcloud(pcl::PointCloud& final_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)) { - distance = (position - toEigen(xyz)).norm(); - if (distance > min_realsense_dist ) { - // subsampling the cloud - PolarPoint p_pol = cartesianToPolarHistogram(toEigen(xyz), position); - Eigen::Vector2i p_ind = polarToHistogramIndex(p_pol, ALPHA_RES / 2); - 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)); - } + distance = (position - toEigen(xyz)).norm(); + if (distance > min_realsense_dist) { + // subsampling the cloud + PolarPoint p_pol = cartesianToPolarHistogram(toEigen(xyz), position); + Eigen::Vector2i p_ind = polarToHistogramIndex(p_pol, ALPHA_RES / 2); + 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)); } + } } } } @@ -48,25 +47,24 @@ void processPointcloud(pcl::PointCloud& final_cloud, for (const pcl::PointXYZI& xyzi : old_cloud) { // adding older points if not expired and space is free according to new // cloud - distance = (position - toEigen(xyzi)).norm(); - - 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 / 2); - - // 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; - } - + distance = (position - toEigen(xyzi)).norm(); + + 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 / 2); + + // 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; + } } final_cloud.header.stamp = complete_cloud[0].header.stamp; diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 2b01b0a01..a58626427 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -19,7 +19,10 @@ void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerN smoothing_margin_degrees_ = static_cast(config.smoothing_margin_degrees_); } -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; @@ -48,7 +51,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; @@ -76,14 +84,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; @@ -91,7 +105,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; } }