indigo-devel branch fully ported to ROS2

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()

frontier_exploration/bounded_explore_layer.xml

@@ -0,0 +1,7 @@
+<library path="frontier_exploration_core">
+  <class name="frontier_exploration/BoundedExploreLayer"
+    type="frontier_exploration::BoundedExploreLayer" 
+    base_class_type="nav2_costmap_2d::Layer">
+    <description>Demo Layer that marks all points that were ever one meter in front of the robot</description>
+  </class>
+</library>

frontier_exploration/include/frontier_exploration/bounded_explore_layer.hpp

@@ -0,0 +1,148 @@
+#ifndef BOUNDED_EXPLORE_LAYER_H_
+#define BOUNDED_EXPLORE_LAYER_H_
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <nav2_costmap_2d/layer.hpp>
+#include <nav2_costmap_2d/layered_costmap.hpp>
+
+#include <geometry_msgs/msg/polygon.hpp>
+#include <geometry_msgs/msg/quaternion.h>
+
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <frontier_msgs/msg/frontier.hpp>
+#include <frontier_msgs/srv/update_boundary_polygon.hpp>
+#include <frontier_msgs/srv/get_next_frontier.hpp>
+
+#include <tf2_ros/buffer.h>
+#include <tf2/LinearMath/Quaternion.h>
+
+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<rclcpp::Node> node = std::make_shared<rclcpp::Node>("bounded_explore_layer");
+
+    geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw);
+
+    rclcpp::Service<frontier_msgs::srv::UpdateBoundaryPolygon>::SharedPtr polygonService_;
+    rclcpp::Service<frontier_msgs::srv::GetNextFrontier>::SharedPtr frontierService_;
+
+    // rclcpp_lifecycle::LifecycleNode::SharedPtr node = node_.lock();
+    // rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("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<rmw_request_id_t> request_header,
+        const std::shared_ptr<frontier_msgs::srv::UpdateBoundaryPolygon::Request> req, 
+        std::shared_ptr<frontier_msgs::srv::UpdateBoundaryPolygon::Response> 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<rmw_request_id_t> request_header,
+        const std::shared_ptr<frontier_msgs::srv::GetNextFrontier::Request> req,
+        std::shared_ptr<frontier_msgs::srv::GetNextFrontier::Response> 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<rclcpp::Parameter> parameters);
+
+    geometry_msgs::msg::Polygon polygon_;
+    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+
+    rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::PointCloud2>::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

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

frontier_exploration/package.xml

@@ -15,11 +15,15 @@
   <build_depend>geometry_msgs</build_depend>
   <build_depend>nav2_costmap_2d</build_depend>
   <build_depend>frontier_msgs</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>sensor_msgs</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>nav2_costmap_2d</exec_depend>
   <exec_depend>frontier_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>pcl_ros</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

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