diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index 2650f7e..e95bbdf 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -22,7 +22,6 @@ struct Frontier { // std::vector points; std::vector vectors_to_points; geometry_msgs::Point reference_robot_pose; - double max_frontier_angular_size{10.0}; // TODO make a parameter on parameter server geometry_msgs::Point toReferenceFrame(const geometry_msgs::Point &pt); geometry_msgs::Point fromReferenceFrame(const geometry_msgs::Point &pt_in_reference_frame); }; @@ -41,7 +40,8 @@ class FrontierSearch * @param costmap Reference to costmap data to search. */ FrontierSearch(costmap_2d::Costmap2D* costmap, double potential_scale, - double gain_scale, double min_frontier_size); + double gain_scale, double min_frontier_size, + int use_every_k_point,double max_frontier_angular_size); /** * @brief Runs search implementation, outward from the start position @@ -54,7 +54,7 @@ class FrontierSearch std::pair approxFrontierByPlanarFarthest(Frontier &fr, geometry_msgs::Point &reference_robot); std::pair approximateFrontierByViewAngle(Frontier &fr); - + std::vector splitFrontier(Frontier& fr); /** @@ -95,6 +95,8 @@ class FrontierSearch unsigned int size_x_, size_y_; double potential_scale_, gain_scale_; double min_frontier_size_; + int use_every_k_point_{1}; + double max_frontier_angular_size_{10.0}; // TODO make a parameter on parameter server }; } #endif diff --git a/explore_lite_revised/src/explore.cpp b/explore_lite_revised/src/explore.cpp index 029f47a..9e2bfe7 100644 --- a/explore_lite_revised/src/explore.cpp +++ b/explore_lite_revised/src/explore.cpp @@ -60,7 +60,8 @@ Explore::Explore() , last_markers_count_(0) { double timeout; - double min_frontier_size; + int use_each_k_point; + double min_frontier_size, max_frontier_angular_size; // parameters for FrontierSearch class private_nh_.param("planner_frequency", planner_frequency_, 1.0); private_nh_.param("progress_timeout", timeout, 30.0); progress_timeout_ = ros::Duration(timeout); @@ -69,10 +70,12 @@ Explore::Explore() private_nh_.param("orientation_scale", orientation_scale_, 0.0); private_nh_.param("gain_scale", gain_scale_, 1.0); private_nh_.param("min_frontier_size", min_frontier_size, 0.5); + private_nh_.param("use_each_k_point", use_each_k_point, 1); + private_nh_.param("max_frontier_angular_size", max_frontier_angular_size, 10.0); search_ = frontier_exploration::FrontierSearch(costmap_client_.getCostmap(), potential_scale_, gain_scale_, - min_frontier_size); + min_frontier_size, use_each_k_point, max_frontier_angular_size); if (visualize_) { marker_array_publisher_ = @@ -127,9 +130,7 @@ void Explore::visualizeFrontiers( markers.push_back(m); ++m.id; m.action = visualization_msgs::Marker::ADD; -// for (; m.id < last_markers_count_; ++m.id) { -// markers.push_back(m); -// } + // TODO when the work on visualisation will stabilize, make a separate functuion or even subpackage for it for (auto& frontier : frontiers) { m.header.frame_id = costmap_client_.getGlobalFrameID(); @@ -200,13 +201,7 @@ void Explore::visualizeFrontiers( m.scale.x = 0.1; m.scale.y = 0.1; m.scale.z = 0.1; -// ROS_ERROR_STREAM(frontier.points_in_reference_coords.size()); -// ROS_ERROR_STREAM("ref: " << frontier.points[0] << frontier.points[1] << frontier.points[2] ); -// ROS_ERROR_STREAM("map_points: " << frontier.vectors_to_points[0] << frontier.vectors_to_points[1] << frontier.vectors_to_points[2] ); -// ROS_ERROR_STREAM(frontier.points_in_reference_coords.size()); -// ROS_ERROR_STREAM(frontier.points.size()); - std::vector translated_vector; - translated_vector.insert(translated_vector.begin(), frontier.vectors_to_points.begin(), frontier.vectors_to_points.end()); + std::vector translated_vector{ frontier.vectors_to_points.begin(), frontier.vectors_to_points.end()}; for (auto &i : translated_vector) { i.x += frontier.reference_robot_pose.x; i.y += frontier.reference_robot_pose.y; @@ -216,37 +211,10 @@ void Explore::visualizeFrontiers( m.header.stamp = ros::Time::now(); markers.push_back(m); - -// for (size_t p_ind = 0; p_ind < frontier.vectors_to_points.size(); ++p_ind){ -// m.type = visualization_msgs::Marker::TEXT_VIEW_FACING; -//// m.header.frame_id = costmap_client_.getBaseFrameID(); -// ++m.id; -// m.pose.position = frontier.vectors_to_points[p_ind]; -// m.text = std::to_string(p_ind); -// m.scale.x = 0.3; -// m.scale.y = 0.3; -// m.scale.z = 0.3; -//// ROS_ERROR_STREAM(frontier.points_in_reference_coords.size()); -// ROS_ERROR_STREAM("ref: " << frontier.points[0] << frontier.points[1] << frontier.points[2] ); -// ROS_ERROR_STREAM("map_points: " << frontier.vectors_to_points[0] << frontier.vectors_to_points[1] << frontier.vectors_to_points[2] ); -//// ROS_ERROR_STREAM(frontier.points_in_reference_coords.size()); -//// ROS_ERROR_STREAM(frontier.points.size()); -// m.points = frontier.vectors_to_points; -// m.color = *red; -// m.header.stamp = ros::Time::now(); -// markers.push_back(m); -// } - } size_t current_markers_count = markers.size(); - // delete previous markers, which are now unused -// m.action = visualization_msgs::Marker::DELETE; -// for (; m.id < last_markers_count_; ++m.id) { -// markers.push_back(m); -// } - last_markers_count_ = current_markers_count; marker_array_publisher_.publish(markers_msg); } @@ -396,13 +364,6 @@ void initialize_variables(){ default_msg_template = new visualization_msgs::Marker(); default_msg_template->ns = "frontiers"; -// default_msg_template->scale.x = 1.0; -// default_msg_template->scale.y = 1.0; -// default_msg_template->scale.z = 1.0; -// default_msg_template->color.r = 0; -// default_msg_template->color.g = 0; -// default_msg_template->color.b = 255; -// default_msg_template->color.a = 255; // lives forever default_msg_template->lifetime = ros::Duration(0); default_msg_template->frame_locked = false; diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index 6aee925..b59c2c8 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -53,11 +53,15 @@ geometry_msgs::Point Frontier::fromReferenceFrame(const geometry_msgs::Point &p FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, double potential_scale, double gain_scale, - double min_frontier_size) + double min_frontier_size, + int use_every_k_point, + double max_frontier_angular_size) : costmap_(costmap) , potential_scale_(potential_scale) , gain_scale_(gain_scale) , min_frontier_size_(min_frontier_size) + , use_every_k_point_(use_every_k_point) + , max_frontier_angular_size_(max_frontier_angular_size) { } std::pair FrontierSearch::approxFrontierByPlanarFarthest(Frontier &fr, @@ -99,7 +103,7 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, return {p1, p2}; } - std::vector splitFrontier(Frontier& fr){ + std::vector FrontierSearch::splitFrontier(Frontier& fr){ Frontier fr1, fr2; std::vector frontiers{fr1, fr2}, vector_buf, res; @@ -124,7 +128,7 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, double fr_angular_dist = angular_vector_distance(this_fr.interpolated_line.first, this_fr.interpolated_line.second); ROS_DEBUG_STREAM("FR has [" << fr_angular_dist << "] deg [" << frontiers[0].vectors_to_points.size() << "] PTS"); - if (fr_angular_dist > this_fr.max_frontier_angular_size + if (fr_angular_dist > this->max_frontier_angular_size_ && fr1.vectors_to_points.size() > 6) // to allways have two subdrontiers with middlepoints { ROS_DEBUG_STREAM("PERFORM FRONTIER RECURSIVE SPLITTING"); @@ -190,8 +194,6 @@ std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) frontier_flag[nbr] = true; std::vector new_frontiers = buildNewFrontier(nbr, pos, frontier_flag); for (auto &new_frontier: new_frontiers){ - /*KD : TODO after moving sparsicng factor to the parameter server rescale those allso*/ - ROS_WARN_STREAM("MIN_FRONTIER" << min_frontier_size_); if (new_frontier.vectors_to_points.size() * costmap_->getResolution() >= min_frontier_size_) { frontier_list.push_back(new_frontier); } @@ -241,8 +243,7 @@ std::vector FrontierSearch::buildNewFrontier(unsigned int initial_cell output.reference_robot_pose.x = reference_x; output.reference_robot_pose.y = reference_y; -// TODO place it on a parameter server -size_t choose_each{1}, cntr{0}; +size_t cntr{0}; while (!bfs.empty()) { unsigned int idx = bfs.front(); @@ -259,7 +260,7 @@ size_t choose_each{1}, cntr{0}; costmap_->mapToWorld(mx, my, wx, wy); // leaving only each n-th point - if (cntr++ % choose_each == 0){ + if (cntr++ % this->use_every_k_point_ == 0){ geometry_msgs::Point reference_scope_point; reference_scope_point.x = wx - reference_x; reference_scope_point.y = wy - reference_y; @@ -268,38 +269,12 @@ size_t choose_each{1}, cntr{0}; output.centroid.y += wy; } - -// geometry_msgs::Point point; -// point.x = wx; -// point.y = wy; -// output.points.push_back(point); - - // update frontier size -// output.size++; // FIXME R U SRIOS!? - - // update centroid of frontier -// output.centroid.x += wx; -// output.centroid.y += wy; - - // determine frontier's distance from robot, going by closest gridcell - // to robot - double distance = std::hypot(reference_x - wx, reference_y - wy); -// if (distance < output.min_distance) { -// output.min_distance = distance; -// output.closest_point.x = wx; -// output.closest_point.y = wy; -// } - // add to queue for breadth first search bfs.push(nbr); } } } - -// output.min_distance = std::hypot(reference_x - output.closest_point.x, reference_y - output.closest_point.y); // average out frontier centroid - - output.centroid.x /= output.vectors_to_points.size(); output.centroid.y /= output.vectors_to_points.size(); /*KD*/ @@ -315,8 +290,8 @@ size_t choose_each{1}, cntr{0}; if (!output.vectors_to_points.empty()){ double degrees_distance = angular_vector_distance(output.interpolated_line.first, output.interpolated_line.second, output.reference_robot_pose); - if (degrees_distance > output.max_frontier_angular_size){ - ROS_WARN_STREAM("Detected too wide frontier [" << degrees_distance <<"]" << "PTS [" < this->max_frontier_angular_size_){ + ROS_INFO_STREAM("Detected wide frontier [" << degrees_distance <<"]" << "PTS [" <