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..fd8c6fb --- /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 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; + } +} + +#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..3bc3219 100644 --- a/explore_lite_revised/include/explore/frontier_search.h +++ b/explore_lite_revised/include/explore/frontier_search.h @@ -5,14 +5,14 @@ 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}; + //todo improve this struct functionality double min_distance{std::numeric_limits::infinity()}; double cost{0.0}; geometry_msgs::Point initial; @@ -25,6 +25,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..5d4b858 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 makePointMsg( + 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 makePointMsg(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() )