diff --git a/src/Nav/aruco_localizer/config/aruco_board_params.yaml b/src/Nav/aruco_localizer/config/aruco_board_params.yaml index 9cf1c66a..e090b16f 100644 --- a/src/Nav/aruco_localizer/config/aruco_board_params.yaml +++ b/src/Nav/aruco_localizer/config/aruco_board_params.yaml @@ -12,7 +12,7 @@ aruco_board_detector_node: # DICT_7X7_50, DICT_7X7_100, DICT_7X7_250, DICT_7X7_1000, # DICT_ARUCO_ORIGINAL, DICT_APRILTAG_16h5, DICT_APRILTAG_25h9, # DICT_APRILTAG_36h10, DICT_APRILTAG_36h11 - aruco_dictionary_id: "DICT_6X6_250" + aruco_dictionary_id: "DICT_4X4_50" # Directory containing board configuration YAML files # This will be set by the launch file diff --git a/src/Nav/gps/launch/rover.launch.py b/src/Nav/gps/launch/rover.launch.py index 514fb8f7..90611321 100644 --- a/src/Nav/gps/launch/rover.launch.py +++ b/src/Nav/gps/launch/rover.launch.py @@ -3,7 +3,7 @@ import launch import launch_ros.actions from ament_index_python.packages import get_package_share_directory -from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.actions import ExecuteProcess, DeclareLaunchArgument from launch.conditions import IfCondition diff --git a/src/Nav/localization/config/ekf/gps_ekf_global.yaml b/src/Nav/localization/config/ekf/gps_ekf_global.yaml index 173ab1c2..3a6ddc15 100644 --- a/src/Nav/localization/config/ekf/gps_ekf_global.yaml +++ b/src/Nav/localization/config/ekf/gps_ekf_global.yaml @@ -62,16 +62,16 @@ global_ekf: # odom0_pose_rejection_threshold: 1000000.0 # odom0_twist_rejection_threshold: 1.0 - odom1: /drive/odom + odom1: /chassis_controller/wheel_odom odom1_config: [ # position false, false, false, # orientation false, false, false, # lineal vel - true, false, false, + true, true, false, # angular vel - false, false, false, + false, false, true, # lineal acc false, false, false, ] diff --git a/src/Nav/localization/config/ekf/gps_ekf_local.yaml b/src/Nav/localization/config/ekf/gps_ekf_local.yaml index 4b859927..ac265202 100644 --- a/src/Nav/localization/config/ekf/gps_ekf_local.yaml +++ b/src/Nav/localization/config/ekf/gps_ekf_local.yaml @@ -62,16 +62,16 @@ local_ekf: # odom0_pose_rejection_threshold: 1000000.0 # odom0_twist_rejection_threshold: 1.0 - odom1: /drive/odom + odom1: /chassis_controller/wheel_odom odom1_config: [ # position false, false, false, # orientation false, false, false, # lineal vel - true, false, false, + true, true, false, # angular vel - false, false, false, + false, false, true, # lineal acc false, false, false, ] diff --git a/src/Nav/localization/config/ekf/indoor_aruco_boards_ekf_global.yaml b/src/Nav/localization/config/ekf/indoor_aruco_boards_ekf_global.yaml index a6e02bb7..c01f7a5f 100644 --- a/src/Nav/localization/config/ekf/indoor_aruco_boards_ekf_global.yaml +++ b/src/Nav/localization/config/ekf/indoor_aruco_boards_ekf_global.yaml @@ -4,7 +4,7 @@ global_ekf: # sensor_timeout: 20.0 use_sim_time: false - two_d_mode: true + two_d_mode: false # transform_time_offset: 0.25 # transform_timeout: 3.0 @@ -49,16 +49,36 @@ global_ekf: pose0_rejection_threshold: 50.0 # High threshold to accept bootstrap poses with high covariance # pose0_pose_rejection_threshold: 5.0 # Old parameter name (deprecated) - odom0: /drive/odom + pose1: /zed/zed_node/pose_with_covariance + pose1_config: [ + # position + true, true, true, + # orientation + false, false, false, + # lineal vel + false, false, false, + # angular vel + false, false, false, + # lineal acc + false, false, false, + ] + # pose1_queue_size: 2 + # pose1_nodelay: false + pose1_differential: true + pose1_relative: false + pose1_pose_rejection_threshold: 5.0 # Mahalanobis Distance + # pose1_twist_rejection_threshold: 1.0 + + odom0: /chassis_controller/wheel_odom odom0_config: [ # position false, false, false, # orientation false, false, false, # lineal vel - true, false, false, + true, true, false, # angular vel - false, false, false, + false, false, true, # lineal acc false, false, false, ] diff --git a/src/Nav/localization/config/ekf/indoor_aruco_boards_ekf_local.yaml b/src/Nav/localization/config/ekf/indoor_aruco_boards_ekf_local.yaml index 084620f2..63e86aa0 100644 --- a/src/Nav/localization/config/ekf/indoor_aruco_boards_ekf_local.yaml +++ b/src/Nav/localization/config/ekf/indoor_aruco_boards_ekf_local.yaml @@ -4,7 +4,7 @@ local_ekf: # sensor_timeout: 20.0 use_sim_time: false - two_d_mode: true + two_d_mode: false # transform_time_offset: 0.25 # transform_timeout: 3.0 @@ -49,16 +49,36 @@ local_ekf: pose0_rejection_threshold: 50.0 # High threshold to accept bootstrap poses with high covariance # pose0_pose_rejection_threshold: 5.0 # Old parameter name (deprecated) - odom0: /drive/odom + pose1: /zed/zed_node/pose_with_covariance + pose1_config: [ + # position + true, true, false, + # orientation + false, false, false, + # lineal vel + false, false, false, + # angular vel + false, false, false, + # lineal acc + false, false, false, + ] + # pose1_queue_size: 2 + # pose1_nodelay: false + pose1_differential: true + pose1_relative: false + pose1_pose_rejection_threshold: 5.0 # Mahalanobis Distance + # pose1_twist_rejection_threshold: 1.0 + + odom0: /chassis_controller/wheel_odom odom0_config: [ # position false, false, false, # orientation false, false, false, # lineal vel - true, false, false, + true, true, false, # angular vel - false, false, false, + false, false, true, # lineal acc false, false, false, ] diff --git a/src/Nav/localization/localization/republish_odometry.py b/src/Nav/localization/localization/republish_odometry.py index 0f301ffc..67315743 100644 --- a/src/Nav/localization/localization/republish_odometry.py +++ b/src/Nav/localization/localization/republish_odometry.py @@ -76,7 +76,7 @@ def main(args=None): rclpy.init(args=args) # Set the frequency (in Hz) at which to republish the odometry - frequency = 10 # Change to your desired frequency (Hz) + frequency = 1.0 # Change to your desired frequency (Hz) # Create the node and start spinning node = OdometryRepublisher(frequency) diff --git a/src/Nav/nav_commanders/nav_commanders/nav_then_search.py b/src/Nav/nav_commanders/nav_commanders/nav_then_search.py new file mode 100644 index 00000000..52d5d0d3 --- /dev/null +++ b/src/Nav/nav_commanders/nav_commanders/nav_then_search.py @@ -0,0 +1,426 @@ +#!/usr/bin/env python3 + +import math +from enum import Enum, auto +from threading import Event +from collections import Counter + +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup +from rclpy.qos import ( + QoSProfile, + ReliabilityPolicy, + DurabilityPolicy, + qos_profile_sensor_data, +) + +from geometry_msgs.msg import PoseStamped, Quaternion +from nav_msgs.msg import Odometry, Path +from std_msgs.msg import Int8, Int32MultiArray +from std_srvs.srv import Trigger + +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from robot_localization.srv import FromLL +from interfaces.srv import NavToGPSGeopose + + +class MissionState(Enum): + DO_NOTHING = auto() + NAV_TO_START = auto() + EXECUTE_SEARCH = auto() + FOUND_TARGET = auto() + NAV_CANCELLED = auto() + + +class SearchPatternCommander(Node): + def __init__(self): + super().__init__("search_pattern_commander") + self.navigator = BasicNavigator("search_pattern_navigator") + self.mission_state = MissionState.DO_NOTHING + + # --- Parameters --- + self.declare_parameter("object_topic", "/aruco_detections") + self.object_topic = ( + self.get_parameter("object_topic").get_parameter_value().string_value + ) + + self.declare_parameter("search_size_meters", 20.0) + self.search_size = ( + self.get_parameter("search_size_meters").get_parameter_value().double_value + ) + + self.declare_parameter("lane_spacing_meters", 4.0) + self.lane_spacing = ( + self.get_parameter("lane_spacing_meters").get_parameter_value().double_value + ) + + self.declare_parameter("frequency", 1 / 5.0) + self.frequency = ( + self.get_parameter("frequency").get_parameter_value().double_value + ) + + self.declare_parameter("robot_pose_topic", "odometry/filtered/global") + self.robot_pose_topic = ( + self.get_parameter("robot_pose_topic").get_parameter_value().string_value + ) + + self.declare_parameter("detection_window_sec", 5.0) + self.detection_window_sec = ( + self.get_parameter("detection_window_sec") + .get_parameter_value() + .double_value + ) + + self.declare_parameter("min_detections", 5) + self.min_detections = ( + self.get_parameter("min_detections").get_parameter_value().integer_value + ) + + self.qos_profile = QoSProfile(reliability=ReliabilityPolicy.RELIABLE, depth=10) + + # Transient Local QoS for the Path so it stays visible in RViz + self.path_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + ) + + # --- Topic Names and Service Names --- + self.nav_fix_service_name = "fromLL" + self.geopose_service_name = "commander/nav_to_gps_geopose" + self.cancel_nav_service_name = "commander/cancel_nav" + self.lights_topic = "/light" + self.intended_path_topic = "/intended_search_path" + + self.nav_activate_light_code = 1 + self.nav_completed_light_code = 3 + self.nav_cancelled_light_code = 2 + + # --- Callback Groups --- + self.localizer_callback_group = MutuallyExclusiveCallbackGroup() + self.pose_callback_group = ReentrantCallbackGroup() + self.goal_callback_group = MutuallyExclusiveCallbackGroup() + self.timer_group = MutuallyExclusiveCallbackGroup() + self.lights_callback_group = MutuallyExclusiveCallbackGroup() + self.detection_callback_group = ReentrantCallbackGroup() + self.cancel_service_callback_group = MutuallyExclusiveCallbackGroup() + + # --- Clients, Services, Subscriptions, Publishers --- + self.localizer_client = self.create_client( + FromLL, + self.nav_fix_service_name, + callback_group=self.localizer_callback_group, + ) + self.geopose_service = self.create_service( + NavToGPSGeopose, + self.geopose_service_name, + self.geopose_server, + callback_group=self.goal_callback_group, + ) + self.cancel_nav_service = self.create_service( + Trigger, + self.cancel_nav_service_name, + self.cancel_nav_server, + callback_group=self.cancel_service_callback_group, + ) + self.robot_pose_subscription = self.create_subscription( + Odometry, + self.robot_pose_topic, + self.robot_pose_callback, + qos_profile_sensor_data, + callback_group=self.pose_callback_group, + ) + self.lights_publisher = self.create_publisher( + Int8, + self.lights_topic, + qos_profile=self.qos_profile, + callback_group=self.lights_callback_group, + ) + + # New Publisher for RViz full path visualization + self.path_publisher = self.create_publisher( + Path, self.intended_path_topic, qos_profile=self.path_qos + ) + + # --- Object Detection --- + self.detections_buffer = [] + self.detection_subscription = self.create_subscription( + Int32MultiArray, + self.object_topic, + self.detection_callback, + qos_profile_sensor_data, + callback_group=self.detection_callback_group, + ) + + # --- State Variables --- + self.final_lat_lon = None + self.current_robot_pose = None + self.goal_handle = None + self.stop_triggered_id = None + self.search_poses = [] + + # --- Timer --- + self.timer = self.create_timer( + 1.0 / self.frequency, self.timer_callback, callback_group=self.timer_group + ) + + # --- Initialization Checks --- + self.get_logger().info( + f"Waiting for {self.nav_fix_service_name} to be active..." + ) + while not self.localizer_client.wait_for_service(timeout_sec=2.0): + self.get_logger().info("Waiting...") + self.get_logger().info(f"{self.nav_fix_service_name} is active") + + self.get_logger().info("Waiting for Nav2 to be active...") + self.navigator.waitUntilNav2Active(localizer="controller_server") + self.get_logger().info("Nav2 is active. Ready to generate search patterns.") + + def robot_pose_callback(self, msg: Odometry): + self.current_robot_pose = msg.pose + + def detection_callback(self, msg: Int32MultiArray): + if self.mission_state not in [ + MissionState.NAV_TO_START, + MissionState.EXECUTE_SEARCH, + ]: + return + + current_time = self.get_clock().now().nanoseconds / 1e9 + self.detections_buffer = [ + (ts, tag_id) + for ts, tag_id in self.detections_buffer + if (current_time - ts) <= self.detection_window_sec + ] + + for detected_id in msg.data: + self.detections_buffer.append((current_time, detected_id)) + + tag_ids_in_window = [tag_id for ts, tag_id in self.detections_buffer] + tag_counts = Counter(tag_ids_in_window) + + for tag_id, count in tag_counts.items(): + if count >= self.min_detections: + self.get_logger().info(f"Object ID {tag_id} detected! Stopping search.") + self.mission_state = MissionState.FOUND_TARGET + self.stop_triggered_id = tag_id + self.navigator.cancelTask() + self.reset_state_variables() + + light_msg = Int8() + light_msg.data = self.nav_completed_light_code + self.lights_publisher.publish(light_msg) + return + + def geopose_server( + self, request: NavToGPSGeopose.Request, response: NavToGPSGeopose.Response + ) -> NavToGPSGeopose.Response: + self.get_logger().info(f"Received new GPS search zone: {request.goal}") + self.reset_state_variables() + self.final_lat_lon = ( + request.goal.position.latitude, + request.goal.position.longitude, + request.goal.position.altitude, + request.goal.orientation, + ) + self.mission_state = MissionState.NAV_TO_START + response.success = True + self.timer.reset() + + msg = Int8() + msg.data = self.nav_activate_light_code + self.lights_publisher.publish(msg) + return response + + def cancel_nav_server( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + self.get_logger().info("Received request to cancel search.") + self.navigator.cancelTask() + self.reset_state_variables() + self.mission_state = MissionState.NAV_CANCELLED + + msg = Int8() + msg.data = self.nav_cancelled_light_code + self.lights_publisher.publish(msg) + + response.success = True + response.message = "Search cancelled by user." + return response + + def convert_lat_lon_to_pose(self, latitude, longitude, altitude, orientation): + req = FromLL.Request() + req.ll_point.longitude = longitude + req.ll_point.latitude = latitude + req.ll_point.altitude = altitude + + try: + event = Event() + + def done_callback(future): + nonlocal event + event.set() + + future = self.localizer_client.call_async(req) + future.add_done_callback(done_callback) + event.wait(timeout=5.0) + + if not future.done(): + self.get_logger().error("fromLL service timeout.") + return None + + result = future.result() + target_pose = PoseStamped() + target_pose.header.frame_id = "map" + target_pose.header.stamp = self.get_clock().now().to_msg() + target_pose.pose.position = result.map_point + target_pose.pose.orientation = orientation + return target_pose + except Exception as e: + self.get_logger().error(f"Error during lat/lon conversion: {e}") + return None + + def yaw_to_quaternion(self, yaw): + q = Quaternion() + q.x, q.y = 0.0, 0.0 + q.z = math.sin(yaw / 2.0) + q.w = math.cos(yaw / 2.0) + return q + + def publish_intended_path(self, poses): + """Publishes the full array of search poses to RViz.""" + path_msg = Path() + path_msg.header.frame_id = "map" + path_msg.header.stamp = self.get_clock().now().to_msg() + path_msg.poses = poses + self.path_publisher.publish(path_msg) + self.get_logger().info( + f"Published intended search path to {self.intended_path_topic}" + ) + + def generate_search_pattern(self, center_pose: PoseStamped): + poses = [] + c_x = center_pose.pose.position.x + c_y = center_pose.pose.position.y + half_size = self.search_size / 2.0 + + start_x = c_x - half_size + start_y = c_y - half_size + num_lanes = int(self.search_size / self.lane_spacing) + 1 + + for i in range(num_lanes): + current_y = start_y + (i * self.lane_spacing) + if i % 2 == 0: + x_start, x_end, yaw = start_x, start_x + self.search_size, 0.0 + else: + x_start, x_end, yaw = start_x + self.search_size, start_x, math.pi + + pose_start = PoseStamped() + pose_start.header.frame_id = "map" + pose_start.pose.position.x = x_start + pose_start.pose.position.y = current_y + pose_start.pose.orientation = self.yaw_to_quaternion(yaw) + poses.append(pose_start) + + pose_end = PoseStamped() + pose_end.header.frame_id = "map" + pose_end.pose.position.x = x_end + pose_end.pose.position.y = current_y + pose_end.pose.orientation = self.yaw_to_quaternion(yaw) + poses.append(pose_end) + + self.get_logger().info( + f"Generated {len(poses)} waypoints for {self.search_size}x{self.search_size}m search area." + ) + + # Immediately publish to RViz so it's clear what the rover is attempting + self.publish_intended_path(poses) + return poses + + def reset_state_variables(self): + self.goal_handle = None + self.final_lat_lon = None + self.detections_buffer = [] + self.stop_triggered_id = None + self.search_poses = [] + + # Clear the RViz path when resetting + empty_path = Path() + empty_path.header.frame_id = "map" + self.path_publisher.publish(empty_path) + + def handle_task_result(self, success_state): + result = self.navigator.getResult() + if result == TaskResult.SUCCEEDED: + if success_state == MissionState.EXECUTE_SEARCH: + self.get_logger().info( + "Arrived at start point. Beginning search pattern!" + ) + self.mission_state = MissionState.EXECUTE_SEARCH + self.goal_handle = None + else: + self.get_logger().info( + "Search pattern fully completed without finding object." + ) + self.mission_state = MissionState.DO_NOTHING + self.reset_state_variables() + msg = Int8() + msg.data = self.nav_completed_light_code + self.lights_publisher.publish(msg) + + elif result == TaskResult.CANCELED: + if self.stop_triggered_id is not None: + self.get_logger().info( + f"Mission cancelled by target detection (ID: {self.stop_triggered_id})." + ) + else: + self.get_logger().info("Mission canceled by user.") + elif result == TaskResult.FAILED: + self.get_logger().error("Navigation task failed!") + self.mission_state = MissionState.DO_NOTHING + self.reset_state_variables() + + def timer_callback(self): + if self.mission_state == MissionState.NAV_TO_START: + if not self.search_poses: + center_pose = self.convert_lat_lon_to_pose(*self.final_lat_lon) + if not center_pose: + return + + self.search_poses = self.generate_search_pattern(center_pose) + + self.get_logger().info("Navigating to the search start point...") + self.goal_handle = self.navigator.goToPose(self.search_poses[0]) + return + + if self.navigator.isTaskComplete(): + self.handle_task_result(success_state=MissionState.EXECUTE_SEARCH) + + elif self.mission_state == MissionState.EXECUTE_SEARCH: + if self.goal_handle is None: + self.get_logger().info("Executing goThroughPoses for search grid...") + self.goal_handle = self.navigator.goThroughPoses(self.search_poses[1:]) + return + + if self.navigator.isTaskComplete(): + self.handle_task_result(success_state=MissionState.DO_NOTHING) + + +def main(args=None): + rclpy.init(args=args) + node = SearchPatternCommander() + executor = MultiThreadedExecutor() + executor.add_node(node) + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/Nav/nav_commanders/nav_commanders/nav_to_gps_aruco.py b/src/Nav/nav_commanders/nav_commanders/nav_to_gps_aruco.py index 479b7f27..2684bf14 100644 --- a/src/Nav/nav_commanders/nav_commanders/nav_to_gps_aruco.py +++ b/src/Nav/nav_commanders/nav_commanders/nav_to_gps_aruco.py @@ -11,7 +11,7 @@ from geometry_msgs.msg import PoseStamped from nav_msgs.msg import Odometry -from std_msgs.msg import Int8, Int32MultiArray +from std_msgs.msg import Int8, Int32 from std_srvs.srv import Trigger from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult @@ -42,7 +42,7 @@ def __init__(self): self.mission_state = MissionState.DO_NOTHING # --- Parameters --- - self.declare_parameter("aruco_topic", "/aruco_detections") + self.declare_parameter("aruco_topic", "/marker_detected") self.aruco_topic = ( self.get_parameter("aruco_topic").get_parameter_value().string_value ) @@ -148,7 +148,7 @@ def __init__(self): # Stores (timestamp, detected_id) tuples for all detected tags within the defined window. self.aruco_detections_buffer = [] self.aruco_subscription = self.create_subscription( - Int32MultiArray, + Int32, self.aruco_topic, self.aruco_callback, qos_profile_sensor_data, @@ -191,7 +191,7 @@ def robot_pose_callback(self, msg: Odometry): """Callback to update the current robot pose from odometry.""" self.current_robot_pose = msg.pose - def aruco_callback(self, msg: Int32MultiArray): + def aruco_callback(self, msg: Int32): """ Callback for Aruco tag detections. Tracks the frequency of all detected tags within a configurable sliding window. @@ -212,12 +212,12 @@ def aruco_callback(self, msg: Int32MultiArray): if (current_time - ts) <= self.aruco_detection_window_sec ] - # Add new detections from the current message to the buffer - for detected_id in msg.data: - self.aruco_detections_buffer.append((current_time, detected_id)) - self.get_logger().debug( - f"Aruco ID {detected_id} detected. Buffer size: {len(self.aruco_detections_buffer)}" - ) + # Add new detection from the current message to the buffer (single marker ID) + detected_id = msg.data + self.aruco_detections_buffer.append((current_time, detected_id)) + self.get_logger().debug( + f"Aruco ID {detected_id} detected. Buffer size: {len(self.aruco_detections_buffer)}" + ) # Count occurrences of each unique tag ID in the current detection window tag_ids_in_window = [tag_id for ts, tag_id in self.aruco_detections_buffer] diff --git a/src/Nav/nav_commanders/nav_commanders/search_pattern_viz.py b/src/Nav/nav_commanders/nav_commanders/search_pattern_viz.py new file mode 100644 index 00000000..f39bed24 --- /dev/null +++ b/src/Nav/nav_commanders/nav_commanders/search_pattern_viz.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 + +import math +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped, Quaternion +from nav_msgs.msg import Path + + +class SearchPatternVisualizer(Node): + """ + A simple ROS2 node that generates a lawnmower search pattern + and continuously publishes it as a Path message for RViz visualization. + """ + + def __init__(self): + super().__init__("search_pattern_visualizer") + + # Publisher for the path topic + self.path_pub = self.create_publisher(Path, "/search_pattern_path", 10) + + # Grid parameters (Modify these to instantly see changes in RViz) + self.search_size = 20.0 # 20x20 meter grid + self.lane_spacing = 4.0 # 4 meters between lanes + + # Publish at 1Hz so it remains persistent in RViz + self.timer = self.create_timer(1.0, self.publish_path) + self.get_logger().info( + "Publishing search pattern to '/search_pattern_path' at 1Hz..." + ) + + def yaw_to_quaternion(self, yaw): + """Converts a yaw angle (in radians) to a geometry_msgs Quaternion.""" + q = Quaternion() + q.x, q.y = 0.0, 0.0 + q.z = math.sin(yaw / 2.0) + q.w = math.cos(yaw / 2.0) + return q + + def publish_path(self): + path_msg = Path() + path_msg.header.frame_id = "map" + path_msg.header.stamp = self.get_clock().now().to_msg() + + # Center of the search area (0,0 for visualization purposes) + center_x, center_y = 0.0, 0.0 + half_size = self.search_size / 2.0 + + start_x = center_x - half_size + start_y = center_y - half_size + num_lanes = int(self.search_size / self.lane_spacing) + 1 + + for i in range(num_lanes): + current_y = start_y + (i * self.lane_spacing) + + # Alternate directions for the boustrophedon sweep + if i % 2 == 0: + x_start, x_end, yaw = start_x, start_x + self.search_size, 0.0 + else: + x_start, x_end, yaw = start_x + self.search_size, start_x, math.pi + + # Entry point of the current lane + pose_start = PoseStamped() + pose_start.header = path_msg.header + pose_start.pose.position.x = x_start + pose_start.pose.position.y = current_y + pose_start.pose.orientation = self.yaw_to_quaternion(yaw) + path_msg.poses.append(pose_start) + + # Exit point of the current lane + pose_end = PoseStamped() + pose_end.header = path_msg.header + pose_end.pose.position.x = x_end + pose_end.pose.position.y = current_y + pose_end.pose.orientation = self.yaw_to_quaternion(yaw) + path_msg.poses.append(pose_end) + + # Send to RViz + self.path_pub.publish(path_msg) + + +def main(args=None): + rclpy.init(args=args) + node = SearchPatternVisualizer() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/Nav/nav_commanders/setup.py b/src/Nav/nav_commanders/setup.py index 10cff416..91073a74 100644 --- a/src/Nav/nav_commanders/setup.py +++ b/src/Nav/nav_commanders/setup.py @@ -25,6 +25,8 @@ "gps_commander_node = nav_commanders.nav_to_gps_coords:main", "incremental_gps_commander_node = nav_commanders.incremental_gps_commander:main", "aruco_gps_commander_node = nav_commanders.nav_to_gps_aruco:main", + "nav_then_search_commander_node = nav_commanders.nav_then_search:main", + "search_pattern_viz_node = nav_commanders.search_pattern_viz:main", ], }, ) diff --git a/src/Nav/navigation/behavior_trees/bt_swerve_dynamic_replanning.xml b/src/Nav/navigation/behavior_trees/bt_swerve_dynamic_replanning.xml index e5be3123..a04b3a34 100644 --- a/src/Nav/navigation/behavior_trees/bt_swerve_dynamic_replanning.xml +++ b/src/Nav/navigation/behavior_trees/bt_swerve_dynamic_replanning.xml @@ -44,7 +44,7 @@ - + diff --git a/src/Nav/navigation/config/elevation_mapping/postprocessing_traversability.yaml b/src/Nav/navigation/config/elevation_mapping/postprocessing_traversability.yaml index bc2e4020..91c74dcc 100644 --- a/src/Nav/navigation/config/elevation_mapping/postprocessing_traversability.yaml +++ b/src/Nav/navigation/config/elevation_mapping/postprocessing_traversability.yaml @@ -69,7 +69,7 @@ elevation_mapping: type: gridMapFilters/MathExpressionFilter params: output_layer: traversability - expression: 0.13 * (slope / 0.6) + 0.24 * (roughness / 0.1) # roughness: 0.3 + expression: 0.7 * (slope / 0.6) + 0.45 * (roughness / 0.1) # roughness: 0.3 # Set lower threshold on traversability. filter7: diff --git a/src/Nav/navigation/nav_launch.md b/src/Nav/navigation/nav_launch.md new file mode 100644 index 00000000..26683fe0 --- /dev/null +++ b/src/Nav/navigation/nav_launch.md @@ -0,0 +1,11 @@ +ros2 launch navigation slam.launch.py > slam_log.txt 2>&1 & +ros2 run localization repub_odom > rpub_odom.txt 2>&1 & +ros2 launch navigation nav2.launch.py > nav2.log 2>&1 & +ros2 launch video_streaming video_streaming.launch.py > video.log 2>&1 & + +ros2 launch bringup control.launch.py + +python3 send_gps_command.py --file p7_feb22_2026_aruco.yaml +ros2 launch nav_commanders incremental_gps_commander.launch.py + +ros2 param set /detect_node detection_type "ARUCO" \ No newline at end of file diff --git a/src/Nav/navigation/params/nav2.yaml b/src/Nav/navigation/params/nav2.yaml index 8023985e..93c3bff1 100644 --- a/src/Nav/navigation/params/nav2.yaml +++ b/src/Nav/navigation/params/nav2.yaml @@ -90,10 +90,11 @@ global_costmap: global_costmap: ros__parameters: update_frequency: 10.0 - publish_frequency: 0.5 + publish_frequency: 2.0 global_frame: map robot_base_frame: base_link rolling_window: true + transform_tolerance: 0.3 height: 100 width: 100 origin_x: -50.0 @@ -145,11 +146,11 @@ planner_server: analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) - minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle (Swerve can turn tighter) + minimum_turning_radius: 0.3 # minimum turning radius in m of path / vehicle (Swerve can turn tighter) reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 - cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + cost_penalty: 1.5 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. retrospective_penalty: 0.015 lookup_table_size: 40.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. cache_obstacle_heuristic: true # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. @@ -161,21 +162,51 @@ planner_server: smoother: max_iterations: 1000 - w_smooth: 0.3 + w_smooth: 0.6 # Increased from 0.3 to produce smoother initial paths w_data: 0.2 tolerance: 1.0e-10 do_refinement: true - refinement_num: 2 + refinement_num: 4 # Increased from 2 for better initial smoothing smoother_server: ros__parameters: use_sim_time: True - smoother_plugins: [simple_smoother] + # smoother_plugins: [simple_smoother] + smoother_plugins: [constrained_smoother] simple_smoother: plugin: nav2_smoother::SimpleSmoother tolerance: 1.0e-10 max_its: 1000 do_refinement: True + constrained_smoother: + plugin: "nav2_constrained_smoother/ConstrainedSmoother" + reversing_enabled: false # whether to detect forward/reverse direction and cusps. Should be set to false for paths without orientations assigned + path_downsampling_factor: 5 # every n-th node of the path is taken (increased from 3 to speed up smoothing) + path_upsampling_factor: 3 # 0 - path remains downsampled, 1 - path is upsampled back to original granularity using cubic bezier, 2... - more upsampling + keep_start_orientation: true # whether to prevent the start orientation from being smoothed + keep_goal_orientation: true # whether to prevent the gpal orientation from being smoothed + minimum_turning_radius: 0.1 # minimum turning radius the robot can perform. Can be set to 0.0 (or w_curve can be set to 0.0 with the same effect) for diff-drive/holonomic robots + w_curve: 150.0 # weight to enforce minimum_turning_radius (increased to strongly prefer larger ~0.8m arcs but allow 0.1m when obstacles require) + w_dist: 0.0 # weight to bind path to original as optional replacement for cost weight + w_smooth: 2000000.0 # weight to maximize smoothness of path + w_cost: 0.5 # weight to steer robot away from collision and cost (HIGH value to strongly respect elevation/traversability costmap and force tighter turns in rough terrain) + + # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes) + # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification + w_cost_cusp_multiplier: 1.0 # option to have higher weight during forward/reverse direction change which is often accompanied with dangerous rotations (set to 1.0 since reversing disabled) + cusp_zone_length: 2.5 # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight equals w_cost*w_cost_cusp_multiplier) + + # Points in robot frame to grab costmap values from. Format: [x1, y1, weight1, x2, y2, weight2, ...] + # IMPORTANT: Requires much higher number of iterations to actually improve the path. Uncomment only if you really need it (highly elongated/asymmetric robots) + # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification + # cost_check_points: [-0.185, 0.0, 1.0] + + optimizer: + max_iterations: 50 # max iterations of smoother (reduced from 70 for faster convergence) + debug_optimizer: false # print debug info + gradient_tol: 0.01 # Relaxed from 0.005 for faster convergence + fn_tol: 1.0e-10 # Relaxed from 1.0e-15 for faster convergence + param_tol: 1.0e-15 # Relaxed from 1.0e-20 for faster convergence controller_server: ros__parameters: @@ -197,18 +228,20 @@ controller_server: general_goal_checker: stateful: True plugin: nav2_controller::SimpleGoalChecker - xy_goal_tolerance: 0.5 + xy_goal_tolerance: 0.6 yaw_goal_tolerance: 0.2 # ~11 degrees FollowPath: - plugin: nav2_rotation_shim_controller::RotationShimController - primary_controller: nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + # plugin: nav2_rotation_shim_controller::RotationShimController + # primary_controller: nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + plugin: nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController + # Rotation Shim Parameters (Swerve Logic - Point Turn Before Driving) - angular_dist_threshold: 0.5 # If path angle > 30deg, rotate in place first + angular_dist_threshold: 0.3 # If path angle > 30deg, rotate in place first forward_sampling_distance: 0.5 # How far ahead to check path orientation rotate_to_heading_angular_vel: 1.0 # Angular velocity during point turn - max_angular_accel: 2.0 # Max angular acceleration + max_angular_accel: 0.8 # Max angular acceleration simulate_ahead_time: 1.0 # Simulation time for collision checking use_rotate_to_heading: false # Disable since using rotation shim controller @@ -216,7 +249,7 @@ controller_server: # Regulated Pure Pursuit Parameters (Driving Logic) transform_tolerance: 1.0 - desired_linear_vel: 0.3 # Increase to 1.0 for outdoor swerve agility + desired_linear_vel: 0.35 # Increase to 1.0 for outdoor swerve agility max_robot_pose_search_dist: 10.0 lookahead_dist: 1.2 @@ -247,13 +280,13 @@ behavior_server: cycle_frequency: 0.4 behavior_plugins: [spin, backup, drive_on_heading, wait] spin: - plugin: nav2_behaviors::Spin + plugin: nav2_behaviors/Spin backup: - plugin: nav2_behaviors::BackUp + plugin: nav2_behaviors/BackUp drive_on_heading: - plugin: nav2_behaviors::DriveOnHeading + plugin: nav2_behaviors/DriveOnHeading wait: - plugin: nav2_behaviors::Wait + plugin: nav2_behaviors/Wait global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 @@ -295,8 +328,8 @@ velocity_smoother: # max_decel: [-0.5, 0.0, -1.5] # INDOOR TESTING # Safe limits for initial testing in confined spaces - max_velocity: [0.3, 0.0, 0.5] # [x, y, theta] in m/s, m/s, rad/s - min_velocity: [-0.3, 0.0, -0.5] - max_accel: [0.2, 0.0, 0.5] # [x, y, theta] in m/s², m/s², rad/s² - max_decel: [-0.2, 0.0, -0.5] + max_velocity: [0.6, 0.0, 0.6] # [x, y, theta] in m/s, m/s, rad/s + min_velocity: [-0.6, 0.0, -0.6] + max_accel: [0.3, 0.0, 0.5] # [x, y, theta] in m/s², m/s², rad/s² + max_decel: [-0.3, 0.0, -0.5] diff --git a/src/Nav/navigation/rviz/costmap_trav.rviz b/src/Nav/navigation/rviz/costmap_trav.rviz new file mode 100644 index 00000000..50a693cf --- /dev/null +++ b/src/Nav/navigation/rviz/costmap_trav.rviz @@ -0,0 +1,421 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1/Frames1 + - /TF1/Tree1 + - /TraversibilityMap1 + - /Path2 + Splitter Ratio: 0.5423280596733093 + Tree Height: 725 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + Antenna_Back: + Value: false + Antenna_Front: + Value: false + EndEffector: + Value: false + Frame: + Value: false + GPS_Left: + Value: false + GPS_Right: + Value: false + Link_1: + Value: false + Link_2: + Value: false + Link_3: + Value: false + Link_4: + Value: false + Link_5: + Value: false + Link_6: + Value: false + Rocker_Left: + Value: false + Rocker_Right: + Value: false + Sus_Arm_Back_Left: + Value: false + Sus_Arm_Back_Right: + Value: false + Sus_Arm_Front_Left: + Value: false + Sus_Arm_Front_Right: + Value: false + Wheel_Arm_Back_Left: + Value: false + Wheel_Arm_Back_Right: + Value: false + Wheel_Arm_Front_Left: + Value: false + Wheel_Arm_Front_Right: + Value: false + Wheel_Back_Left: + Value: false + Wheel_Back_Right: + Value: false + Wheel_Front_Left: + Value: false + Wheel_Front_Right: + Value: false + Zed: + Value: false + base_link: + Value: true + gps_link: + Value: false + imu_link: + Value: false + lidar_link: + Value: false + map: + Value: false + odom: + Value: false + zed_camera_center: + Value: true + zed_camera_link: + Value: false + zed_imu_link: + Value: false + zed_left_camera_frame: + Value: false + zed_left_camera_optical_frame: + Value: false + zed_right_camera_frame: + Value: false + zed_right_camera_optical_frame: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_link: + Frame: + Antenna_Back: + {} + Antenna_Front: + {} + GPS_Left: + {} + GPS_Right: + {} + Rocker_Left: + Sus_Arm_Back_Left: + Wheel_Arm_Back_Left: + Wheel_Back_Left: + {} + Sus_Arm_Front_Left: + Wheel_Arm_Front_Left: + Wheel_Front_Left: + {} + Rocker_Right: + Sus_Arm_Back_Right: + Wheel_Arm_Back_Right: + Wheel_Back_Right: + {} + Sus_Arm_Front_Right: + Wheel_Arm_Front_Right: + Wheel_Front_Right: + {} + Zed: + {} + Link_1: + Link_2: + Link_3: + Link_4: + Link_5: + Link_6: + EndEffector: + {} + gps_link: + {} + imu_link: + {} + lidar_link: + {} + zed_camera_link: + zed_camera_center: + zed_left_camera_frame: + zed_imu_link: + {} + zed_left_camera_optical_frame: + {} + zed_right_camera_frame: + zed_right_camera_optical_frame: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: traversability + Color Transformer: IntensityLayer + Enabled: true + Height Layer: elevation + Height Transformer: Layer + History Length: 1 + Invert Rainbow: false + Max Color: 0; 255; 0 + Max Intensity: 1 + Min Color: 170; 0; 0 + Min Intensity: 0 + Name: TraversibilityMap + Show Grid Lines: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /traversability_map + Use Rainbow: true + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: GlobalMap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /aruco_markers_viz + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /unsmoothed_plan + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 23.05002212524414 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 3.423001289367676 + Y: 2.539494276046753 + Z: 0.7042691707611084 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.399796485900879 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.1303207874298096 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 1016 + Hide Left Dock: true + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016d0000035efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b0000035e000000c700fffffffb0000000a0049006d0061006700650000000298000001010000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001750000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ea0000003efc0100000002fb0000000800540069006d00650100000000000003ea0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000003ea0000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1002 + X: 70 + Y: 27 diff --git a/src/Nav/navigation/scripts/record_nav_bag.sh b/src/Nav/navigation/scripts/record_nav_bag.sh new file mode 100755 index 00000000..7caa301d --- /dev/null +++ b/src/Nav/navigation/scripts/record_nav_bag.sh @@ -0,0 +1,26 @@ +#!/bin/bash + +# Define the bag name with a timestamp so you don't overwrite previous bags +BAG_NAME="autonomy_debug_bag_$(date +%Y_%m_%d_%H_%M_%S)" + +echo "Starting ROS 2 bag recording: $BAG_NAME" +echo "Press Ctrl+C to stop recording." + +# Run the ros2 bag record command +ros2 bag record -o "$BAG_NAME" \ + /odometry/filtered/global \ + /global_costmap/costmap \ + /global_costmap/costmap_updates \ + /traversability_map \ + /plan \ + /unsmoothed_plan \ + /goal_pose \ + /goal \ + /zed/zed_node/left/image_rect_color/compressed \ + /zed/zed_node/left/camera_info \ + /tf \ + /tf_static \ + /marker_detected \ + /intended_search_path + +# Note: /tf and /tf_static were added (see explanation below) \ No newline at end of file diff --git a/src/Nav/navigation/scripts/record_zed_svo.sh b/src/Nav/navigation/scripts/record_zed_svo.sh new file mode 100755 index 00000000..562fa3ef --- /dev/null +++ b/src/Nav/navigation/scripts/record_zed_svo.sh @@ -0,0 +1,51 @@ +#!/bin/bash + +# Default recording directory (matches your example) +REC_DIR="/usr/local/zed/recordings" + +# Help Menu +if [[ "$1" == "--help" || "$1" == "-h" ]]; then + echo "Usage: $0 " + echo "" + echo "Description:" + echo " Starts a ZED camera SVO recording via ROS2 service call." + echo " Files are saved to: $REC_DIR" + echo "" + echo "Arguments:" + echo " filename The name of the output file (e.g., 'mission_test')." + echo " The script automatically appends '.svo2' if missing." + echo "" + echo "Options:" + echo " -h, --help Show this help message and exit." + exit 0 +fi + +# Check for argument +if [ -z "$1" ]; then + echo "Error: No filename provided." + echo "Try '$0 --help' for usage." + exit 1 +fi + +# Construct filename +FILENAME="$1" +# Append extension if user didn't provide it +if [[ "$FILENAME" != *.svo2 ]]; then + FILENAME="${FILENAME}.svo2" +fi + +# Ensure full path +FULL_PATH="${REC_DIR}/${FILENAME}" + +echo "Starting ZED recording..." +echo "Saving to: $FULL_PATH" + +# Execute Service Call +# compression_mode: 2 (H.265/HEVC - High compression, good quality) +ros2 service call /zed/zed_node/start_svo_rec zed_msgs/srv/StartSvoRec "{ + svo_filename: '$FULL_PATH', + compression_mode: 2, + bitrate: 0, + target_framerate: 0, + input_transcode: false +}" \ No newline at end of file