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 69dd2d5..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), @@ -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'), diff --git a/src/rove_navigation/rove_navigation/exploration.py b/src/rove_navigation/rove_navigation/exploration.py new file mode 100644 index 0000000..093905f --- /dev/null +++ b/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() 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 9e8d29f..a4ae75a 100644 --- a/src/rove_navigation/setup.py +++ b/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', ], }, )