From 53012ffb007e006996981abf55a33ba24ddc136b Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Mon, 11 Mar 2019 15:06:24 +0000 Subject: [PATCH 01/17] initialisation of constant variables took out from code for colors --- explore_lite_revised/src/explore.cpp | 117 +++++++++++++++++++-------- 1 file changed, 84 insertions(+), 33 deletions(-) diff --git a/explore_lite_revised/src/explore.cpp b/explore_lite_revised/src/explore.cpp index 6be6cb7..2326ab1 100644 --- a/explore_lite_revised/src/explore.cpp +++ b/explore_lite_revised/src/explore.cpp @@ -50,6 +50,7 @@ inline static bool operator==(const geometry_msgs::Point& one, namespace explore { + Explore::Explore() : private_nh_("~") , tf_listener_(ros::Duration(10.0)) @@ -92,43 +93,33 @@ Explore::~Explore() stop(); } +std_msgs::ColorRGBA *blue; +std_msgs::ColorRGBA *red; +std_msgs::ColorRGBA *green; + + visualization_msgs::Marker *default_msg_template; + visualization_msgs::Marker *point_msg_template; + visualization_msgs::Marker *figure_msg_template; + + +void set_default_frontier_marker(visualization_msgs::Marker& m){ + m.header.stamp = ros::Time::now(); + m.ns = "frontiers"; +} + +void get_points_marker(){} +void set_figure_marker(){} + void Explore::visualizeFrontiers( const std::vector& frontiers) { - std_msgs::ColorRGBA blue; - blue.r = 0; - blue.g = 0; - blue.b = 1.0; - blue.a = 1.0; - std_msgs::ColorRGBA red; - red.r = 1.0; - red.g = 0; - red.b = 0; - red.a = 1.0; - std_msgs::ColorRGBA green; - green.r = 0; - green.g = 1.0; - green.b = 0; - green.a = 1.0; ROS_DEBUG("visualising %lu frontiers", frontiers.size()); visualization_msgs::MarkerArray markers_msg; std::vector& markers = markers_msg.markers; visualization_msgs::Marker m; - + set_default_frontier_marker(m); m.header.frame_id = costmap_client_.getGlobalFrameID(); - m.header.stamp = ros::Time::now(); - m.ns = "frontiers"; - m.scale.x = 1.0; - m.scale.y = 1.0; - m.scale.z = 1.0; - m.color.r = 0; - m.color.g = 0; - m.color.b = 255; - m.color.a = 255; - // lives forever - m.lifetime = ros::Duration(0); - m.frame_locked = true; // weighted frontiers are always sorted double min_cost = frontiers.empty() ? 0. : frontiers.front().cost; @@ -144,13 +135,14 @@ void Explore::visualizeFrontiers( m.scale.z = 0.1; m.points = frontier.points; if (goalOnBlacklist(frontier.centroid)) { - m.color = red; - } else { - m.color = blue; + m.color = *red; + } + else { + m.color = *blue; } markers.push_back(m); ++id; - m.type = visualization_msgs::Marker::ARROW; + m.type = visualization_msgs::Marker::SPHERE; m.id = int(id); m.pose.position = frontier.initial; // scale frontier according to its cost (costier frontiers will be smaller) @@ -159,7 +151,7 @@ void Explore::visualizeFrontiers( m.scale.y = scale; m.scale.z = scale; m.points = {}; - m.color = green; + m.color = *green; markers.push_back(m); ++id; } @@ -188,6 +180,11 @@ void Explore::makePlan() } if (frontiers.empty()) { + visualization_msgs::MarkerArray empty_marker_msg; +// empty_marker_msg.markers + marker_array_publisher_.publish(empty_marker_msg); + visualizeFrontiers(frontiers); + ROS_WARN_STREAM("NO FRONTIERS LEFT, PUBLISHING EMPTY LIST"); stop(); return; } @@ -287,16 +284,70 @@ void Explore::start() void Explore::stop() { +// marker_array_publisher_.publish(); move_base_client_.cancelAllGoals(); exploring_timer_.stop(); ROS_INFO("Exploration stopped."); } +void initialize_variables(){ +// TODO rewrite in better style with alocator + red = new std_msgs::ColorRGBA(); + blue = new std_msgs::ColorRGBA(); + green = new std_msgs::ColorRGBA(); + + blue->r = 0; + blue->g = 0; + blue->b = 1.0; + blue->a = 1.0; + + red->r = 1.0; + red->g = 0; + red->b = 0; + red->a = 1.0; + + green->r = 0; + green->g = 1.0; + green->b = 0; + green->a = 1.0; + + default_msg_template = new visualization_msgs::Marker(); + 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 = true; + + + point_msg_template = new visualization_msgs::Marker(); + point_msg_template->scale.x = 0.1; + point_msg_template->scale.y = 0.1; + point_msg_template->scale.z = 0.1; + point_msg_template->color = *blue; + point_msg_template->pose.position = {}; + +// point_msg_template->color.g = 0; +// point_msg_template->color.b = 255; +// point_msg_template->color.a = 255; + // lives forever + point_msg_template->lifetime = ros::Duration(0); + point_msg_template->frame_locked = true; +}; + } // namespace explore + + + int main(int argc, char** argv) { ros::init(argc, argv, "explore"); + explore::initialize_variables(); if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) { ros::console::notifyLoggerLevelsChanged(); From e4c04f023a82ac0bbd2ff02d3a40cb58c38294e6 Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Mon, 11 Mar 2019 18:28:32 +0000 Subject: [PATCH 02/17] minor refactoring of initialization --- .../include/explore/explore.h | 1 + explore_lite_revised/src/explore.cpp | 59 ++++++------------- explore_stack/launch/combined_bringup.launch | 4 +- 3 files changed, 22 insertions(+), 42 deletions(-) diff --git a/explore_lite_revised/include/explore/explore.h b/explore_lite_revised/include/explore/explore.h index 52cdbcf..da9e811 100644 --- a/explore_lite_revised/include/explore/explore.h +++ b/explore_lite_revised/include/explore/explore.h @@ -90,6 +90,7 @@ class Explore ros::Publisher marker_array_publisher_; tf::TransformListener tf_listener_; + Costmap2DClient costmap_client_; actionlib::SimpleActionClient move_base_client_; diff --git a/explore_lite_revised/src/explore.cpp b/explore_lite_revised/src/explore.cpp index 2326ab1..73556b6 100644 --- a/explore_lite_revised/src/explore.cpp +++ b/explore_lite_revised/src/explore.cpp @@ -98,8 +98,6 @@ std_msgs::ColorRGBA *red; std_msgs::ColorRGBA *green; visualization_msgs::Marker *default_msg_template; - visualization_msgs::Marker *point_msg_template; - visualization_msgs::Marker *figure_msg_template; void set_default_frontier_marker(visualization_msgs::Marker& m){ @@ -107,8 +105,6 @@ void set_default_frontier_marker(visualization_msgs::Marker& m){ m.ns = "frontiers"; } -void get_points_marker(){} -void set_figure_marker(){} void Explore::visualizeFrontiers( const std::vector& frontiers) @@ -118,32 +114,30 @@ void Explore::visualizeFrontiers( visualization_msgs::MarkerArray markers_msg; std::vector& markers = markers_msg.markers; visualization_msgs::Marker m; - set_default_frontier_marker(m); + m = *default_msg_template; m.header.frame_id = costmap_client_.getGlobalFrameID(); // weighted frontiers are always sorted double min_cost = frontiers.empty() ? 0. : frontiers.front().cost; m.action = visualization_msgs::Marker::ADD; - size_t id = 0; + m.id = 0; + for (auto& frontier : frontiers) { + m.type = visualization_msgs::Marker::POINTS; - m.id = int(id); + ++m.id; m.pose.position = {}; m.scale.x = 0.1; m.scale.y = 0.1; m.scale.z = 0.1; m.points = frontier.points; - if (goalOnBlacklist(frontier.centroid)) { - m.color = *red; - } - else { - m.color = *blue; - } + m.color = goalOnBlacklist(frontier.centroid) ? *red : *blue; + m.header.stamp = ros::Time::now(); markers.push_back(m); - ++id; + m.type = visualization_msgs::Marker::SPHERE; - m.id = int(id); + ++m.id; m.pose.position = frontier.initial; // scale frontier according to its cost (costier frontiers will be smaller) double scale = std::min(std::abs(min_cost * 0.4 / frontier.cost), 0.5); @@ -152,15 +146,14 @@ void Explore::visualizeFrontiers( m.scale.z = scale; m.points = {}; m.color = *green; + m.header.stamp = ros::Time::now(); markers.push_back(m); - ++id; } size_t current_markers_count = markers.size(); // delete previous markers, which are now unused m.action = visualization_msgs::Marker::DELETE; - for (; id < last_markers_count_; ++id) { - m.id = int(id); + for (; m.id < last_markers_count_; ++m.id) { markers.push_back(m); } @@ -312,31 +305,17 @@ void initialize_variables(){ green->a = 1.0; default_msg_template = new visualization_msgs::Marker(); - 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; + 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 = true; - - - point_msg_template = new visualization_msgs::Marker(); - point_msg_template->scale.x = 0.1; - point_msg_template->scale.y = 0.1; - point_msg_template->scale.z = 0.1; - point_msg_template->color = *blue; - point_msg_template->pose.position = {}; - -// point_msg_template->color.g = 0; -// point_msg_template->color.b = 255; -// point_msg_template->color.a = 255; - // lives forever - point_msg_template->lifetime = ros::Duration(0); - point_msg_template->frame_locked = true; }; } // namespace explore diff --git a/explore_stack/launch/combined_bringup.launch b/explore_stack/launch/combined_bringup.launch index 62545b0..f766327 100644 --- a/explore_stack/launch/combined_bringup.launch +++ b/explore_stack/launch/combined_bringup.launch @@ -16,8 +16,8 @@ - - + + From aa662ee296b9971f8468bb17dec0fccb23171958 Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Tue, 12 Mar 2019 11:54:40 +0000 Subject: [PATCH 03/17] refactored distance calculation to use of std::hypot introduced search of two farthest from centroid points in discource of frontier buidling --- .../include/explore/frontier_search.h | 5 +++ explore_lite_revised/src/explore.cpp | 22 +++++++++++ explore_lite_revised/src/frontier_search.cpp | 39 +++++++++++++++++-- 3 files changed, 63 insertions(+), 3 deletions(-) diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index e0e228e..7574453 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -16,6 +16,7 @@ struct Frontier { geometry_msgs::Point initial; geometry_msgs::Point centroid; geometry_msgs::Point middle; + std::pair interpolated_line; std::vector points; }; @@ -45,6 +46,10 @@ class FrontierSearch std::vector searchFrom(geometry_msgs::Point position); protected: + std::pair getFrontierEdges(Frontier &fr, geometry_msgs::Point &reference_robot); + + + /** * @brief Starting from an initial cell, build a frontier from valid adjacent * cells diff --git a/explore_lite_revised/src/explore.cpp b/explore_lite_revised/src/explore.cpp index 73556b6..b74cbd3 100644 --- a/explore_lite_revised/src/explore.cpp +++ b/explore_lite_revised/src/explore.cpp @@ -148,6 +148,28 @@ void Explore::visualizeFrontiers( m.color = *green; m.header.stamp = ros::Time::now(); markers.push_back(m); + + m.type = visualization_msgs::Marker::SPHERE; + ++m.id; + m.pose.position = frontier.interpolated_line.first; + m.scale.x = 0.1; + m.scale.y = 0.1; + m.scale.z = 0.2; + m.points = {}; + m.color = *red; + m.header.stamp = ros::Time::now(); + markers.push_back(m); + + ++m.id; + m.pose.position = frontier.interpolated_line.second; + m.scale.x = 0.1; + m.scale.y = 0.1; + m.scale.z = 0.2; + m.points = {}; + m.color = *red; + m.header.stamp = ros::Time::now(); + markers.push_back(m); + } size_t current_markers_count = markers.size(); diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index c873a9c..19994fa 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -8,6 +8,8 @@ #include +#include + namespace frontier_exploration { using costmap_2d::LETHAL_OBSTACLE; @@ -24,6 +26,31 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, { } + std::pair FrontierSearch::getFrontierEdges(Frontier &fr, geometry_msgs::Point &reference_robot){ + ROS_ERROR_STREAM("GOT REFERENCE POINT [" << reference_robot.x << ":"<< reference_robot.y << "]"); + ROS_ERROR_STREAM("GOT CENTROID POINT [" << fr.centroid.x << ":"<< fr.centroid.y << "]"); + double max_dist = 0.0; + double centroid_dist{0.0}, point_point_dist{0.0}; + geometry_msgs::Point p1, p2; + for (auto &point: fr.points){ + centroid_dist = std::hypot(point.x - fr.centroid.x , point.y - fr.centroid.y); + if (centroid_dist > max_dist){ + max_dist = centroid_dist; + p1 = point; + } + } + max_dist = 0.0; + for (auto &point: fr.points){ + centroid_dist = std::hypot(point.x - fr.centroid.x , point.y - fr.centroid.y); + point_point_dist = std::hypot(point.x - p1.x , point.y - p1.y); + if (centroid_dist + point_point_dist> max_dist){ + max_dist = centroid_dist + point_point_dist; + p2 = point; + } + } + return {p1, p2}; + } + std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) { std::vector frontier_list; @@ -119,7 +146,9 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, double reference_x, reference_y; costmap_->indexToCells(reference, rx, ry); costmap_->mapToWorld(rx, ry, reference_x, reference_y); - + geometry_msgs::Point reference_robot_pose; + reference_robot_pose.x = reference_x; + reference_robot_pose.y = reference_y; while (!bfs.empty()) { unsigned int idx = bfs.front(); bfs.pop(); @@ -149,8 +178,7 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, // determine frontier's distance from robot, going by closest gridcell // to robot - double distance = sqrt(pow((double(reference_x) - double(wx)), 2.0) + - pow((double(reference_y) - double(wy)), 2.0)); + double distance = std::hypot(reference_x - wx, reference_x - wx); if (distance < output.min_distance) { output.min_distance = distance; output.middle.x = wx; @@ -164,11 +192,15 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, } // average out frontier centroid + output.centroid.x /= output.size; output.centroid.y /= output.size; + output.interpolated_line = getFrontierEdges(output, reference_robot_pose); return output; } + + bool FrontierSearch::isNewFrontierCell(unsigned int idx, const std::vector& frontier_flag) { @@ -194,4 +226,5 @@ double FrontierSearch::frontierCost(const Frontier& frontier) costmap_->getResolution()) - (gain_scale_ * frontier.size * costmap_->getResolution()); } + } From 75c7118e4ca6ab64233b509f04b3ae3ddb72829b Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Tue, 12 Mar 2019 12:29:33 +0000 Subject: [PATCH 04/17] added approximation by hypotetical bounding points --- explore_lite_revised/src/explore.cpp | 14 +++++++------- explore_lite_revised/src/frontier_search.cpp | 19 +++++++++++-------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/explore_lite_revised/src/explore.cpp b/explore_lite_revised/src/explore.cpp index b74cbd3..f8ec273 100644 --- a/explore_lite_revised/src/explore.cpp +++ b/explore_lite_revised/src/explore.cpp @@ -149,23 +149,23 @@ void Explore::visualizeFrontiers( m.header.stamp = ros::Time::now(); markers.push_back(m); - m.type = visualization_msgs::Marker::SPHERE; + m.type = visualization_msgs::Marker::LINE_STRIP; ++m.id; - m.pose.position = frontier.interpolated_line.first; + m.pose.position = {}; m.scale.x = 0.1; m.scale.y = 0.1; - m.scale.z = 0.2; - m.points = {}; + m.scale.z = 0.1; + m.points = {frontier.interpolated_line.first, frontier.centroid}; m.color = *red; m.header.stamp = ros::Time::now(); markers.push_back(m); ++m.id; - m.pose.position = frontier.interpolated_line.second; + m.pose.position = {}; m.scale.x = 0.1; m.scale.y = 0.1; - m.scale.z = 0.2; - m.points = {}; + m.scale.z = 0.1; + m.points = {frontier.interpolated_line.second, frontier.centroid}; m.color = *red; m.header.stamp = ros::Time::now(); markers.push_back(m); diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index 19994fa..c9c855a 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -30,21 +30,24 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, ROS_ERROR_STREAM("GOT REFERENCE POINT [" << reference_robot.x << ":"<< reference_robot.y << "]"); ROS_ERROR_STREAM("GOT CENTROID POINT [" << fr.centroid.x << ":"<< fr.centroid.y << "]"); double max_dist = 0.0; - double centroid_dist{0.0}, point_point_dist{0.0}; + double heuristics_dist {0.0}; geometry_msgs::Point p1, p2; for (auto &point: fr.points){ - centroid_dist = std::hypot(point.x - fr.centroid.x , point.y - fr.centroid.y); - if (centroid_dist > max_dist){ - max_dist = centroid_dist; + heuristics_dist = + std::hypot(point.x - reference_robot.x , point.y - reference_robot.y) + /* robot_point_dist */ + std::hypot(point.x - fr.centroid.x , point.y - fr.centroid.y); /* centroid_point dist */ + if (heuristics_dist > max_dist){ + max_dist = heuristics_dist; p1 = point; } } max_dist = 0.0; for (auto &point: fr.points){ - centroid_dist = std::hypot(point.x - fr.centroid.x , point.y - fr.centroid.y); - point_point_dist = std::hypot(point.x - p1.x , point.y - p1.y); - if (centroid_dist + point_point_dist> max_dist){ - max_dist = centroid_dist + point_point_dist; + heuristics_dist = + std::hypot(point.x - p1.x , point.y - p1.y) + /* point-point dist */ + std::hypot(point.x - fr.centroid.x , point.y - fr.centroid.y); /* centroid_point dist */ + if (heuristics_dist > max_dist){ + max_dist = heuristics_dist; p2 = point; } } From dc1e6b85132e6aef021c755a503001ec065939c8 Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Tue, 12 Mar 2019 13:46:30 +0000 Subject: [PATCH 05/17] refactored individual deletes with DELETEALL clearing msg added function for angular measurement of frontier size --- explore_lite_revised/src/explore.cpp | 20 +++++++++++----- explore_lite_revised/src/frontier_search.cpp | 25 ++++++++++++++++++-- 2 files changed, 37 insertions(+), 8 deletions(-) diff --git a/explore_lite_revised/src/explore.cpp b/explore_lite_revised/src/explore.cpp index f8ec273..89d4bb2 100644 --- a/explore_lite_revised/src/explore.cpp +++ b/explore_lite_revised/src/explore.cpp @@ -120,9 +120,17 @@ void Explore::visualizeFrontiers( // weighted frontiers are always sorted double min_cost = frontiers.empty() ? 0. : frontiers.front().cost; - m.action = visualization_msgs::Marker::ADD; + m.id = 0; + m.action = visualization_msgs::Marker::DELETEALL; + markers.push_back(m); + ++m.id; + m.action = visualization_msgs::Marker::ADD; +// for (; m.id < last_markers_count_; ++m.id) { +// markers.push_back(m); +// } + for (auto& frontier : frontiers) { m.type = visualization_msgs::Marker::POINTS; @@ -174,10 +182,10 @@ void Explore::visualizeFrontiers( 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); - } +// 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); @@ -337,7 +345,7 @@ void initialize_variables(){ // default_msg_template->color.a = 255; // lives forever default_msg_template->lifetime = ros::Duration(0); - default_msg_template->frame_locked = true; + default_msg_template->frame_locked = false; }; } // namespace explore diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index c9c855a..cc4711a 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -9,6 +9,8 @@ #include #include +#include +#include namespace frontier_exploration { @@ -27,8 +29,8 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, } std::pair FrontierSearch::getFrontierEdges(Frontier &fr, geometry_msgs::Point &reference_robot){ - ROS_ERROR_STREAM("GOT REFERENCE POINT [" << reference_robot.x << ":"<< reference_robot.y << "]"); - ROS_ERROR_STREAM("GOT CENTROID POINT [" << fr.centroid.x << ":"<< fr.centroid.y << "]"); +// ROS_ERROR_STREAM("GOT REFERENCE POINT [" << reference_robot.x << ":"<< reference_robot.y << "]"); +// ROS_ERROR_STREAM("GOT CENTROID POINT [" << fr.centroid.x << ":"<< fr.centroid.y << "]"); double max_dist = 0.0; double heuristics_dist {0.0}; geometry_msgs::Point p1, p2; @@ -124,6 +126,21 @@ std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) return frontier_list; } + +double getDegreesDistance(geometry_msgs::Point p1,geometry_msgs::Point p2,geometry_msgs::Point reference){ + // using vector dotproduct to get angle between two reference points v_A dot v_B == |v_A| * |v_B| * cos(angle(A^B)) + double v1_x{p1.x - reference.x}, + v1_y{p1.y - reference.y}, + v2_x{p2.x - reference.x}, + v2_y{p2.y - reference.y}, + len_v1 = std::hypot(v1_x, v1_y), + len_v2 = std::hypot(v2_x, v2_y); + double dot_prod = v1_x * v2_x + v1_y * v2_y; + double angle = std::acos(dot_prod / (len_v1 * len_v2)) * 180.0 / M_PI; + return angle; +} + + Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, unsigned int reference, std::vector& frontier_flag) @@ -198,7 +215,11 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, output.centroid.x /= output.size; output.centroid.y /= output.size; + /*KD*/ output.interpolated_line = getFrontierEdges(output, reference_robot_pose); + ROS_ERROR_STREAM("distance in degrees = " << getDegreesDistance(output.interpolated_line.first, output.interpolated_line.second, reference_robot_pose)); + // next splitting onto pieces while they are more then 10deg from robots perspective + return output; } From 13134770a68e479087f82424060f78b6ebf1cf78 Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Wed, 13 Mar 2019 09:25:39 +0000 Subject: [PATCH 06/17] visualisation update --- explore_lite_revised/src/explore.cpp | 60 +++++++++++++++----- explore_lite_revised/src/frontier_search.cpp | 5 ++ 2 files changed, 52 insertions(+), 13 deletions(-) diff --git a/explore_lite_revised/src/explore.cpp b/explore_lite_revised/src/explore.cpp index 89d4bb2..1ac8413 100644 --- a/explore_lite_revised/src/explore.cpp +++ b/explore_lite_revised/src/explore.cpp @@ -130,7 +130,7 @@ void Explore::visualizeFrontiers( // 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.type = visualization_msgs::Marker::POINTS; @@ -157,26 +157,60 @@ void Explore::visualizeFrontiers( m.header.stamp = ros::Time::now(); markers.push_back(m); - m.type = visualization_msgs::Marker::LINE_STRIP; + m.type = visualization_msgs::Marker::SPHERE_LIST; + ++m.id; + m.pose.position = {}; + m.scale.x = 0.2; + m.scale.y = 0.2; + m.scale.z = 0.2; + m.points = {frontier.interpolated_line.first, frontier.centroid, frontier.interpolated_line.second}; + m.color = *red; + m.header.stamp = ros::Time::now(); + markers.push_back(m); + + +// m.type = visualization_msgs::Marker::SPHERE; +// ++m.id; +// m.pose.position = frontier.interpolated_line.first; +// m.scale.x = 0.2; +// m.scale.y = 0.2; +// m.scale.z = 0.2; +// m.points = {}; +// m.color = *red; +// m.header.stamp = ros::Time::now(); +// markers.push_back(m); +// +// m.type = visualization_msgs::Marker::SPHERE; +// ++m.id; +// m.pose.position = frontier.interpolated_line.second; +// m.scale.x = 0.2; +// m.scale.y = 0.2; +// m.scale.z = 0.2; +// m.points = {}; +// m.color = *red; +// m.header.stamp = ros::Time::now(); +// markers.push_back(m); + + m.type = visualization_msgs::Marker::LINE_STRIP; ++m.id; m.pose.position = {}; m.scale.x = 0.1; m.scale.y = 0.1; m.scale.z = 0.1; - m.points = {frontier.interpolated_line.first, frontier.centroid}; - m.color = *red; - m.header.stamp = ros::Time::now(); - markers.push_back(m); - - ++m.id; - m.pose.position = {}; - m.scale.x = 0.1; - m.scale.y = 0.1; - m.scale.z = 0.1; - m.points = {frontier.interpolated_line.second, frontier.centroid}; + m.points = {frontier.interpolated_line.first, frontier.centroid, frontier.interpolated_line.second}; m.color = *red; m.header.stamp = ros::Time::now(); markers.push_back(m); +// +// ++m.id; +// m.pose.position = {}; +// m.scale.x = 0.1; +// m.scale.y = 0.1; +// m.scale.z = 0.1; +// m.points = {frontier.interpolated_line.second, frontier.centroid}; +// m.color = *red; +// m.header.stamp = ros::Time::now(); +// markers.push_back(m); } size_t current_markers_count = markers.size(); diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index cc4711a..7a5a2d3 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -217,6 +217,11 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, output.centroid.y /= output.size; /*KD*/ output.interpolated_line = getFrontierEdges(output, reference_robot_pose); + double degrees_distance = getDegreesDistance(output.interpolated_line.first, output.interpolated_line.second, reference_robot_pose); + if (degrees_distance > 10.0){ + + } + ROS_ERROR_STREAM("distance in degrees = " << getDegreesDistance(output.interpolated_line.first, output.interpolated_line.second, reference_robot_pose)); // next splitting onto pieces while they are more then 10deg from robots perspective From 56b7ae49a2e1266ca8927d038fc022935a161960 Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Wed, 13 Mar 2019 16:08:52 +0000 Subject: [PATCH 07/17] milestone on working 'split-frontier-ahalf' --- .../include/explore/frontier_search.h | 10 +- explore_lite_revised/src/explore.cpp | 103 +++++++++++-- explore_lite_revised/src/frontier_search.cpp | 138 +++++++++++++++--- 3 files changed, 214 insertions(+), 37 deletions(-) diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index 7574453..19c0646 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -18,6 +18,7 @@ struct Frontier { geometry_msgs::Point middle; std::pair interpolated_line; std::vector points; + std::vector vectors_to_points; }; /** @@ -46,7 +47,10 @@ class FrontierSearch std::vector searchFrom(geometry_msgs::Point position); protected: - std::pair getFrontierEdges(Frontier &fr, geometry_msgs::Point &reference_robot); + std::pair approxFrontierPlanarFarthest(Frontier &fr, + geometry_msgs::Point &reference_robot); + std::pair approximateFrontierViewAngle(Frontier &fr, + geometry_msgs::Point &reference_robot); @@ -59,8 +63,8 @@ class FrontierSearch * as frontiers * @return new frontier */ - Frontier buildNewFrontier(unsigned int initial_cell, unsigned int reference, - std::vector& frontier_flag); + std::vector buildNewFrontier(unsigned int initial_cell, unsigned int reference, + std::vector &frontier_flag); /** * @brief isNewFrontierCell Evaluate if candidate cell is a valid candidate diff --git a/explore_lite_revised/src/explore.cpp b/explore_lite_revised/src/explore.cpp index 1ac8413..a5a667e 100644 --- a/explore_lite_revised/src/explore.cpp +++ b/explore_lite_revised/src/explore.cpp @@ -99,7 +99,7 @@ std_msgs::ColorRGBA *green; visualization_msgs::Marker *default_msg_template; - +// todo consicer cleanup void set_default_frontier_marker(visualization_msgs::Marker& m){ m.header.stamp = ros::Time::now(); m.ns = "frontiers"; @@ -132,7 +132,7 @@ void Explore::visualizeFrontiers( // } // 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(); m.type = visualization_msgs::Marker::POINTS; ++m.id; m.pose.position = {}; @@ -144,18 +144,18 @@ void Explore::visualizeFrontiers( m.header.stamp = ros::Time::now(); markers.push_back(m); - m.type = visualization_msgs::Marker::SPHERE; - ++m.id; - m.pose.position = frontier.initial; - // scale frontier according to its cost (costier frontiers will be smaller) - double scale = std::min(std::abs(min_cost * 0.4 / frontier.cost), 0.5); - m.scale.x = scale; - m.scale.y = scale; - m.scale.z = scale; - m.points = {}; - m.color = *green; - m.header.stamp = ros::Time::now(); - markers.push_back(m); +// m.type = visualization_msgs::Marker::SPHERE; +// ++m.id; +// m.pose.position = frontier.initial; +// // scale frontier according to its cost (costier frontiers will be smaller) +// double scale = std::min(std::abs(min_cost * 0.4 / frontier.cost), 0.5); +// m.scale.x = scale; +// m.scale.y = scale; +// m.scale.z = scale; +// m.points = {}; +// m.color = *green; +// m.header.stamp = ros::Time::now(); +// markers.push_back(m); m.type = visualization_msgs::Marker::SPHERE_LIST; ++m.id; @@ -201,6 +201,42 @@ void Explore::visualizeFrontiers( m.color = *red; m.header.stamp = ros::Time::now(); markers.push_back(m); + + m.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + ++m.id; + m.text = "F"; + m.pose.position = frontier.interpolated_line.first; + m.scale.x = 0.2; + m.scale.y = 0.2; + m.scale.z = 0.2; + m.points = {}; + m.color = *blue; + m.header.stamp = ros::Time::now(); + markers.push_back(m); + m.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + ++m.id; + m.text = "M"; + m.pose.position = frontier.centroid; + m.scale.x = 0.2; + m.scale.y = 0.2; + m.scale.z = 0.2; + m.points = {}; + m.color = *blue; + m.header.stamp = ros::Time::now(); + markers.push_back(m); + m.type = visualization_msgs::Marker::TEXT_VIEW_FACING; + ++m.id; + m.text = "L"; + m.pose.position = frontier.interpolated_line.second; + m.scale.x = 0.2; + m.scale.y = 0.2; + m.scale.z = 0.2; + m.points = {}; + m.color = *blue; + m.header.stamp = ros::Time::now(); + markers.push_back(m); + + // // ++m.id; // m.pose.position = {}; @@ -212,7 +248,46 @@ void Explore::visualizeFrontiers( // m.header.stamp = ros::Time::now(); // markers.push_back(m); + m.type = visualization_msgs::Marker::POINTS; +// m.header.frame_id = costmap_client_.getBaseFrameID(); + ++m.id; + m.pose.position = {}; + 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 = *green; + 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 diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index 7a5a2d3..326e07f 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -28,9 +28,8 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, { } - std::pair FrontierSearch::getFrontierEdges(Frontier &fr, geometry_msgs::Point &reference_robot){ -// ROS_ERROR_STREAM("GOT REFERENCE POINT [" << reference_robot.x << ":"<< reference_robot.y << "]"); -// ROS_ERROR_STREAM("GOT CENTROID POINT [" << fr.centroid.x << ":"<< fr.centroid.y << "]"); + std::pair FrontierSearch::approxFrontierPlanarFarthest(Frontier &fr, + geometry_msgs::Point &reference_robot){ double max_dist = 0.0; double heuristics_dist {0.0}; geometry_msgs::Point p1, p2; @@ -53,9 +52,46 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, p2 = point; } } - return {p1, p2}; + return {p1, p2}; +} + + + std::pair FrontierSearch::approximateFrontierViewAngle( + frontier_exploration::Frontier &fr, geometry_msgs::Point &reference_robot) { + geometry_msgs::Point p1, p2; + if (!fr.vectors_to_points.empty()){ + p1.x = fr.vectors_to_points.front().x + reference_robot.x; + p1.y = fr.vectors_to_points.front().y + reference_robot.y; + p2.x = fr.vectors_to_points.back().x + reference_robot.x; + p2.y = fr.vectors_to_points.back().y + reference_robot.y; + ROS_DEBUG_STREAM( " should " << fr.vectors_to_points.front() << fr.vectors_to_points.back()); + } // else default zero values + return {p1, p2}; } + std::vector splitFrontier(Frontier& fr, geometry_msgs::Point reference_robot_pose){ + Frontier fr1, fr2; + assert(!fr.vectors_to_points.empty() > 0); + size_t fr1_pts_number = fr.vectors_to_points.size() / 2; + // TODO remove it, while for backward compatibility + fr1.points = fr.points; + fr2.points = fr.points; +// fr1.min_distance + fr1.vectors_to_points = std::vector(fr.vectors_to_points.begin(), fr.vectors_to_points.begin() + fr1_pts_number); + fr1.interpolated_line = {fr.interpolated_line.first, fr.middle}; + fr1.centroid = fr1.vectors_to_points[fr1.vectors_to_points.size() / 2]; + fr1.centroid.x += reference_robot_pose.x; + fr1.centroid.y += reference_robot_pose.y; + + + fr2.vectors_to_points = std::vector( fr.vectors_to_points.begin() + fr1_pts_number, fr.vectors_to_points.end()); + fr2.interpolated_line = {fr.middle, fr.interpolated_line.second}; + fr2.centroid = fr2.vectors_to_points[fr2.vectors_to_points.size() / 2]; + fr2.centroid.x += reference_robot_pose.x; + fr2.centroid.y += reference_robot_pose.y; + + return {fr1, fr2}; +} std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) { std::vector frontier_list; @@ -106,10 +142,11 @@ std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) // neighbour) } else if (isNewFrontierCell(nbr, frontier_flag)) { frontier_flag[nbr] = true; - Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag); - if (new_frontier.size * costmap_->getResolution() >= - min_frontier_size_) { - frontier_list.push_back(new_frontier); + std::vector new_frontiers = buildNewFrontier(nbr, pos, frontier_flag); + for (auto &new_frontier: new_frontiers){ + if (new_frontier.size * costmap_->getResolution() >= min_frontier_size_) { + frontier_list.push_back(new_frontier); + } } } } @@ -141,9 +178,14 @@ double getDegreesDistance(geometry_msgs::Point p1,geometry_msgs::Point p2,geomet } -Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, - unsigned int reference, - std::vector& frontier_flag) +bool cmp_by_angle(geometry_msgs::Point p1, geometry_msgs::Point p2){ + return atan2(p1.y, p1.x) < atan2(p2.y, p2.x); +} + + +std::vector FrontierSearch::buildNewFrontier(unsigned int initial_cell, + unsigned int reference, + std::vector &frontier_flag) { // initialize frontier structure Frontier output; @@ -155,8 +197,12 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, // record initial contact point for frontier unsigned int ix, iy; costmap_->indexToCells(initial_cell, ix, iy); + ROS_ERROR_STREAM(ix << " --- " << iy); + costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y); + ROS_ERROR_STREAM(output.initial.x << " --- " << output.initial.y); + // push initial gridcell onto queue std::queue bfs; bfs.push(initial_cell); @@ -169,10 +215,16 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, geometry_msgs::Point reference_robot_pose; reference_robot_pose.x = reference_x; reference_robot_pose.y = reference_y; + +// ROS_ERROR_STREAM( reference_robot_pose.x << " " << reference_robot_pose.y); +// TODO make a parameter +size_t choose_each = 6; + while (!bfs.empty()) { unsigned int idx = bfs.front(); bfs.pop(); - + double world_scale_X = costmap_->getSizeInMetersX(); + double world_scale_Y = costmap_->getSizeInMetersY(); // try adding cells in 8-connected neighborhood to frontier for (unsigned int nbr : nhood8(idx, *costmap_)) { // check if neighbour is a potential frontier cell @@ -184,13 +236,23 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, costmap_->indexToCells(nbr, mx, my); costmap_->mapToWorld(mx, my, wx, wy); + // selecting sparce points + if (output.points.size() % choose_each == 0){ + geometry_msgs::Point reference_scope_point; + reference_scope_point.x = wx - reference_x; + reference_scope_point.y = wy - reference_y; + output.vectors_to_points.push_back(reference_scope_point); + } + +// costmap_. + geometry_msgs::Point point; point.x = wx; point.y = wy; output.points.push_back(point); // update frontier size - output.size++; + output.size++; // FIXME R U SRIOS!? // update centroid of frontier output.centroid.x += wx; @@ -198,7 +260,7 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, // determine frontier's distance from robot, going by closest gridcell // to robot - double distance = std::hypot(reference_x - wx, reference_x - wx); + double distance = std::hypot(reference_x - wx, reference_y - wy); if (distance < output.min_distance) { output.min_distance = distance; output.middle.x = wx; @@ -211,21 +273,51 @@ Frontier FrontierSearch::buildNewFrontier(unsigned int initial_cell, } } +// output.min_distance = std::hypot(reference_x - output.middle.x, reference_y - output.middle.y); // average out frontier centroid output.centroid.x /= output.size; output.centroid.y /= output.size; /*KD*/ - output.interpolated_line = getFrontierEdges(output, reference_robot_pose); - double degrees_distance = getDegreesDistance(output.interpolated_line.first, output.interpolated_line.second, reference_robot_pose); + +// todo doit with lambda + std::sort(output.vectors_to_points.begin(), output.vectors_to_points.end(), cmp_by_angle); + + // fixme find out why there are occuring empty frontiers (empty raw points) +// ROS_WARN_STREAM(output.vectors_to_points.size() << " VEC LENGTH"); +// if (output.vectors_to_points.size() > 0) +// ROS_WARN_STREAM(output.vectors_to_points.front() << " VEC LENGTH" << output.vectors_to_points.back()); +// else +// ROS_WARN_STREAM(output.points.size() << " FULL LENGTH "); + output.interpolated_line = approximateFrontierViewAngle(output, reference_robot_pose); + + +// angular distance for vectors +// TODO move to approppriate frontier +if (!output.vectors_to_points.empty()){ + double degrees_distance = std::abs( atan2(output.interpolated_line.first.y, output.interpolated_line.first.x) - atan2(output.interpolated_line.second.y, output.interpolated_line.second.x) ) + * 180.0 / M_PI; + getDegreesDistance(output.interpolated_line.first, output.interpolated_line.second, reference_robot_pose); + if (degrees_distance > 10.0){ + ROS_WARN_STREAM("Detected too wide frontier, " << degrees_distance << "splitting on two"); + + auto splitted = splitFrontier(output, reference_robot_pose); + for (auto &fr: splitted){ + degrees_distance = std::abs( atan2(fr.interpolated_line.first.y, fr.interpolated_line.first.x) - atan2(fr.interpolated_line.second.y, fr.interpolated_line.second.x) ) + * 180.0 / M_PI; + ROS_WARN_STREAM("Ner frontier, " << degrees_distance); + } + return splitted; } +} ROS_ERROR_STREAM("distance in degrees = " << getDegreesDistance(output.interpolated_line.first, output.interpolated_line.second, reference_robot_pose)); // next splitting onto pieces while they are more then 10deg from robots perspective - return output; + + return {output}; } @@ -251,9 +343,15 @@ bool FrontierSearch::isNewFrontierCell(unsigned int idx, double FrontierSearch::frontierCost(const Frontier& frontier) { - return (potential_scale_ * frontier.min_distance * - costmap_->getResolution()) - - (gain_scale_ * frontier.size * costmap_->getResolution()); + return (potential_scale_ * frontier.min_distance + // * costmap_->getResolution() + ) + - + (gain_scale_ + /*KD*/ * frontier.vectors_to_points.size() + // * frontier.size + // * costmap_->getResolution() + ); } } From a8c90d2aa77686d83d91f5f2f8e832f0fecf8ee9 Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Thu, 14 Mar 2019 08:19:30 +0000 Subject: [PATCH 08/17] frontier had beeng sparcified and split --- .../include/explore/frontier_search.h | 4 +- explore_lite_revised/src/explore.cpp | 76 ++---------- explore_lite_revised/src/frontier_search.cpp | 116 +++++++++++++----- .../src/velocity_topic_watchdog.cpp | 4 +- 4 files changed, 100 insertions(+), 100 deletions(-) diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index 19c0646..1c5d798 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -19,6 +19,7 @@ struct Frontier { std::pair interpolated_line; std::vector points; std::vector vectors_to_points; + geometry_msgs::Point reference_robot_pose; }; /** @@ -49,8 +50,7 @@ class FrontierSearch protected: std::pair approxFrontierPlanarFarthest(Frontier &fr, geometry_msgs::Point &reference_robot); - std::pair approximateFrontierViewAngle(Frontier &fr, - geometry_msgs::Point &reference_robot); + std::pair approximateFrontierViewAngle(Frontier &fr); diff --git a/explore_lite_revised/src/explore.cpp b/explore_lite_revised/src/explore.cpp index a5a667e..714b9b2 100644 --- a/explore_lite_revised/src/explore.cpp +++ b/explore_lite_revised/src/explore.cpp @@ -133,29 +133,6 @@ void Explore::visualizeFrontiers( // 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(); - m.type = visualization_msgs::Marker::POINTS; - ++m.id; - m.pose.position = {}; - m.scale.x = 0.1; - m.scale.y = 0.1; - m.scale.z = 0.1; - m.points = frontier.points; - m.color = goalOnBlacklist(frontier.centroid) ? *red : *blue; - m.header.stamp = ros::Time::now(); - markers.push_back(m); - -// m.type = visualization_msgs::Marker::SPHERE; -// ++m.id; -// m.pose.position = frontier.initial; -// // scale frontier according to its cost (costier frontiers will be smaller) -// double scale = std::min(std::abs(min_cost * 0.4 / frontier.cost), 0.5); -// m.scale.x = scale; -// m.scale.y = scale; -// m.scale.z = scale; -// m.points = {}; -// m.color = *green; -// m.header.stamp = ros::Time::now(); -// markers.push_back(m); m.type = visualization_msgs::Marker::SPHERE_LIST; ++m.id; @@ -169,27 +146,6 @@ void Explore::visualizeFrontiers( markers.push_back(m); -// m.type = visualization_msgs::Marker::SPHERE; -// ++m.id; -// m.pose.position = frontier.interpolated_line.first; -// m.scale.x = 0.2; -// m.scale.y = 0.2; -// m.scale.z = 0.2; -// m.points = {}; -// m.color = *red; -// m.header.stamp = ros::Time::now(); -// markers.push_back(m); -// -// m.type = visualization_msgs::Marker::SPHERE; -// ++m.id; -// m.pose.position = frontier.interpolated_line.second; -// m.scale.x = 0.2; -// m.scale.y = 0.2; -// m.scale.z = 0.2; -// m.points = {}; -// m.color = *red; -// m.header.stamp = ros::Time::now(); -// markers.push_back(m); m.type = visualization_msgs::Marker::LINE_STRIP; ++m.id; @@ -237,31 +193,25 @@ void Explore::visualizeFrontiers( markers.push_back(m); -// -// ++m.id; -// m.pose.position = {}; -// m.scale.x = 0.1; -// m.scale.y = 0.1; -// m.scale.z = 0.1; -// m.points = {frontier.interpolated_line.second, frontier.centroid}; -// m.color = *red; -// m.header.stamp = ros::Time::now(); -// markers.push_back(m); - m.type = visualization_msgs::Marker::POINTS; -// m.header.frame_id = costmap_client_.getBaseFrameID(); ++m.id; m.pose.position = {}; - m.scale.x = 0.3; - m.scale.y = 0.3; - m.scale.z = 0.3; + 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("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 = *green; + std::vector translated_vector; + translated_vector.insert(translated_vector.begin(), 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; + } + m.points = translated_vector; + m.color = goalOnBlacklist(frontier.centroid) ? *red : *blue; m.header.stamp = ros::Time::now(); markers.push_back(m); diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index 326e07f..90b83bd 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -18,6 +18,11 @@ using costmap_2d::LETHAL_OBSTACLE; using costmap_2d::NO_INFORMATION; using costmap_2d::FREE_SPACE; + + inline double angular_vector_distance(geometry_msgs::Point v1, geometry_msgs::Point v2){ + return std::abs( atan2(v1.y, v1.x) - atan2(v2.y, v2.x) ) * 180.0 / M_PI; + } + FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, double potential_scale, double gain_scale, double min_frontier_size) @@ -57,41 +62,87 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, std::pair FrontierSearch::approximateFrontierViewAngle( - frontier_exploration::Frontier &fr, geometry_msgs::Point &reference_robot) { + frontier_exploration::Frontier &fr) { geometry_msgs::Point p1, p2; if (!fr.vectors_to_points.empty()){ - p1.x = fr.vectors_to_points.front().x + reference_robot.x; - p1.y = fr.vectors_to_points.front().y + reference_robot.y; - p2.x = fr.vectors_to_points.back().x + reference_robot.x; - p2.y = fr.vectors_to_points.back().y + reference_robot.y; - ROS_DEBUG_STREAM( " should " << fr.vectors_to_points.front() << fr.vectors_to_points.back()); + p1.x = fr.vectors_to_points.front().x + fr.reference_robot_pose.x; + p1.y = fr.vectors_to_points.front().y + fr.reference_robot_pose.y; + p2.x = fr.vectors_to_points.back().x + fr.reference_robot_pose.x; + p2.y = fr.vectors_to_points.back().y + fr.reference_robot_pose.y; + ROS_DEBUG_STREAM( " approximated borders with view angle " << fr.vectors_to_points.front() << fr.vectors_to_points.back()); } // else default zero values return {p1, p2}; } - std::vector splitFrontier(Frontier& fr, geometry_msgs::Point reference_robot_pose){ - Frontier fr1, fr2; - assert(!fr.vectors_to_points.empty() > 0); + std::vector splitFrontier(Frontier& fr){ +// ROS_WARN_STREAM(0); + Frontier fr1(fr), fr2(fr); +// assert(!fr.vectors_to_points.empty() > 0); size_t fr1_pts_number = fr.vectors_to_points.size() / 2; - // TODO remove it, while for backward compatibility + // TODO remove it, while now for backward compatibility fr1.points = fr.points; fr2.points = fr.points; // fr1.min_distance fr1.vectors_to_points = std::vector(fr.vectors_to_points.begin(), fr.vectors_to_points.begin() + fr1_pts_number); - fr1.interpolated_line = {fr.interpolated_line.first, fr.middle}; - fr1.centroid = fr1.vectors_to_points[fr1.vectors_to_points.size() / 2]; - fr1.centroid.x += reference_robot_pose.x; - fr1.centroid.y += reference_robot_pose.y; + fr1.interpolated_line = {fr.interpolated_line.first, fr.centroid}; + fr1.centroid = fr1.vectors_to_points[fr1.vectors_to_points.size() / 2]; // not true centroid, but who cares... + + // moving from relative vector to absolute coordinates + fr1.centroid.x += fr.reference_robot_pose.x; + fr1.centroid.y += fr.reference_robot_pose.y; fr2.vectors_to_points = std::vector( fr.vectors_to_points.begin() + fr1_pts_number, fr.vectors_to_points.end()); - fr2.interpolated_line = {fr.middle, fr.interpolated_line.second}; + fr2.interpolated_line = {fr.centroid, fr.interpolated_line.second}; fr2.centroid = fr2.vectors_to_points[fr2.vectors_to_points.size() / 2]; - fr2.centroid.x += reference_robot_pose.x; - fr2.centroid.y += reference_robot_pose.y; + fr2.centroid.x += fr.reference_robot_pose.x; + fr2.centroid.y += fr.reference_robot_pose.y; + fr1.middle = fr1.centroid; // todo change it to closest if needed + fr2.middle = fr2.centroid; // todo change it to closest if needed + std::vector v1, v2, res; +// ROS_WARN_STREAM(2); + double f1_angular_dist = angular_vector_distance(fr1.interpolated_line.first, fr1.interpolated_line.second); + ROS_WARN_STREAM("FR1 has [" << f1_angular_dist << "] deg [" << fr1.vectors_to_points.size() << "] PTS"); + if (f1_angular_dist > 10.0 + && fr1.vectors_to_points.size() > 6) // to allways have two subdrontiers with middlepoints + { + ROS_WARN_STREAM("PERFORM RECURSIVE SPLITTING"); + v1 = splitFrontier(fr1); + ROS_WARN_STREAM("RECURSIVE SPLITTING PREFORMED OK"); + } else{ + v1 = {fr1}; + } + double f2_angular_dist = angular_vector_distance(fr1.interpolated_line.first, fr1.interpolated_line.second); + ROS_WARN_STREAM("FR1 has [" << f1_angular_dist << "] deg [" << fr1.vectors_to_points.size() << "] PTS"); + if (f2_angular_dist > 10.0 + && fr2.vectors_to_points.size() > 6) // to allways have two subdrontiers with middlepoints + { + ROS_WARN_STREAM("PERFORM F2 RECURSIVE SPLITTING"); + v2 = splitFrontier(fr2); + ROS_WARN_STREAM("RECURSIVE SPLITTING PREFORMED OK"); + } else{ + v2 = {fr2}; + } - return {fr1, fr2}; +// if (angular_vector_distance(fr1.interpolated_line.first, fr1.interpolated_line.second) > 10.0 ) { +// v1 = splitFrontier(fr1, reference_robot_pose); +// } else +// v1 = {fr1}; +// +// if (angular_vector_distance(fr2.interpolated_line.first, fr2.interpolated_line.second) > 10.0 ) { +// v2 = splitFrontier(fr2, reference_robot_pose); +// } else +// v2 = {fr2}; +// + res.insert(res.end(), v1.begin(), v1.end()); + res.insert(res.end(), v2.begin(), v2.end()); +// +// ROS_WARN_STREAM(3); + return res; +// return {fr1, fr2}; } + + std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) { std::vector frontier_list; @@ -197,11 +248,11 @@ std::vector FrontierSearch::buildNewFrontier(unsigned int initial_cell // record initial contact point for frontier unsigned int ix, iy; costmap_->indexToCells(initial_cell, ix, iy); - ROS_ERROR_STREAM(ix << " --- " << iy); +// ROS_ERROR_STREAM(ix << " --- " << iy); costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y); - ROS_ERROR_STREAM(output.initial.x << " --- " << output.initial.y); +// ROS_ERROR_STREAM(output.initial.x << " --- " << output.initial.y); // push initial gridcell onto queue std::queue bfs; @@ -212,13 +263,12 @@ std::vector FrontierSearch::buildNewFrontier(unsigned int initial_cell double reference_x, reference_y; costmap_->indexToCells(reference, rx, ry); costmap_->mapToWorld(rx, ry, reference_x, reference_y); - geometry_msgs::Point reference_robot_pose; - reference_robot_pose.x = reference_x; - reference_robot_pose.y = reference_y; + output.reference_robot_pose.x = reference_x; + output.reference_robot_pose.y = reference_y; // ROS_ERROR_STREAM( reference_robot_pose.x << " " << reference_robot_pose.y); // TODO make a parameter -size_t choose_each = 6; +size_t choose_each = 3; while (!bfs.empty()) { unsigned int idx = bfs.front(); @@ -289,7 +339,7 @@ size_t choose_each = 6; // ROS_WARN_STREAM(output.vectors_to_points.front() << " VEC LENGTH" << output.vectors_to_points.back()); // else // ROS_WARN_STREAM(output.points.size() << " FULL LENGTH "); - output.interpolated_line = approximateFrontierViewAngle(output, reference_robot_pose); + output.interpolated_line = approximateFrontierViewAngle(output); // angular distance for vectors @@ -297,23 +347,23 @@ size_t choose_each = 6; if (!output.vectors_to_points.empty()){ double degrees_distance = std::abs( atan2(output.interpolated_line.first.y, output.interpolated_line.first.x) - atan2(output.interpolated_line.second.y, output.interpolated_line.second.x) ) * 180.0 / M_PI; - getDegreesDistance(output.interpolated_line.first, output.interpolated_line.second, reference_robot_pose); - - if (degrees_distance > 10.0){ - ROS_WARN_STREAM("Detected too wide frontier, " << degrees_distance << "splitting on two"); + getDegreesDistance(output.interpolated_line.first, output.interpolated_line.second, output.reference_robot_pose); + double max_view_angle_for_frontier = 10.0; // todo make the parameter + if (degrees_distance > max_view_angle_for_frontier){ + ROS_WARN_STREAM("Detected too wide frontier [" << degrees_distance <<"]" << "PTS [" <publish(*stop_msg_ptr); - ROS_WARN_STREAM("STOP MSG PUBLISHED to [" << pub_ptr->getTopic() << "]"); + ROS_INFO_STREAM("STOP MSG PUBLISHED to [" << pub_ptr->getTopic() << "]"); global_timer->stop(); } From cd0533fc395d73fc1c5588757cc193d8082f360d Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Thu, 14 Mar 2019 09:34:38 +0000 Subject: [PATCH 09/17] refactoring in progress - optimise angular_measurement function call - cleaned comments --- .../include/explore/frontier_search.h | 6 +- explore_lite_revised/src/frontier_search.cpp | 128 ++++++------------ 2 files changed, 47 insertions(+), 87 deletions(-) diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index 1c5d798..213e85f 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -48,9 +48,9 @@ class FrontierSearch std::vector searchFrom(geometry_msgs::Point position); protected: - std::pair approxFrontierPlanarFarthest(Frontier &fr, - geometry_msgs::Point &reference_robot); - std::pair approximateFrontierViewAngle(Frontier &fr); + std::pair approxFrontierByPlanarFarthest(Frontier &fr, + geometry_msgs::Point &reference_robot); + std::pair approximateFrontierByViewAngle(Frontier &fr); diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index 90b83bd..bfc10cd 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -19,9 +19,20 @@ using costmap_2d::NO_INFORMATION; using costmap_2d::FREE_SPACE; - inline double angular_vector_distance(geometry_msgs::Point v1, geometry_msgs::Point v2){ - return std::abs( atan2(v1.y, v1.x) - atan2(v2.y, v2.x) ) * 180.0 / M_PI; - } +inline double angular_vector_distance(const double x1,const double y1,const double x2,const double y2){ + return std::abs( atan2(y1, x1) - atan2(y2, x2) ) * 180.0 / M_PI; +} +inline double angular_vector_distance(const geometry_msgs::Point &v1,const geometry_msgs::Point &v2) { + return angular_vector_distance(v1.x, v1.y, v2.x, v2.y); +} +// for the case when vectors are presented by points with single point-of-origin +inline double angular_vector_distance(const geometry_msgs::Point &p1,const geometry_msgs::Point &p2,const geometry_msgs::Point &reference_pt){ + return angular_vector_distance(p1.x - reference_pt.x, p1.y - reference_pt.y, p2.x - reference_pt.x, p2.y - reference_pt.y); +} + +inline bool cmp_by_angle(const geometry_msgs::Point &p1,const geometry_msgs::Point &p2) { + return atan2(p1.y, p1.x) < atan2(p2.y, p2.x); +} FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, double potential_scale, double gain_scale, @@ -32,9 +43,8 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, , min_frontier_size_(min_frontier_size) { } - - std::pair FrontierSearch::approxFrontierPlanarFarthest(Frontier &fr, - geometry_msgs::Point &reference_robot){ + std::pair FrontierSearch::approxFrontierByPlanarFarthest(Frontier &fr, + geometry_msgs::Point &reference_robot){ double max_dist = 0.0; double heuristics_dist {0.0}; geometry_msgs::Point p1, p2; @@ -61,7 +71,7 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, } - std::pair FrontierSearch::approximateFrontierViewAngle( + std::pair FrontierSearch::approximateFrontierByViewAngle( frontier_exploration::Frontier &fr) { geometry_msgs::Point p1, p2; if (!fr.vectors_to_points.empty()){ @@ -80,9 +90,8 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, // assert(!fr.vectors_to_points.empty() > 0); size_t fr1_pts_number = fr.vectors_to_points.size() / 2; // TODO remove it, while now for backward compatibility - fr1.points = fr.points; - fr2.points = fr.points; -// fr1.min_distance +// fr1.points = fr.points; +// fr2.points = fr.points; fr1.vectors_to_points = std::vector(fr.vectors_to_points.begin(), fr.vectors_to_points.begin() + fr1_pts_number); fr1.interpolated_line = {fr.interpolated_line.first, fr.centroid}; fr1.centroid = fr1.vectors_to_points[fr1.vectors_to_points.size() / 2]; // not true centroid, but who cares... @@ -99,48 +108,23 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, fr2.centroid.y += fr.reference_robot_pose.y; fr1.middle = fr1.centroid; // todo change it to closest if needed fr2.middle = fr2.centroid; // todo change it to closest if needed - std::vector v1, v2, res; -// ROS_WARN_STREAM(2); - double f1_angular_dist = angular_vector_distance(fr1.interpolated_line.first, fr1.interpolated_line.second); - ROS_WARN_STREAM("FR1 has [" << f1_angular_dist << "] deg [" << fr1.vectors_to_points.size() << "] PTS"); - if (f1_angular_dist > 10.0 - && fr1.vectors_to_points.size() > 6) // to allways have two subdrontiers with middlepoints - { - ROS_WARN_STREAM("PERFORM RECURSIVE SPLITTING"); - v1 = splitFrontier(fr1); - ROS_WARN_STREAM("RECURSIVE SPLITTING PREFORMED OK"); - } else{ - v1 = {fr1}; - } - double f2_angular_dist = angular_vector_distance(fr1.interpolated_line.first, fr1.interpolated_line.second); - ROS_WARN_STREAM("FR1 has [" << f1_angular_dist << "] deg [" << fr1.vectors_to_points.size() << "] PTS"); - if (f2_angular_dist > 10.0 - && fr2.vectors_to_points.size() > 6) // to allways have two subdrontiers with middlepoints - { - ROS_WARN_STREAM("PERFORM F2 RECURSIVE SPLITTING"); - v2 = splitFrontier(fr2); - ROS_WARN_STREAM("RECURSIVE SPLITTING PREFORMED OK"); - } else{ - v2 = {fr2}; + std::vector frontiers{fr1, fr2}, vector_buf, res; + for (auto& fr: frontiers){ + double fr_angular_dist = angular_vector_distance(fr.interpolated_line.first, fr.interpolated_line.second); + ROS_DEBUG_STREAM("FR1 has [" << fr_angular_dist << "] deg [" << fr1.vectors_to_points.size() << "] PTS"); + if (fr_angular_dist > 10.0 + && fr1.vectors_to_points.size() > 6) // to allways have two subdrontiers with middlepoints + { + ROS_DEBUG_STREAM("PERFORM Fr1 RECURSIVE SPLITTING"); + vector_buf = splitFrontier(fr); + ROS_DEBUG_STREAM("RECURSIVE SPLITTING PREFORMED OK"); + } else{ + vector_buf = {fr}; + } + res.insert(res.end(), vector_buf.begin(), vector_buf.end()); } - -// if (angular_vector_distance(fr1.interpolated_line.first, fr1.interpolated_line.second) > 10.0 ) { -// v1 = splitFrontier(fr1, reference_robot_pose); -// } else -// v1 = {fr1}; -// -// if (angular_vector_distance(fr2.interpolated_line.first, fr2.interpolated_line.second) > 10.0 ) { -// v2 = splitFrontier(fr2, reference_robot_pose); -// } else -// v2 = {fr2}; -// - res.insert(res.end(), v1.begin(), v1.end()); - res.insert(res.end(), v2.begin(), v2.end()); -// -// ROS_WARN_STREAM(3); return res; -// return {fr1, fr2}; -} + } std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) @@ -215,23 +199,6 @@ std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) } -double getDegreesDistance(geometry_msgs::Point p1,geometry_msgs::Point p2,geometry_msgs::Point reference){ - // using vector dotproduct to get angle between two reference points v_A dot v_B == |v_A| * |v_B| * cos(angle(A^B)) - double v1_x{p1.x - reference.x}, - v1_y{p1.y - reference.y}, - v2_x{p2.x - reference.x}, - v2_y{p2.y - reference.y}, - len_v1 = std::hypot(v1_x, v1_y), - len_v2 = std::hypot(v2_x, v2_y); - double dot_prod = v1_x * v2_x + v1_y * v2_y; - double angle = std::acos(dot_prod / (len_v1 * len_v2)) * 180.0 / M_PI; - return angle; -} - - -bool cmp_by_angle(geometry_msgs::Point p1, geometry_msgs::Point p2){ - return atan2(p1.y, p1.x) < atan2(p2.y, p2.x); -} std::vector FrontierSearch::buildNewFrontier(unsigned int initial_cell, @@ -252,7 +219,6 @@ std::vector FrontierSearch::buildNewFrontier(unsigned int initial_cell costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y); -// ROS_ERROR_STREAM(output.initial.x << " --- " << output.initial.y); // push initial gridcell onto queue std::queue bfs; @@ -265,16 +231,12 @@ std::vector FrontierSearch::buildNewFrontier(unsigned int initial_cell costmap_->mapToWorld(rx, ry, reference_x, reference_y); output.reference_robot_pose.x = reference_x; output.reference_robot_pose.y = reference_y; - -// ROS_ERROR_STREAM( reference_robot_pose.x << " " << reference_robot_pose.y); -// TODO make a parameter -size_t choose_each = 3; +// TODO place it on a parameter server +size_t choose_each = 1; while (!bfs.empty()) { unsigned int idx = bfs.front(); bfs.pop(); - double world_scale_X = costmap_->getSizeInMetersX(); - double world_scale_Y = costmap_->getSizeInMetersY(); // try adding cells in 8-connected neighborhood to frontier for (unsigned int nbr : nhood8(idx, *costmap_)) { // check if neighbour is a potential frontier cell @@ -294,7 +256,6 @@ size_t choose_each = 3; output.vectors_to_points.push_back(reference_scope_point); } -// costmap_. geometry_msgs::Point point; point.x = wx; @@ -339,25 +300,24 @@ size_t choose_each = 3; // ROS_WARN_STREAM(output.vectors_to_points.front() << " VEC LENGTH" << output.vectors_to_points.back()); // else // ROS_WARN_STREAM(output.points.size() << " FULL LENGTH "); - output.interpolated_line = approximateFrontierViewAngle(output); + output.interpolated_line = approximateFrontierByViewAngle(output); // angular distance for vectors // TODO move to approppriate frontier if (!output.vectors_to_points.empty()){ - double degrees_distance = std::abs( atan2(output.interpolated_line.first.y, output.interpolated_line.first.x) - atan2(output.interpolated_line.second.y, output.interpolated_line.second.x) ) - * 180.0 / M_PI; - getDegreesDistance(output.interpolated_line.first, output.interpolated_line.second, output.reference_robot_pose); + + double degrees_distance = angular_vector_distance(output.interpolated_line.first, output.interpolated_line.second, output.reference_robot_pose); double max_view_angle_for_frontier = 10.0; // todo make the parameter if (degrees_distance > max_view_angle_for_frontier){ ROS_WARN_STREAM("Detected too wide frontier [" << degrees_distance <<"]" << "PTS [" < Date: Thu, 14 Mar 2019 10:02:25 +0000 Subject: [PATCH 10/17] refactored frontier structure and structure init --- explore_lite_revised/include/explore/frontier_search.h | 7 ++++--- explore_lite_revised/src/frontier_search.cpp | 10 +++------- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index 213e85f..6a065b1 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -10,9 +10,9 @@ namespace frontier_exploration * */ struct Frontier { - std::uint32_t size; - double min_distance; - double cost; + std::uint32_t size{0}; + double min_distance{std::numeric_limits::infinity()}; + double cost{0.0}; geometry_msgs::Point initial; geometry_msgs::Point centroid; geometry_msgs::Point middle; @@ -20,6 +20,7 @@ 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 }; /** diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index bfc10cd..f2c1325 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -85,9 +85,7 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, } std::vector splitFrontier(Frontier& fr){ -// ROS_WARN_STREAM(0); Frontier fr1(fr), fr2(fr); -// assert(!fr.vectors_to_points.empty() > 0); size_t fr1_pts_number = fr.vectors_to_points.size() / 2; // TODO remove it, while now for backward compatibility // fr1.points = fr.points; @@ -194,6 +192,7 @@ std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) std::sort( frontier_list.begin(), frontier_list.end(), [](const Frontier& f1, const Frontier& f2) { return f1.cost < f2.cost; }); + // TODO do we need sort if only min is interesting? maybe once we have cache behaviour prtial sort will be ok... return frontier_list; } @@ -209,17 +208,14 @@ std::vector FrontierSearch::buildNewFrontier(unsigned int initial_cell Frontier output; output.centroid.x = 0; output.centroid.y = 0; - output.size = 1; - output.min_distance = std::numeric_limits::infinity(); +// output.size = 1; +// output.min_distance = std::numeric_limits::infinity(); // record initial contact point for frontier unsigned int ix, iy; costmap_->indexToCells(initial_cell, ix, iy); -// ROS_ERROR_STREAM(ix << " --- " << iy); - costmap_->mapToWorld(ix, iy, output.initial.x, output.initial.y); - // push initial gridcell onto queue std::queue bfs; bfs.push(initial_cell); From ab790b58c980184cbfb175902045807681fd4cc6 Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Thu, 14 Mar 2019 11:27:55 +0000 Subject: [PATCH 11/17] return bug removed, code minor refactored --- .../include/explore/frontier_search.h | 2 +- explore_lite_revised/src/frontier_search.cpp | 45 +++++-------------- 2 files changed, 12 insertions(+), 35 deletions(-) diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index 6a065b1..6dfe51d 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -15,7 +15,7 @@ struct Frontier { double cost{0.0}; geometry_msgs::Point initial; geometry_msgs::Point centroid; - geometry_msgs::Point middle; + geometry_msgs::Point closest_point; std::pair interpolated_line; std::vector points; std::vector vectors_to_points; diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index f2c1325..c0f7aef 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -104,13 +104,13 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, fr2.centroid = fr2.vectors_to_points[fr2.vectors_to_points.size() / 2]; fr2.centroid.x += fr.reference_robot_pose.x; fr2.centroid.y += fr.reference_robot_pose.y; - fr1.middle = fr1.centroid; // todo change it to closest if needed - fr2.middle = fr2.centroid; // todo change it to closest if needed + fr1.closest_point = fr1.centroid; // todo change it to closest if needed + fr2.closest_point = fr2.centroid; // todo change it to closest if needed std::vector frontiers{fr1, fr2}, vector_buf, res; for (auto& fr: frontiers){ double fr_angular_dist = angular_vector_distance(fr.interpolated_line.first, fr.interpolated_line.second); ROS_DEBUG_STREAM("FR1 has [" << fr_angular_dist << "] deg [" << fr1.vectors_to_points.size() << "] PTS"); - if (fr_angular_dist > 10.0 + if (fr_angular_dist > fr.max_frontier_angular_size && fr1.vectors_to_points.size() > 6) // to allways have two subdrontiers with middlepoints { ROS_DEBUG_STREAM("PERFORM Fr1 RECURSIVE SPLITTING"); @@ -208,8 +208,6 @@ std::vector FrontierSearch::buildNewFrontier(unsigned int initial_cell Frontier output; output.centroid.x = 0; output.centroid.y = 0; -// output.size = 1; -// output.min_distance = std::numeric_limits::infinity(); // record initial contact point for frontier unsigned int ix, iy; @@ -227,6 +225,7 @@ std::vector FrontierSearch::buildNewFrontier(unsigned int initial_cell costmap_->mapToWorld(rx, ry, reference_x, reference_y); 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; @@ -270,8 +269,8 @@ size_t choose_each = 1; double distance = std::hypot(reference_x - wx, reference_y - wy); if (distance < output.min_distance) { output.min_distance = distance; - output.middle.x = wx; - output.middle.y = wy; + output.closest_point.x = wx; + output.closest_point.y = wy; } // add to queue for breadth first search @@ -280,7 +279,7 @@ size_t choose_each = 1; } } -// output.min_distance = std::hypot(reference_x - output.middle.x, reference_y - output.middle.y); +// 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.size; @@ -291,39 +290,17 @@ size_t choose_each = 1; std::sort(output.vectors_to_points.begin(), output.vectors_to_points.end(), cmp_by_angle); // fixme find out why there are occuring empty frontiers (empty raw points) -// ROS_WARN_STREAM(output.vectors_to_points.size() << " VEC LENGTH"); -// if (output.vectors_to_points.size() > 0) -// ROS_WARN_STREAM(output.vectors_to_points.front() << " VEC LENGTH" << output.vectors_to_points.back()); -// else -// ROS_WARN_STREAM(output.points.size() << " FULL LENGTH "); output.interpolated_line = approximateFrontierByViewAngle(output); - - -// angular distance for vectors -// TODO move to approppriate frontier + std::vector splitted_frontiers{output}; if (!output.vectors_to_points.empty()){ double degrees_distance = angular_vector_distance(output.interpolated_line.first, output.interpolated_line.second, output.reference_robot_pose); - double max_view_angle_for_frontier = 10.0; // todo make the parameter - if (degrees_distance > max_view_angle_for_frontier){ + if (degrees_distance > output.max_frontier_angular_size){ ROS_WARN_STREAM("Detected too wide frontier [" << degrees_distance <<"]" << "PTS [" < - - - - - - - - - - - - + \ No newline at end of file diff --git a/explore_lite_revised/launch/explore_costmap.launch b/explore_lite_revised/launch/explore_costmap.launch index 3ff1fb0..2b0acae 100644 --- a/explore_lite_revised/launch/explore_costmap.launch +++ b/explore_lite_revised/launch/explore_costmap.launch @@ -1,15 +1,5 @@ - - - - - - - - - - - + diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index c0f7aef..efb0a88 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -48,20 +48,20 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, double max_dist = 0.0; double heuristics_dist {0.0}; geometry_msgs::Point p1, p2; - for (auto &point: fr.points){ + for (auto &point: fr.vectors_to_points){ heuristics_dist = - std::hypot(point.x - reference_robot.x , point.y - reference_robot.y) + /* robot_point_dist */ - std::hypot(point.x - fr.centroid.x , point.y - fr.centroid.y); /* centroid_point dist */ + std::hypot(point.x , point.y ) + /* robot_point_dist */ + std::hypot(point.x , point.y ); /* centroid_point dist */ if (heuristics_dist > max_dist){ max_dist = heuristics_dist; p1 = point; } } max_dist = 0.0; - for (auto &point: fr.points){ + for (auto &point: fr.vectors_to_points){ heuristics_dist = std::hypot(point.x - p1.x , point.y - p1.y) + /* point-point dist */ - std::hypot(point.x - fr.centroid.x , point.y - fr.centroid.y); /* centroid_point dist */ + std::hypot( (/*transforming to map frame*/point.x + reference_robot.x) - fr.centroid.x , (point.y + reference_robot.y) - fr.centroid.y); /* centroid_point dist */ if (heuristics_dist > max_dist){ max_dist = heuristics_dist; p2 = point; @@ -87,9 +87,6 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, std::vector splitFrontier(Frontier& fr){ Frontier fr1(fr), fr2(fr); size_t fr1_pts_number = fr.vectors_to_points.size() / 2; - // TODO remove it, while now for backward compatibility -// fr1.points = fr.points; -// fr2.points = fr.points; fr1.vectors_to_points = std::vector(fr.vectors_to_points.begin(), fr.vectors_to_points.begin() + fr1_pts_number); fr1.interpolated_line = {fr.interpolated_line.first, fr.centroid}; fr1.centroid = fr1.vectors_to_points[fr1.vectors_to_points.size() / 2]; // not true centroid, but who cares... @@ -109,11 +106,11 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, std::vector frontiers{fr1, fr2}, vector_buf, res; for (auto& fr: frontiers){ double fr_angular_dist = angular_vector_distance(fr.interpolated_line.first, fr.interpolated_line.second); - ROS_DEBUG_STREAM("FR1 has [" << fr_angular_dist << "] deg [" << fr1.vectors_to_points.size() << "] PTS"); + ROS_DEBUG_STREAM("FR has [" << fr_angular_dist << "] deg [" << fr1.vectors_to_points.size() << "] PTS"); if (fr_angular_dist > fr.max_frontier_angular_size && fr1.vectors_to_points.size() > 6) // to allways have two subdrontiers with middlepoints { - ROS_DEBUG_STREAM("PERFORM Fr1 RECURSIVE SPLITTING"); + ROS_DEBUG_STREAM("PERFORM FRONTIER RECURSIVE SPLITTING"); vector_buf = splitFrontier(fr); ROS_DEBUG_STREAM("RECURSIVE SPLITTING PREFORMED OK"); } else{ @@ -126,7 +123,7 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, std::vector FrontierSearch::searchFrom(geometry_msgs::Point position) -{ + { std::vector frontier_list; // Sanity check that robot is inside costmap bounds before searching @@ -177,7 +174,9 @@ 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){ - if (new_frontier.size * costmap_->getResolution() >= min_frontier_size_) { + /*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); } } @@ -227,7 +226,7 @@ std::vector FrontierSearch::buildNewFrontier(unsigned int initial_cell output.reference_robot_pose.y = reference_y; // TODO place it on a parameter server -size_t choose_each = 1; +size_t choose_each{1}, cntr{0}; while (!bfs.empty()) { unsigned int idx = bfs.front(); @@ -243,26 +242,28 @@ size_t choose_each = 1; costmap_->indexToCells(nbr, mx, my); costmap_->mapToWorld(mx, my, wx, wy); - // selecting sparce points - if (output.points.size() % choose_each == 0){ + // leaving only each n-th point + if (cntr++ % choose_each == 0){ geometry_msgs::Point reference_scope_point; reference_scope_point.x = wx - reference_x; reference_scope_point.y = wy - reference_y; output.vectors_to_points.push_back(reference_scope_point); + output.centroid.x += wx; // map frame coords + output.centroid.y += wy; } - geometry_msgs::Point point; - point.x = wx; - point.y = wy; - output.points.push_back(point); +// geometry_msgs::Point point; +// point.x = wx; +// point.y = wy; +// output.points.push_back(point); // update frontier size - output.size++; // FIXME R U SRIOS!? +// output.size++; // FIXME R U SRIOS!? // update centroid of frontier - output.centroid.x += wx; - output.centroid.y += wy; +// output.centroid.x += wx; +// output.centroid.y += wy; // determine frontier's distance from robot, going by closest gridcell // to robot @@ -282,8 +283,9 @@ size_t choose_each = 1; // 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.size; - output.centroid.y /= output.size; + + output.centroid.x /= output.vectors_to_points.size(); + output.centroid.y /= output.vectors_to_points.size(); /*KD*/ // todo doit with lambda diff --git a/explore_stack/param/explore_lite_revised.yaml b/explore_stack/param/explore_lite_revised.yaml index 34f83cc..599d6fd 100644 --- a/explore_stack/param/explore_lite_revised.yaml +++ b/explore_stack/param/explore_lite_revised.yaml @@ -1,5 +1,5 @@ robot_base_frame: base_link -min_frontier_size: 0.75 +min_frontier_size: 0.4 costmap_topic: map costmap_updates_topic: map_updates visualize: true From a0b59d440ce1a1bcd82a005bda6cc07e9221e829 Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Thu, 14 Mar 2019 13:06:02 +0000 Subject: [PATCH 13/17] removed 'closest_point' usage, as expensive and not usable since we have sorted vectors --- .../include/explore/frontier_search.h | 2 +- explore_lite_revised/src/explore.cpp | 3 +- explore_lite_revised/src/frontier_search.cpp | 54 +++++++++++-------- 3 files changed, 35 insertions(+), 24 deletions(-) diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index b791a13..f9b2b69 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -15,7 +15,7 @@ struct Frontier { double cost{0.0}; geometry_msgs::Point initial; geometry_msgs::Point centroid; - geometry_msgs::Point closest_point; +// geometry_msgs::Point closest_point; // for now we ommit it as recomputing is qwestionable std::pair interpolated_line; // std::vector points; std::vector vectors_to_points; diff --git a/explore_lite_revised/src/explore.cpp b/explore_lite_revised/src/explore.cpp index 714b9b2..029f47a 100644 --- a/explore_lite_revised/src/explore.cpp +++ b/explore_lite_revised/src/explore.cpp @@ -169,6 +169,7 @@ void Explore::visualizeFrontiers( m.color = *blue; m.header.stamp = ros::Time::now(); markers.push_back(m); + m.type = visualization_msgs::Marker::TEXT_VIEW_FACING; ++m.id; m.text = "M"; @@ -180,6 +181,7 @@ void Explore::visualizeFrontiers( m.color = *blue; m.header.stamp = ros::Time::now(); markers.push_back(m); + m.type = visualization_msgs::Marker::TEXT_VIEW_FACING; ++m.id; m.text = "L"; @@ -192,7 +194,6 @@ void Explore::visualizeFrontiers( m.header.stamp = ros::Time::now(); markers.push_back(m); - m.type = visualization_msgs::Marker::POINTS; ++m.id; m.pose.position = {}; diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index efb0a88..972655b 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -30,8 +30,12 @@ inline double angular_vector_distance(const geometry_msgs::Point &p1,const geom return angular_vector_distance(p1.x - reference_pt.x, p1.y - reference_pt.y, p2.x - reference_pt.x, p2.y - reference_pt.y); } -inline bool cmp_by_angle(const geometry_msgs::Point &p1,const geometry_msgs::Point &p2) { - return atan2(p1.y, p1.x) < atan2(p2.y, p2.x); +// for now is not used +inline geometry_msgs::Point getClosestPointTo(const std::vector &distance_vectors, const geometry_msgs::Point &goal){ + auto min_elem_iter = std::min_element(distance_vectors.begin(), distance_vectors.end(), + [](const geometry_msgs::Point &p1,const geometry_msgs::Point &p2) + {return p1.x + p1.y < p2.x + p2.y;}); //using Manhatten distance + return *min_elem_iter; } FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, @@ -85,26 +89,30 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, } std::vector splitFrontier(Frontier& fr){ - Frontier fr1(fr), fr2(fr); - size_t fr1_pts_number = fr.vectors_to_points.size() / 2; - fr1.vectors_to_points = std::vector(fr.vectors_to_points.begin(), fr.vectors_to_points.begin() + fr1_pts_number); - fr1.interpolated_line = {fr.interpolated_line.first, fr.centroid}; - fr1.centroid = fr1.vectors_to_points[fr1.vectors_to_points.size() / 2]; // not true centroid, but who cares... + Frontier fr1, fr2; - // moving from relative vector to absolute coordinates - fr1.centroid.x += fr.reference_robot_pose.x; - fr1.centroid.y += fr.reference_robot_pose.y; + fr1.reference_robot_pose = fr.reference_robot_pose; + fr2.reference_robot_pose = fr.reference_robot_pose; + size_t fr1_pts_number = fr.vectors_to_points.size() / 2; + fr1.vectors_to_points = std::vector(fr.vectors_to_points.begin(), fr.vectors_to_points.begin() + fr1_pts_number); + fr1.interpolated_line = {fr.interpolated_line.first, fr.centroid}; + fr1.centroid = fr1.vectors_to_points[fr1.vectors_to_points.size() / 2]; // not true centroid, but who cares... //todo maybe + + // moving from relative vector to absolute coordinates + fr1.centroid.x += fr.reference_robot_pose.x; + fr1.centroid.y += fr.reference_robot_pose.y; + + fr2.vectors_to_points = std::vector( fr.vectors_to_points.begin() + fr1_pts_number, fr.vectors_to_points.end()); + fr2.interpolated_line = {fr.centroid, fr.interpolated_line.second}; + fr2.centroid = fr2.vectors_to_points[fr2.vectors_to_points.size() / 2]; - fr2.vectors_to_points = std::vector( fr.vectors_to_points.begin() + fr1_pts_number, fr.vectors_to_points.end()); - fr2.interpolated_line = {fr.centroid, fr.interpolated_line.second}; - fr2.centroid = fr2.vectors_to_points[fr2.vectors_to_points.size() / 2]; fr2.centroid.x += fr.reference_robot_pose.x; fr2.centroid.y += fr.reference_robot_pose.y; - fr1.closest_point = fr1.centroid; // todo change it to closest if needed - fr2.closest_point = fr2.centroid; // todo change it to closest if needed + std::vector frontiers{fr1, fr2}, vector_buf, res; for (auto& fr: frontiers){ + double fr_angular_dist = angular_vector_distance(fr.interpolated_line.first, fr.interpolated_line.second); ROS_DEBUG_STREAM("FR has [" << fr_angular_dist << "] deg [" << fr1.vectors_to_points.size() << "] PTS"); if (fr_angular_dist > fr.max_frontier_angular_size @@ -268,11 +276,11 @@ size_t choose_each{1}, cntr{0}; // 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; - } +// 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); @@ -288,8 +296,10 @@ size_t choose_each{1}, cntr{0}; output.centroid.y /= output.vectors_to_points.size(); /*KD*/ -// todo doit with lambda - std::sort(output.vectors_to_points.begin(), output.vectors_to_points.end(), cmp_by_angle); + std::sort(output.vectors_to_points.begin(), output.vectors_to_points.end(), + [](const geometry_msgs::Point &p1,const geometry_msgs::Point &p2) + {return atan2(p1.y, p1.x) < atan2(p2.y, p2.x);} + ); // fixme find out why there are occuring empty frontiers (empty raw points) output.interpolated_line = approximateFrontierByViewAngle(output); From 502e7d8bc8e23d51ba1a96c231c8d69897981b2f Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Thu, 14 Mar 2019 14:53:15 +0000 Subject: [PATCH 14/17] refactored frontier splittig, added functons to Frontier struct for translating from coord frame and to it --- .../include/explore/frontier_search.h | 8 +++ explore_lite_revised/src/frontier_search.cpp | 50 +++++++++---------- 2 files changed, 33 insertions(+), 25 deletions(-) diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index f9b2b69..1f9679e 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -21,6 +21,14 @@ struct Frontier { 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 + void toReferenceFrame(geometry_msgs::Point &pt){ + pt.x -= reference_robot_pose.x; + pt.y -= reference_robot_pose.y; + } + void fromReferenceFrame(geometry_msgs::Point &pt){ + pt.x += reference_robot_pose.x; + pt.y += reference_robot_pose.y; + } }; /** diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index 972655b..d9cea1f 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -91,38 +91,38 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, std::vector splitFrontier(Frontier& fr){ Frontier fr1, fr2; - fr1.reference_robot_pose = fr.reference_robot_pose; - fr2.reference_robot_pose = fr.reference_robot_pose; - size_t fr1_pts_number = fr.vectors_to_points.size() / 2; - - fr1.vectors_to_points = std::vector(fr.vectors_to_points.begin(), fr.vectors_to_points.begin() + fr1_pts_number); - fr1.interpolated_line = {fr.interpolated_line.first, fr.centroid}; - fr1.centroid = fr1.vectors_to_points[fr1.vectors_to_points.size() / 2]; // not true centroid, but who cares... //todo maybe - - // moving from relative vector to absolute coordinates - fr1.centroid.x += fr.reference_robot_pose.x; - fr1.centroid.y += fr.reference_robot_pose.y; - - fr2.vectors_to_points = std::vector( fr.vectors_to_points.begin() + fr1_pts_number, fr.vectors_to_points.end()); - fr2.interpolated_line = {fr.centroid, fr.interpolated_line.second}; - fr2.centroid = fr2.vectors_to_points[fr2.vectors_to_points.size() / 2]; - - fr2.centroid.x += fr.reference_robot_pose.x; - fr2.centroid.y += fr.reference_robot_pose.y; - std::vector frontiers{fr1, fr2}, vector_buf, res; - for (auto& fr: frontiers){ + size_t splits = frontiers.size(); + size_t current_element {0}, split_size {fr.vectors_to_points.size() / splits}; + for (auto& this_fr: frontiers){ + this_fr.reference_robot_pose = fr.reference_robot_pose; + this_fr.vectors_to_points = std::vector(fr.vectors_to_points.begin() + current_element, fr.vectors_to_points.begin() + current_element + split_size); + current_element += split_size; + this_fr.interpolated_line = {this_fr.vectors_to_points.front(), this_fr.vectors_to_points.back()}; + this_fr.centroid = *(this_fr.vectors_to_points.begin() + split_size / 2); // not true centroid, but who cares... //todo maybe +// for (geometry_msgs::Point pt: std::vector{this_fr.centroid, this_fr.interpolated_line.first, this_fr.interpolated_line.second} ){ +// this_fr.fromReferenceFrame(pt); +// } // todo find the way to iterate over it + this_fr.fromReferenceFrame(this_fr.centroid); + this_fr.fromReferenceFrame(this_fr.interpolated_line.first); + this_fr.fromReferenceFrame(this_fr.interpolated_line.second); + + if (fr.vectors_to_points.end() - fr.vectors_to_points.begin() - current_element < split_size) { + this_fr.vectors_to_points.insert(this_fr.vectors_to_points.end(), + fr.vectors_to_points.begin() + current_element, + fr.vectors_to_points.end()); // uppend last chunk in case of division leftover + } - double fr_angular_dist = angular_vector_distance(fr.interpolated_line.first, fr.interpolated_line.second); - ROS_DEBUG_STREAM("FR has [" << fr_angular_dist << "] deg [" << fr1.vectors_to_points.size() << "] PTS"); - if (fr_angular_dist > fr.max_frontier_angular_size + 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 && fr1.vectors_to_points.size() > 6) // to allways have two subdrontiers with middlepoints { ROS_DEBUG_STREAM("PERFORM FRONTIER RECURSIVE SPLITTING"); - vector_buf = splitFrontier(fr); + vector_buf = splitFrontier(this_fr); ROS_DEBUG_STREAM("RECURSIVE SPLITTING PREFORMED OK"); } else{ - vector_buf = {fr}; + vector_buf = {this_fr}; } res.insert(res.end(), vector_buf.begin(), vector_buf.end()); } From de358c1fb19c9a1646dc1c1679bc277998bfef5e Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Fri, 15 Mar 2019 08:10:47 +0000 Subject: [PATCH 15/17] refactored method for frontier points translation from and to robot's reference frame --- .../include/explore/frontier_search.h | 10 ++------ explore_lite_revised/src/frontier_search.cpp | 25 +++++++++++++------ 2 files changed, 20 insertions(+), 15 deletions(-) diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index 1f9679e..53ecb3e 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -21,14 +21,8 @@ struct Frontier { 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 - void toReferenceFrame(geometry_msgs::Point &pt){ - pt.x -= reference_robot_pose.x; - pt.y -= reference_robot_pose.y; - } - void fromReferenceFrame(geometry_msgs::Point &pt){ - pt.x += reference_robot_pose.x; - pt.y += reference_robot_pose.y; - } + geometry_msgs::Point toReferenceFrame(const geometry_msgs::Point &pt); + geometry_msgs::Point fromReferenceFrame(const geometry_msgs::Point &pt_in_reference_frame); }; /** diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index d9cea1f..d5fdbf9 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -38,6 +38,19 @@ inline geometry_msgs::Point getClosestPointTo(const std::vector{this_fr.centroid, this_fr.interpolated_line.first, this_fr.interpolated_line.second} ){ // this_fr.fromReferenceFrame(pt); // } // todo find the way to iterate over it - this_fr.fromReferenceFrame(this_fr.centroid); - this_fr.fromReferenceFrame(this_fr.interpolated_line.first); - this_fr.fromReferenceFrame(this_fr.interpolated_line.second); + this_fr.centroid = this_fr.fromReferenceFrame(this_fr.centroid); + this_fr.interpolated_line.first = this_fr.fromReferenceFrame(this_fr.interpolated_line.first); + this_fr.interpolated_line.second = this_fr.fromReferenceFrame(this_fr.interpolated_line.second); if (fr.vectors_to_points.end() - fr.vectors_to_points.begin() - current_element < split_size) { this_fr.vectors_to_points.insert(this_fr.vectors_to_points.end(), From 8fab7ce88a64e8e8fef28c732a5fd71c9a957d9e Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Fri, 15 Mar 2019 08:16:20 +0000 Subject: [PATCH 16/17] cosmetics --- .../include/explore/frontier_search.h | 2 ++ explore_lite_revised/src/frontier_search.cpp | 27 +++++++++---------- 2 files changed, 14 insertions(+), 15 deletions(-) diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index 53ecb3e..2650f7e 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -9,7 +9,9 @@ namespace frontier_exploration * @brief Represents a frontier * */ + struct Frontier { + //todo make a class of it, make methods static methods of class // std::uint32_t size{0}; double min_distance{std::numeric_limits::infinity()}; double cost{0.0}; diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index d5fdbf9..6aee925 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -38,18 +38,18 @@ inline geometry_msgs::Point getClosestPointTo(const std::vector{this_fr.centroid, this_fr.interpolated_line.first, this_fr.interpolated_line.second} ){ -// this_fr.fromReferenceFrame(pt); -// } // todo find the way to iterate over it + this_fr.centroid = this_fr.fromReferenceFrame(this_fr.centroid); this_fr.interpolated_line.first = this_fr.fromReferenceFrame(this_fr.interpolated_line.first); this_fr.interpolated_line.second = this_fr.fromReferenceFrame(this_fr.interpolated_line.second); @@ -131,7 +129,6 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, { ROS_DEBUG_STREAM("PERFORM FRONTIER RECURSIVE SPLITTING"); vector_buf = splitFrontier(this_fr); - ROS_DEBUG_STREAM("RECURSIVE SPLITTING PREFORMED OK"); } else{ vector_buf = {this_fr}; } From 25e9db355506004da40f4c34029a384327488549 Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Mon, 18 Mar 2019 15:45:16 +0000 Subject: [PATCH 17/17] refactoring parameters moved to the parameter server prepeared for merging --- .../include/explore/frontier_search.h | 8 +-- explore_lite_revised/src/explore.cpp | 53 +++---------------- explore_lite_revised/src/frontier_search.cpp | 47 ++++------------ explore_stack/param/explore_lite_revised.yaml | 2 + 4 files changed, 25 insertions(+), 85 deletions(-) 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 [" <