From 952232232b3d13fef9d430b9e82c87734bb94ef9 Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Tue, 19 Mar 2019 08:11:57 +0000 Subject: [PATCH 1/2] point_msg builder added --- explore_lite_revised/CMakeLists.txt | 3 +- .../include/explore/explore.h | 2 + .../include/explore/explore_utils.h | 18 +++++++++ .../include/explore/frontier_search.h | 4 +- explore_lite_revised/src/frontier_search.cpp | 38 +++++++++++-------- 5 files changed, 46 insertions(+), 19 deletions(-) create mode 100644 explore_lite_revised/include/explore/explore_utils.h diff --git a/explore_lite_revised/CMakeLists.txt b/explore_lite_revised/CMakeLists.txt index c40eae5..55d6503 100644 --- a/explore_lite_revised/CMakeLists.txt +++ b/explore_lite_revised/CMakeLists.txt @@ -51,8 +51,7 @@ include_directories( add_executable(explore_lite_revised src/costmap_client.cpp src/explore.cpp - src/frontier_search.cpp -) + src/frontier_search.cpp) add_dependencies(explore_lite_revised ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(explore_lite_revised ${catkin_LIBRARIES}) diff --git a/explore_lite_revised/include/explore/explore.h b/explore_lite_revised/include/explore/explore.h index d2aaa50..6166465 100644 --- a/explore_lite_revised/include/explore/explore.h +++ b/explore_lite_revised/include/explore/explore.h @@ -53,6 +53,8 @@ namespace explore { + + /** * @class Explore * @brief A class adhering to the robot_actions::Action interface that moves the diff --git a/explore_lite_revised/include/explore/explore_utils.h b/explore_lite_revised/include/explore/explore_utils.h new file mode 100644 index 0000000..89b96da --- /dev/null +++ b/explore_lite_revised/include/explore/explore_utils.h @@ -0,0 +1,18 @@ +// +// Created by user on 3/19/19. +// + +#ifndef PROJECT_EXPLORE_UTILS_H +#define PROJECT_EXPLORE_UTILS_H + +#include + +namespace frontier_exploration{ + static geometry_msgs::Point pointMsgBuilder(const double x, const double y, const double z){ + geometry_msgs::Point p; + p.x = x; p.y = y; p.z = z; + return p; + } +} + +#endif //PROJECT_EXPLORE_UTILS_H diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index 2cb7586..1e4978e 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -5,7 +5,8 @@ namespace frontier_exploration { -/** + + /** * @brief Represents a frontier * */ @@ -25,6 +26,7 @@ struct Frontier { geometry_msgs::Point toReferenceFrame(const geometry_msgs::Point &pt); geometry_msgs::Point fromReferenceFrame(const geometry_msgs::Point &pt_in_reference_frame); bool hidden{false}; + Frontier() = default; }; /** diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index f091de3..e147500 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -1,4 +1,3 @@ -#include #include @@ -6,12 +5,15 @@ #include #include -#include - #include #include #include +#include +#include +#include + + namespace frontier_exploration { using costmap_2d::LETHAL_OBSTACLE; @@ -34,21 +36,20 @@ inline double angular_vector_distance(const geometry_msgs::Point &p1,const geom 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 p1.x + p1.y < p2.x + p2.y;}); //using Manhatten distance return *min_elem_iter; } -geometry_msgs::Point Frontier::toReferenceFrame(const geometry_msgs::Point &pt_in_absolute_frame){ - geometry_msgs::Point pt_in_reference_frame; - pt_in_reference_frame.x = pt_in_absolute_frame.x - reference_robot_pose.x; - pt_in_reference_frame.y = pt_in_absolute_frame.y - reference_robot_pose.y; - return pt_in_reference_frame; +inline geometry_msgs::Point Frontier::toReferenceFrame(const geometry_msgs::Point &pt_in_absolute_frame){ + return pointMsgBuilder( + pt_in_absolute_frame.x - reference_robot_pose.x, + pt_in_absolute_frame.y - reference_robot_pose.y, + 0); } -geometry_msgs::Point Frontier::fromReferenceFrame(const geometry_msgs::Point &pt_in_reference_frame){ - geometry_msgs::Point pt_in_absolute_frame; - pt_in_absolute_frame.x = pt_in_reference_frame.x + reference_robot_pose.x; - pt_in_absolute_frame.y = pt_in_reference_frame.y + reference_robot_pose.y; - return pt_in_absolute_frame; +inline geometry_msgs::Point Frontier::fromReferenceFrame(const geometry_msgs::Point &pt_in_reference_frame){ + return pointMsgBuilder(pt_in_reference_frame.x + reference_robot_pose.x, + pt_in_reference_frame.y + reference_robot_pose.y, + 0); } FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, @@ -105,13 +106,15 @@ FrontierSearch::FrontierSearch(costmap_2d::Costmap2D* costmap, std::vector FrontierSearch::splitFrontier(Frontier& fr){ Frontier fr1, fr2; - std::vector frontiers{fr1, fr2}, vector_buf, res; 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); + 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 @@ -326,6 +329,9 @@ bool FrontierSearch::is_hidden(frontier_exploration::Frontier &fr, double thresh double FrontierSearch::frontierCost(const Frontier& frontier) { + // TODO add rotation angle heuristics + // TODO geodesial distance heuristics + // todo add wall reachability for successfull mapping return (potential_scale_ * frontier.min_distance // * costmap_->getResolution() ) From 834a3fa93561b49418eebccb78f8cf56a40ccf16 Mon Sep 17 00:00:00 2001 From: DmytroKushnir Date: Thu, 21 Mar 2019 10:58:13 +0000 Subject: [PATCH 2/2] makePointMsg provided --- explore_lite_revised/include/explore/explore_utils.h | 2 +- explore_lite_revised/include/explore/frontier_search.h | 3 +-- explore_lite_revised/src/frontier_search.cpp | 8 ++++---- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/explore_lite_revised/include/explore/explore_utils.h b/explore_lite_revised/include/explore/explore_utils.h index 89b96da..fd8c6fb 100644 --- a/explore_lite_revised/include/explore/explore_utils.h +++ b/explore_lite_revised/include/explore/explore_utils.h @@ -8,7 +8,7 @@ #include namespace frontier_exploration{ - static geometry_msgs::Point pointMsgBuilder(const double x, const double y, const double z){ + static geometry_msgs::Point makePointMsg(const double x, const double y, const double z){ geometry_msgs::Point p; p.x = x; p.y = y; p.z = z; return p; diff --git a/explore_lite_revised/include/explore/frontier_search.h b/explore_lite_revised/include/explore/frontier_search.h index 1e4978e..3bc3219 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -12,8 +12,7 @@ namespace frontier_exploration */ struct Frontier { - //todo make a class of it, make methods static methods of class -// std::uint32_t size{0}; + //todo improve this struct functionality double min_distance{std::numeric_limits::infinity()}; double cost{0.0}; geometry_msgs::Point initial; diff --git a/explore_lite_revised/src/frontier_search.cpp b/explore_lite_revised/src/frontier_search.cpp index e147500..5d4b858 100644 --- a/explore_lite_revised/src/frontier_search.cpp +++ b/explore_lite_revised/src/frontier_search.cpp @@ -41,15 +41,15 @@ inline geometry_msgs::Point getClosestPointTo(const std::vector