Exploration behavior

src/rove_bringup/launch/sim.launch.py

@@ -61,18 +61,18 @@ def generate_launch_description():
 
     # Spawn robot
     spawn_rove = Node(
-    package='ros_gz_sim',
-    executable='create',
-    arguments=[
-        '-name', 'rove',
-        '-topic', 'robot_description',
-        '-x', '0',
-        '-y', '0',
-        '-z', '0.1',
-        '-Y', str(yaw),
-    ],
-    output='screen',
-)
+        package='ros_gz_sim',
+        executable='create',
+        arguments=[
+            '-name', 'rove',
+            '-topic', 'robot_description',
+            '-x', '0',
+            '-y', '0',
+            '-z', '0.1',
+            '-Y', str(yaw),
+        ],
+        output='screen',
+    )
 
     # Takes the description and joint angles as inputs and publishes
     # the 3D poses of the robot links
@@ -120,9 +120,9 @@ def generate_launch_description():
     return LaunchDescription([
             gz_sim,
             bridge,
+            robot_state_publisher,
             spawn_walls,
             spawn_actor,
-            robot_state_publisher,
             spawn_rove,
             common,
             human_tracker,

src/rove_description/config/basic.rviz

@@ -4,7 +4,6 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /Global Options1
         - /DepthCloud1/Auto Size1
       Splitter Ratio: 0.5
     Tree Height: 325
@@ -422,6 +421,7 @@ Visualization Manager:
         TF: true
         Value: true
         filtered position: true
+        frontier_detection: true
       Zoom Factor: 1
     - Alpha: 1
       Class: rviz_default_plugins/PointStamped
@@ -438,6 +438,40 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /person_position
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 0; 255; 255
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 0
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: frontier_detection
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.5
+      Style: Flat Squares
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /frontier_points
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -484,25 +518,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 12.128095626831055
+      Distance: 36.84917449951172
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.19487914443016052
-        Y: -0.5717933773994446
-        Z: 2.147127151489258
+        X: 0.4458295702934265
+        Y: 3.5056796073913574
+        Z: -1.5910545587539673
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6703999042510986
+      Pitch: 0.914797306060791
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.9836294651031494
+      Yaw: 4.8786468505859375
     Saved: ~
 Window Geometry:
   Camera:

src/rove_navigation/launch/navigation.launch.py

@@ -31,6 +31,20 @@ def generate_launch_description():
             output='screen',
         )
 
+    frontier_publisher_node = Node(
+            package="rove_navigation",
+            executable='frontier_publisher',
+            name='frontier_publisher',
+            output='screen',
+        )
+
+    explorer_node = Node(
+        package='rove_navigation',
+        executable='exploration',
+        name='exploration',
+        output='screen',
+    )
+
 
     nav = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(navigation_launch_path),
@@ -41,7 +55,8 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
-        person_following_node,
+        #person_following_node,
+        frontier_publisher_node,
         GroupAction(
             actions=[
                 SetRemap(src='cmd_vel', dst='nav_vel'),

src/rove_navigation/rove_navigation/exploration.py

@@ -0,0 +1,106 @@
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+from sensor_msgs.msg import PointCloud2
+import sensor_msgs_py.point_cloud2 as pc2
+from geometry_msgs.msg import Point, PoseStamped
+from nav2_msgs.action import NavigateToPose
+import numpy as np
+from std_msgs.msg import Header
+from sklearn.cluster import DBSCAN
+from nav_msgs.msg import Odometry
+
+class NavigateToFrontier(Node):
+    def __init__(self):
+        super().__init__('navigate_to_frontier_node')
+        self.subscription = self.create_subscription(
+            PointCloud2, 'frontier_points', self.frontier_callback, 10)
+        self.odometry_subscription = self.create_subscription(
+            Odometry, '/odometry/local', self.odometry_callback, 10)
+        self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
+        self.min_frontier_size = 20  # Minimal number of points to consider a frontier valid
+        self.cluster_epsilon = 0.5  # Maximal distance between points in the same cluster
+        self.current_position = Point(x=0.0, y=0.0, z=0.0)  # Initial position
+
+    def odometry_callback(self, msg):
+        # Update current position based on odometry data
+        self.current_position.x = msg.pose.pose.position.x
+        self.current_position.y = msg.pose.pose.position.y
+        self.current_position.z = msg.pose.pose.position.z
+
+    def frontier_callback(self, msg):
+        # Parse the PointCloud2 message
+        points = list(pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True))
+        # Filter and cluster the points into frontiers
+        frontiers = self.process_frontiers(points)
+        if not frontiers:
+            self.get_logger().info('No valid frontiers found.')
+            return
+        # Calculate distances and find the closest frontier
+        closest_frontier = self.find_closest_frontier(frontiers)
+        if closest_frontier is not None:
+            self.navigate_to_frontier(closest_frontier)
+
+    def process_frontiers(self, points):
+        # Convert points to numpy array for clustering
+        points_array = np.array([[point[0], point[1]] for point in points])
+        # DBSCAN clustering
+        clustering = DBSCAN(eps=self.cluster_epsilon, min_samples=self.min_frontier_size).fit(points_array)
+        labels = clustering.labels_
+        # Number of clusters, ignoring noise if present.
+        n_clusters_ = len(set(labels)) - (1 if -1 in labels else 0)
+        self.get_logger().info(f'Number of clusters found: {n_clusters_}')
+        # Filter clusters to form frontiers
+        frontiers = []
+        for k in range(n_clusters_):
+            class_member_mask = (labels == k)
+            xy = points_array[class_member_mask]
+            if len(xy) > self.min_frontier_size:
+                frontiers.append(xy)
+        return frontiers
+
+    def find_closest_frontier(self, frontiers):
+        min_distance = float('inf')
+        closest_frontier = None
+        for frontier in frontiers:
+            centroid = np.mean(frontier, axis=0)
+            distance = np.linalg.norm(centroid - np.array([self.current_position.x, self.current_position.y]))
+            if distance < min_distance:
+                min_distance = distance
+                closest_frontier = frontier
+        return closest_frontier
+
+    def navigate_to_frontier(self, frontier):
+        # Prepare the goal message and send it
+        goal_pose = PoseStamped()
+        goal_pose.header = Header(frame_id='map')
+
+        x = float(np.mean(frontier[:, 0]))
+        y = float(np.mean(frontier[:, 1]))
+
+        goal_pose.pose.position = Point(x=x, y=y, z=0.0)
+        goal_pose.pose.orientation.w = 1.0  # Assuming no orientation preference
+
+        self.get_logger().info(f'Navigating to frontier at ({x}, {y})')
+        self.action_client.wait_for_server()
+        self.send_goal(goal_pose)
+
+    def send_goal(self, goal_pose):
+        goal_msg = NavigateToPose.Goal()
+        goal_msg.pose = goal_pose
+        self.action_client.send_goal_async(goal_msg)
+        self.get_logger().info('Goal sent to navigation system.')
+
+    def get_current_position(self):
+        # Return the current position from the odometry data
+        return self.current_position
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = NavigateToFrontier()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rove_navigation/rove_navigation/frontier_publisher.py

@@ -0,0 +1,94 @@
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import OccupancyGrid
+from sensor_msgs.msg import PointCloud2, PointField
+from std_msgs.msg import Header
+import numpy as np
+from queue import Queue
+from threading import Lock
+import sensor_msgs_py.point_cloud2 as pc2
+
+
+class FrontierExploration(Node):
+    def __init__(self):
+        super().__init__('frontier_exploration_node')
+        self.map_subscription = self.create_subscription(
+            OccupancyGrid, 'map', self.map_callback, 10)
+        self.frontier_pub = self.create_publisher(
+            PointCloud2, 'frontier_points', 10)
+
+        self.map_data = None
+        self.map_lock = Lock()
+
+    def map_callback(self, msg):
+        with self.map_lock:
+            self.map_data = msg
+        self.process_map_data()
+
+    def process_map_data(self):
+        if self.map_data is None:
+            return
+
+        frontiers = self.find_frontiers()
+        if frontiers:
+            largest_frontier = max(frontiers, key=len)
+            self.publish_frontier(largest_frontier)
+        else:
+            self.get_logger().info('No frontiers detected.')
+
+    def find_frontiers(self):
+        with self.map_lock:
+            map_array = np.array(self.map_data.data).reshape(
+                (self.map_data.info.height, self.map_data.info.width))
+
+        frontiers = []
+        visited = np.zeros_like(map_array, dtype=bool)
+        frontier_flag = np.zeros_like(map_array, dtype=bool)
+
+        for y in range(map_array.shape[0]):
+            for x in range(map_array.shape[1]):
+                if map_array[y, x] == 0 and not visited[y, x]:  # Free space
+                    queue = Queue()
+                    queue.put((x, y))
+                    new_frontier = []
+                    while not queue.empty():
+                        px, py = queue.get()
+                        if visited[py, px]:
+                            continue
+                        visited[py, px] = True
+                        for dx, dy in [(-1, 0), (1, 0), (0, -1), (0, 1)]:  # 4-connectivity
+                            nx, ny = px + dx, py + dy
+                            if 0 <= nx < map_array.shape[1] and 0 <= ny < map_array.shape[0]:
+                                if map_array[ny, nx] == -1 and not frontier_flag[ny, nx]:  # Unknown space
+                                    frontier_flag[ny, nx] = True
+                                    new_frontier.append((nx, ny))
+                                elif map_array[ny, nx] == 0 and not visited[ny, nx]:
+                                    queue.put((nx, ny))
+                    if new_frontier:
+                        frontiers.append(new_frontier)
+        return frontiers
+
+    def publish_frontier(self, frontier):
+        header = Header()
+        header.stamp = self.get_clock().now().to_msg()
+        header.frame_id = self.map_data.header.frame_id
+        points = [(x * self.map_data.info.resolution + self.map_data.info.origin.position.x,
+                   y * self.map_data.info.resolution + self.map_data.info.origin.position.y, 0.0) for x, y in frontier]
+        fields = [
+            PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
+            PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
+            PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
+        ]        
+        cloud = pc2.create_cloud(header,fields, points)
+        self.frontier_pub.publish(cloud)
+        self.get_logger().info(f'Published {len(frontier)} points representing the largest frontier.')
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = FrontierExploration()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/rove_navigation/setup.py

@@ -28,6 +28,8 @@
             'green_person_tracker = rove_navigation.green_person_tracker:main',
             'behavior_square_pattroling = rove_navigation.behavior_square_pattroling:main',
             'person_following = rove_navigation.person_following:main',
+            'frontier_publisher = rove_navigation.frontier_publisher:main',
+            'exploration = rove_navigation.exploration:main',
         ],
     },
 )