diff --git a/riptide_descriptions/config/talos.yaml b/riptide_descriptions/config/talos.yaml index 72b149b..6380591 100644 --- a/riptide_descriptions/config/talos.yaml +++ b/riptide_descriptions/config/talos.yaml @@ -6,7 +6,11 @@ base_link: [-0.14, 0.03, -0.09] # Vehicle mass properties mass: 31.998 -com: [-0.159, 0.042, -0.056] +# COM no IVC +# com: [-0.151, 0.041, -0.044] + +# COM w/ IVC +com: [-0.157, 0.040, -0.048] inertia: [0.72604692, 1.59322252, 1.65134701] # kg*m^2 hull_volume: .11 #make this real @@ -24,8 +28,8 @@ controller: drag_enable: [1, 1, 0, 0, 0, 0] feed_forward: - #base_wrench: [0.03, 0.01, -5.28, 0.57, 1.77, -0.020] - base_wrench: [-1.0, -1.0, 0.0, -0.95, 2.1, 0.0] + base_wrench: [-0.25, -0.1, -3.0, 0.0, 1.4, -0.005] + # base_wrench: [-1.0, -1.0, 0.0, 0.0, 0.0, 0.0] auto_tune: #positional error threshold to begin autotuning @@ -123,13 +127,14 @@ controller: #[x,y,z,r,p,y] #first order eta - eta_order_0: [1.8, 0.5, 10.0, 0.2, 0.2, 0.3] + eta_order_0: [0.8, 1.5, 5.0, 0.2, 0.2, 1.3] #second order eta - eta_order_1: [0.08, 0.08, 0.3, 0.0, 0.0, 0.0] + eta_order_1: [0.1, 0.04, 0.3, 0.0, 0.0, 0.2] #lambda - lambda: [0.25, 0.25, 1.0, 10.0, 10.0, 0.35] + lambda: [0.8, 0.2, 1.0, 10.0, 10.0, 0.35] + #format: 'a, m' for each degree of freedom #where: @@ -137,7 +142,7 @@ controller: # m = gravity well saturation error velocity_curve_params: [8.0, 0.8, #x 8.0, 0.8, #y - 10.0, 0.5, #z + 8.0, 0.5, #z 15.0, 1.0, #r 15.0, 1.0, #p 1.5, 0.3] #yaw @@ -152,10 +157,14 @@ controller: #linear unit are newtons per meter, angular are newton meter per radian #the default gains for normal operation - p_gains: [3.0, 18.0, 200.0, 3.5, 2.0, 2.5] - i_gains: [0.0, 0.0, 0.0, 0.01, 1.0, 0.0] - d_gains: [5.0, 5.0, 90.0, 1.0, 0.05, 0.9] - + p_gains: [3.0, 18.0, 200.0, 10.0, 8.0, 15.0] + i_gains: [0.0, 0.0, 0.0, 0.01, 0.005, 0.5] + d_gains: [5.0, 5.0, 90.0, 0.02, 0.1, 0.02] + + # #the default gains for normal operation + # p_gains: [3.0, 18.0, 200.0, 2.0, 3.5, 8.0] + # i_gains: [0.0, 0.0, 0.0, 0.0, 0.0, 2.0] + # d_gains: [5.0, 5.0, 90.0, 0.25, 0.25, 0.2] # the control value thresholds for each DOF -- body frame max_control_thresholds: [30, 30, 30, 15, 15, 15] reset_threshold: 0.5 @@ -289,27 +298,31 @@ poker: pose: [0.2286, 0.0127, -0.17145, 0, 0, 0] torpedoes: - pose: [0.048, 0.17200625, -0.0954, 0.0, 0.0, 0.0] #real z: -0.1524, real y: 0.17700625 + pose: [0.048, 0.18200625, -0.1524, 0.0, 0.0, 0.0] #real z: -0.1524, real y: 0.17700625 baseline: 0.0365125 droppers: - pose: [0.048, 0.17200625, -0.1501, 0.0, 0.0, 0.0] + pose: [0.0, 0.17200625, -0.1501, 0.0, 0.0, 0.0] hitch: pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +claw: + pose: [0.19, 0.0655, -0.3712, 0.0, 0.0, 0.0] + cameras: [ { type: zed, name: ffc, - baseline: 0.12, # m. Distance between cameras (manually measured) - pose: [-0.0195, -0.0889, 0, 0, 0, 0] + baseline: 0.050, # m. Distance between cameras (manually measured) + pose: [-0.0195, -0.0889, 0.02, -0.01, 0.05, 0] }, { type: zed, name: dfc, baseline: 0.045, - pose: [-0.145, -0.080, -0.08, 1.5707, 1.5707, 0] + # pose: [-0.145, -0.080, -0.08, 1.5707, 1.5707, 0] + pose: [-0.24347, -0.065568, -0.15943, 1.5507, 1.5107, 0] } ] @@ -359,4 +372,4 @@ thrusters: [ type: 1, pose: [-0.134, 0.440, -0.244, 0.0, 0.0, 0.0] } -] +] \ No newline at end of file diff --git a/riptide_descriptions/urdf/robot_actuators.xacro b/riptide_descriptions/urdf/robot_actuators.xacro index 75d6b92..010ba49 100644 --- a/riptide_descriptions/urdf/robot_actuators.xacro +++ b/riptide_descriptions/urdf/robot_actuators.xacro @@ -106,4 +106,12 @@ parent="${namespace}/origin" pose="${config['hitch']['pose']}" /> + + + + diff --git a/riptide_hardware/cfg/dfc_config.yaml b/riptide_hardware/cfg/dfc_config.yaml index a42807b..1502e50 100644 --- a/riptide_hardware/cfg/dfc_config.yaml +++ b/riptide_hardware/cfg/dfc_config.yaml @@ -1,13 +1,16 @@ /**: ros__parameters: general: - camera_name: "talos/dfc" + camera_model: zedxm serial_number: 51491740 - grab_resolution: 'HD1200' # For zedx/zedxm use HD1200, for zed2i use HD2K - optional_opencv_calibration_file: "/home/ros/zed_cals/zed_dfc_calibration2.yaml" + camera_id: 1 + camera_name: "talos/dfc" + grab_resolution: "HD1200" # For zedx/zedxm use HD1200, for zed2i use HD2K + optional_opencv_calibration_file: "/home/ros/zed_cals/dfc_charuco_test.yaml" + self_calib: false pub_resolution: "NATIVE" # The resolution used for image and depth map publishing. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission - pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' - pub_frame_rate: 30.0 + #pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' + grab_frame_rate: 15 pos_tracking: publish_tf: false debug: diff --git a/riptide_hardware/cfg/ffc_config.yaml b/riptide_hardware/cfg/ffc_config.yaml index cec331f..748cade 100644 --- a/riptide_hardware/cfg/ffc_config.yaml +++ b/riptide_hardware/cfg/ffc_config.yaml @@ -1,13 +1,24 @@ /**: ros__parameters: general: + camera_model: zedxm + serial_number: 55348591 + camera_id: 0 camera_name: "talos/ffc" pub_resolution: "NATIVE" # The resolution used for image and depth map publishing. 'NATIVE' to use the same `general.grab_resolution` - `CUSTOM` to apply the `general.pub_downscale_factor` downscale factory to reduce bandwidth in transmission pub_downscale_factor: 2.0 # rescale factor used to rescale image before publishing when 'pub_resolution' is 'CUSTOM' - pub_frame_rate: 30.0 - optional_opencv_calibration_file: "/home/ros/zed_cals/taloszedxffc_cal5.yaml" + # pub_frame_rate: 30.0 + optional_opencv_calibration_file: "/home/ros/zed_cals/tirpak_ffc_charuco_2_tan.yaml" grab_resolution: 'HD1200' + grab_frame_rate: 15 self_calib: false + sensors: + sensors_image_sync: true + # svo: + # svo_path: "/home/ros/svos/prequal/prequal1.svo2" + depth: + depth_mode: "NEURAL_LIGHT" + max_depth: 4.0 pos_tracking: publish_tf: false debug: diff --git a/riptide_hardware/cfg/talos_ekf.yaml b/riptide_hardware/cfg/talos_ekf.yaml index 9e80c86..90c2bc4 100644 --- a/riptide_hardware/cfg/talos_ekf.yaml +++ b/riptide_hardware/cfg/talos_ekf.yaml @@ -9,7 +9,7 @@ publish_acceleration: true #make camera happy - # predict_to_current_time: true + predict_to_current_time: true # transform_time_offset: -0.1 diff --git a/riptide_hardware/launch/apriltag.launch.py b/riptide_hardware/launch/apriltag.launch.py index 41a034a..9b68174 100644 --- a/riptide_hardware/launch/apriltag.launch.py +++ b/riptide_hardware/launch/apriltag.launch.py @@ -25,7 +25,7 @@ def generate_launch_description(): namespace="apriltag", package='rclcpp_components', executable='component_container', - arguments=['--ros-args', '--log-level', 'error'], + arguments=['--ros-args', '--log-level', 'info'], composable_node_descriptions=[ ComposableNode( name='apriltag_36h11', diff --git a/riptide_hardware/launch/diagnostics.launch.py b/riptide_hardware/launch/diagnostics.launch.py index 3fc03d3..46a9b2a 100644 --- a/riptide_hardware/launch/diagnostics.launch.py +++ b/riptide_hardware/launch/diagnostics.launch.py @@ -77,7 +77,7 @@ def generate_launch_description(): electrical_monitor_node, firmware_monitor_node, voltage_monitor_node, - sensor_monitor_node, + # sensor_monitor_node, computer_monitor_node, aggregator ]) \ No newline at end of file diff --git a/riptide_hardware/launch/hardware.launch.py b/riptide_hardware/launch/hardware.launch.py index 1866813..b656b9c 100644 --- a/riptide_hardware/launch/hardware.launch.py +++ b/riptide_hardware/launch/hardware.launch.py @@ -15,11 +15,6 @@ "launch", "copro_agent.launch.py", ) -diagnostics_launch_file = os.path.join( - get_package_share_directory('riptide_hardware2'), - "launch", "diagnostics.launch.py" -) - dvl_launch_file = os.path.join( get_package_share_directory('riptide_hardware2'), "launch", "dvl.launch.py" @@ -49,14 +44,8 @@ def generate_launch_description(): PushRosNamespace( LC("robot") ), - # IncludeLaunchDescription( - # AnyLaunchDescriptionSource(copro_agent_launch_file), - # launch_arguments=[ - # ('robot', LC('robot')), - # ] - # ), IncludeLaunchDescription( - AnyLaunchDescriptionSource(diagnostics_launch_file), + AnyLaunchDescriptionSource(copro_agent_launch_file), launch_arguments=[ ('robot', LC('robot')), ] @@ -110,13 +99,6 @@ def generate_launch_description(): "imu_pwr_cycle.yaml" ) ] - ), - Node( - package='riptide_hardware2', - executable='pressure_monitor.py', - name='pressure_monitor', - output='screen', - parameters=[{"robot":LC('robot')}] ) ], scoped=True) ]) diff --git a/riptide_hardware/launch/hardware_fake_dvl.launch.py b/riptide_hardware/launch/hardware_fake_dvl.launch.py index ac887a1..60f147e 100644 --- a/riptide_hardware/launch/hardware_fake_dvl.launch.py +++ b/riptide_hardware/launch/hardware_fake_dvl.launch.py @@ -15,11 +15,6 @@ "launch", "copro_agent.launch.py", ) -diagnostics_launch_file = os.path.join( - get_package_share_directory('riptide_hardware2'), - "launch", "diagnostics.launch.py" -) - imu_launch_file = os.path.join( get_package_share_directory('riptide_imu'), "launch", "imu.launch.py" @@ -56,14 +51,8 @@ def generate_launch_description(): # name='zenoh_router', # output='screen', # ), - # IncludeLaunchDescription( - # AnyLaunchDescriptionSource(copro_agent_launch_file), - # launch_arguments=[ - # ('robot', LC('robot')), - # ] - # ), IncludeLaunchDescription( - AnyLaunchDescriptionSource(diagnostics_launch_file), + AnyLaunchDescriptionSource(copro_agent_launch_file), launch_arguments=[ ('robot', LC('robot')), ] @@ -112,13 +101,6 @@ def generate_launch_description(): executable='imu_power_cycle.py', name='imu_power_cycle', output='screen', - ), - Node( - package='riptide_hardware2', - executable='pressure_monitor.py', - name='pressure_monitor', - output='screen', - parameters=[{"robot":LC('robot')}] - ) + ) ], scoped=True) ]) diff --git a/riptide_hardware/launch/navigation.launch.py b/riptide_hardware/launch/navigation.launch.py index 30f3c1c..fad72ce 100644 --- a/riptide_hardware/launch/navigation.launch.py +++ b/riptide_hardware/launch/navigation.launch.py @@ -87,7 +87,7 @@ def evaluate_xacro(context, *args, **kwargs): # try: if robot == "talos": - nodes.append(get_zed_description("ffc", "zedx", robot, context, debug)) + nodes.append(get_zed_description("ffc", "zedxm", robot, context, debug)) nodes.append(get_zed_description("dfc", "zedxm", robot, context, debug)) if robot == "liltank": diff --git a/riptide_hardware/launch/zed.launch.py b/riptide_hardware/launch/zed.launch.py index 651e436..3cd2ad0 100644 --- a/riptide_hardware/launch/zed.launch.py +++ b/riptide_hardware/launch/zed.launch.py @@ -1,184 +1,65 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, Shutdown +from launch.actions import DeclareLaunchArgument, GroupAction from launch_ros.actions import PushRosNamespace, ComposableNodeContainer, Node from launch_ros.descriptions import ComposableNode -from launch.conditions import IfCondition -from launch.substitutions import PythonExpression, LaunchConfiguration as LC +from launch.substitutions import LaunchConfiguration as LC from ament_index_python import get_package_share_directory import os - - def generate_launch_description(): + zed_wrapper_share = get_package_share_directory("zed_wrapper") + riptide_share = get_package_share_directory("riptide_hardware2") - # Define common configuration path - zed_config_path = os.path.join( - get_package_share_directory('zed_wrapper'), - "config", - "common_stereo.yaml" - ) + zed_common = os.path.join(zed_wrapper_share, "config", "common_stereo.yaml") + zedxm = os.path.join(zed_wrapper_share, "config", "zedxm.yaml") + ffc_cfg = os.path.join(riptide_share, "cfg", "ffc_config.yaml") + dfc_cfg = os.path.join(riptide_share, "cfg", "dfc_config.yaml") - # Paths for individual camera configurations - - # FFC Zed 2i - zed2i_camera_path = os.path.join( - get_package_share_directory('zed_wrapper'), - 'config', - 'zed2i.yaml' - ) - - # FFC Zed X - zedx_camera_path = os.path.join( - get_package_share_directory('zed_wrapper'), - 'config', - 'zedx.yaml' - ) - - # DFC Zed X Mini - zedxm_camera_path = os.path.join( - get_package_share_directory('zed_wrapper'), - 'config', - 'zedxm.yaml' - ) - - # FFC Overrides - ffc_config_path = os.path.join( - get_package_share_directory('riptide_hardware2'), - 'cfg', - 'ffc_config.yaml' + ffc_node = ComposableNode( + package="zed_components", + plugin="stereolabs::ZedCamera", + namespace="ffc", + name="zed_node", + parameters=[zed_common, zedxm, ffc_cfg], ) - # DFC Overrides - dfc_config_path = os.path.join( - get_package_share_directory('riptide_hardware2'), - 'cfg', - 'dfc_config.yaml' - ) - - tank_config_path = os.path.join( - get_package_share_directory('riptide_hardware2'), - 'cfg', - 'tank_ffc_config.yaml' + dfc_node = ComposableNode( + package="zed_components", + plugin="stereolabs::ZedCamera", + namespace="dfc", + name="zed_node", + parameters=[zed_common, zedxm, dfc_cfg], ) - - zed_compression_path = os.path.join( - get_package_share_directory('riptide_hardware2'), - 'cfg', - 'zed_compression.yaml' - ) - + return LaunchDescription([ - DeclareLaunchArgument( - name="robot", - default_value="tempest", - description="name of the robot" - ), - # Group actions under the robot namespace + DeclareLaunchArgument("robot", default_value="tempest"), + GroupAction([ PushRosNamespace(LC("robot")), ComposableNodeContainer( - name="ffc_container", - namespace="", - package="rclcpp_components", - executable="component_container", - output="screen", - respawn=True, - composable_node_descriptions=[ - # First ZED Node (FFC) - ComposableNode( - package="zed_components", - plugin="stereolabs::ZedCamera", - namespace="ffc", - name="zed_node", - parameters=[ - zed_config_path, - zedx_camera_path, - ffc_config_path, - ] - ), - ], - condition=IfCondition( - PythonExpression(["'", LC("robot"), "' == 'talos'"])) - ), - - ComposableNodeContainer( - name="dfc_container", + name="zed_container", namespace="", package="rclcpp_components", executable="component_container", output="screen", - respawn=True, - composable_node_descriptions=[ - # Second ZED Node (DFC) - ComposableNode( - package="zed_components", - plugin="stereolabs::ZedCamera", - namespace="dfc", - name="zed_node", - parameters=[ - zed_config_path, - zedxm_camera_path, - dfc_config_path, - ] - ), - ], - condition=IfCondition( - PythonExpression(["'", LC("robot"), "' == 'talos'"])) + respawn=False, + composable_node_descriptions=[ffc_node, dfc_node], ), - - # ComposableNodeContainer( - # name="ffc_container", - # namespace="/", - # package="rclcpp_components", - # executable="component_container", - # output="screen", - # respawn=True, - # composable_node_descriptions=[ - # # The tank camera - # ComposableNode( - # package="zed_components", - # plugin="stereolabs::ZedCamera", - # namespace="ffc", - # name="zed_node", - # parameters=[ - # # zedxm_camera_path, - # zed_config_path, - # tank_config_path, - # # zed_compression_path, - # {"general.camera_name": "liltank/ffc"}, - # {"mapping.clicked_point_topic": "/clicked_point"}, - # ] - # ), - # ], - # condition=IfCondition( - # PythonExpression(["'", LC("robot"), "' == 'liltank'"])) - # ), - - # Disabled for now as both are on by default - # # Launch the ZedManager node - # Node( - # package='riptide_hardware2', - # executable='zed_manager.py', - # name='ZedManager', - # output='screen' - # ) - - # Node( - # package='riptide_hardware2', - # executable='picture_taker.py', - # name='picture_taker', - # output='screen', - # parameters=[ - # {"robot_namespace": LC("robot")}, - # {"camera_name": "ffc"}, - # {"save_stereo": True}, - # {"save_split": True} - # ] - # ) - - ], scoped=True), - + Node( + package='riptide_hardware2', + executable='picture_taker.py', + name='picture_taker', + output='screen', + parameters=[ + {"robot_namespace": LC("robot")}, + {"camera_name": "ffc"}, + {"subscription_enabled": True}, + {"save_stereo": False}, + {"save_split": True} + ] + ) + ], scoped=True), ]) \ No newline at end of file diff --git a/riptide_hardware/riptide_hardware2/depth_converter.py b/riptide_hardware/riptide_hardware2/depth_converter.py index 1de7402..42147fc 100755 --- a/riptide_hardware/riptide_hardware2/depth_converter.py +++ b/riptide_hardware/riptide_hardware2/depth_converter.py @@ -51,7 +51,7 @@ def depthCb(self, msg): outMsg.header.frame_id = "odom" outMsg.pose.pose.position.z = msg.depth + addedDepth outMsg.pose.covariance[14] = msg.variance - outMsg.header.stamp = self.get_clock().now().to_msg() + outMsg.header.stamp = msg.header.stamp self.pub.publish(outMsg) diff --git a/riptide_hardware/riptide_hardware2/picture_taker.py b/riptide_hardware/riptide_hardware2/picture_taker.py index d41ed80..7514780 100644 --- a/riptide_hardware/riptide_hardware2/picture_taker.py +++ b/riptide_hardware/riptide_hardware2/picture_taker.py @@ -4,6 +4,7 @@ from rclpy.node import Node from sensor_msgs.msg import Image from std_srvs.srv import Trigger, SetBool +from rcl_interfaces.msg import SetParametersResult from cv_bridge import CvBridge import cv2 import os @@ -18,40 +19,37 @@ def __init__(self): self.declare_parameter('robot_namespace', 'talos') self.declare_parameter('camera_name', 'ffc') self.declare_parameter('save_directory', '/home/ros/cal_images') + self.declare_parameter('subscription_enabled', True) self.declare_parameter('save_stereo', False) self.declare_parameter('save_split', True) + self.add_on_set_parameters_callback(self.on_param_change) # Get parameter values - robot_namespace = self.get_parameter('robot_namespace').get_parameter_value().string_value - camera_name = self.get_parameter('camera_name').get_parameter_value().string_value + self.robot_namespace = self.get_parameter('robot_namespace').get_parameter_value().string_value + self.camera_name = self.get_parameter('camera_name').get_parameter_value().string_value self.save_directory = self.get_parameter('save_directory').get_parameter_value().string_value + self.subscription_enabled = self.get_parameter('subscription_enabled').get_parameter_value().bool_value self.save_stereo = self.get_parameter('save_stereo').get_parameter_value().bool_value self.save_split = self.get_parameter('save_split').get_parameter_value().bool_value # Build the full image topic path - self.image_topic = f"/{robot_namespace}/{camera_name}/zed_node/stereo_raw/image_raw_color" + self.image_topic = f"/{self.robot_namespace}/{self.camera_name}/zed_node/stereo_raw/image_raw_color" - # Create save directory if it doesn't exist - os.makedirs(self.save_directory, exist_ok=True) - - # Create subdirectories for split images if needed - if self.save_split: - os.makedirs(os.path.join(self.save_directory, 'left'), exist_ok=True) - os.makedirs(os.path.join(self.save_directory, 'right'), exist_ok=True) + self.create_save_dir() # Initialize CV bridge for image conversion self.bridge = CvBridge() # Store the latest image and subscription state self.latest_image = None - self.subscription_enabled = True self.image_subscriber = None # Log the topic we're subscribing to self.get_logger().info(f"Image topic: {self.image_topic}") # Create initial subscriber for image topic - self.create_image_subscriber() + if self.subscription_enabled: + self.create_image_subscriber() # Create service for capturing images self.capture_service = self.create_service( @@ -67,6 +65,60 @@ def __init__(self): self.enable_subscription_callback ) + def create_save_dir(self): + # Create save directory if it doesn't exist + os.makedirs(self.save_directory, exist_ok=True) + + # Create subdirectories for split images if needed + if self.save_split: + os.makedirs(os.path.join(self.save_directory, 'left'), exist_ok=True) + os.makedirs(os.path.join(self.save_directory, 'right'), exist_ok=True) + + def on_param_change(self, params): + changed = [] + for p in params: + if hasattr(self, p.name): + current = getattr(self, p.name) + if current != p.value: + setattr(self, p.name, p.value) + self.get_logger().info(f"Parameter '{p.name}' changed: {current} -> {p.value}") + changed.append(p) + + + self.handle_param_update(changed) + return SetParametersResult(successful=True) + + def handle_param_update(self, params): + topic_changed = update_save = update_sub = False + for p in params: + if p.name in ['camera_name', 'robot_namespace']: + topic_changed = True + elif p.name in ['save_directory', 'save_split']: + update_save = True + elif p.name == 'subscription_enabled': + update_sub = True + + if update_save: + self.create_save_dir() + + if topic_changed: + self.image_topic = f"/{self.robot_namespace}/{self.camera_name}/zed_node/stereo_raw/image_raw_color" + + if update_sub: + if self.subscription_enabled: + if self.image_subscriber is None: + self.create_image_subscriber() + else: + if topic_changed: + self.destroy_image_subscriber() + self.create_image_subscriber() + else: + self.destroy_image_subscriber() + else: + if topic_changed: + self.destroy_image_subscriber() + self.create_image_subscriber() + def create_image_subscriber(self): """Create the image subscriber""" if self.image_subscriber is None: @@ -99,12 +151,18 @@ def capture_image_callback(self, request, response): return response try: + + # if not self.save_stereo or self.save_split: + # return + # Generate filename with timestamp - timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f")[:-3] + timestamp = datetime.now().strftime("%Y_%m_%d__%H_%M_%S_%f")[:-3] camera_name = self.get_parameter('camera_name').get_parameter_value().string_value saved_files = [] + self.create_save_dir() + # Save stereo image if enabled if self.save_stereo: stereo_filename = f"captured_image_{camera_name}_{timestamp}.png" @@ -112,7 +170,7 @@ def capture_image_callback(self, request, response): success = cv2.imwrite(stereo_filepath, self.latest_image, [cv2.IMWRITE_PNG_COMPRESSION, 0]) if success: - saved_files.append(f"stereo: {stereo_filename}") + saved_files.append(f"\nStereo: {stereo_filepath}") self.get_logger().info(f"Stereo image saved: {stereo_filepath}") else: self.get_logger().error("Failed to save stereo image") @@ -137,13 +195,13 @@ def capture_image_callback(self, request, response): right_success = cv2.imwrite(right_filepath, right_image, [cv2.IMWRITE_PNG_COMPRESSION, 0]) if left_success: - saved_files.append(f"left: {left_filename}") + saved_files.append(f"\nLeft: {left_filepath}") self.get_logger().info(f"Left image saved: {left_filepath}") else: self.get_logger().error("Failed to save left image") if right_success: - saved_files.append(f"right: {right_filename}") + saved_files.append(f"\nRight: {right_filepath}") self.get_logger().info(f"Right image saved: {right_filepath}") else: self.get_logger().error("Failed to save right image") diff --git a/riptide_msgs/srv/SetString.srv b/riptide_msgs/srv/SetString.srv new file mode 100644 index 0000000..7377b4c --- /dev/null +++ b/riptide_msgs/srv/SetString.srv @@ -0,0 +1,4 @@ +string data +--- +bool success +string message \ No newline at end of file diff --git a/riptide_msgs/srv/StartBinaryClassifier.srv b/riptide_msgs/srv/StartBinaryClassifier.srv new file mode 100644 index 0000000..0eb09cf --- /dev/null +++ b/riptide_msgs/srv/StartBinaryClassifier.srv @@ -0,0 +1,6 @@ +string class_name +string object1_name +string object2_name +--- +bool success +string message \ No newline at end of file