From 067f7651b8cb9739094d2e1f2b8633cfebe09966 Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Thu, 15 Aug 2019 15:51:47 +0200 Subject: [PATCH] Fix sensor range sqr issue, reduce point density slightly --- local_planner/cfg/local_planner_node.cfg | 2 +- local_planner/src/nodes/planner_functions.cpp | 43 +++++++++++-------- local_planner/test/test_planner_functions.cpp | 6 +-- 3 files changed, 28 insertions(+), 23 deletions(-) diff --git a/local_planner/cfg/local_planner_node.cfg b/local_planner/cfg/local_planner_node.cfg index 224327fc5..033ba6834 100755 --- a/local_planner/cfg/local_planner_node.cfg +++ b/local_planner/cfg/local_planner_node.cfg @@ -7,7 +7,7 @@ gen = ParameterGenerator() # local_planner -gen.add("max_sensor_range_", double_t, 0, "Data points farther away will be discarded", 15.0, 0, 20) +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("yaw_cost_param_", double_t, 0, "Cost function weight for constant heading", 3.0, 0, 20.0) diff --git a/local_planner/src/nodes/planner_functions.cpp b/local_planner/src/nodes/planner_functions.cpp index d4ebe06bf..23f1c84df 100644 --- a/local_planner/src/nodes/planner_functions.cpp +++ b/local_planner/src/nodes/planner_functions.cpp @@ -14,7 +14,7 @@ void processPointcloud(pcl::PointCloud& final_cloud, 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 = 4; + const int SCALE_FACTOR = 3; pcl::PointCloud old_cloud; std::swap(final_cloud, old_cloud); final_cloud.points.clear(); @@ -25,12 +25,14 @@ void processPointcloud(pcl::PointCloud& final_cloud, 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;}; + 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 distance = (position - toEigen(xyz)).squaredNorm(); - if (min_sensor_range < distance && distance < max_sensor_range) { + 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); @@ -45,22 +47,25 @@ 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 - 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; + 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; + } } } diff --git a/local_planner/test/test_planner_functions.cpp b/local_planner/test/test_planner_functions.cpp index 377fd6895..2b665546c 100644 --- a/local_planner/test/test_planner_functions.cpp +++ b/local_planner/test/test_planner_functions.cpp @@ -122,10 +122,10 @@ TEST(PlannerFunctionsTests, processPointcloud) { // THEN: we expect the first cloud to have 5 points // the second cloud should contain all 6 points - EXPECT_EQ(5, processed_cloud1.size()); - EXPECT_EQ(6, processed_cloud2.size()); + EXPECT_EQ(7, processed_cloud1.size()); + EXPECT_EQ(8, processed_cloud2.size()); EXPECT_TRUE(pointInsideFOV(FOV_regular, memory_point_polar)); - EXPECT_EQ(5, processed_cloud3.size()); // since memory point is inside FOV, it isn't remembered + EXPECT_EQ(7, processed_cloud3.size()); // since memory point is inside FOV, it isn't remembered } TEST(PlannerFunctions, getSetpointFromPath) {