HuberoPlannerROS & Config - added limiting obstacles number based…

… on distance criterion (#87)

cfg/HuberoPlanner.cfg

@@ -27,6 +27,7 @@ group_general.add("person_model_radius", double_t, 0, "The radius of the circula
 group_general.add("person_fov", double_t, 0, "Half of the field of view of a detected person", 3.31613 / 2.0, 0.0, +3.1415)
 group_general.add("publish_traj_pcl", bool_t, 0, "Whether to publish PCL with explored trajectories", True)
 group_general.add("publish_cost_grid_pcl", bool_t, 0, "Whether to publish PCL with the potential field generated by the cost function", True)
+group_general.add("obstacles_closest_polygons_num", int_t, 0, "Number of closest polygons taken into consideration as obstacles; -1 represents all polygons; mainly for performance reasons", 7, -1, 100)
 
 # SFM group parameters
 group_sfm = gen.add_group("SFM", type="tab")

include/hubero_local_planner/hubero_config.h

@@ -42,6 +42,8 @@ namespace hubero_local_planner {
 		bool publish_traj_pcl = true;
 		/// Whether to publish PCL with the potential field generated by the cost function
 		bool publish_cost_grid_pcl = true;
+		/// Number of closest polygons taken into consideration as obstacles; -1 represents all polygons
+		int obstacles_closest_polygons_num = -1;
 	};
 
 	/// \brief Declaration of an SfmParams typedef'ed struct;

src/hubero_config_ros.cpp

@@ -19,6 +19,7 @@ void HuberoConfigROS::loadFromParamServer(const ros::NodeHandle& nh) {
 	nh.param("sim_time", general_->sim_time, general_->sim_time);
 	nh.param("sim_granularity", general_->sim_granularity, general_->sim_granularity);
 	nh.param("angular_sim_granularity", general_->angular_sim_granularity, general_->angular_sim_granularity);
+	nh.param("obstacles_closest_polygons_num", general_->obstacles_closest_polygons_num, general_->obstacles_closest_polygons_num);
 
 	// `sim_period` is handled differently - derived from `controller_frequency`
 	std::string controller_frequency_param;
@@ -87,6 +88,7 @@ void HuberoConfigROS::reconfigure(HuberoPlannerConfig& cfg) {
 	general_->planning_approach = cfg.planning_approach;
 	general_->publish_traj_pcl = cfg.publish_traj_pcl;
 	general_->publish_cost_grid_pcl = cfg.publish_cost_grid_pcl;
+	general_->obstacles_closest_polygons_num = cfg.obstacles_closest_polygons_num;
 
 	sfm_->fov = cfg.fov;
 	sfm_->mass = cfg.mass;

src/hubero_planner_ros.cpp

@@ -429,6 +429,42 @@ bool HuberoPlannerROS::updateObstacleContainerWithCostmapConverter() {
 			obstacles_->back()->setCentroidVelocity(obstacles->obstacles[i].velocities, obstacles->obstacles[i].orientation);
 		}
 	}
+
+	// compute distance to each obstacle and select only N-closest
+	if (obstacles_->size() <= cfg_->getGeneral()->obstacles_closest_polygons_num) {
+		// no need to select closest since there are too few of them
+		return true;
+	}
+
+	// obstacles are in the same frame since costmap_converter_ shares costmap with the planner -> transform not needed
+	geometry_msgs::PoseStamped robot_pose;
+	costmap_ros_->getRobotPose(robot_pose);
+	// circular robot -> let's simplify to the point representation
+	Eigen::Vector2d robot_pos(robot_pose.pose.position.x, robot_pose.pose.position.y);
+
+	// pair with distance and obstacle pointer
+	std::vector<std::pair<double, ObstaclePtr>> obstacles_sort;
+	// save distances from robot to each obstacle
+	for (size_t i = 0; i < obstacles_->size(); i++) {
+		double distance = obstacles_->at(i)->getMinimumDistance(robot_pos);
+		obstacles_sort.push_back({distance, obstacles_->at(i)});
+	}
+	std::sort(
+		obstacles_sort.begin(),
+		obstacles_sort.end(),
+		[](const auto& left, const auto& right) {
+			return left.first < right.first;
+		}
+	);
+	// overwrite obstacles set with the N-closest
+	obstacles_->clear();
+	for (const auto& obstacle_wdist: obstacles_sort) {
+		obstacles_->push_back(obstacle_wdist.second);
+		// check if enough collected
+		if (obstacles_->size() >= cfg_->getGeneral()->obstacles_closest_polygons_num) {
+			break;
+		}
+	}
 	return true;
 }