From b084dc7549871f4b2329c2a4269fc906216db479 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 22 Jun 2024 21:26:27 +0200 Subject: [PATCH 1/2] exploration basics (not working) --- .../launch/navigation.launch.py | 2 +- .../rove_navigation/exploration.py | 140 ++++++++++++++++++ src/rove_navigation/setup.py | 1 + 3 files changed, 142 insertions(+), 1 deletion(-) create mode 100644 src/rove_navigation/rove_navigation/exploration.py diff --git a/src/rove_navigation/launch/navigation.launch.py b/src/rove_navigation/launch/navigation.launch.py index 69dd2d5..c2b5404 100644 --- a/src/rove_navigation/launch/navigation.launch.py +++ b/src/rove_navigation/launch/navigation.launch.py @@ -41,7 +41,7 @@ def generate_launch_description(): ) return LaunchDescription([ - person_following_node, + #person_following_node, GroupAction( actions=[ SetRemap(src='cmd_vel', dst='nav_vel'), diff --git a/src/rove_navigation/rove_navigation/exploration.py b/src/rove_navigation/rove_navigation/exploration.py new file mode 100644 index 0000000..8d00261 --- /dev/null +++ b/src/rove_navigation/rove_navigation/exploration.py @@ -0,0 +1,140 @@ +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient +from nav_msgs.msg import OccupancyGrid +from geometry_msgs.msg import Point, PoseStamped +from nav2_msgs.action import NavigateToPose +from std_msgs.msg import Bool +import numpy as np +from queue import Queue +from threading import Lock + +class FrontierExploration(Node): + def __init__(self): + super().__init__('frontier_exploration_node') + + self.map_subscription = self.create_subscription( + OccupancyGrid, 'map', self.map_callback, 10) + self.map_data = None + self.map_lock = Lock() + + self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + self.explore_timer = self.create_timer(1.0, self.explore_callback) + + self.map_received = False + + def map_callback(self, msg): + with self.map_lock: + self.map_data = msg + self.map_received = True + + + + 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 explore_callback(self): + if not self.map_received: + self.get_logger().info('Waiting for map data...') + return + + frontiers = self.find_frontiers() + if frontiers: + best_frontier = self.select_best_frontier(frontiers) + self.navigate_to_frontier(best_frontier) + else: + self.get_logger().info('No more frontiers to explore.') + + def select_best_frontier(self, frontiers): + best_score = -float('inf') + best_frontier = None + + for frontier in frontiers: + size = len(frontier) + score = self.score_frontier(size) + + if score > best_score: + best_score = score + best_frontier = frontier + + return best_frontier + + + + def calculate_centroid(self, frontier): + centroid_x = sum(p[0] for p in frontier) / len(frontier) + centroid_y = sum(p[1] for p in frontier) / len(frontier) + return Point(x=centroid_x, y=centroid_y) + + + def score_frontier(self, size): + # Adjust weights as needed. Higher `size_weight` favors larger frontiers. + # Higher `distance_weight` favors more distant frontiers. + size_weight = 1.0 + return size * size_weight + + def navigate_to_frontier(self, frontier): + # Estimate centroid as the target position + centroid_x = sum(p[0] for p in frontier) / len(frontier) + centroid_y = sum(p[1] for p in frontier) / len(frontier) + + # Convert to world coordinates + wx = centroid_x * self.map_data.info.resolution + self.map_data.info.origin.position.x + wy = centroid_y * self.map_data.info.resolution + self.map_data.info.origin.position.y + + goal_pose = PoseStamped() + goal_pose.header.frame_id = self.map_data.header.frame_id + goal_pose.pose.position = Point(x=wx, y=wy, z=0.0) + goal_pose.pose.orientation.w = 1.0 # Neutral orientation + + self.get_logger().info(f'Navigating to new frontier at ({wx}, {wy})') + + 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...') + +def main(args=None): + rclpy.init(args=args) + node = FrontierExploration() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/rove_navigation/setup.py b/src/rove_navigation/setup.py index 9e8d29f..1b36b94 100644 --- a/src/rove_navigation/setup.py +++ b/src/rove_navigation/setup.py @@ -28,6 +28,7 @@ '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', + 'exploration = rove_navigation.exploration:main', ], }, ) From 562691ca45a1663062ffcfb61ecc96b955eb72dd Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 22 Jun 2024 23:14:14 +0200 Subject: [PATCH 2/2] frontier exploration --- src/rove_bringup/launch/sim.launch.py | 26 +-- src/rove_description/config/basic.rviz | 48 ++++- .../launch/navigation.launch.py | 15 ++ .../rove_navigation/exploration.py | 180 +++++++----------- .../rove_navigation/frontier_publisher.py | 94 +++++++++ src/rove_navigation/setup.py | 1 + 6 files changed, 237 insertions(+), 127 deletions(-) create mode 100644 src/rove_navigation/rove_navigation/frontier_publisher.py diff --git a/src/rove_bringup/launch/sim.launch.py b/src/rove_bringup/launch/sim.launch.py index 52eb91e..3db90ea 100644 --- a/src/rove_bringup/launch/sim.launch.py +++ b/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, diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz index 3d4e9d3..881b1b1 100644 --- a/src/rove_description/config/basic.rviz +++ b/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: Value: Orbit (rviz) - Yaw: 2.9836294651031494 + Yaw: 4.8786468505859375 Saved: ~ Window Geometry: Camera: diff --git a/src/rove_navigation/launch/navigation.launch.py b/src/rove_navigation/launch/navigation.launch.py index c2b5404..79433f1 100644 --- a/src/rove_navigation/launch/navigation.launch.py +++ b/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), @@ -42,6 +56,7 @@ def generate_launch_description(): return LaunchDescription([ #person_following_node, + frontier_publisher_node, GroupAction( actions=[ SetRemap(src='cmd_vel', dst='nav_vel'), diff --git a/src/rove_navigation/rove_navigation/exploration.py b/src/rove_navigation/rove_navigation/exploration.py index 8d00261..093905f 100644 --- a/src/rove_navigation/rove_navigation/exploration.py +++ b/src/rove_navigation/rove_navigation/exploration.py @@ -1,137 +1,103 @@ import rclpy from rclpy.node import Node from rclpy.action import ActionClient -from nav_msgs.msg import OccupancyGrid +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 -from std_msgs.msg import Bool import numpy as np -from queue import Queue -from threading import Lock +from std_msgs.msg import Header +from sklearn.cluster import DBSCAN +from nav_msgs.msg import Odometry -class FrontierExploration(Node): +class NavigateToFrontier(Node): def __init__(self): - super().__init__('frontier_exploration_node') - - self.map_subscription = self.create_subscription( - OccupancyGrid, 'map', self.map_callback, 10) - self.map_data = None - self.map_lock = Lock() - + 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.explore_timer = self.create_timer(1.0, self.explore_callback) - - self.map_received = False - - def map_callback(self, msg): - with self.map_lock: - self.map_data = msg - self.map_received = True - - - - 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)) - + 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 = [] - 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) + 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 explore_callback(self): - if not self.map_received: - self.get_logger().info('Waiting for map data...') - return - - frontiers = self.find_frontiers() - if frontiers: - best_frontier = self.select_best_frontier(frontiers) - self.navigate_to_frontier(best_frontier) - else: - self.get_logger().info('No more frontiers to explore.') - - def select_best_frontier(self, frontiers): - best_score = -float('inf') - best_frontier = None - + def find_closest_frontier(self, frontiers): + min_distance = float('inf') + closest_frontier = None for frontier in frontiers: - size = len(frontier) - score = self.score_frontier(size) - - if score > best_score: - best_score = score - best_frontier = frontier - - return best_frontier - - - - def calculate_centroid(self, frontier): - centroid_x = sum(p[0] for p in frontier) / len(frontier) - centroid_y = sum(p[1] for p in frontier) / len(frontier) - return Point(x=centroid_x, y=centroid_y) - - - def score_frontier(self, size): - # Adjust weights as needed. Higher `size_weight` favors larger frontiers. - # Higher `distance_weight` favors more distant frontiers. - size_weight = 1.0 - return size * size_weight + 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): - # Estimate centroid as the target position - centroid_x = sum(p[0] for p in frontier) / len(frontier) - centroid_y = sum(p[1] for p in frontier) / len(frontier) - - # Convert to world coordinates - wx = centroid_x * self.map_data.info.resolution + self.map_data.info.origin.position.x - wy = centroid_y * self.map_data.info.resolution + self.map_data.info.origin.position.y - + # Prepare the goal message and send it goal_pose = PoseStamped() - goal_pose.header.frame_id = self.map_data.header.frame_id - goal_pose.pose.position = Point(x=wx, y=wy, z=0.0) - goal_pose.pose.orientation.w = 1.0 # Neutral orientation - - self.get_logger().info(f'Navigating to new frontier at ({wx}, {wy})') + 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...') + 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 = FrontierExploration() + node = NavigateToFrontier() rclpy.spin(node) node.destroy_node() rclpy.shutdown() diff --git a/src/rove_navigation/rove_navigation/frontier_publisher.py b/src/rove_navigation/rove_navigation/frontier_publisher.py new file mode 100644 index 0000000..0bc4a92 --- /dev/null +++ b/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() diff --git a/src/rove_navigation/setup.py b/src/rove_navigation/setup.py index 1b36b94..a4ae75a 100644 --- a/src/rove_navigation/setup.py +++ b/src/rove_navigation/setup.py @@ -28,6 +28,7 @@ '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', ], },