diff --git a/local_planner/CMakeLists.txt b/local_planner/CMakeLists.txt index d65f94c80..130963ed5 100644 --- a/local_planner/CMakeLists.txt +++ b/local_planner/CMakeLists.txt @@ -138,7 +138,6 @@ include_directories( set(LOCAL_PLANNER_CPP_FILES "src/nodes/local_planner.cpp" "src/nodes/waypoint_generator.cpp" "src/nodes/tree_node.cpp" - "src/nodes/box.cpp" "src/nodes/star_planner.cpp" "src/nodes/planner_functions.cpp" "src/nodes/local_planner_node.cpp" diff --git a/local_planner/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg index 1de224be5..6eceac661 100755 --- a/local_planner/cfg/local_planner_node.cfg +++ b/local_planner/cfg/local_planner_node.cfg @@ -7,10 +7,10 @@ gen = ParameterGenerator() # local_planner -gen.add("box_radius_", 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", 150.0, 0, 500.0) -gen.add("yaw_cost_param_", double_t, 0, "Cost function weight for constant heading", 6.0, 0, 20.0) -gen.add("velocity_cost_param_", double_t, 0, "Cost function weight for path smoothness", 35000.0, 0.0, 50000.0) +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", 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("goal_z_param", double_t, 0, "Height of the goal position", 3.5, -20.0, 20.0) diff --git a/local_planner/include/local_planner/box.h b/local_planner/include/local_planner/box.h deleted file mode 100644 index ce66dc6a9..000000000 --- a/local_planner/include/local_planner/box.h +++ /dev/null @@ -1,44 +0,0 @@ - -#ifndef BOX_H -#define BOX_H - -#include -namespace avoidance { -class Box { - public: - Box(); - Box(const float& radius); - ~Box() = default; - - /** - * @brief sets the bounding box coordinates limits around the vehicle - *position - * @param[in] pos, vehicle current position - * @param[in] ground_distance, distance to the ground [m] - **/ - void setBoxLimits(const Eigen::Vector3f& pos, const float ground_distance); - - /** - * @brief checks if a pointcloud point is within the bounding box - * @param[in] x, x-coordinate of the point - * @param[in] y, y-coordinate of the point - * @param[in] z, z-coordinate of the point - * @returns true, if the point is within the bounding box - **/ - inline bool isPointWithinBox(const float& x, const float& y, const float& z) const { - return x < xmax_ && x > xmin_ && y < ymax_ && y > ymin_ && z < zmax_ && z > zmin_; - } - - float radius_; - float box_dist_to_ground_ = 2.0; - float zmin_; - - private: - float xmin_; - float xmax_; - float ymin_; - float ymax_; - float zmax_; -}; -} -#endif // BOX_H diff --git a/local_planner/include/local_planner/local_planner.h b/local_planner/include/local_planner/local_planner.h index 9050cac3f..5d7220dc9 100644 --- a/local_planner/include/local_planner/local_planner.h +++ b/local_planner/include/local_planner/local_planner.h @@ -4,7 +4,6 @@ #include #include "avoidance/histogram.h" #include "avoidance_output.h" -#include "box.h" #include "candidate_direction.h" #include "cost_parameters.h" #include "planner_functions.h" @@ -77,6 +76,7 @@ class LocalPlanner { float max_point_age_s_ = 10; float yaw_fcu_frame_deg_ = 0.0f; float pitch_fcu_frame_deg_ = 0.0f; + float sensor_range_ = 12.0f; std::vector fov_fcu_frame_; @@ -120,7 +120,6 @@ class LocalPlanner { void generateHistogramImage(Histogram& histogram); public: - Box histogram_box_; std::vector histogram_image_data_; std::vector cost_image_data_; bool use_vel_setpoints_; @@ -131,7 +130,6 @@ class LocalPlanner { double timeout_critical_; double timeout_termination_; double starting_height_ = 0.0; - float ground_distance_ = 2.0; ModelParameters px4_; // PX4 Firmware paramters @@ -172,6 +170,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_; } /** * @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 0f5ccc55d..383484d8a 100644 --- a/local_planner/include/local_planner/local_planner_visualization.h +++ b/local_planner/include/local_planner/local_planner_visualization.h @@ -48,15 +48,6 @@ class LocalPlannerVisualization { **/ void publishGoal(const geometry_msgs::Point& goal) const; - /** - * @brief Visualization of the bounding box used to crop the pointcloud - * @params[in] drone_pos, current position of the drone - * @params[in] box_radius, the radius of the bounding box - * @params[in] plane_height, the height above ground at which the pointcloud - * is additionally cropped - **/ - void publishBox(const Eigen::Vector3f& drone_pos, float box_radius, float plane_height) const; - /** * @brief Visualization of the data used during takeoff * @params[in] take_off_pose, pose at which the vehicle was armed @@ -118,14 +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 ground - * @params[in] drone_pos, location of the drone at the current timestep - * @params[in] box_radius, the radius of the bounding box - * @params[in] ground_distance, measured distance to ground - **/ - void publishGround(const Eigen::Vector3f& drone_pos, float box_radius, float ground_distance) 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 5214bef12..d537e49a9 100644 --- a/local_planner/include/local_planner/planner_functions.h +++ b/local_planner/include/local_planner/planner_functions.h @@ -3,7 +3,6 @@ #include "avoidance/common.h" #include "avoidance/histogram.h" -#include "box.h" #include "candidate_direction.h" #include "cost_parameters.h" @@ -22,7 +21,6 @@ namespace avoidance { * the data from the last timestep * @param final_cloud, processed data to be used for planning * @param[in] complete_cloud, array of pointclouds from the sensors -* @param[in] histogram_box, geometry definition of the bounding box * @param[in] FOV, struct defining current field of view * @param[in] position, current vehicle position * @param[in] min_realsense_dist, minimum sensor range [m] @@ -33,10 +31,9 @@ namespace avoidance { * a valid input here) **/ void processPointcloud(pcl::PointCloud& final_cloud, - const std::vector>& complete_cloud, const Box& histogram_box, - const std::vector& fov, float yaw_fcu_frame_deg, float pitch_fcu_frame_deg, - const Eigen::Vector3f& position, float min_realsense_dist, float 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, 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 f293e3c1c..d5fb4ba68 100644 --- a/local_planner/include/local_planner/star_planner.h +++ b/local_planner/include/local_planner/star_planner.h @@ -2,7 +2,6 @@ #define STAR_PLANNER_H #include "avoidance/histogram.h" -#include "box.h" #include "cost_parameters.h" #include diff --git a/local_planner/src/nodes/box.cpp b/local_planner/src/nodes/box.cpp deleted file mode 100644 index be0c4d7bd..000000000 --- a/local_planner/src/nodes/box.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include "local_planner/box.h" - -namespace avoidance { - -Box::Box(const float& radius) - : xmin_{0.0f}, - xmax_{0.0f}, - ymin_{0.0f}, - ymax_{0.0f}, - zmin_{0.0f}, - zmax_{0.0f}, - radius_{radius}, - box_dist_to_ground_{1.0f} {} - -Box::Box() - : xmin_{0.0f}, - xmax_{0.0f}, - ymin_{0.0f}, - ymax_{0.0f}, - zmin_{0.0f}, - zmax_{0.0f}, - radius_{0.0f}, - box_dist_to_ground_{1.0f} {} - -// update bounding box limit coordinates around a new UAV pose -void Box::setBoxLimits(const Eigen::Vector3f& pos, const float ground_distance) { - float zmin_close_to_ground = std::min(pos.z() + 0.8f, pos.z() - ground_distance + box_dist_to_ground_); - zmin_ = std::max(zmin_close_to_ground, pos.z() - 1.0f); - xmin_ = pos.x() - radius_; - ymin_ = pos.y() - radius_; - xmax_ = pos.x() + radius_; - ymax_ = pos.y() + radius_; - zmax_ = pos.z() + radius_; -} -} diff --git a/local_planner/src/nodes/local_planner.cpp b/local_planner/src/nodes/local_planner.cpp index 7d6a3fdeb..4d5e5fbf4 100644 --- a/local_planner/src/nodes/local_planner.cpp +++ b/local_planner/src/nodes/local_planner.cpp @@ -28,7 +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_); + // 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_; @@ -78,12 +79,10 @@ void LocalPlanner::runPlanner() { ROS_INFO("\033[1;35m[OA] Planning started, using %i cameras\n \033[0m", static_cast(original_cloud_vector_.size())); - histogram_box_.setBoxLimits(position_, ground_distance_); - float elapsed_since_last_processing = static_cast((ros::Time::now() - last_pointcloud_process_time_).toSec()); - processPointcloud(final_cloud_, original_cloud_vector_, histogram_box_, 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(); @@ -114,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 / histogram_box_.radius_ : 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))); } } @@ -167,7 +166,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 = histogram_box_.radius_; + msg.range_max = sensor_range_; msg.ranges.reserve(GRID_LENGTH_Z); for (int i = 0; i < GRID_LENGTH_Z; ++i) { @@ -176,7 +175,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 : histogram_box_.radius_ + 1.0f); + msg.ranges.push_back(dist > min_realsense_dist_ ? dist : sensor_range_ + 1.0f); } distance_data_ = msg; @@ -188,7 +187,7 @@ void LocalPlanner::updateObstacleDistanceMsg() { 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 = histogram_box_.radius_; + msg.range_max = sensor_range_; distance_data_ = msg; } @@ -236,7 +235,7 @@ avoidanceOutput LocalPlanner::getAvoidanceOutput() const { 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 * histogram_box_.radius_; + float c = 2 * -px4_.param_mpc_acc_hor * sensor_range_; float limited_speed = (-b + std::sqrt(b * b - 4 * a * c)) / (2 * a); float max_speed = std::min(px4_.param_mpc_xy_cruise, limited_speed); diff --git a/local_planner/src/nodes/local_planner_node.cpp b/local_planner/src/nodes/local_planner_node.cpp index 1b18a572d..0899d3bb6 100644 --- a/local_planner/src/nodes/local_planner_node.cpp +++ b/local_planner/src/nodes/local_planner_node.cpp @@ -185,14 +185,6 @@ void LocalPlannerNode::updatePlannerInfo() { new_goal_ = false; } - // update ground distance - if (ros::Time::now() - ground_distance_msg_.header.stamp < ros::Duration(0.5)) { - local_planner_->ground_distance_ = ground_distance_msg_.bottom_clearance; - } else { - local_planner_->ground_distance_ = 2.0; // in case where no range data is - // available assume vehicle is close to ground - } - // update last sent waypoint local_planner_->last_sent_waypoint_ = toEigen(newest_waypoint_position_); } @@ -359,8 +351,6 @@ void LocalPlannerNode::fcuInputGoalCallback(const mavros_msgs::Trajectory& msg) void LocalPlannerNode::distanceSensorCallback(const mavros_msgs::Altitude& msg) { if (!std::isnan(msg.bottom_clearance)) { ground_distance_msg_ = msg; - visualizer_.publishGround(local_planner_->getPosition(), local_planner_->histogram_box_.radius_, - local_planner_->ground_distance_); } } diff --git a/local_planner/src/nodes/local_planner_visualization.cpp b/local_planner/src/nodes/local_planner_visualization.cpp index 2a560c620..21768e34e 100644 --- a/local_planner/src/nodes/local_planner_visualization.cpp +++ b/local_planner/src/nodes/local_planner_visualization.cpp @@ -52,18 +52,12 @@ void LocalPlannerVisualization::visualizePlannerData(const LocalPlanner& planner // visualize goal publishGoal(toPoint(planner.getGoal())); - // publish bounding box of pointcloud - publishBox(planner.getPosition(), planner.histogram_box_.radius_, planner.histogram_box_.zmin_); - - // publish data related to takeoff maneuver - publishReachHeight(planner.take_off_pose_, planner.starting_height_); - // publish histogram image publishDataImages(planner.histogram_image_data_, planner.cost_image_data_, newest_waypoint_position, newest_adapted_waypoint_position, newest_pose); // publish the FOV - publishFOV(planner.getFOV(), planner.histogram_box_.radius_); + publishFOV(planner.getFOV(), planner.getSensorRange()); } void LocalPlannerVisualization::publishFOV(const std::vector& fov_vec, float max_range) const { @@ -213,54 +207,6 @@ void LocalPlannerVisualization::publishGoal(const geometry_msgs::Point& goal) co marker_goal_pub_.publish(marker_goal); } -void LocalPlannerVisualization::publishBox(const Eigen::Vector3f& drone_pos, float box_radius, - float plane_height) const { - visualization_msgs::MarkerArray marker_array; - - visualization_msgs::Marker box; - box.header.frame_id = "local_origin"; - box.header.stamp = ros::Time::now(); - box.id = 0; - box.type = visualization_msgs::Marker::SPHERE; - box.action = visualization_msgs::Marker::ADD; - box.pose.position = toPoint(drone_pos); - box.pose.orientation.x = 0.0; - box.pose.orientation.y = 0.0; - box.pose.orientation.z = 0.0; - box.pose.orientation.w = 1.0; - box.scale.x = 2.0 * box_radius; - box.scale.y = 2.0 * box_radius; - box.scale.z = 2.0 * box_radius; - box.color.a = 0.5; - box.color.r = 0.0; - box.color.g = 1.0; - box.color.b = 0.0; - marker_array.markers.push_back(box); - - visualization_msgs::Marker plane; - plane.header.frame_id = "local_origin"; - plane.header.stamp = ros::Time::now(); - plane.id = 1; - plane.type = visualization_msgs::Marker::CUBE; - plane.action = visualization_msgs::Marker::ADD; - plane.pose.position = toPoint(drone_pos); - plane.pose.position.z = plane_height; - plane.pose.orientation.x = 0.0; - plane.pose.orientation.y = 0.0; - plane.pose.orientation.z = 0.0; - plane.pose.orientation.w = 1.0; - plane.scale.x = 2.0 * box_radius; - plane.scale.y = 2.0 * box_radius; - plane.scale.z = 0.001; - plane.color.a = 0.5; - plane.color.r = 0.0; - plane.color.g = 1.0; - plane.color.b = 0.0; - marker_array.markers.push_back(plane); - - bounding_box_pub_.publish(marker_array); -} - void LocalPlannerVisualization::publishReachHeight(const Eigen::Vector3f& take_off_pose, float starting_height) const { visualization_msgs::Marker m; m.header.frame_id = "local_origin"; @@ -540,30 +486,4 @@ void LocalPlannerVisualization::publishCurrentSetpoint(const geometry_msgs::Twis current_waypoint_pub_.publish(setpoint); } - -void LocalPlannerVisualization::publishGround(const Eigen::Vector3f& drone_pos, float box_radius, - float ground_distance) const { - visualization_msgs::Marker plane; - - plane.header.frame_id = "local_origin"; - plane.header.stamp = ros::Time::now(); - plane.id = 1; - plane.type = visualization_msgs::Marker::CUBE; - plane.action = visualization_msgs::Marker::ADD; - plane.pose.position = toPoint(drone_pos); - plane.pose.position.z = drone_pos.z() - static_cast(ground_distance); - plane.pose.orientation.x = 0.0; - plane.pose.orientation.y = 0.0; - plane.pose.orientation.z = 0.0; - plane.pose.orientation.w = 1.0; - plane.scale.x = 2.0 * box_radius; - plane.scale.y = 2.0 * box_radius; - plane.scale.z = 0.001; - ; - plane.color.a = 0.5; - plane.color.r = 0.0; - plane.color.g = 0.0; - plane.color.b = 1.0; - ground_measurement_pub_.publish(plane); -} } diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index 618416106..aa8f025b3 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -8,13 +8,11 @@ namespace avoidance { -// trim the point cloud so that only points inside the bounding box are -// considered +// 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 Box& histogram_box, - const std::vector& fov, float yaw_fcu_frame_deg, float pitch_fcu_frame_deg, - const Eigen::Vector3f& position, float min_realsense_dist, float 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, float max_age, float elapsed_s, int min_num_points_per_cell) { const int SCALE_FACTOR = 4; pcl::PointCloud old_cloud; std::swap(final_cloud, old_cloud); @@ -32,16 +30,14 @@ 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)) { - if (histogram_box.isPointWithinBox(xyz.x, xyz.y, xyz.z)) { - distance = (position - toEigen(xyz)).norm(); - if (distance > min_realsense_dist && distance < histogram_box.radius_) { - // 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)); - } + 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 / 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)); } } } @@ -50,28 +46,22 @@ void processPointcloud(pcl::PointCloud& final_cloud, // combine with old cloud for (const pcl::PointXYZI& xyzi : old_cloud) { - // adding older points if not expired and space is free according to new - // cloud - if (histogram_box.isPointWithinBox(xyzi.x, xyzi.y, xyzi.z)) { - distance = (position - toEigen(xyzi)).norm(); - if (distance < histogram_box.radius_) { - 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; - } - } + // 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; } } diff --git a/local_planner/src/nodes/star_planner.cpp b/local_planner/src/nodes/star_planner.cpp index 4086f777e..2e5ef6e80 100644 --- a/local_planner/src/nodes/star_planner.cpp +++ b/local_planner/src/nodes/star_planner.cpp @@ -15,7 +15,7 @@ void StarPlanner::dynamicReconfigureSetStarParams(const avoidance::LocalPlannerN children_per_node_ = config.children_per_node_; n_expanded_nodes_ = config.n_expanded_nodes_; tree_node_distance_ = static_cast(config.tree_node_distance_); - max_path_length_ = static_cast(config.box_radius_); + max_path_length_ = static_cast(config.sensor_range_); smoothing_margin_degrees_ = static_cast(config.smoothing_margin_degrees_); tree_heuristic_weight_ = static_cast(config.tree_heuristic_weight_); } diff --git a/local_planner/test/test_planner_functions.cpp b/local_planner/test/test_planner_functions.cpp index 37874e6f3..e081d7b30 100644 --- a/local_planner/test/test_planner_functions.cpp +++ b/local_planner/test/test_planner_functions.cpp @@ -86,15 +86,13 @@ TEST(PlannerFunctionsTests, processPointcloud) { p1.push_back(toXYZ(position + Eigen::Vector3f(-1.0f, -1.1f, 3.5f))); pcl::PointCloud p2; - p2.push_back(toXYZ(position + Eigen::Vector3f(1.0f, 5.0f, 1.0f))); // > histogram_box.radius - p2.push_back(toXYZ(position + Eigen::Vector3f(100.0f, 5.0f, 1.0f))); // > histogram_box.radius + p2.push_back(toXYZ(position + Eigen::Vector3f(1.0f, 5.0f, 1.0f))); + p2.push_back(toXYZ(position + Eigen::Vector3f(100.0f, 5.0f, 1.0f))); p2.push_back(toXYZ(position + Eigen::Vector3f(0.1f, 0.05f, 0.05f))); // < min_realsense_dist std::vector> complete_cloud; complete_cloud.push_back(p1); complete_cloud.push_back(p2); - Box histogram_box(5.0f); - histogram_box.setBoxLimits(position, 4.5f); float min_realsense_dist = 0.2f; pcl::PointCloud processed_cloud1, processed_cloud2, processed_cloud3; @@ -111,22 +109,22 @@ TEST(PlannerFunctionsTests, processPointcloud) { FOV_regular.push_back(FOV(0.0f, 1.0f, 85.0f, 65.0f)); // WHEN: we filter the PointCloud with different values max_age - processPointcloud(processed_cloud1, complete_cloud, histogram_box, FOV_zero, 0.0f, 0.0f, position, min_realsense_dist, - 0.0f, 0.5f, 1); + processPointcloud(processed_cloud1, complete_cloud, FOV_zero, 0.0f, 0.0f, position, min_realsense_dist, 0.0f, 0.5f, + 1); - processPointcloud(processed_cloud2, complete_cloud, histogram_box, FOV_zero, 0.0f, - 0.0f, // todo: test different yaw and pitch - position, min_realsense_dist, 10.0f, .5f, 1); + // todo: test different yaw and pitch + processPointcloud(processed_cloud2, complete_cloud, FOV_zero, 0.0f, 0.0f, position, min_realsense_dist, 10.0f, .5f, + 1); - processPointcloud(processed_cloud3, complete_cloud, histogram_box, FOV_regular, 0.0f, 0.0f, position, - min_realsense_dist, 10.0f, 0.5f, 1); + processPointcloud(processed_cloud3, complete_cloud, FOV_regular, 0.0f, 0.0f, position, min_realsense_dist, 10.0f, + 0.5f, 1); - // THEN: we expect the first cloud to have 6 points - // the second cloud should contain 7 points - EXPECT_EQ(6, processed_cloud1.size()); - EXPECT_EQ(7, processed_cloud2.size()); + // THEN: we expect the first cloud to have 8 points + // the second cloud should contain all 9 points + EXPECT_EQ(8, processed_cloud1.size()); + EXPECT_EQ(9, processed_cloud2.size()); EXPECT_TRUE(pointInsideFOV(FOV_regular, memory_point_polar)); - EXPECT_EQ(6, processed_cloud3.size()); + EXPECT_EQ(8, processed_cloud3.size()); // since memory point is inside FOV, it isn't remembered } TEST(PlannerFunctions, getSetpointFromPath) {