diff --git a/frontier_exploration/CMakeLists.txt b/frontier_exploration/CMakeLists.txt
index adfa9e6..e043524 100644
--- a/frontier_exploration/CMakeLists.txt
+++ b/frontier_exploration/CMakeLists.txt
@@ -4,24 +4,59 @@ project(frontier_exploration)
# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
find_package(geometry_msgs REQUIRED)
+find_package(action_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
-find_package(frontier_msgs)
+find_package(frontier_msgs REQUIRED)
+find_package(pcl_ros REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(pluginlib REQUIRED)
+
+find_package(PCL REQUIRED)
include_directories(
include
+ ${PCL_INCLUDE_DIRS}
)
-# Add executable
-add_executable(frontier_search src/frontier_search.cpp)
-ament_target_dependencies(frontier_search rclcpp geometry_msgs nav2_costmap_2d frontier_msgs)
+#Add libraries
+add_library(frontier_exploration_core SHARED plugins/bounded_explore_layer.cpp src/frontier_search.cpp)
+ament_target_dependencies(frontier_exploration_core rclcpp pluginlib geometry_msgs nav2_costmap_2d sensor_msgs frontier_msgs pcl_ros pcl_conversions PCL)
+target_link_libraries(frontier_exploration_core ${PCL_LIBRARIES})
+
+
+# Add executable 1
+add_executable(explore_client src/explore_client.cpp)
+ament_target_dependencies(explore_client rclcpp geometry_msgs std_msgs nav2_msgs nav2_costmap_2d visualization_msgs frontier_msgs rclcpp_action)
+
+# Add executable 2
+add_executable(explore_server src/explore_server.cpp)
+ament_target_dependencies(explore_server rclcpp nav2_msgs nav2_costmap_2d visualization_msgs frontier_msgs rclcpp_action action_msgs)
-# Install executable
-install(TARGETS frontier_search
- DESTINATION lib/${PROJECT_NAME})
+# # Add executable 3 (Temporary)
+# add_executable(bounded_explore_layer src/frontier_search.cpp)
+# ament_target_dependencies(bounded_explore_layer rclcpp pluginlib geometry_msgs nav2_costmap_2d sensor_msgs frontier_msgs pcl_ros pcl_conversions PCL)
+# Add executable 4
+add_executable(polygon_point_publisher src/polygon_point_publisher.cpp)
+ament_target_dependencies(polygon_point_publisher rclcpp geometry_msgs)
+
+# TODO
+# bounded_explore_layer
+install(TARGETS explore_server explore_client frontier_exploration_core polygon_point_publisher
+DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY launch params
+DESTINATION share/${PROJECT_NAME}
+)
+pluginlib_export_plugin_description_file(nav2_costmap_2d bounded_explore_layer.xml)
ament_export_dependencies(rosidl_default_runtime)
ament_package()
\ No newline at end of file
diff --git a/frontier_exploration/bounded_explore_layer.xml b/frontier_exploration/bounded_explore_layer.xml
new file mode 100644
index 0000000..e482612
--- /dev/null
+++ b/frontier_exploration/bounded_explore_layer.xml
@@ -0,0 +1,7 @@
+
+
+ Demo Layer that marks all points that were ever one meter in front of the robot
+
+
\ No newline at end of file
diff --git a/frontier_exploration/include/frontier_exploration/bounded_explore_layer.hpp b/frontier_exploration/include/frontier_exploration/bounded_explore_layer.hpp
new file mode 100644
index 0000000..022129b
--- /dev/null
+++ b/frontier_exploration/include/frontier_exploration/bounded_explore_layer.hpp
@@ -0,0 +1,148 @@
+#ifndef BOUNDED_EXPLORE_LAYER_H_
+#define BOUNDED_EXPLORE_LAYER_H_
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+
+namespace frontier_exploration
+{
+
+/**
+ * @brief costmap_2d layer plugin that holds the state for a bounded frontier exploration task.
+ * Manages the boundary polygon, superimposes the polygon on the overall exploration costmap,
+ * and processes costmap to find next frontier to explore.
+ */
+class BoundedExploreLayer : public nav2_costmap_2d::Layer, public nav2_costmap_2d::Costmap2D
+{
+public:
+ BoundedExploreLayer();
+ ~BoundedExploreLayer();
+
+ /**
+ * @brief Loads default values and initialize exploration costmap.
+ */
+ virtual void onInitialize();
+
+ /**
+ * @brief Calculate bounds of costmap window to update
+ */
+ virtual void updateBounds(double origin_x, double origin_y, double origin_yaw, double* polygon_min_x, double* polygon_min_y, double* polygon_max_x,
+ double* polygon_max_y);
+
+ /**
+ * @brief Update requested costmap window
+ */
+ virtual void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+
+ bool isDiscretized()
+ {
+ return true;
+ }
+
+ /**
+ * @brief Match dimensions and origin of parent costmap
+ */
+ void matchSize() override;
+
+ /**
+ * @brief If clearing operations should be processed on this layer or not
+ */
+ virtual bool isClearable() {return false;}
+
+ /**
+ * @brief Reset exploration progress
+ */
+ virtual void reset();
+
+ // std::shared_ptr node = std::make_shared("bounded_explore_layer");
+
+ geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw);
+
+ rclcpp::Service::SharedPtr polygonService_;
+ rclcpp::Service::SharedPtr frontierService_;
+
+ // rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
+ // rclcpp::Node::SharedPtr node = std::make_shared("bounded_explore_layer");
+
+protected:
+
+ /**
+ * @brief ROS Service wrapper for updateBoundaryPolygon
+ * @param req Service request
+ * @param res Service response
+ * @return True on service success, false otherwise
+ */
+ /**
+ * @brief Load polygon boundary to draw on map with each update
+ * @param polygon_stamped polygon boundary
+ * @return True if polygon successfully loaded, false otherwise
+ */
+ void updateBoundaryPolygonService(
+ const std::shared_ptr request_header,
+ const std::shared_ptr req,
+ std::shared_ptr res);
+
+ /**
+ * @brief ROS Service wrapper for getNextFrontier
+ * @param req Service request
+ * @param res Service response
+ * @return True on service success, false otherwise
+ */
+ /**
+ * @brief Search the costmap for next reachable frontier to explore
+ * @param start_pose Pose from which to start search
+ * @param next_frontier Pose of found frontier
+ * @return True if a reachable frontier was found, false otherwise
+ */
+ void getNextFrontierService(
+ const std::shared_ptr request_header,
+ const std::shared_ptr req,
+ std::shared_ptr res);
+
+private:
+
+ /**
+ * @brief Update the map with exploration boundary data
+ * @param master_grid Reference to master costmap
+ */
+ void mapUpdateKeepObstacles(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+
+ /**
+ * @brief Callback executed when a parameter change is detected
+ * @param event ParameterEvent message
+ */
+ rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters);
+
+ geometry_msgs::msg::Polygon polygon_;
+ std::shared_ptr tf_buffer_;
+
+ rclcpp_lifecycle::LifecyclePublisher::SharedPtr frontier_cloud_pub;
+
+ bool enabledLayer_;
+ bool configured_, marked_;
+ bool explore_clear_space_;
+ bool resize_to_boundary_;
+ std::string frontier_travel_point_;
+ int max;
+ double frontierDetectRadius_;
+
+ // Dynamic parameters handler
+ rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+};
+
+}
+#endif
diff --git a/frontier_exploration/launch/exploration.launch.py b/frontier_exploration/launch/exploration.launch.py
new file mode 100644
index 0000000..bc86055
--- /dev/null
+++ b/frontier_exploration/launch/exploration.launch.py
@@ -0,0 +1,72 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.actions import Node
+from launch_ros.descriptions import ComposableNode
+from nav2_common.launch import RewrittenYaml
+
+
+def generate_launch_description():
+ # Get the launch directory
+ exploration_package_dir = get_package_share_directory('frontier_exploration')
+
+ namespace = LaunchConfiguration('namespace')
+ params_file = LaunchConfiguration('params_file')
+ log_level = LaunchConfiguration('log_level')
+
+ # Create our own temporary YAML files that include substitutions
+ param_substitutions = {}
+
+ configured_params = RewrittenYaml(
+ source_file=params_file,
+ root_key=namespace,
+ param_rewrites=param_substitutions,
+ convert_types=True)
+
+ declare_params_file_cmd = DeclareLaunchArgument(
+ 'params_file',
+ default_value=os.path.join(exploration_package_dir, 'params', 'exploration_params.yaml'),
+ description='Full path to the ROS2 parameters file to use for all launched nodes')
+
+ declare_use_composition_cmd = DeclareLaunchArgument(
+ 'use_composition', default_value='False',
+ description='Use composed bringup if True')
+
+ declare_log_level_cmd = DeclareLaunchArgument(
+ 'log_level', default_value='info',
+ description='log level')
+
+ declare_namespace_cmd = DeclareLaunchArgument(
+ 'namespace',
+ default_value='',
+ description='Top-level namespace')
+
+ load_nodes = GroupAction(
+ actions=[
+ Node(
+ package='frontier_exploration',
+ executable='explore_server',
+ output='screen',
+ parameters=[configured_params],
+ arguments=['--ros-args', '--log-level', log_level]),
+ ]
+ )
+
+ # Create the launch description and populate
+ ld = LaunchDescription()
+
+ ld.add_action(declare_namespace_cmd)
+ # Declare the launch options
+ ld.add_action(declare_params_file_cmd)
+ ld.add_action(declare_use_composition_cmd)
+ ld.add_action(declare_log_level_cmd)
+ # Add the actions to launch all of the navigation nodes
+ ld.add_action(load_nodes)
+
+ return ld
\ No newline at end of file
diff --git a/frontier_exploration/package.xml b/frontier_exploration/package.xml
index 1edfe76..5a6bfc6 100644
--- a/frontier_exploration/package.xml
+++ b/frontier_exploration/package.xml
@@ -15,11 +15,15 @@
geometry_msgs
nav2_costmap_2d
frontier_msgs
+ pcl_ros
+ sensor_msgs
rclcpp
geometry_msgs
nav2_costmap_2d
frontier_msgs
+ sensor_msgs
+ pcl_ros
ament_cmake
diff --git a/frontier_exploration/params/exploration_params.yaml b/frontier_exploration/params/exploration_params.yaml
new file mode 100644
index 0000000..ec16d1f
--- /dev/null
+++ b/frontier_exploration/params/exploration_params.yaml
@@ -0,0 +1,44 @@
+explore_costmap:
+ explore_costmap:
+ ros__parameters:
+ footprint: "[ [0.48, 0.38], [0.48, -0.38], [-0.48, -0.38], [ -0.48, 0.38 ] ]"
+ update_frequency: 1.0
+ publish_frequency: 1.0
+ global_frame: map
+ robot_base_frame: base_link
+ use_sim_time: True
+ # robot_radius: 0.22
+ resolution: 0.05
+ track_unknown_space: true
+ plugins: ["static_layer", "obstacle_layer", "bounded_explore_layer"]
+ # plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+ obstacle_layer:
+ plugin: "nav2_costmap_2d::ObstacleLayer"
+ enabled: True
+ observation_sources: scan
+ scan:
+ topic: /camera/points
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ data_type: "PointCloud2"
+ raytrace_max_range: 3.0
+ raytrace_min_range: 0.0
+ obstacle_max_range: 2.5
+ obstacle_min_range: 0.0
+ max_obstacle_height: 0.4
+ static_layer:
+ plugin: "nav2_costmap_2d::StaticLayer"
+ map_subscribe_transient_local: True
+ bounded_explore_layer:
+ plugin: "frontier_exploration/BoundedExploreLayer"
+ enabled: true
+ resize_to_boundary: false
+ frontier_travel_point: middle
+ explore_clear_space: false
+ always_send_full_costmap: True
+
+explore_server:
+ ros__parameters:
+ frequency: 1.0
+ goal_aliasing: 2.0
\ No newline at end of file
diff --git a/frontier_exploration/plugins/bounded_explore_layer.cpp b/frontier_exploration/plugins/bounded_explore_layer.cpp
new file mode 100644
index 0000000..955cebd
--- /dev/null
+++ b/frontier_exploration/plugins/bounded_explore_layer.cpp
@@ -0,0 +1,425 @@
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+
+
+namespace frontier_exploration
+{
+
+ using nav2_costmap_2d::LETHAL_OBSTACLE;
+ using nav2_costmap_2d::NO_INFORMATION;
+ using nav2_costmap_2d::FREE_SPACE;
+ using rcl_interfaces::msg::ParameterType;
+
+ BoundedExploreLayer::BoundedExploreLayer(){}
+
+ BoundedExploreLayer::~BoundedExploreLayer(){
+ polygonService_.reset();
+ frontierService_.reset();
+ dyn_params_handler_.reset();
+ }
+
+ void BoundedExploreLayer::onInitialize(){
+
+ configured_ = false;
+ marked_ = false;
+
+ declareParameter("explore_clear_space", rclcpp::ParameterValue(true));
+ declareParameter("resize_to_boundary", rclcpp::ParameterValue(true));
+ declareParameter("frontier_travel_point", rclcpp::ParameterValue(std::string("closest")));
+ declareParameter("enabled", rclcpp::ParameterValue(true));
+
+ auto node = node_.lock();
+ if (!node) {
+ throw std::runtime_error{"Failed to lock node bounded_explore_layer"};
+ }
+
+ node->get_parameter("explore_clear_space", explore_clear_space_);
+ node->get_parameter("resize_to_boundary", resize_to_boundary_);
+ node->get_parameter("frontier_travel_point", frontier_travel_point_);
+ node->get_parameter("enabled", enabledLayer_);
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("bounded_explore_layer"),"Enable parameter is" << enabledLayer_);
+ // TODO. Fix later
+ explore_clear_space_ = false;
+ resize_to_boundary_ = false;
+ frontier_travel_point_ = "initial";
+ enabledLayer_ = true;
+ frontierDetectRadius_ = 2.0;
+
+ if(explore_clear_space_){
+ default_value_ = NO_INFORMATION;
+ }else{
+ default_value_ = FREE_SPACE;
+ }
+
+ auto custom_qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable();
+
+ frontier_cloud_pub = node->create_publisher("frontiers", custom_qos);
+ frontier_cloud_pub->on_activate();
+ polygonService_ = node->create_service(
+ "update_boundary_polygon",
+ std::bind(&BoundedExploreLayer::updateBoundaryPolygonService, this,
+ std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+ frontierService_ = node->create_service(
+ "get_next_frontier",
+ std::bind(&BoundedExploreLayer::getNextFrontierService, this,
+ std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
+
+ dyn_params_handler_ = node->add_on_set_parameters_callback(
+ std::bind(
+ &BoundedExploreLayer::dynamicParametersCallback,
+ this, std::placeholders::_1));
+
+ tf_buffer_ = std::make_shared(node->get_clock());
+
+ matchSize();
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("bounded_explore_layer"),"Bounded explore layer Initialized with enabled as: " << enabledLayer_);
+ }
+
+
+ void BoundedExploreLayer::matchSize(){
+ Costmap2D* master = layered_costmap_->getCostmap();
+ resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
+ master->getOriginX(), master->getOriginY());
+ }
+
+ rcl_interfaces::msg::SetParametersResult BoundedExploreLayer::dynamicParametersCallback(std::vector parameters)
+ {
+ std::lock_guard guard(*getMutex());
+ rcl_interfaces::msg::SetParametersResult result;
+
+ for (auto parameter : parameters) {
+ const auto & param_type = parameter.get_type();
+ const auto & param_name = parameter.get_name();
+
+ if (param_type == ParameterType::PARAMETER_BOOL) {
+ if (param_name == "explore_clear_space") {
+ explore_clear_space_ = parameter.as_bool();
+ } else if (param_name == "resize_to_boundary") {
+ resize_to_boundary_ = parameter.as_bool();
+ } else if (param_name == "enabled") {
+ enabledLayer_ = parameter.as_bool();
+ }
+ }
+
+ else if(param_type == ParameterType::PARAMETER_STRING) {
+ if(param_name == "frontier_travel_point") {
+ frontier_travel_point_ = parameter.as_string();
+ }
+ }
+
+ }
+
+ result.successful = true;
+ return result;
+ }
+
+ void BoundedExploreLayer::getNextFrontierService(const std::shared_ptr, const std::shared_ptr req, std::shared_ptr res){
+ //wait for costmap to get marked with boundary
+ // TODO
+ RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Get Next Frontier Service called");
+ rclcpp::Rate r(10);
+ while (!marked_)
+ {
+ // auto n = std::dynamic_pointer_cast(node->get_node_base_interface());
+ // rclcpp::spin(node);
+ r.sleep();
+ }
+ RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Waiting to be marked done");
+
+ if(req->start_pose.header.frame_id != layered_costmap_->getGlobalFrameID()){
+ //error out if no transform available
+ std::string tf_error;
+ if(!tf_buffer_->canTransform(layered_costmap_->getGlobalFrameID(), req->start_pose.header.frame_id,tf2::TimePointZero, &tf_error)) {
+ RCLCPP_ERROR_STREAM(rclcpp::get_logger("bounded_explore_layer"),"Couldn't transform from map "<<" to "<< req->start_pose.header.frame_id.c_str());
+ res->success = false;
+ }
+ geometry_msgs::msg::PoseStamped temp_pose = req->start_pose;
+ tf_buffer_->transform(temp_pose,req->start_pose,layered_costmap_->getGlobalFrameID(),tf2::durationFromSec(10));
+ }
+
+ RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Transformation done");
+
+ //initialize frontier search implementation
+ frontier_exploration::FrontierSearch frontierSearch(*(layered_costmap_->getCostmap()));
+ //get list of frontiers from search implementation
+ std::list frontier_list = frontierSearch.searchFrom(req->start_pose.pose.position);
+
+ RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Frontier_search done");
+
+ //create placeholder for selected frontier
+ frontier_msgs::msg::Frontier selected;
+ selected.min_distance = std::numeric_limits::infinity();
+
+ if(frontier_list.size() == 0){
+ RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"), "No frontiers found, exploration complete");
+ res->success = false;
+ res->next_frontier.pose.position = selected.centroid;
+ return;
+ }
+
+ //pointcloud for visualization purposes
+ pcl::PointCloud frontier_cloud_viz;
+ pcl::PointXYZI frontier_point_viz(50);
+
+ RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Publishing routine started");
+
+ // FRONTIER SELECTION - CHANGE TO NEW FUNCTION IF YOU WISH TO ADD INTELLIGENCE.
+ bool frontierSelectionFlag = false;
+ for (const auto& frontier : frontier_list) {
+ //load frontier into visualization poitncloud
+ frontier_point_viz.x = frontier.middle.x;
+ frontier_point_viz.y = frontier.middle.y;
+ frontier_cloud_viz.push_back(frontier_point_viz);
+
+ //check if this frontier is the nearest to robot and ignore if very close (frontier_detect_radius)
+ if(frontier.min_distance > frontierDetectRadius_){
+ if (frontier.min_distance < selected.min_distance){
+ selected = frontier;
+ max = frontier_cloud_viz.size()-1;
+ frontierSelectionFlag = true;
+ }
+ }
+ }
+ if(frontierSelectionFlag == false){
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("bounded_explore_layer"),"FrontierSelection Flag is: " << frontierSelectionFlag);
+ res->success = false;
+ return;
+ }
+
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("bounded_explore_layer"),"Current selected frontier is: x:" << selected.middle.x << ", y: " << selected.middle.y);
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("bounded_explore_layer"),"Frontier cloud size is: " << frontier_cloud_viz.size());
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("bounded_explore_layer"),"Max is: " << max);
+ if(frontier_cloud_viz.size() > 0) {
+ frontier_cloud_viz[max].intensity = 100;
+ }
+ // RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Color set");
+
+ //publish visualization point cloud
+ sensor_msgs::msg::PointCloud2 frontier_viz_output;
+ pcl::toROSMsg(frontier_cloud_viz,frontier_viz_output);
+ // RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"costmap frame id to set");
+ frontier_viz_output.header.frame_id = layered_costmap_->getGlobalFrameID();
+ // RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Header frame ID set to global frame id.");
+ frontier_viz_output.header.stamp = rclcpp::Clock().now();
+ frontier_cloud_pub->publish(frontier_viz_output);
+ RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Get Next Frontier Service called and published new frontier for visualization");
+
+ //set goal pose to next frontier
+ res->next_frontier.header.frame_id = layered_costmap_->getGlobalFrameID();
+ res->next_frontier.header.stamp = rclcpp::Clock().now();
+
+ //
+ if(frontier_travel_point_ == "closest"){
+ res->next_frontier.pose.position = selected.initial;
+ }else if(frontier_travel_point_ == "middle"){
+ res->next_frontier.pose.position = selected.middle;
+ }else if(frontier_travel_point_ == "centroid"){
+ res->next_frontier.pose.position = selected.centroid;
+ }else{
+ RCLCPP_ERROR(rclcpp::get_logger("bounded_explore_layer"),"Invalid 'frontier_travel_point' parameter, falling back to 'closest'");
+ res->next_frontier.pose.position = selected.initial;
+ }
+
+ res->next_frontier.pose.orientation = createQuaternionMsgFromYaw(yawOfVector(req->start_pose.pose.position, res->next_frontier.pose.position) );
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("bounded_explore_layer"),"The result to be sent is x: " << res->next_frontier.pose.position.x << ", y:" << res->next_frontier.pose.position.y);
+ res->success = true;
+
+ RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Bounded explore layer Get Next Frontier Service Call completed");
+
+ }
+
+ geometry_msgs::msg::Quaternion BoundedExploreLayer::createQuaternionMsgFromYaw(double yaw)
+ {
+ tf2::Quaternion q;
+ q.setRPY(0, 0, yaw);
+ return tf2::toMsg(q);
+ }
+
+ void BoundedExploreLayer::updateBoundaryPolygonService(const std::shared_ptr, const std::shared_ptr req, std::shared_ptr res){
+ //clear existing boundary, if any
+ polygon_.points.clear();
+ std::string tf_error;
+ //error if no transform available between polygon and costmap
+ if(!tf_buffer_->canTransform(layered_costmap_->getGlobalFrameID(), req->explore_boundary.header.frame_id,tf2::TimePointZero, &tf_error)) {
+ RCLCPP_ERROR_STREAM(rclcpp::get_logger("bounded_explore_layer"),"Couldn't transform from " << layered_costmap_->getGlobalFrameID().c_str() << " to "<< req->explore_boundary.header.frame_id.c_str());
+ res-> success = false;
+ }
+
+ //Transform all points of boundary polygon into costmap frame
+ geometry_msgs::msg::PointStamped in;
+ geometry_msgs::msg::PointStamped out;
+ in.header = req->explore_boundary.header;
+ for (const auto& point32 : req->explore_boundary.polygon.points) {
+ in.point = nav2_costmap_2d::toPoint(point32);
+ tf_buffer_->transform(in,out,layered_costmap_->getGlobalFrameID(),tf2::durationFromSec(10));
+ polygon_.points.push_back(nav2_costmap_2d::toPoint32(out.point));
+ }
+
+ //if empty boundary provided, set to whole map
+ if(polygon_.points.empty()){
+ geometry_msgs::msg::Point32 temp;
+ temp.x = getOriginX();
+ temp.y = getOriginY();
+ polygon_.points.push_back(temp);
+ temp.y = getSizeInMetersY();
+ polygon_.points.push_back(temp);
+ temp.x = getSizeInMetersX();
+ polygon_.points.push_back(temp);
+ temp.y = getOriginY();
+ polygon_.points.push_back(temp);
+ }
+
+ if(resize_to_boundary_){
+ updateOrigin(0,0);
+
+ //Find map size and origin by finding min/max points of polygon
+ double min_x_polygon = std::numeric_limits::infinity();
+ double min_y_polygon = std::numeric_limits::infinity();
+ double max_x_polygon = -std::numeric_limits::infinity();
+ double max_y_polygon = -std::numeric_limits::infinity();
+
+ for (const auto& point : polygon_.points) {
+ min_x_polygon = std::min(min_x_polygon,(double)point.x);
+ min_y_polygon = std::min(min_y_polygon,(double)point.y);
+ max_x_polygon = std::max(max_x_polygon,(double)point.x);
+ max_y_polygon = std::max(max_y_polygon,(double)point.y);
+ }
+
+ //resize the costmap to polygon boundaries, don't change resolution
+ int size_x, size_y;
+ worldToMapNoBounds(max_x_polygon - min_x_polygon, max_y_polygon - min_y_polygon, size_x, size_y);
+ layered_costmap_->resizeMap(size_x, size_y, layered_costmap_->getCostmap()->getResolution(), min_x_polygon, min_y_polygon);
+ matchSize();
+ }
+
+ configured_ = true;
+ marked_ = false;
+ res->success = true;
+ RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Update boundary polygon service finished");
+ }
+
+ void BoundedExploreLayer::reset(){
+
+ RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Reset function called");
+
+ //reset costmap_ char array to default values
+ marked_ = false;
+ configured_ = false;
+ memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
+
+ }
+
+
+ void BoundedExploreLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
+ double* min_y, double* max_x, double* max_y){
+
+ //check if layer is enabled and configured with a boundary
+ if (!enabledLayer_ || !configured_){ return; }
+
+ //update the whole costmap
+ // *min_x = getOriginX();
+ // *min_y = getOriginY();
+ // RCLCPP_INFO_STREAM(rclcpp::get_logger("bounded_explore_layer")," Update bounds Min X and Min Y: " << *min_x << "," << *min_y);
+ // *max_x = getSizeInMetersX()+getOriginX();
+ // *max_y = getSizeInMetersY()+getOriginY();
+
+ *min_x = *min_x;
+ *max_x = *max_x;
+ *min_y = *min_y;
+ *max_y = *max_y;
+
+ RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Bounded explore layer Updated bounds");
+
+ }
+
+ void BoundedExploreLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){
+ //check if layer is enabled and configured with a boundary
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("bounded_explore_layer"),"Marked is set as: " << marked_);
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("bounded_explore_layer"),"Configured is set as" << configured_);
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("bounded_explore_layer"),"Enabled is set as" << enabledLayer_);
+ if (!enabledLayer_ || !configured_){
+ return;
+ }
+
+ //draw lines between each point in polygon
+ MarkCell marker(costmap_, LETHAL_OBSTACLE);
+
+ //circular iterator
+ for(int i = 0, j = polygon_.points.size()-1; i < polygon_.points.size(); j = i++){
+ int x_1, y_1, x_2, y_2;
+ worldToMapEnforceBounds(polygon_.points[i].x, polygon_.points[i].y, x_1,y_1);
+ worldToMapEnforceBounds(polygon_.points[j].x, polygon_.points[j].y, x_2,y_2);
+
+ raytraceLine(marker,x_1,y_1,x_2,y_2);
+ }
+ //update the master grid from the internal costmap
+ mapUpdateKeepObstacles(master_grid, min_i, min_j, max_i, max_j);
+
+ // RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Bounded explore layer updated costs");
+
+ }
+
+ void BoundedExploreLayer::mapUpdateKeepObstacles(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){
+ if (!enabledLayer_)
+ return;
+
+ unsigned char* master = master_grid.getCharMap();
+ unsigned int span = master_grid.getSizeInCellsX();
+ // RCLCPP_INFO_STREAM(rclcpp::get_logger("bounded_explore_layer"),"Values:" << max_j << ","< master[it])){
+ master[it] = costmap_[it];
+ }
+ it++;
+
+ }
+ }
+ marked_ = true;
+ // RCLCPP_INFO(rclcpp::get_logger("bounded_explore_layer"),"Update costs function called, marked set to true");
+ }
+}
+
+#include
+PLUGINLIB_EXPORT_CLASS(frontier_exploration::BoundedExploreLayer, nav2_costmap_2d::Layer)
+
+// int main(int argc, char** argv) {
+// rclcpp::init(argc, argv);
+
+// // Create an instance of your class
+// frontier_exploration::BoundedExploreLayer bel;
+
+// // You can now access the node using the class instance
+// // For example, call a member function that uses the node
+
+// // auto n = std::dynamic_pointer_cast(bel.node->get_node_base_interface());
+// // rclcpp::spin(n); // Spin the node to handle callbacks
+
+// rclcpp::shutdown();
+// return 0;
+// }
\ No newline at end of file
diff --git a/frontier_exploration/src/explore_client.cpp b/frontier_exploration/src/explore_client.cpp
new file mode 100644
index 0000000..2d33f3d
--- /dev/null
+++ b/frontier_exploration/src/explore_client.cpp
@@ -0,0 +1,158 @@
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+
+namespace frontier_exploration{
+
+/**
+ * @brief Client for FrontierExplorationServer that receives control points from rviz, and creates boundary polygon for frontier exploration
+ */
+class FrontierExplorationClient : public rclcpp::Node {
+
+private:
+
+ rclcpp::Subscription::SharedPtr point_;
+ rclcpp::Publisher::SharedPtr point_viz_pub_;
+ rclcpp::TimerBase::SharedPtr point_viz_timer_;
+ geometry_msgs::msg::PolygonStamped input_;
+
+ bool waiting_for_center_;
+
+ /**
+ * @brief Publish markers for visualization of points for boundary polygon.
+ */
+ void vizPubCb(){
+
+ visualization_msgs::msg::Marker points, line_strip;
+
+ points.header = line_strip.header = input_.header;
+ points.ns = line_strip.ns = "explore_points";
+
+ points.id = 0;
+ line_strip.id = 1;
+
+ points.type = visualization_msgs::msg::Marker::SPHERE_LIST;
+ line_strip.type = visualization_msgs::msg::Marker::LINE_STRIP;
+
+ if(!input_.polygon.points.empty()){
+
+ points.action = line_strip.action = visualization_msgs::msg::Marker::ADD;
+ points.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
+
+ points.scale.x = points.scale.y = 0.1;
+ line_strip.scale.x = 0.05;
+
+ for (const auto& point : input_.polygon.points) {
+ line_strip.points.push_back(nav2_costmap_2d::toPoint(point));
+ points.points.push_back(nav2_costmap_2d::toPoint(point));
+ }
+
+ if(waiting_for_center_){
+ line_strip.points.push_back(nav2_costmap_2d::toPoint(input_.polygon.points.front()));
+ points.color.a = points.color.r = line_strip.color.r = line_strip.color.a = 1.0;
+ }else{
+ points.color.a = points.color.b = line_strip.color.b = line_strip.color.a = 1.0;
+ }
+ }else{
+ points.action = line_strip.action = visualization_msgs::msg::Marker::DELETE;
+ }
+ point_viz_pub_->publish(points);
+ point_viz_pub_->publish(line_strip);
+
+ }
+
+ /**
+ * @brief Build boundary polygon from points received through rviz gui.
+ * @param point Received point from rviz
+ */
+ void pointCb(const std::shared_ptr point){
+ RCLCPP_INFO(rclcpp::get_logger("explore_client"), "Point clicked");
+
+ double average_distance = polygonPerimeter(input_.polygon) / input_.polygon.points.size();
+
+ if(waiting_for_center_){
+ //flag is set so this is the last point of boundary polygon, i.e. center
+
+ if(!pointInPolygon(point->point,input_.polygon)){
+ RCLCPP_ERROR(rclcpp::get_logger("explore_client"),"Center not inside polygon, restarting");
+ }else{
+ auto exploreClient = rclcpp_action::create_client(this, "explore_action");
+ frontier_msgs::action::ExploreTask::Goal goal;
+ goal.explore_center = *point;
+ goal.explore_boundary = input_;
+ exploreClient->async_send_goal(goal);
+ }
+ waiting_for_center_ = false;
+ input_.polygon.points.clear();
+
+ }else if(input_.polygon.points.empty()){
+ //first control point, so initialize header of boundary polygon
+
+ input_.header = point->header;
+ input_.polygon.points.push_back(nav2_costmap_2d::toPoint32(point->point));
+
+ }else if(input_.header.frame_id != point->header.frame_id){
+ RCLCPP_ERROR(rclcpp::get_logger("explore_client"),"Frame mismatch, restarting polygon selection");
+ input_.polygon.points.clear();
+
+ }else if(input_.polygon.points.size() > 1 && pointsNearby(input_.polygon.points.front(), point->point,
+ average_distance*0.1)){
+ //check if last boundary point, i.e. nearby to first point
+
+ if(input_.polygon.points.size() < 3){
+ RCLCPP_ERROR(rclcpp::get_logger("explore_client"),"Not a valid polygon, restarting");
+ input_.polygon.points.clear();
+ }else{
+ waiting_for_center_ = true;
+ RCLCPP_WARN(rclcpp::get_logger("explore_client"),"Please select an initial point for exploration inside the polygon");
+ }
+
+ }else{
+
+ //otherwise, must be a regular point inside boundary polygon
+ input_.polygon.points.push_back(nav2_costmap_2d::toPoint32(point->point));
+ input_.header.stamp = rclcpp::Clock().now();
+ RCLCPP_INFO(rclcpp::get_logger("explore_client"),"Point taken into account.");
+ }
+
+ }
+
+public:
+
+ /**
+ * @brief Constructor for the client.
+ */
+ FrontierExplorationClient() : Node("explore_client"), waiting_for_center_(false)
+ {
+ input_.header.frame_id = "map";
+ point_ = create_subscription(
+ "/clicked_point", 10, std::bind(&FrontierExplorationClient::pointCb, this, std::placeholders::_1));
+ point_viz_pub_ = create_publisher("exploration_polygon_marker", 10);
+ point_viz_timer_ = create_wall_timer(std::chrono::milliseconds(100), std::bind(&FrontierExplorationClient::vizPubCb, this));
+ RCLCPP_INFO(rclcpp::get_logger("explore_client"),"Please use the 'Point' tool in Rviz to select an exporation boundary.");
+ }
+
+};
+
+}
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+ auto node = std::make_shared();
+ rclcpp::spin(node);
+ rclcpp::shutdown();
+}
\ No newline at end of file
diff --git a/frontier_exploration/src/explore_server.cpp b/frontier_exploration/src/explore_server.cpp
new file mode 100644
index 0000000..ee944f6
--- /dev/null
+++ b/frontier_exploration/src/explore_server.cpp
@@ -0,0 +1,390 @@
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+
+#include
+#include
+#include
+
+#include
+
+#include
+
+
+namespace frontier_exploration{
+
+/**
+ * @brief Server for frontier exploration action, runs the state machine associated with a
+ * structured frontier exploration task and manages robot movement through move_base.
+ */
+class FrontierExplorationServer : public rclcpp::Node {
+public:
+
+ using GoalHandleExplore = rclcpp_action::ServerGoalHandle;
+ using NavigateToPose = nav2_msgs::action::NavigateToPose;
+ using GoalHandleNav2 = rclcpp_action::ClientGoalHandle;
+
+ /**
+ * @brief Constructor for the server, sets up this node's ActionServer for exploration and ActionClient to move_base for robot movement.
+ * @param name Name for SimpleActionServer
+ */
+ FrontierExplorationServer() : Node("explore_server_node")
+ {
+ this->declare_parameter("frequency", 0.0);
+ this->declare_parameter("goal_aliasing", 0.1);
+ this->get_parameter("frequency", frequency_);
+ this->get_parameter("goal_aliasing",goal_aliasing_);
+ frequency_ = 0.0;
+ goal_aliasing_ = 0.1;
+ nav2Client_ = rclcpp_action::create_client(this, "navigate_to_pose"); //was previously move_base, true
+ action_server_ = rclcpp_action::create_server(
+ this,
+ "explore_action",
+ std::bind(&FrontierExplorationServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
+ std::bind(&FrontierExplorationServer::handle_cancel, this, std::placeholders::_1),
+ std::bind(&FrontierExplorationServer::handle_accepted, this, std::placeholders::_1));
+ explore_costmap_ros_ = std::make_shared("explore_costmap",std::string{get_namespace()},"explore_costmap");
+ explore_costmap_ros_->configure();
+ // Launch a thread to run the costmap node
+ explore_costmap_thread_ = std::make_unique(explore_costmap_ros_);
+ explore_costmap_ros_->activate();
+
+ retry_ = 30;
+ nav2WaitTime_ = 15;
+ tf_listener_ = std::make_shared(this->get_clock());
+ nav2Publisher_ = this->create_publisher("goal_pose", 10);
+ nav2Subscriber_ = this->create_subscription("navigate_to_pose/_action/status",10, std::bind(&FrontierExplorationServer::feedbackNav2cb, this, std::placeholders::_1));
+ }
+
+ ~FrontierExplorationServer()
+ {
+ explore_costmap_ros_->deactivate();
+ explore_costmap_ros_->cleanup();
+ explore_costmap_thread_.reset();
+ }
+
+private:
+
+ std::shared_ptr tf_listener_;
+ rclcpp_action::Server::SharedPtr action_server_;
+
+ std::shared_ptr explore_costmap_ros_;
+ std::unique_ptr explore_costmap_thread_;
+ // nav2_costmap_2d::Costmap2DROS* explore_costmap_ros_;
+ double frequency_, goal_aliasing_;
+ bool success_, moving_;
+ int retry_;
+ int nav2WaitTime_;
+
+ rclcpp_action::Client::SharedPtr nav2Client_;
+ std::mutex nav2Clientlock_;
+ rclcpp::Publisher::SharedPtr nav2Publisher_;
+ rclcpp::Subscription::SharedPtr nav2Subscriber_;
+ NavigateToPose::Goal goal_msg; //previously was goal_msg
+ std::shared_ptr frontier_goal;
+
+ rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal)
+ {
+ RCLCPP_INFO(this->get_logger(), "Received goal");
+ (void)uuid;
+ frontier_goal = goal;
+ return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+ }
+
+ rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle)
+ {
+ nav2Client_->async_cancel_goals_before(rclcpp::Clock().now());
+ RCLCPP_WARN(rclcpp::get_logger("explore_server"),"Current exploration task cancelled");
+
+ (void)goal_handle;
+ return rclcpp_action::CancelResponse::ACCEPT;
+ }
+
+ void handle_accepted(const std::shared_ptr goal_handle)
+ {
+ std::thread{std::bind(&FrontierExplorationServer::executeCb, this, std::placeholders::_1, std::placeholders::_2),goal_handle, frontier_goal}.detach();
+ }
+
+
+ geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw)
+ {
+ tf2::Quaternion q;
+ q.setRPY(0, 0, yaw);
+ return tf2::toMsg(q);
+ }
+
+ /**
+ * @brief Execute callback for actionserver, run after accepting a new goal
+ * @param goal ActionGoal containing boundary of area to explore, and a valid centerpoint for the area.
+ */
+
+ void executeCb(const std::shared_ptr goal_handle, std::shared_ptr goal)
+ {
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"TempExecute callback called");
+ success_ = false;
+ moving_ = false;
+ auto feedback_ = std::make_shared();
+ auto res = std::make_shared();
+
+ // Don't compute a plan until costmap is valid (after clear costmap)
+ rclcpp::Rate r(100);
+ while (!explore_costmap_ros_->isCurrent()) {
+ r.sleep();
+ }
+ rclcpp::Client::SharedPtr updateBoundaryPolygon = this->create_client("explore_costmap/update_boundary_polygon");
+ rclcpp::Client::SharedPtr getNextFrontier = this->create_client("explore_costmap/get_next_frontier");
+
+ //wait for move_base and costmap services
+ if(!nav2Client_->wait_for_action_server(std::chrono::seconds(10)) || !updateBoundaryPolygon->wait_for_service(std::chrono::seconds(10)) || !getNextFrontier->wait_for_service(std::chrono::seconds(10))){
+ goal_handle->abort(res);
+ RCLCPP_ERROR(rclcpp::get_logger("explore_server"),"Action server or service is not available.");
+ return;
+ }
+
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"TempAction Services available");
+
+ //set region boundary on costmap
+ if(rclcpp::ok() && goal_handle->is_active()){
+ auto srv_req = std::make_shared();
+ srv_req->explore_boundary = goal->explore_boundary;
+ auto resultBoundaryPolygon = updateBoundaryPolygon->async_send_request(srv_req);
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"TempUpdate Boundary Polygon result recieved.");
+ std::future_status fstatus = resultBoundaryPolygon.wait_for(std::chrono::seconds(10));
+ if(fstatus == std::future_status::ready){
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"Region boundary set");
+ }else{
+ RCLCPP_ERROR(rclcpp::get_logger("explore_server"),"Failed to set region boundary");
+ goal_handle->abort(res);
+ return;
+ }
+ }
+
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"TempCompleted region boundary setting. Failed or passed indicated.");
+
+ //loop until all frontiers are explored
+ rclcpp::Rate rate(frequency_);
+ while(rclcpp::ok() && goal_handle->is_active()){
+
+ auto srv_req = std::make_shared();
+ auto srv_res = std::make_shared();
+
+ //placeholder for next goal to be sent to move base
+ geometry_msgs::msg::PoseStamped goal_pose;
+
+ //get current robot pose in frame of exploration boundary
+ geometry_msgs::msg::PoseStamped robot_pose;
+ explore_costmap_ros_->getRobotPose(robot_pose);
+ srv_req->start_pose = robot_pose;
+
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("explore_server"),"TempThe robot pose is: " << robot_pose.pose.position.x << " ," << robot_pose.pose.position.y);
+
+ //evaluate if robot is within exploration boundary using robot_pose in boundary frame
+ geometry_msgs::msg::PoseStamped eval_pose = srv_req->start_pose;
+ if(eval_pose.header.frame_id != goal->explore_boundary.header.frame_id){
+ tf_listener_->transform(srv_req->start_pose, eval_pose,goal->explore_boundary.header.frame_id);
+ }
+
+ auto resultNextFrontier = getNextFrontier->async_send_request(srv_req);
+ std::future_status fstatus = resultNextFrontier.wait_for(std::chrono::seconds(10));
+ srv_res = resultNextFrontier.get();
+ //check if robot is not within exploration boundary and needs to return to center of search area
+ if(goal->explore_boundary.polygon.points.size() > 0 && !pointInPolygon(eval_pose.pose.position,goal->explore_boundary.polygon)){
+
+ //check if robot has explored at least one frontier, and promote debug message to warning
+ if(success_){
+ RCLCPP_WARN(rclcpp::get_logger("explore_server"),"Robot left exploration boundary, returning to center");
+ }else{
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"Robot not initially in exploration boundary, traveling to center");
+ }
+ //get current robot position in frame of exploration center
+ geometry_msgs::msg::PointStamped eval_point;
+ eval_point.header = eval_pose.header;
+ eval_point.point = eval_pose.pose.position;
+ if(eval_point.header.frame_id != goal->explore_center.header.frame_id){
+ geometry_msgs::msg::PointStamped temp = eval_point;
+ tf_listener_->transform(temp, eval_point, goal->explore_center.header.frame_id);
+ }
+
+ //set goal pose to exploration center
+ goal_pose.header = goal->explore_center.header;
+ goal_pose.pose.position = goal->explore_center.point;
+ goal_pose.pose.orientation = FrontierExplorationServer::createQuaternionMsgFromYaw(yawOfVector(eval_point.point, goal->explore_center.point));
+
+ }
+ else if(fstatus == std::future_status::ready && srv_res->success == true){
+
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"Found frontier to explore");
+ success_ = true;
+ // goal_pose = feedback_->next_frontier = srv_res->next_frontier;
+ goal_pose = srv_res->next_frontier;
+ auto service_result = srv_res->success;
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("explore_server"),"The goal pose is set to is x: " << goal_pose.pose.position.x << ", y:" << goal_pose.pose.position.y);
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("explore_server"),"The result of the service is: " << service_result);
+ }
+ else if(fstatus == std::future_status::ready && srv_res->success == false){ //if no frontier found, check if search is successful
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"Couldn't find a frontier");
+
+ //search is succesful
+ if(retry_ == 0 && success_){
+ RCLCPP_WARN(rclcpp::get_logger("explore_server"),"Finished exploring room");
+ goal_handle->succeed(res);
+ std::unique_lock lock(nav2Clientlock_);
+ nav2Client_->async_cancel_goals_before(rclcpp::Clock().now());
+ return;
+
+ }else if(retry_ == 0 || !rclcpp::ok()){ //search is not successful
+
+ RCLCPP_ERROR(rclcpp::get_logger("explore_server"),"Failed exploration");
+ goal_handle->abort(res);
+ return;
+ }
+
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"Retrying...");
+ retry_--;
+ //try to find frontier again, without moving robot
+ if(retry_ <= 7){
+ performBackupAction();
+ rclcpp::Rate(2).sleep();
+ }
+ continue;
+ }
+ //if above conditional does not escape this loop step, search has a valid goal_pose
+
+ //check if new goal is close to old goal, hence no need to resend
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("explore_server"),"Nearby? is: " << pointsNearby(goal_msg.pose.pose.position,goal_pose.pose.position,goal_aliasing_*0.5));
+ // RCLCPP_INFO_STREAM(rclcpp::get_logger("explore_server"),"The goal_msg x is: " << goal_msg.pose.pose.position.x);
+ // RCLCPP_INFO_STREAM(rclcpp::get_logger("explore_server"),"The goal_pose x is: " << goal_pose.pose.position.x);
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("explore_server"),"Moving is: " << moving_);
+ if((!moving_ || !pointsNearby(goal_msg.pose.pose.position,goal_pose.pose.position,goal_aliasing_*0.5)) && srv_res->success == true){
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"New exploration goal");
+ goal_msg.pose = goal_pose;
+ // std::unique_lock lock(nav2Clientlock_);
+ if(goal_handle->is_active()){
+
+ // auto send_goal_options = rclcpp_action::Client::SendGoalOptions();
+ // send_goal_options.feedback_callback = std::bind(&FrontierExplorationServer::feedbackMovingCb, this, std::placeholders::_1);
+ // send_goal_options.result_callback = std::bind(&FrontierExplorationServer::doneMovingCb, this, std::placeholders::_1);
+ // auto goal_handle_future = nav2Client_->async_send_goal(goal_msg, send_goal_options);
+ nav2Publisher_->publish(goal_pose);
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("explore_server"),"Published new goal x:" << goal_pose.pose.position.x << ",y: "<< goal_pose.pose.position.y);
+ moving_ = true;
+ int waitCount_ = 0;
+ bool minuteWait = false;
+ while(moving_ && minuteWait == false){
+ rclcpp::Rate(1).sleep();
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"Waiting to finish moving with new goal");
+ waitCount_++;
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("explore_server"),"Wait count goal is: " << waitCount_);
+ if( waitCount_ > nav2WaitTime_){
+ minuteWait = true;
+ }
+ }
+ }
+ // lock.unlock();
+ }
+ }
+
+ //goal should never be active at this point
+ // ROS_ASSERT(!goal_handle->is_active());
+ if(!goal_handle->is_active()){
+ RCLCPP_ERROR(rclcpp::get_logger("explore_server"),"Goal Active when it should not be.");
+ }
+ }
+
+ void performBackupAction(){
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"Frontier Exploration backup");
+ geometry_msgs::msg::PoseStamped robot_pose;
+ explore_costmap_ros_->getRobotPose(robot_pose);
+ tf2::Quaternion quaternion;
+ tf2::fromMsg(robot_pose.pose.orientation, quaternion);
+ double current_yaw = tf2::getYaw(quaternion);
+ double desired_rad = M_PI / 2;
+ double new_yaw = current_yaw + desired_rad;
+ quaternion.setRPY(0, 0, new_yaw);
+ geometry_msgs::msg::PoseStamped new_pose;
+ new_pose.header.frame_id = robot_pose.header.frame_id;
+ new_pose.pose.position = robot_pose.pose.position;
+ new_pose.pose.orientation = tf2::toMsg(quaternion);
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"Rotating in place to get new frontiers");
+ nav2Publisher_->publish(new_pose);
+ moving_ = true;
+ int waitCount_ = 0;
+ bool minuteWait = false;
+ while(moving_ && minuteWait == false){
+ rclcpp::WallRate(1.0).sleep();
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"Waiting to finish moving while rotating");
+ waitCount_++;
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("explore_server"),"Wait count rotation is: " << waitCount_);
+ // TODO: Assign different wait times for goal and rotate.
+ if( waitCount_ > (nav2WaitTime_ / 3)){
+ minuteWait = true;
+ }
+ }
+ }
+
+
+ /**
+ * @brief Preempt callback for the server, cancels the current running goal and all associated movement actions.
+ */
+
+ /**
+ * @brief Feedback callback for the move_base client, republishes as feedback for the exploration server
+ * @param feedback Feedback from the move_base client
+ */
+ void feedbackMovingCb(GoalHandleNav2::SharedPtr, const std::shared_ptr feedback){
+
+ auto feedback_ = std::make_shared();
+ feedback_->current_pose = feedback->current_pose;
+ // action_server_->publish_feedback(feedback_);
+ }
+
+ /**
+ * @brief Done callback for the move_base client, checks for errors and aborts exploration task if necessary
+ * @param state State from the move_base client
+ * @param result Result from the move_base client
+ */
+ void doneMovingCb(const GoalHandleNav2::WrappedResult & result){
+
+ if (result.code == rclcpp_action::ResultCode::ABORTED){
+ RCLCPP_ERROR(rclcpp::get_logger("explore_server"),"Failed to move");
+ // action_server_.setAborted();
+ }else if(result.code == rclcpp_action::ResultCode::SUCCEEDED){
+ moving_ = false;
+ }
+ }
+
+ void feedbackNav2cb(const action_msgs::msg::GoalStatusArray::SharedPtr status){
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"Nav2 feedback Callback called.");
+ auto current_goal_status = status->status_list.back().status;
+ if(current_goal_status == action_msgs::msg::GoalStatus::STATUS_SUCCEEDED){
+ moving_ = false;
+ RCLCPP_INFO(rclcpp::get_logger("explore_server"),"Moving completed");
+ }
+ else if(current_goal_status == action_msgs::msg::GoalStatus::STATUS_ABORTED){
+ RCLCPP_ERROR(rclcpp::get_logger("explore_server"),"Failed to move");
+ moving_ = false;
+ }
+ else if(current_goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING){
+ // RCLCPP_INFO(rclcpp::get_logger("explore_server"),"Currently moving");
+ }
+ }
+
+};
+
+}
+
+int main(int argc, char** argv)
+{
+ rclcpp::init(argc, argv);
+ auto node = std::make_shared();
+ rclcpp::spin(node);
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/frontier_exploration/src/frontier_search.cpp b/frontier_exploration/src/frontier_search.cpp
index 2ac3b44..1053f5a 100644
--- a/frontier_exploration/src/frontier_search.cpp
+++ b/frontier_exploration/src/frontier_search.cpp
@@ -17,7 +17,9 @@ using nav2_costmap_2d::LETHAL_OBSTACLE;
using nav2_costmap_2d::NO_INFORMATION;
using nav2_costmap_2d::FREE_SPACE;
-FrontierSearch::FrontierSearch(nav2_costmap_2d::Costmap2D &costmap) : costmap_(costmap) { }
+FrontierSearch::FrontierSearch(nav2_costmap_2d::Costmap2D &costmap) : costmap_(costmap) {
+ RCLCPP_INFO(rclcpp::get_logger("frontier_search"), "Frontier search constructor");
+}
std::list FrontierSearch::searchFrom(geometry_msgs::msg::Point position){
@@ -70,6 +72,7 @@ std::list FrontierSearch::searchFrom(geometry_msgs
else if(isNewFrontierCell(nbr, frontier_flag)){
frontier_flag[nbr] = true;
frontier_msgs::msg::Frontier new_frontier = buildNewFrontier(nbr, pos, frontier_flag);
+ // TODO: Set minimum frontier point size.
if(new_frontier.size > 1){
frontier_list.push_back(new_frontier);
}
@@ -252,20 +255,20 @@ void localizationPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStampe
}
-int main(int argc, char** argv) {
- rclcpp::init(argc, argv);
- auto node = std::make_shared("frontier_search");
+// int main(int argc, char** argv) {
+// rclcpp::init(argc, argv);
+// auto node = std::make_shared("frontier_search");
- auto map_sub = node->create_subscription(
- "map", 10, mapCallback);
+// auto map_sub = node->create_subscription(
+// "/map", 10, mapCallback);
- auto localization_pose_sub = node->create_subscription(
- "localization_pose", 10, localizationPoseCallback);
+// auto localization_pose_sub = node->create_subscription(
+// "localization_pose", 10, localizationPoseCallback);
- marker_pub_initial = node->create_publisher("frontier_markers_initial", 10);
- marker_pub_centroid = node->create_publisher("frontier_markers_centroid", 10);
- rclcpp::spin(node);
- rclcpp::shutdown();
+// marker_pub_initial = node->create_publisher("frontier_markers_initial", 10);
+// marker_pub_centroid = node->create_publisher("frontier_markers_centroid", 10);
+// rclcpp::spin(node);
+// rclcpp::shutdown();
- return 0;
-}
\ No newline at end of file
+// return 0;
+// }
\ No newline at end of file
diff --git a/frontier_exploration/src/polygon_point_publisher.cpp b/frontier_exploration/src/polygon_point_publisher.cpp
new file mode 100644
index 0000000..1503492
--- /dev/null
+++ b/frontier_exploration/src/polygon_point_publisher.cpp
@@ -0,0 +1,82 @@
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/point_stamped.hpp"
+
+class PointPublisherNode : public rclcpp::Node
+{
+public:
+ PointPublisherNode() : Node("point_publisher_node"), count_(1)
+ {
+ publisher_ = this->create_publisher("/clicked_point", 10);
+ count_ = 1;
+ timer_ = this->create_wall_timer(std::chrono::seconds(2), std::bind(&PointPublisherNode::publishPoint, this));
+ squareHalfSideLength_ = 25.0;
+ }
+
+private:
+ void publishPoint()
+ {
+ auto point_msg = std::make_shared();
+ point_msg->header.stamp = this->now();
+ point_msg->header.frame_id = "map"; // Modify the frame_id according to your needs
+ point_msg->point.x = -squareHalfSideLength_;
+ point_msg->point.y = squareHalfSideLength_;
+ point_msg->point.z = 0.002;
+
+ switch (count_)
+ {
+ case 1:
+ point_msg->point.x = -squareHalfSideLength_;
+ point_msg->point.y = squareHalfSideLength_;
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("polygon_point_publisher"), "Point: " << count_);
+ break;
+ case 2:
+ point_msg->point.x = squareHalfSideLength_;
+ point_msg->point.y = squareHalfSideLength_;
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("polygon_point_publisher"), "Point: " << count_);
+ break;
+ case 3:
+ point_msg->point.x = squareHalfSideLength_;
+ point_msg->point.y = -squareHalfSideLength_;
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("polygon_point_publisher"), "Point: " << count_);
+ break;
+ case 4:
+ point_msg->point.x = -squareHalfSideLength_;
+ point_msg->point.y = -squareHalfSideLength_;
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("polygon_point_publisher"), "Point: " << count_);
+ break;
+ case 5:
+ point_msg->point.x = -squareHalfSideLength_;
+ point_msg->point.y = squareHalfSideLength_;
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("polygon_point_publisher"), "Point: " << count_);
+ break;
+ case 6:
+ point_msg->point.x = 0.0;
+ point_msg->point.y = 0.0;
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("polygon_point_publisher"), "Point: " << count_);
+ break;
+ default:
+ // Reset the counter if it exceeds 4
+ count_ = 0;
+ timer_->cancel();
+ RCLCPP_INFO_STREAM(rclcpp::get_logger("polygon_point_publisher"), "Point: " << count_);
+ rclcpp::shutdown();
+ break;
+ }
+
+ publisher_->publish(*point_msg);
+ count_++;
+ }
+
+ rclcpp::Publisher::SharedPtr publisher_;
+ rclcpp::TimerBase::SharedPtr timer_;
+ int count_;
+ double squareHalfSideLength_;
+};
+
+int main(int argc, char **argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::spin(std::make_shared());
+ rclcpp::shutdown();
+ return 0;
+}
\ No newline at end of file
diff --git a/frontier_msgs/CMakeLists.txt b/frontier_msgs/CMakeLists.txt
index c46e452..450f8bd 100644
--- a/frontier_msgs/CMakeLists.txt
+++ b/frontier_msgs/CMakeLists.txt
@@ -7,7 +7,6 @@ find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(nav2_costmap_2d REQUIRED)
-find_package(frontier_msgs)
include_directories(
include
@@ -15,6 +14,9 @@ include_directories(
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Frontier.msg"
+"srv/GetNextFrontier.srv"
+"srv/UpdateBoundaryPolygon.srv"
+"action/ExploreTask.action"
DEPENDENCIES builtin_interfaces geometry_msgs
)
diff --git a/frontier_msgs/action/ExploreTask.action b/frontier_msgs/action/ExploreTask.action
new file mode 100644
index 0000000..51b2d60
--- /dev/null
+++ b/frontier_msgs/action/ExploreTask.action
@@ -0,0 +1,10 @@
+#Boundary for frontier exploration
+geometry_msgs/PolygonStamped explore_boundary
+#Center point for frontier exploration, inside explore_boundary
+geometry_msgs/PointStamped explore_center
+---
+---
+#Current target frontier to explore
+geometry_msgs/PoseStamped next_frontier
+#Current position of robot
+geometry_msgs/PoseStamped base_position
\ No newline at end of file
diff --git a/frontier_msgs/srv/GetNextFrontier.srv b/frontier_msgs/srv/GetNextFrontier.srv
new file mode 100644
index 0000000..3ded170
--- /dev/null
+++ b/frontier_msgs/srv/GetNextFrontier.srv
@@ -0,0 +1,4 @@
+geometry_msgs/PoseStamped start_pose
+---
+bool success
+geometry_msgs/PoseStamped next_frontier
diff --git a/frontier_msgs/srv/UpdateBoundaryPolygon.srv b/frontier_msgs/srv/UpdateBoundaryPolygon.srv
new file mode 100644
index 0000000..cf814ad
--- /dev/null
+++ b/frontier_msgs/srv/UpdateBoundaryPolygon.srv
@@ -0,0 +1,3 @@
+geometry_msgs/PolygonStamped explore_boundary
+---
+bool success
\ No newline at end of file