From a19003aaacd14b03e2e39e5539126640d66d6875 Mon Sep 17 00:00:00 2001 From: Barrett Date: Sat, 21 Feb 2026 11:56:02 -0600 Subject: [PATCH 01/20] fix(feedback): use wrapping cksum math --- lib/feedback/src/lib.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/lib/feedback/src/lib.rs b/lib/feedback/src/lib.rs index 2b7554b0..7b8eecfb 100644 --- a/lib/feedback/src/lib.rs +++ b/lib/feedback/src/lib.rs @@ -45,7 +45,7 @@ impl Wheels { right, // see electrical ebox teensy code - checksum: 255_u8.overflowing_add(left + right).0, + checksum: 255_u8.wrapping_add(left.wrapping_add(right)), } } } From d3ead674d47fb503465153b3a3b2111e8a8c3cfc Mon Sep 17 00:00:00 2001 From: Barrett Date: Sat, 21 Feb 2026 11:56:21 -0600 Subject: [PATCH 02/20] fix(drive): tweak nav2 parameters --- src/drive_launcher/params/nav2.yaml | 43 +++++++++++++++-------------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/src/drive_launcher/params/nav2.yaml b/src/drive_launcher/params/nav2.yaml index 32a1ec32..50f9e953 100644 --- a/src/drive_launcher/params/nav2.yaml +++ b/src/drive_launcher/params/nav2.yaml @@ -49,8 +49,8 @@ controller_server: # pogress checker parameters progress_checker: plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.2 - movement_time_allowance: 20.0 + required_movement_radius: 0.05 + movement_time_allowance: 40.0 general_goal_checker: stateful: True plugin: "nav2_controller::SimpleGoalChecker" @@ -60,29 +60,29 @@ controller_server: FollowPath: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: true - min_vel_x: 0.0 + min_vel_x: 0.8 min_vel_y: 0.0 - max_vel_x: 0.5 + max_vel_x: 3.5 max_vel_y: 0.0 max_vel_theta: 1.0 min_speed_xy: 0.0 - max_speed_xy: 0.5 + max_speed_xy: 3.5 min_speed_theta: 0.0 - acc_lim_x: 2.5 + acc_lim_x: 3.5 acc_lim_y: 0.0 - acc_lim_theta: 3.2 + acc_lim_theta: 1.5 #CHANGED THIS! decel_lim_x: -2.5 decel_lim_y: 0.0 decel_lim_theta: -3.2 - vx_samples: 20 + vx_samples: 30 vy_samples: 5 - vtheta_samples: 20 - sim_time: 1.7 + vtheta_samples: 30 + sim_time: 1.2 linear_granularity: 0.05 angular_granularity: 0.025 transform_tolerance: 0.2 xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 + trans_stopped_velocity: 0.05 short_circuit_trajectory_evaluation: True limit_vel_cmd_in_traj: False stateful: True @@ -97,14 +97,14 @@ controller_server: "GoalDist", ] BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - GoalAlign.scale: 24.0 + PathAlign.scale: 8.0 + GoalAlign.scale: 4.0 PathAlign.forward_point_distance: 0.1 GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 + PathDist.scale: 24.0 + GoalDist.scale: 16.0 + RotateToGoal.scale: 16.0 + RotateToGoal.slowing_factor: 2.0 RotateToGoal.lookahead_time: -1.0 /local_costmap/local_costmap: @@ -289,6 +289,7 @@ behavior_server: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] spin: plugin: "nav2_behaviors/Spin" + time_allowance: 25.0 backup: plugin: "nav2_behaviors/BackUp" backup_speed: 0.3 @@ -305,10 +306,10 @@ behavior_server: global_frame: map robot_base_frame: base_link transform_tolerance: 2.0 - simulate_ahead_time: 1.0 - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 3.2 + simulate_ahead_time: 0.5 + max_rotational_vel: 0.6 + min_rotational_vel: 0.2 + rotational_acc_lim: 1.0 waypoint_follower: ros__parameters: From 2639c91fe6a272a659f035c19590ac9dccb5afff Mon Sep 17 00:00:00 2001 From: Barrett Date: Sat, 21 Feb 2026 11:57:27 -0600 Subject: [PATCH 03/20] fix(nav): use "correct" lights srv path it should actually be `/control/lights`, but i guess that changed at some point??? oh well --- src/navigator/navigator_node/main.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/navigator/navigator_node/main.py b/src/navigator/navigator_node/main.py index 0fdb92a6..acf2d3ee 100644 --- a/src/navigator/navigator_node/main.py +++ b/src/navigator/navigator_node/main.py @@ -180,7 +180,7 @@ def __init__(self): # create a service client for lights node self._lights_client = self.create_client( srv_type=Lights, - srv_name="/control/lights", + srv_name="lights_service", ) # wait for the lights service to come up From 01e20d8b79d84ff2e1ede1a7ead90e38c5f6e104 Mon Sep 17 00:00:00 2001 From: Barrett Date: Sat, 21 Feb 2026 11:58:02 -0600 Subject: [PATCH 04/20] fix(nav): use right qos settings in utm node --- src/navigator/utm_conversion_node/main.py | 42 +++++++++++++++++------ 1 file changed, 32 insertions(+), 10 deletions(-) diff --git a/src/navigator/utm_conversion_node/main.py b/src/navigator/utm_conversion_node/main.py index 65a48a0f..62b7a017 100644 --- a/src/navigator/utm_conversion_node/main.py +++ b/src/navigator/utm_conversion_node/main.py @@ -2,23 +2,37 @@ from dataclasses import dataclass import rclpy +from custom_interfaces.srv import GnssToMap +from custom_interfaces.srv._gnss_to_map import ( + GnssToMap_Request, + GnssToMap_Response, +) from geodesy import utm from geodesy.utm import UTMPoint as UtmPoint from geometry_msgs.msg import Point from loguru import logger as llogger from nav_msgs.msg import Odometry from rclpy.node import Node, Service, Subscription -from rclpy.qos import QoSPresetProfiles, QoSProfile +from rclpy.qos import ( + QoSDurabilityPolicy, + QoSHistoryPolicy, + QoSPresetProfiles, + QoSProfile, + QoSReliabilityPolicy, +) from sensor_msgs.msg import NavSatFix from typing_extensions import override -from custom_interfaces.srv import GnssToMap -from custom_interfaces.srv._gnss_to_map import ( - GnssToMap_Request, - GnssToMap_Response, +GPS_QOS_PROFILE: QoSProfile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=5, + durability=QoSDurabilityPolicy.VOLATILE, ) -QOS_PROFILE: QoSProfile = QoSPresetProfiles.SENSOR_DATA.value +ODOM_QOS_PROFILE: QoSProfile = QoSPresetProfiles.SENSOR_DATA.value +ODOM_QOS_PROFILE.reliability = QoSReliabilityPolicy.RELIABLE +ODOM_QOS_PROFILE.depth = 10 @dataclass(kw_only=True) @@ -37,7 +51,7 @@ def __init__(self): NavSatFix, "/sensors/gps", self._on_filtered_gps_message, - QOS_PROFILE, + GPS_QOS_PROFILE, ) # also, grab odom stream @@ -45,7 +59,7 @@ def __init__(self): Odometry, "/odometry/filtered", self._on_odom_message, - QOS_PROFILE, + ODOM_QOS_PROFILE, ) # finally, make a service to convert gnss coords -> map coords @@ -67,9 +81,17 @@ def _handle_gnss_conversion( self, req: GnssToMap_Request, resp: GnssToMap_Response ) -> GnssToMap_Response: # if we don't have the offset, early return... - if self._last_known_odom is None or self._last_known_utm is None: + if self._last_known_odom is None: + llogger.warning( + "srv called, but `odom` (`/odometry/filtered`) isn't available yet. \ + try again in a moment..." + ) + resp.success = False + return resp + if self._last_known_utm is None: llogger.warning( - "srv called, but utm/odom isn't available yet. try again in a moment..." + "srv called, and `odom` is available, but `utm` (`/sensors/gps`) isn't available yet. \ + try again in a moment..." ) resp.success = False return resp From 1892af22ee776c3ce8efd706a036b2d41fbf704f Mon Sep 17 00:00:00 2001 From: Barrett Date: Sat, 21 Feb 2026 11:59:17 -0600 Subject: [PATCH 05/20] fix(soro_lidar): add minimum range to lidar to prevent us from seeing the rover chassis/antenna post as an obstacle --- src/soro_lidar/launch/launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/soro_lidar/launch/launch.py b/src/soro_lidar/launch/launch.py index 48c91735..82a55a1e 100644 --- a/src/soro_lidar/launch/launch.py +++ b/src/soro_lidar/launch/launch.py @@ -20,7 +20,7 @@ def generate_launch_description(): {"work_mode": 0}, {"use_system_timestamp": True}, {"publish_tf": False}, - {"range_min": 0.0}, + {"range_min": 1.2}, {"range_max": 100.0}, {"cloud_scan_num": 18}, { From 42a591b2f06785b7bdd8020f4e74da79e830fc07 Mon Sep 17 00:00:00 2001 From: Barrett Date: Sat, 21 Feb 2026 11:59:50 -0600 Subject: [PATCH 06/20] fix(nav): open asyncio task first in nav node --- src/navigator/navigator_node/main.py | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/src/navigator/navigator_node/main.py b/src/navigator/navigator_node/main.py index acf2d3ee..1d60c86f 100644 --- a/src/navigator/navigator_node/main.py +++ b/src/navigator/navigator_node/main.py @@ -741,23 +741,28 @@ def main(args: list[str] | None = None): rclpy.init(args=args) navigator_node: NavigatorNode = NavigatorNode() - # spin the ros 2 node and run our logic... at the same time! + # Spin the ROS 2 node and run navigation logic concurrently. # - # for more info, see: https://github.com/m2-farzan/ros2-asyncio - future = asyncio.wait( - [ - asyncio.create_task(ros_spin(navigator_node)), - asyncio.create_task(navigator_node.navigator()), - ] - ) - _ = asyncio.get_event_loop().run_until_complete(future) + # `asyncio.create_task` requires a running loop, so we enter async context + # with `asyncio.run(...)` first. + async def _run() -> None: + _ = await asyncio.gather( + ros_spin(navigator_node), + navigator_node.navigator(), + ) + + try: + asyncio.run(_run()) + except KeyboardInterrupt: + llogger.info("Navigator interrupted; shutting down.") # destroy the Node explicitly # # this is optional - otherwise, the garbage collector does it automatically # when it runs. navigator_node.destroy_node() - rclpy.shutdown() + if rclpy.ok(): + rclpy.shutdown() def shutdown(n: NavigatorNode): From 914a30bdd0f9a3b105ce7cf735ada7ace5e4ed17 Mon Sep 17 00:00:00 2001 From: Barrett Date: Tue, 24 Feb 2026 09:36:20 -0600 Subject: [PATCH 07/20] chore(drive): rm assisted_teleop plugin from nav2 we don't use it --- src/drive_launcher/params/nav2.yaml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/drive_launcher/params/nav2.yaml b/src/drive_launcher/params/nav2.yaml index 50f9e953..345d6fba 100644 --- a/src/drive_launcher/params/nav2.yaml +++ b/src/drive_launcher/params/nav2.yaml @@ -286,7 +286,7 @@ behavior_server: global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 behavior_plugins: - ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + ["spin", "backup", "drive_on_heading", "wait"] spin: plugin: "nav2_behaviors/Spin" time_allowance: 25.0 @@ -300,8 +300,6 @@ behavior_server: minimum_speed: 0.25 wait: plugin: "nav2_behaviors/Wait" - assisted_teleop: - plugin: "nav2_behaviors/AssistedTeleop" local_frame: odom global_frame: map robot_base_frame: base_link From 6c0ae7e96283076986bf43d2517a13b7f0829e64 Mon Sep 17 00:00:00 2001 From: Barrett Date: Tue, 24 Feb 2026 09:37:27 -0600 Subject: [PATCH 08/20] fix(drive): make nav2 drive without weird jitters now, it drives smoothly without problems :D --- src/drive_launcher/params/nav2.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drive_launcher/params/nav2.yaml b/src/drive_launcher/params/nav2.yaml index 345d6fba..7f9d1c0d 100644 --- a/src/drive_launcher/params/nav2.yaml +++ b/src/drive_launcher/params/nav2.yaml @@ -60,7 +60,7 @@ controller_server: FollowPath: plugin: "dwb_core::DWBLocalPlanner" debug_trajectory_details: true - min_vel_x: 0.8 + min_vel_x: 0.35 min_vel_y: 0.0 max_vel_x: 3.5 max_vel_y: 0.0 @@ -326,10 +326,10 @@ velocity_smoother: smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [0.5, 0.0, 2.0] - min_velocity: [-0.5, 0.0, -2.0] - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] + max_velocity: [3.5, 0.0, 2.0] + min_velocity: [-3.5, 0.0, -2.0] + max_accel: [3.5, 0.0, 3.2] + max_decel: [-3.5, 0.0, -3.2] odom_topic: "odom" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] From d44e5c5fc63b902a5c3abc6497f11edcbf515c7c Mon Sep 17 00:00:00 2001 From: Barrett Date: Tue, 24 Feb 2026 09:37:54 -0600 Subject: [PATCH 09/20] fix(drive): mk nav2's global_costmap more precise --- src/drive_launcher/params/nav2.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drive_launcher/params/nav2.yaml b/src/drive_launcher/params/nav2.yaml index 7f9d1c0d..9c11384b 100644 --- a/src/drive_launcher/params/nav2.yaml +++ b/src/drive_launcher/params/nav2.yaml @@ -199,9 +199,9 @@ controller_server: # # see: https://docs.nav2.org/tutorials/docs/navigation2_with_gps.html#setup-navigation-system rolling_window: True - resolution: 0.75 - width: 2000 - height: 2000 + resolution: 0.25 + width: 8000 + height: 8000 # allow higher latency from sensors transform_tolerance: 2.0 From df0473e3087645f18eaf6c80b0e028a15d1ccae1 Mon Sep 17 00:00:00 2001 From: Barrett Date: Mon, 6 Apr 2026 19:27:56 -0500 Subject: [PATCH 10/20] chore(ifaces): update Rust <=> ROS 2 bindings --- .../bindings/builtin_interfaces/cksum | 2 +- .../bindings/custom_interfaces/cksum | 2 +- .../bindings/custom_interfaces/src/action.rs | 3 + .../src/action/find_aruco_with_pose.rs | 1292 +++++++++++++++++ .../bindings/geometry_msgs/cksum | 2 +- .../bindings/geometry_msgs/src/msg.rs | 6 + .../geometry_msgs/src/msg/polygon_instance.rs | 147 ++ .../src/msg/polygon_instance_stamped.rs | 147 ++ .../bindings/unique_identifier_msgs/cksum | 2 +- 9 files changed, 1599 insertions(+), 4 deletions(-) create mode 100644 src/custom_interfaces/bindings/custom_interfaces/src/action/find_aruco_with_pose.rs create mode 100644 src/custom_interfaces/bindings/geometry_msgs/src/msg/polygon_instance.rs create mode 100644 src/custom_interfaces/bindings/geometry_msgs/src/msg/polygon_instance_stamped.rs diff --git a/src/custom_interfaces/bindings/builtin_interfaces/cksum b/src/custom_interfaces/bindings/builtin_interfaces/cksum index 97b43098..a9a6344b 100644 --- a/src/custom_interfaces/bindings/builtin_interfaces/cksum +++ b/src/custom_interfaces/bindings/builtin_interfaces/cksum @@ -1,3 +1,3 @@ 0.2.6 Version("0.4.3") -5DdPufd6t1CqEwE58UfyY3doZ+7Q6m7aLG6KIa5U4omVihNXuNGM+7VHCcDdFe6PhoFMHNFxd25cazWnAj1ZlQ== \ No newline at end of file +G9htJh3lZoodPoLgaU8lMnuRZE3rb8JSntvjconQ6tL9/xIY3EB7dViK41mP9bxEjbWnfEYEU0xfhnToMalHYQ== \ No newline at end of file diff --git a/src/custom_interfaces/bindings/custom_interfaces/cksum b/src/custom_interfaces/bindings/custom_interfaces/cksum index a6cfe460..6288aa4b 100644 --- a/src/custom_interfaces/bindings/custom_interfaces/cksum +++ b/src/custom_interfaces/bindings/custom_interfaces/cksum @@ -1,3 +1,3 @@ 0.2.6 Version("0.4.3") -7ZI0m1vPHiJskzbQyls9I7ZFRX3ONCd6RzaIqtaZs+Kodl9L/Rcl0c7PeOPEI6DdtK6Mv0iKxpsIuOqpU70G9Q== \ No newline at end of file +lpM62I+VSPRY1hx2+d2XwBxhGXjh1hkDqfPaYmFe91KzDbdAbMWK1+tcifH6O/pYX4oh2HWdGCjgvjUPSYZPpQ== \ No newline at end of file diff --git a/src/custom_interfaces/bindings/custom_interfaces/src/action.rs b/src/custom_interfaces/bindings/custom_interfaces/src/action.rs index 8b97c381..c2be58f7 100644 --- a/src/custom_interfaces/bindings/custom_interfaces/src/action.rs +++ b/src/custom_interfaces/bindings/custom_interfaces/src/action.rs @@ -1,3 +1,6 @@ +pub mod find_aruco_with_pose; +pub use find_aruco_with_pose::*; + pub mod track_aruco_marker; pub use track_aruco_marker::*; diff --git a/src/custom_interfaces/bindings/custom_interfaces/src/action/find_aruco_with_pose.rs b/src/custom_interfaces/bindings/custom_interfaces/src/action/find_aruco_with_pose.rs new file mode 100644 index 00000000..31aa735a --- /dev/null +++ b/src/custom_interfaces/bindings/custom_interfaces/src/action/find_aruco_with_pose.rs @@ -0,0 +1,1292 @@ +use safe_drive::{msg::{ActionMsg, ActionGoal, ActionResult, GetUUID, GoalResponse, ResultResponse, TypeSupport, builtin_interfaces::UnsafeTime, unique_identifier_msgs}, rcl::{self, size_t}}; + +extern "C" { + fn rosidl_typesupport_c__get_action_type_support_handle__custom_interfaces__action__FindArucoWithPose() -> *const rcl::rosidl_action_type_support_t; +} + +#[derive(Debug)] +pub struct FindArucoWithPose; + +impl ActionMsg for FindArucoWithPose { + type Goal = FindArucoWithPose_SendGoal; + type Result = FindArucoWithPose_GetResult; + type Feedback = FindArucoWithPose_FeedbackMessage; + fn type_support() -> *const rcl::rosidl_action_type_support_t { + unsafe { + rosidl_typesupport_c__get_action_type_support_handle__custom_interfaces__action__FindArucoWithPose() + } + } + + type GoalContent = FindArucoWithPose_Goal; + + fn new_goal_request( + goal: Self::GoalContent, + uuid: [u8; 16], + ) -> ::Request { + FindArucoWithPose_SendGoal_Request { + goal, + goal_id: unique_identifier_msgs::msg::UUID { uuid }, + } + } + + type ResultContent = FindArucoWithPose_Result; + + fn new_result_response( + status: u8, + result: Self::ResultContent, + ) -> ::Response { + FindArucoWithPose_GetResult_Response { status, result } + } + + type FeedbackContent = FindArucoWithPose_Feedback; + + fn new_feedback_message(feedback: Self::FeedbackContent, uuid: [u8; 16]) -> Self::Feedback { + FindArucoWithPose_FeedbackMessage { + feedback, + goal_id: unique_identifier_msgs::msg::UUID { uuid }, + } + } +} + +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_SendGoal_Request { + pub goal_id: unique_identifier_msgs::msg::UUID, + pub goal: FindArucoWithPose_Goal, +} + +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_SendGoal_Response { + pub accepted: bool, + pub stamp: UnsafeTime, +} + +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_GetResult_Request { + pub goal_id: unique_identifier_msgs::msg::UUID, +} + +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_GetResult_Response { + pub status: u8, + pub result: FindArucoWithPose_Result, +} + +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_FeedbackMessage { + pub goal_id: unique_identifier_msgs::msg::UUID, + pub feedback: FindArucoWithPose_Feedback, +} + +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_Goal { + pub structure_needs_at_least_one_member: u8, +} + +extern "C" { + fn custom_interfaces__action__FindArucoWithPose_Goal__init(msg: *mut FindArucoWithPose_Goal) -> bool; + fn custom_interfaces__action__FindArucoWithPose_Goal__fini(msg: *mut FindArucoWithPose_Goal); + fn custom_interfaces__action__FindArucoWithPose_Goal__are_equal(lhs: *const FindArucoWithPose_Goal, rhs: *const FindArucoWithPose_Goal) -> bool; + fn custom_interfaces__action__FindArucoWithPose_Goal__Sequence__init(msg: *mut FindArucoWithPose_GoalSeqRaw, size: usize) -> bool; + fn custom_interfaces__action__FindArucoWithPose_Goal__Sequence__fini(msg: *mut FindArucoWithPose_GoalSeqRaw); + fn custom_interfaces__action__FindArucoWithPose_Goal__Sequence__are_equal(lhs: *const FindArucoWithPose_GoalSeqRaw, rhs: *const FindArucoWithPose_GoalSeqRaw) -> bool; + fn rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_Goal() -> *const rcl::rosidl_message_type_support_t; +} + +impl TypeSupport for FindArucoWithPose_Goal { + fn type_support() -> *const rcl::rosidl_message_type_support_t { + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_Goal() + } + } +} + +impl PartialEq for FindArucoWithPose_Goal { + fn eq(&self, other: &Self) -> bool { + unsafe { + custom_interfaces__action__FindArucoWithPose_Goal__are_equal(self, other) + } + } +} + +impl PartialEq for FindArucoWithPose_GoalSeq { + fn eq(&self, other: &Self) -> bool { + unsafe { + let msg1 = FindArucoWithPose_GoalSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + let msg2 = FindArucoWithPose_GoalSeqRaw{ data: other.data, size: other.size, capacity: other.capacity }; + custom_interfaces__action__FindArucoWithPose_Goal__Sequence__are_equal(&msg1, &msg2) + } + } +} + +impl FindArucoWithPose_Goal { + pub fn new() -> Option { + let mut msg: Self = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_Goal__init(&mut msg) } { + Some(msg) + } else { + None + } + } +} + +impl Drop for FindArucoWithPose_Goal { + fn drop(&mut self) { + unsafe { custom_interfaces__action__FindArucoWithPose_Goal__fini(self) }; + } +} + +#[repr(C)] +#[derive(Debug)] +struct FindArucoWithPose_GoalSeqRaw { + data: *mut FindArucoWithPose_Goal, + size: size_t, + capacity: size_t, +} + +/// Sequence of FindArucoWithPose_Goal. +/// `N` is the maximum number of elements. +/// If `N` is `0`, the size is unlimited. +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_GoalSeq { + data: *mut FindArucoWithPose_Goal, + size: size_t, + capacity: size_t, +} + +impl FindArucoWithPose_GoalSeq { + /// Create a sequence of. + /// `N` represents the maximum number of elements. + /// If `N` is `0`, the sequence is unlimited. + pub fn new(size: usize) -> Option { + if N != 0 && size > N { + // the size exceeds in the maximum number + return None; + } + let mut msg: FindArucoWithPose_GoalSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_Goal__Sequence__init(&mut msg, size) } { + Some(Self { data: msg.data, size: msg.size, capacity: msg.capacity }) + } else { + None + } + } + + pub fn null() -> Self { + let msg: FindArucoWithPose_GoalSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + Self { data: msg.data, size: msg.size, capacity: msg.capacity } + } + + pub fn as_slice(&self) -> &[FindArucoWithPose_Goal] { + if self.data.is_null() { + &[] + } else { + let s = unsafe { std::slice::from_raw_parts(self.data, self.size as _) }; + s + } + } + + pub fn as_slice_mut(&mut self) -> &mut [FindArucoWithPose_Goal] { + if self.data.is_null() { + &mut [] + } else { + let s = unsafe { std::slice::from_raw_parts_mut(self.data, self.size as _) }; + s + } + } + + pub fn iter(&self) -> std::slice::Iter<'_, FindArucoWithPose_Goal> { + self.as_slice().iter() + } + + pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, FindArucoWithPose_Goal> { + self.as_slice_mut().iter_mut() + } + + pub fn len(&self) -> usize { + self.as_slice().len() + } + + pub fn is_empty(&self) -> bool { + self.len() == 0 + } +} + +impl Drop for FindArucoWithPose_GoalSeq { + fn drop(&mut self) { + let mut msg = FindArucoWithPose_GoalSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + unsafe { custom_interfaces__action__FindArucoWithPose_Goal__Sequence__fini(&mut msg) }; + } +} + +unsafe impl Send for FindArucoWithPose_GoalSeq {} +unsafe impl Sync for FindArucoWithPose_GoalSeq {} + +extern "C" { + fn custom_interfaces__action__FindArucoWithPose_SendGoal_Request__init(msg: *mut FindArucoWithPose_SendGoal_Request) -> bool; + fn custom_interfaces__action__FindArucoWithPose_SendGoal_Request__fini(msg: *mut FindArucoWithPose_SendGoal_Request); + fn custom_interfaces__action__FindArucoWithPose_SendGoal_Request__are_equal(lhs: *const FindArucoWithPose_SendGoal_Request, rhs: *const FindArucoWithPose_SendGoal_Request) -> bool; + fn custom_interfaces__action__FindArucoWithPose_SendGoal_Request__Sequence__init(msg: *mut FindArucoWithPose_SendGoal_RequestSeqRaw, size: usize) -> bool; + fn custom_interfaces__action__FindArucoWithPose_SendGoal_Request__Sequence__fini(msg: *mut FindArucoWithPose_SendGoal_RequestSeqRaw); + fn custom_interfaces__action__FindArucoWithPose_SendGoal_Request__Sequence__are_equal(lhs: *const FindArucoWithPose_SendGoal_RequestSeqRaw, rhs: *const FindArucoWithPose_SendGoal_RequestSeqRaw) -> bool; + fn rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_SendGoal_Request() -> *const rcl::rosidl_message_type_support_t; +} + +impl TypeSupport for FindArucoWithPose_SendGoal_Request { + fn type_support() -> *const rcl::rosidl_message_type_support_t { + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_SendGoal_Request() + } + } +} + +impl PartialEq for FindArucoWithPose_SendGoal_Request { + fn eq(&self, other: &Self) -> bool { + unsafe { + custom_interfaces__action__FindArucoWithPose_SendGoal_Request__are_equal(self, other) + } + } +} + +impl PartialEq for FindArucoWithPose_SendGoal_RequestSeq { + fn eq(&self, other: &Self) -> bool { + unsafe { + let msg1 = FindArucoWithPose_SendGoal_RequestSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + let msg2 = FindArucoWithPose_SendGoal_RequestSeqRaw{ data: other.data, size: other.size, capacity: other.capacity }; + custom_interfaces__action__FindArucoWithPose_SendGoal_Request__Sequence__are_equal(&msg1, &msg2) + } + } +} + +impl FindArucoWithPose_SendGoal_Request { + pub fn new() -> Option { + let mut msg: Self = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_SendGoal_Request__init(&mut msg) } { + Some(msg) + } else { + None + } + } +} + +impl Drop for FindArucoWithPose_SendGoal_Request { + fn drop(&mut self) { + unsafe { custom_interfaces__action__FindArucoWithPose_SendGoal_Request__fini(self) }; + } +} + +#[repr(C)] +#[derive(Debug)] +struct FindArucoWithPose_SendGoal_RequestSeqRaw { + data: *mut FindArucoWithPose_SendGoal_Request, + size: size_t, + capacity: size_t, +} + +/// Sequence of FindArucoWithPose_SendGoal_Request. +/// `N` is the maximum number of elements. +/// If `N` is `0`, the size is unlimited. +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_SendGoal_RequestSeq { + data: *mut FindArucoWithPose_SendGoal_Request, + size: size_t, + capacity: size_t, +} + +impl FindArucoWithPose_SendGoal_RequestSeq { + /// Create a sequence of. + /// `N` represents the maximum number of elements. + /// If `N` is `0`, the sequence is unlimited. + pub fn new(size: usize) -> Option { + if N != 0 && size > N { + // the size exceeds in the maximum number + return None; + } + let mut msg: FindArucoWithPose_SendGoal_RequestSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_SendGoal_Request__Sequence__init(&mut msg, size) } { + Some(Self { data: msg.data, size: msg.size, capacity: msg.capacity }) + } else { + None + } + } + + pub fn null() -> Self { + let msg: FindArucoWithPose_SendGoal_RequestSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + Self { data: msg.data, size: msg.size, capacity: msg.capacity } + } + + pub fn as_slice(&self) -> &[FindArucoWithPose_SendGoal_Request] { + if self.data.is_null() { + &[] + } else { + let s = unsafe { std::slice::from_raw_parts(self.data, self.size as _) }; + s + } + } + + pub fn as_slice_mut(&mut self) -> &mut [FindArucoWithPose_SendGoal_Request] { + if self.data.is_null() { + &mut [] + } else { + let s = unsafe { std::slice::from_raw_parts_mut(self.data, self.size as _) }; + s + } + } + + pub fn iter(&self) -> std::slice::Iter<'_, FindArucoWithPose_SendGoal_Request> { + self.as_slice().iter() + } + + pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, FindArucoWithPose_SendGoal_Request> { + self.as_slice_mut().iter_mut() + } + + pub fn len(&self) -> usize { + self.as_slice().len() + } + + pub fn is_empty(&self) -> bool { + self.len() == 0 + } +} + +impl Drop for FindArucoWithPose_SendGoal_RequestSeq { + fn drop(&mut self) { + let mut msg = FindArucoWithPose_SendGoal_RequestSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + unsafe { custom_interfaces__action__FindArucoWithPose_SendGoal_Request__Sequence__fini(&mut msg) }; + } +} + +unsafe impl Send for FindArucoWithPose_SendGoal_RequestSeq {} +unsafe impl Sync for FindArucoWithPose_SendGoal_RequestSeq {} + +extern "C" { + fn custom_interfaces__action__FindArucoWithPose_SendGoal_Response__init(msg: *mut FindArucoWithPose_SendGoal_Response) -> bool; + fn custom_interfaces__action__FindArucoWithPose_SendGoal_Response__fini(msg: *mut FindArucoWithPose_SendGoal_Response); + fn custom_interfaces__action__FindArucoWithPose_SendGoal_Response__are_equal(lhs: *const FindArucoWithPose_SendGoal_Response, rhs: *const FindArucoWithPose_SendGoal_Response) -> bool; + fn custom_interfaces__action__FindArucoWithPose_SendGoal_Response__Sequence__init(msg: *mut FindArucoWithPose_SendGoal_ResponseSeqRaw, size: usize) -> bool; + fn custom_interfaces__action__FindArucoWithPose_SendGoal_Response__Sequence__fini(msg: *mut FindArucoWithPose_SendGoal_ResponseSeqRaw); + fn custom_interfaces__action__FindArucoWithPose_SendGoal_Response__Sequence__are_equal(lhs: *const FindArucoWithPose_SendGoal_ResponseSeqRaw, rhs: *const FindArucoWithPose_SendGoal_ResponseSeqRaw) -> bool; + fn rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_SendGoal_Response() -> *const rcl::rosidl_message_type_support_t; +} + +impl TypeSupport for FindArucoWithPose_SendGoal_Response { + fn type_support() -> *const rcl::rosidl_message_type_support_t { + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_SendGoal_Response() + } + } +} + +impl PartialEq for FindArucoWithPose_SendGoal_Response { + fn eq(&self, other: &Self) -> bool { + unsafe { + custom_interfaces__action__FindArucoWithPose_SendGoal_Response__are_equal(self, other) + } + } +} + +impl PartialEq for FindArucoWithPose_SendGoal_ResponseSeq { + fn eq(&self, other: &Self) -> bool { + unsafe { + let msg1 = FindArucoWithPose_SendGoal_ResponseSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + let msg2 = FindArucoWithPose_SendGoal_ResponseSeqRaw{ data: other.data, size: other.size, capacity: other.capacity }; + custom_interfaces__action__FindArucoWithPose_SendGoal_Response__Sequence__are_equal(&msg1, &msg2) + } + } +} + +impl FindArucoWithPose_SendGoal_Response { + pub fn new() -> Option { + let mut msg: Self = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_SendGoal_Response__init(&mut msg) } { + Some(msg) + } else { + None + } + } +} + +impl Drop for FindArucoWithPose_SendGoal_Response { + fn drop(&mut self) { + unsafe { custom_interfaces__action__FindArucoWithPose_SendGoal_Response__fini(self) }; + } +} + +#[repr(C)] +#[derive(Debug)] +struct FindArucoWithPose_SendGoal_ResponseSeqRaw { + data: *mut FindArucoWithPose_SendGoal_Response, + size: size_t, + capacity: size_t, +} + +/// Sequence of FindArucoWithPose_SendGoal_Response. +/// `N` is the maximum number of elements. +/// If `N` is `0`, the size is unlimited. +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_SendGoal_ResponseSeq { + data: *mut FindArucoWithPose_SendGoal_Response, + size: size_t, + capacity: size_t, +} + +impl FindArucoWithPose_SendGoal_ResponseSeq { + /// Create a sequence of. + /// `N` represents the maximum number of elements. + /// If `N` is `0`, the sequence is unlimited. + pub fn new(size: usize) -> Option { + if N != 0 && size > N { + // the size exceeds in the maximum number + return None; + } + let mut msg: FindArucoWithPose_SendGoal_ResponseSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_SendGoal_Response__Sequence__init(&mut msg, size) } { + Some(Self { data: msg.data, size: msg.size, capacity: msg.capacity }) + } else { + None + } + } + + pub fn null() -> Self { + let msg: FindArucoWithPose_SendGoal_ResponseSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + Self { data: msg.data, size: msg.size, capacity: msg.capacity } + } + + pub fn as_slice(&self) -> &[FindArucoWithPose_SendGoal_Response] { + if self.data.is_null() { + &[] + } else { + let s = unsafe { std::slice::from_raw_parts(self.data, self.size as _) }; + s + } + } + + pub fn as_slice_mut(&mut self) -> &mut [FindArucoWithPose_SendGoal_Response] { + if self.data.is_null() { + &mut [] + } else { + let s = unsafe { std::slice::from_raw_parts_mut(self.data, self.size as _) }; + s + } + } + + pub fn iter(&self) -> std::slice::Iter<'_, FindArucoWithPose_SendGoal_Response> { + self.as_slice().iter() + } + + pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, FindArucoWithPose_SendGoal_Response> { + self.as_slice_mut().iter_mut() + } + + pub fn len(&self) -> usize { + self.as_slice().len() + } + + pub fn is_empty(&self) -> bool { + self.len() == 0 + } +} + +impl Drop for FindArucoWithPose_SendGoal_ResponseSeq { + fn drop(&mut self) { + let mut msg = FindArucoWithPose_SendGoal_ResponseSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + unsafe { custom_interfaces__action__FindArucoWithPose_SendGoal_Response__Sequence__fini(&mut msg) }; + } +} + +unsafe impl Send for FindArucoWithPose_SendGoal_ResponseSeq {} +unsafe impl Sync for FindArucoWithPose_SendGoal_ResponseSeq {} + +extern "C" { + fn rosidl_typesupport_c__get_service_type_support_handle__custom_interfaces__action__FindArucoWithPose_SendGoal() -> *const rcl::rosidl_service_type_support_t; +} + +#[derive(Debug)] +pub struct FindArucoWithPose_SendGoal; + +impl ActionGoal for FindArucoWithPose_SendGoal { + type Request = FindArucoWithPose_SendGoal_Request; + type Response = FindArucoWithPose_SendGoal_Response; + fn type_support() -> *const rcl::rosidl_service_type_support_t { + unsafe { + rosidl_typesupport_c__get_service_type_support_handle__custom_interfaces__action__FindArucoWithPose_SendGoal() + } + } +} + +impl GetUUID for FindArucoWithPose_SendGoal_Request { + fn get_uuid(&self) -> &[u8; 16] { + &self.goal_id.uuid + } +} + +impl GoalResponse for FindArucoWithPose_SendGoal_Response { + fn is_accepted(&self) -> bool { + self.accepted + } + + fn get_time_stamp(&self) -> UnsafeTime { + UnsafeTime { + sec: self.stamp.sec, + nanosec: self.stamp.nanosec, + } + } + + fn new(accepted: bool, stamp: UnsafeTime) -> Self { + Self { accepted, stamp } + } +} + + +#[repr(C)] +#[derive(Clone, Debug)] +pub struct FindArucoWithPose_Result { + pub structure_needs_at_least_one_member: u8, +} + +extern "C" { + fn custom_interfaces__action__FindArucoWithPose_Result__init(msg: *mut FindArucoWithPose_Result) -> bool; + fn custom_interfaces__action__FindArucoWithPose_Result__fini(msg: *mut FindArucoWithPose_Result); + fn custom_interfaces__action__FindArucoWithPose_Result__are_equal(lhs: *const FindArucoWithPose_Result, rhs: *const FindArucoWithPose_Result) -> bool; + fn custom_interfaces__action__FindArucoWithPose_Result__Sequence__init(msg: *mut FindArucoWithPose_ResultSeqRaw, size: usize) -> bool; + fn custom_interfaces__action__FindArucoWithPose_Result__Sequence__fini(msg: *mut FindArucoWithPose_ResultSeqRaw); + fn custom_interfaces__action__FindArucoWithPose_Result__Sequence__are_equal(lhs: *const FindArucoWithPose_ResultSeqRaw, rhs: *const FindArucoWithPose_ResultSeqRaw) -> bool; + fn rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_Result() -> *const rcl::rosidl_message_type_support_t; +} + +impl TypeSupport for FindArucoWithPose_Result { + fn type_support() -> *const rcl::rosidl_message_type_support_t { + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_Result() + } + } +} + +impl PartialEq for FindArucoWithPose_Result { + fn eq(&self, other: &Self) -> bool { + unsafe { + custom_interfaces__action__FindArucoWithPose_Result__are_equal(self, other) + } + } +} + +impl PartialEq for FindArucoWithPose_ResultSeq { + fn eq(&self, other: &Self) -> bool { + unsafe { + let msg1 = FindArucoWithPose_ResultSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + let msg2 = FindArucoWithPose_ResultSeqRaw{ data: other.data, size: other.size, capacity: other.capacity }; + custom_interfaces__action__FindArucoWithPose_Result__Sequence__are_equal(&msg1, &msg2) + } + } +} + +impl FindArucoWithPose_Result { + pub fn new() -> Option { + let mut msg: Self = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_Result__init(&mut msg) } { + Some(msg) + } else { + None + } + } +} + +impl Drop for FindArucoWithPose_Result { + fn drop(&mut self) { + unsafe { custom_interfaces__action__FindArucoWithPose_Result__fini(self) }; + } +} + +#[repr(C)] +#[derive(Debug)] +struct FindArucoWithPose_ResultSeqRaw { + data: *mut FindArucoWithPose_Result, + size: size_t, + capacity: size_t, +} + +/// Sequence of FindArucoWithPose_Result. +/// `N` is the maximum number of elements. +/// If `N` is `0`, the size is unlimited. +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_ResultSeq { + data: *mut FindArucoWithPose_Result, + size: size_t, + capacity: size_t, +} + +impl FindArucoWithPose_ResultSeq { + /// Create a sequence of. + /// `N` represents the maximum number of elements. + /// If `N` is `0`, the sequence is unlimited. + pub fn new(size: usize) -> Option { + if N != 0 && size > N { + // the size exceeds in the maximum number + return None; + } + let mut msg: FindArucoWithPose_ResultSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_Result__Sequence__init(&mut msg, size) } { + Some(Self { data: msg.data, size: msg.size, capacity: msg.capacity }) + } else { + None + } + } + + pub fn null() -> Self { + let msg: FindArucoWithPose_ResultSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + Self { data: msg.data, size: msg.size, capacity: msg.capacity } + } + + pub fn as_slice(&self) -> &[FindArucoWithPose_Result] { + if self.data.is_null() { + &[] + } else { + let s = unsafe { std::slice::from_raw_parts(self.data, self.size as _) }; + s + } + } + + pub fn as_slice_mut(&mut self) -> &mut [FindArucoWithPose_Result] { + if self.data.is_null() { + &mut [] + } else { + let s = unsafe { std::slice::from_raw_parts_mut(self.data, self.size as _) }; + s + } + } + + pub fn iter(&self) -> std::slice::Iter<'_, FindArucoWithPose_Result> { + self.as_slice().iter() + } + + pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, FindArucoWithPose_Result> { + self.as_slice_mut().iter_mut() + } + + pub fn len(&self) -> usize { + self.as_slice().len() + } + + pub fn is_empty(&self) -> bool { + self.len() == 0 + } +} + +impl Drop for FindArucoWithPose_ResultSeq { + fn drop(&mut self) { + let mut msg = FindArucoWithPose_ResultSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + unsafe { custom_interfaces__action__FindArucoWithPose_Result__Sequence__fini(&mut msg) }; + } +} + +unsafe impl Send for FindArucoWithPose_ResultSeq {} +unsafe impl Sync for FindArucoWithPose_ResultSeq {} + +extern "C" { + fn custom_interfaces__action__FindArucoWithPose_GetResult_Request__init(msg: *mut FindArucoWithPose_GetResult_Request) -> bool; + fn custom_interfaces__action__FindArucoWithPose_GetResult_Request__fini(msg: *mut FindArucoWithPose_GetResult_Request); + fn custom_interfaces__action__FindArucoWithPose_GetResult_Request__are_equal(lhs: *const FindArucoWithPose_GetResult_Request, rhs: *const FindArucoWithPose_GetResult_Request) -> bool; + fn custom_interfaces__action__FindArucoWithPose_GetResult_Request__Sequence__init(msg: *mut FindArucoWithPose_GetResult_RequestSeqRaw, size: usize) -> bool; + fn custom_interfaces__action__FindArucoWithPose_GetResult_Request__Sequence__fini(msg: *mut FindArucoWithPose_GetResult_RequestSeqRaw); + fn custom_interfaces__action__FindArucoWithPose_GetResult_Request__Sequence__are_equal(lhs: *const FindArucoWithPose_GetResult_RequestSeqRaw, rhs: *const FindArucoWithPose_GetResult_RequestSeqRaw) -> bool; + fn rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_GetResult_Request() -> *const rcl::rosidl_message_type_support_t; +} + +impl TypeSupport for FindArucoWithPose_GetResult_Request { + fn type_support() -> *const rcl::rosidl_message_type_support_t { + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_GetResult_Request() + } + } +} + +impl PartialEq for FindArucoWithPose_GetResult_Request { + fn eq(&self, other: &Self) -> bool { + unsafe { + custom_interfaces__action__FindArucoWithPose_GetResult_Request__are_equal(self, other) + } + } +} + +impl PartialEq for FindArucoWithPose_GetResult_RequestSeq { + fn eq(&self, other: &Self) -> bool { + unsafe { + let msg1 = FindArucoWithPose_GetResult_RequestSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + let msg2 = FindArucoWithPose_GetResult_RequestSeqRaw{ data: other.data, size: other.size, capacity: other.capacity }; + custom_interfaces__action__FindArucoWithPose_GetResult_Request__Sequence__are_equal(&msg1, &msg2) + } + } +} + +impl FindArucoWithPose_GetResult_Request { + pub fn new() -> Option { + let mut msg: Self = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_GetResult_Request__init(&mut msg) } { + Some(msg) + } else { + None + } + } +} + +impl Drop for FindArucoWithPose_GetResult_Request { + fn drop(&mut self) { + unsafe { custom_interfaces__action__FindArucoWithPose_GetResult_Request__fini(self) }; + } +} + +#[repr(C)] +#[derive(Debug)] +struct FindArucoWithPose_GetResult_RequestSeqRaw { + data: *mut FindArucoWithPose_GetResult_Request, + size: size_t, + capacity: size_t, +} + +/// Sequence of FindArucoWithPose_GetResult_Request. +/// `N` is the maximum number of elements. +/// If `N` is `0`, the size is unlimited. +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_GetResult_RequestSeq { + data: *mut FindArucoWithPose_GetResult_Request, + size: size_t, + capacity: size_t, +} + +impl FindArucoWithPose_GetResult_RequestSeq { + /// Create a sequence of. + /// `N` represents the maximum number of elements. + /// If `N` is `0`, the sequence is unlimited. + pub fn new(size: usize) -> Option { + if N != 0 && size > N { + // the size exceeds in the maximum number + return None; + } + let mut msg: FindArucoWithPose_GetResult_RequestSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_GetResult_Request__Sequence__init(&mut msg, size) } { + Some(Self { data: msg.data, size: msg.size, capacity: msg.capacity }) + } else { + None + } + } + + pub fn null() -> Self { + let msg: FindArucoWithPose_GetResult_RequestSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + Self { data: msg.data, size: msg.size, capacity: msg.capacity } + } + + pub fn as_slice(&self) -> &[FindArucoWithPose_GetResult_Request] { + if self.data.is_null() { + &[] + } else { + let s = unsafe { std::slice::from_raw_parts(self.data, self.size as _) }; + s + } + } + + pub fn as_slice_mut(&mut self) -> &mut [FindArucoWithPose_GetResult_Request] { + if self.data.is_null() { + &mut [] + } else { + let s = unsafe { std::slice::from_raw_parts_mut(self.data, self.size as _) }; + s + } + } + + pub fn iter(&self) -> std::slice::Iter<'_, FindArucoWithPose_GetResult_Request> { + self.as_slice().iter() + } + + pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, FindArucoWithPose_GetResult_Request> { + self.as_slice_mut().iter_mut() + } + + pub fn len(&self) -> usize { + self.as_slice().len() + } + + pub fn is_empty(&self) -> bool { + self.len() == 0 + } +} + +impl Drop for FindArucoWithPose_GetResult_RequestSeq { + fn drop(&mut self) { + let mut msg = FindArucoWithPose_GetResult_RequestSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + unsafe { custom_interfaces__action__FindArucoWithPose_GetResult_Request__Sequence__fini(&mut msg) }; + } +} + +unsafe impl Send for FindArucoWithPose_GetResult_RequestSeq {} +unsafe impl Sync for FindArucoWithPose_GetResult_RequestSeq {} + +extern "C" { + fn custom_interfaces__action__FindArucoWithPose_GetResult_Response__init(msg: *mut FindArucoWithPose_GetResult_Response) -> bool; + fn custom_interfaces__action__FindArucoWithPose_GetResult_Response__fini(msg: *mut FindArucoWithPose_GetResult_Response); + fn custom_interfaces__action__FindArucoWithPose_GetResult_Response__are_equal(lhs: *const FindArucoWithPose_GetResult_Response, rhs: *const FindArucoWithPose_GetResult_Response) -> bool; + fn custom_interfaces__action__FindArucoWithPose_GetResult_Response__Sequence__init(msg: *mut FindArucoWithPose_GetResult_ResponseSeqRaw, size: usize) -> bool; + fn custom_interfaces__action__FindArucoWithPose_GetResult_Response__Sequence__fini(msg: *mut FindArucoWithPose_GetResult_ResponseSeqRaw); + fn custom_interfaces__action__FindArucoWithPose_GetResult_Response__Sequence__are_equal(lhs: *const FindArucoWithPose_GetResult_ResponseSeqRaw, rhs: *const FindArucoWithPose_GetResult_ResponseSeqRaw) -> bool; + fn rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_GetResult_Response() -> *const rcl::rosidl_message_type_support_t; +} + +impl TypeSupport for FindArucoWithPose_GetResult_Response { + fn type_support() -> *const rcl::rosidl_message_type_support_t { + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_GetResult_Response() + } + } +} + +impl PartialEq for FindArucoWithPose_GetResult_Response { + fn eq(&self, other: &Self) -> bool { + unsafe { + custom_interfaces__action__FindArucoWithPose_GetResult_Response__are_equal(self, other) + } + } +} + +impl PartialEq for FindArucoWithPose_GetResult_ResponseSeq { + fn eq(&self, other: &Self) -> bool { + unsafe { + let msg1 = FindArucoWithPose_GetResult_ResponseSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + let msg2 = FindArucoWithPose_GetResult_ResponseSeqRaw{ data: other.data, size: other.size, capacity: other.capacity }; + custom_interfaces__action__FindArucoWithPose_GetResult_Response__Sequence__are_equal(&msg1, &msg2) + } + } +} + +impl FindArucoWithPose_GetResult_Response { + pub fn new() -> Option { + let mut msg: Self = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_GetResult_Response__init(&mut msg) } { + Some(msg) + } else { + None + } + } +} + +impl Drop for FindArucoWithPose_GetResult_Response { + fn drop(&mut self) { + unsafe { custom_interfaces__action__FindArucoWithPose_GetResult_Response__fini(self) }; + } +} + +#[repr(C)] +#[derive(Debug)] +struct FindArucoWithPose_GetResult_ResponseSeqRaw { + data: *mut FindArucoWithPose_GetResult_Response, + size: size_t, + capacity: size_t, +} + +/// Sequence of FindArucoWithPose_GetResult_Response. +/// `N` is the maximum number of elements. +/// If `N` is `0`, the size is unlimited. +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_GetResult_ResponseSeq { + data: *mut FindArucoWithPose_GetResult_Response, + size: size_t, + capacity: size_t, +} + +impl FindArucoWithPose_GetResult_ResponseSeq { + /// Create a sequence of. + /// `N` represents the maximum number of elements. + /// If `N` is `0`, the sequence is unlimited. + pub fn new(size: usize) -> Option { + if N != 0 && size > N { + // the size exceeds in the maximum number + return None; + } + let mut msg: FindArucoWithPose_GetResult_ResponseSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_GetResult_Response__Sequence__init(&mut msg, size) } { + Some(Self { data: msg.data, size: msg.size, capacity: msg.capacity }) + } else { + None + } + } + + pub fn null() -> Self { + let msg: FindArucoWithPose_GetResult_ResponseSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + Self { data: msg.data, size: msg.size, capacity: msg.capacity } + } + + pub fn as_slice(&self) -> &[FindArucoWithPose_GetResult_Response] { + if self.data.is_null() { + &[] + } else { + let s = unsafe { std::slice::from_raw_parts(self.data, self.size as _) }; + s + } + } + + pub fn as_slice_mut(&mut self) -> &mut [FindArucoWithPose_GetResult_Response] { + if self.data.is_null() { + &mut [] + } else { + let s = unsafe { std::slice::from_raw_parts_mut(self.data, self.size as _) }; + s + } + } + + pub fn iter(&self) -> std::slice::Iter<'_, FindArucoWithPose_GetResult_Response> { + self.as_slice().iter() + } + + pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, FindArucoWithPose_GetResult_Response> { + self.as_slice_mut().iter_mut() + } + + pub fn len(&self) -> usize { + self.as_slice().len() + } + + pub fn is_empty(&self) -> bool { + self.len() == 0 + } +} + +impl Drop for FindArucoWithPose_GetResult_ResponseSeq { + fn drop(&mut self) { + let mut msg = FindArucoWithPose_GetResult_ResponseSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + unsafe { custom_interfaces__action__FindArucoWithPose_GetResult_Response__Sequence__fini(&mut msg) }; + } +} + +unsafe impl Send for FindArucoWithPose_GetResult_ResponseSeq {} +unsafe impl Sync for FindArucoWithPose_GetResult_ResponseSeq {} + +extern "C" { + fn rosidl_typesupport_c__get_service_type_support_handle__custom_interfaces__action__FindArucoWithPose_GetResult() -> *const rcl::rosidl_service_type_support_t; +} + +#[derive(Debug)] +pub struct FindArucoWithPose_GetResult; + +impl ActionResult for FindArucoWithPose_GetResult { + type Request = FindArucoWithPose_GetResult_Request; + type Response = FindArucoWithPose_GetResult_Response; + fn type_support() -> *const rcl::rosidl_service_type_support_t { + unsafe { + rosidl_typesupport_c__get_service_type_support_handle__custom_interfaces__action__FindArucoWithPose_GetResult() + } + } +} + +impl GetUUID for FindArucoWithPose_GetResult_Request { + fn get_uuid(&self) -> &[u8; 16] { + &self.goal_id.uuid + } +} + +impl ResultResponse for FindArucoWithPose_GetResult_Response { + fn get_status(&self) -> u8 { + self.status + } +} + + +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_Feedback { + pub marker_poses: geometry_msgs::msg::PoseSeq<0>, + pub marker_ids: safe_drive::msg::U8Seq<0>, + pub time_last_image_arrived: builtin_interfaces::msg::Time, +} + +extern "C" { + fn custom_interfaces__action__FindArucoWithPose_Feedback__init(msg: *mut FindArucoWithPose_Feedback) -> bool; + fn custom_interfaces__action__FindArucoWithPose_Feedback__fini(msg: *mut FindArucoWithPose_Feedback); + fn custom_interfaces__action__FindArucoWithPose_Feedback__are_equal(lhs: *const FindArucoWithPose_Feedback, rhs: *const FindArucoWithPose_Feedback) -> bool; + fn custom_interfaces__action__FindArucoWithPose_Feedback__Sequence__init(msg: *mut FindArucoWithPose_FeedbackSeqRaw, size: usize) -> bool; + fn custom_interfaces__action__FindArucoWithPose_Feedback__Sequence__fini(msg: *mut FindArucoWithPose_FeedbackSeqRaw); + fn custom_interfaces__action__FindArucoWithPose_Feedback__Sequence__are_equal(lhs: *const FindArucoWithPose_FeedbackSeqRaw, rhs: *const FindArucoWithPose_FeedbackSeqRaw) -> bool; + fn rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_Feedback() -> *const rcl::rosidl_message_type_support_t; +} + +impl TypeSupport for FindArucoWithPose_Feedback { + fn type_support() -> *const rcl::rosidl_message_type_support_t { + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_Feedback() + } + } +} + +impl PartialEq for FindArucoWithPose_Feedback { + fn eq(&self, other: &Self) -> bool { + unsafe { + custom_interfaces__action__FindArucoWithPose_Feedback__are_equal(self, other) + } + } +} + +impl PartialEq for FindArucoWithPose_FeedbackSeq { + fn eq(&self, other: &Self) -> bool { + unsafe { + let msg1 = FindArucoWithPose_FeedbackSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + let msg2 = FindArucoWithPose_FeedbackSeqRaw{ data: other.data, size: other.size, capacity: other.capacity }; + custom_interfaces__action__FindArucoWithPose_Feedback__Sequence__are_equal(&msg1, &msg2) + } + } +} + +impl FindArucoWithPose_Feedback { + pub fn new() -> Option { + let mut msg: Self = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_Feedback__init(&mut msg) } { + Some(msg) + } else { + None + } + } +} + +impl Drop for FindArucoWithPose_Feedback { + fn drop(&mut self) { + unsafe { custom_interfaces__action__FindArucoWithPose_Feedback__fini(self) }; + } +} + +#[repr(C)] +#[derive(Debug)] +struct FindArucoWithPose_FeedbackSeqRaw { + data: *mut FindArucoWithPose_Feedback, + size: size_t, + capacity: size_t, +} + +/// Sequence of FindArucoWithPose_Feedback. +/// `N` is the maximum number of elements. +/// If `N` is `0`, the size is unlimited. +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_FeedbackSeq { + data: *mut FindArucoWithPose_Feedback, + size: size_t, + capacity: size_t, +} + +impl FindArucoWithPose_FeedbackSeq { + /// Create a sequence of. + /// `N` represents the maximum number of elements. + /// If `N` is `0`, the sequence is unlimited. + pub fn new(size: usize) -> Option { + if N != 0 && size > N { + // the size exceeds in the maximum number + return None; + } + let mut msg: FindArucoWithPose_FeedbackSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_Feedback__Sequence__init(&mut msg, size) } { + Some(Self { data: msg.data, size: msg.size, capacity: msg.capacity }) + } else { + None + } + } + + pub fn null() -> Self { + let msg: FindArucoWithPose_FeedbackSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + Self { data: msg.data, size: msg.size, capacity: msg.capacity } + } + + pub fn as_slice(&self) -> &[FindArucoWithPose_Feedback] { + if self.data.is_null() { + &[] + } else { + let s = unsafe { std::slice::from_raw_parts(self.data, self.size as _) }; + s + } + } + + pub fn as_slice_mut(&mut self) -> &mut [FindArucoWithPose_Feedback] { + if self.data.is_null() { + &mut [] + } else { + let s = unsafe { std::slice::from_raw_parts_mut(self.data, self.size as _) }; + s + } + } + + pub fn iter(&self) -> std::slice::Iter<'_, FindArucoWithPose_Feedback> { + self.as_slice().iter() + } + + pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, FindArucoWithPose_Feedback> { + self.as_slice_mut().iter_mut() + } + + pub fn len(&self) -> usize { + self.as_slice().len() + } + + pub fn is_empty(&self) -> bool { + self.len() == 0 + } +} + +impl Drop for FindArucoWithPose_FeedbackSeq { + fn drop(&mut self) { + let mut msg = FindArucoWithPose_FeedbackSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + unsafe { custom_interfaces__action__FindArucoWithPose_Feedback__Sequence__fini(&mut msg) }; + } +} + +unsafe impl Send for FindArucoWithPose_FeedbackSeq {} +unsafe impl Sync for FindArucoWithPose_FeedbackSeq {} + +extern "C" { + fn custom_interfaces__action__FindArucoWithPose_FeedbackMessage__init(msg: *mut FindArucoWithPose_FeedbackMessage) -> bool; + fn custom_interfaces__action__FindArucoWithPose_FeedbackMessage__fini(msg: *mut FindArucoWithPose_FeedbackMessage); + fn custom_interfaces__action__FindArucoWithPose_FeedbackMessage__are_equal(lhs: *const FindArucoWithPose_FeedbackMessage, rhs: *const FindArucoWithPose_FeedbackMessage) -> bool; + fn custom_interfaces__action__FindArucoWithPose_FeedbackMessage__Sequence__init(msg: *mut FindArucoWithPose_FeedbackMessageSeqRaw, size: usize) -> bool; + fn custom_interfaces__action__FindArucoWithPose_FeedbackMessage__Sequence__fini(msg: *mut FindArucoWithPose_FeedbackMessageSeqRaw); + fn custom_interfaces__action__FindArucoWithPose_FeedbackMessage__Sequence__are_equal(lhs: *const FindArucoWithPose_FeedbackMessageSeqRaw, rhs: *const FindArucoWithPose_FeedbackMessageSeqRaw) -> bool; + fn rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_FeedbackMessage() -> *const rcl::rosidl_message_type_support_t; +} + +impl TypeSupport for FindArucoWithPose_FeedbackMessage { + fn type_support() -> *const rcl::rosidl_message_type_support_t { + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__custom_interfaces__action__FindArucoWithPose_FeedbackMessage() + } + } +} + +impl PartialEq for FindArucoWithPose_FeedbackMessage { + fn eq(&self, other: &Self) -> bool { + unsafe { + custom_interfaces__action__FindArucoWithPose_FeedbackMessage__are_equal(self, other) + } + } +} + +impl PartialEq for FindArucoWithPose_FeedbackMessageSeq { + fn eq(&self, other: &Self) -> bool { + unsafe { + let msg1 = FindArucoWithPose_FeedbackMessageSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + let msg2 = FindArucoWithPose_FeedbackMessageSeqRaw{ data: other.data, size: other.size, capacity: other.capacity }; + custom_interfaces__action__FindArucoWithPose_FeedbackMessage__Sequence__are_equal(&msg1, &msg2) + } + } +} + +impl FindArucoWithPose_FeedbackMessage { + pub fn new() -> Option { + let mut msg: Self = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_FeedbackMessage__init(&mut msg) } { + Some(msg) + } else { + None + } + } +} + +impl Drop for FindArucoWithPose_FeedbackMessage { + fn drop(&mut self) { + unsafe { custom_interfaces__action__FindArucoWithPose_FeedbackMessage__fini(self) }; + } +} + +#[repr(C)] +#[derive(Debug)] +struct FindArucoWithPose_FeedbackMessageSeqRaw { + data: *mut FindArucoWithPose_FeedbackMessage, + size: size_t, + capacity: size_t, +} + +/// Sequence of FindArucoWithPose_FeedbackMessage. +/// `N` is the maximum number of elements. +/// If `N` is `0`, the size is unlimited. +#[repr(C)] +#[derive(Debug)] +pub struct FindArucoWithPose_FeedbackMessageSeq { + data: *mut FindArucoWithPose_FeedbackMessage, + size: size_t, + capacity: size_t, +} + +impl FindArucoWithPose_FeedbackMessageSeq { + /// Create a sequence of. + /// `N` represents the maximum number of elements. + /// If `N` is `0`, the sequence is unlimited. + pub fn new(size: usize) -> Option { + if N != 0 && size > N { + // the size exceeds in the maximum number + return None; + } + let mut msg: FindArucoWithPose_FeedbackMessageSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { custom_interfaces__action__FindArucoWithPose_FeedbackMessage__Sequence__init(&mut msg, size) } { + Some(Self { data: msg.data, size: msg.size, capacity: msg.capacity }) + } else { + None + } + } + + pub fn null() -> Self { + let msg: FindArucoWithPose_FeedbackMessageSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + Self { data: msg.data, size: msg.size, capacity: msg.capacity } + } + + pub fn as_slice(&self) -> &[FindArucoWithPose_FeedbackMessage] { + if self.data.is_null() { + &[] + } else { + let s = unsafe { std::slice::from_raw_parts(self.data, self.size as _) }; + s + } + } + + pub fn as_slice_mut(&mut self) -> &mut [FindArucoWithPose_FeedbackMessage] { + if self.data.is_null() { + &mut [] + } else { + let s = unsafe { std::slice::from_raw_parts_mut(self.data, self.size as _) }; + s + } + } + + pub fn iter(&self) -> std::slice::Iter<'_, FindArucoWithPose_FeedbackMessage> { + self.as_slice().iter() + } + + pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, FindArucoWithPose_FeedbackMessage> { + self.as_slice_mut().iter_mut() + } + + pub fn len(&self) -> usize { + self.as_slice().len() + } + + pub fn is_empty(&self) -> bool { + self.len() == 0 + } +} + +impl Drop for FindArucoWithPose_FeedbackMessageSeq { + fn drop(&mut self) { + let mut msg = FindArucoWithPose_FeedbackMessageSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + unsafe { custom_interfaces__action__FindArucoWithPose_FeedbackMessage__Sequence__fini(&mut msg) }; + } +} + +unsafe impl Send for FindArucoWithPose_FeedbackMessageSeq {} +unsafe impl Sync for FindArucoWithPose_FeedbackMessageSeq {} + +impl GetUUID for FindArucoWithPose_FeedbackMessage { + fn get_uuid(&self) -> &[u8; 16] { + &self.goal_id.uuid + } +} diff --git a/src/custom_interfaces/bindings/geometry_msgs/cksum b/src/custom_interfaces/bindings/geometry_msgs/cksum index 7c036762..4dd8b85f 100644 --- a/src/custom_interfaces/bindings/geometry_msgs/cksum +++ b/src/custom_interfaces/bindings/geometry_msgs/cksum @@ -1,3 +1,3 @@ 0.2.6 Version("0.4.3") -91NZRnFO3CKWWnuEZRWtG79k6+iMXlBPvVv+jI/4qQW6P0lLzxSo2Z4AbwH3vp2cr0CWhVB5xxIdCCmfR4e6Ew== \ No newline at end of file +Qgh9T4pKrFDVLIn11CxrHZCUzQ46Vrc00pbJ1ptJZfZwcHoFjuHdLUZvsRwEGR+fXUpG0VBS/xENqZ52ByG7LQ== \ No newline at end of file diff --git a/src/custom_interfaces/bindings/geometry_msgs/src/msg.rs b/src/custom_interfaces/bindings/geometry_msgs/src/msg.rs index 7e01a372..25a650ff 100644 --- a/src/custom_interfaces/bindings/geometry_msgs/src/msg.rs +++ b/src/custom_interfaces/bindings/geometry_msgs/src/msg.rs @@ -28,6 +28,12 @@ pub use point_stamped::*; pub mod polygon; pub use polygon::*; +pub mod polygon_instance; +pub use polygon_instance::*; + +pub mod polygon_instance_stamped; +pub use polygon_instance_stamped::*; + pub mod polygon_stamped; pub use polygon_stamped::*; diff --git a/src/custom_interfaces/bindings/geometry_msgs/src/msg/polygon_instance.rs b/src/custom_interfaces/bindings/geometry_msgs/src/msg/polygon_instance.rs new file mode 100644 index 00000000..5688ba19 --- /dev/null +++ b/src/custom_interfaces/bindings/geometry_msgs/src/msg/polygon_instance.rs @@ -0,0 +1,147 @@ +use safe_drive::{msg::TypeSupport, rcl::{self, size_t}}; + +#[repr(C)] +#[derive(Debug)] +pub struct PolygonInstance { + pub polygon: crate::msg::Polygon, + pub id: i64, +} + +extern "C" { + fn geometry_msgs__msg__PolygonInstance__init(msg: *mut PolygonInstance) -> bool; + fn geometry_msgs__msg__PolygonInstance__fini(msg: *mut PolygonInstance); + fn geometry_msgs__msg__PolygonInstance__are_equal(lhs: *const PolygonInstance, rhs: *const PolygonInstance) -> bool; + fn geometry_msgs__msg__PolygonInstance__Sequence__init(msg: *mut PolygonInstanceSeqRaw, size: usize) -> bool; + fn geometry_msgs__msg__PolygonInstance__Sequence__fini(msg: *mut PolygonInstanceSeqRaw); + fn geometry_msgs__msg__PolygonInstance__Sequence__are_equal(lhs: *const PolygonInstanceSeqRaw, rhs: *const PolygonInstanceSeqRaw) -> bool; + fn rosidl_typesupport_c__get_message_type_support_handle__geometry_msgs__msg__PolygonInstance() -> *const rcl::rosidl_message_type_support_t; +} + +impl TypeSupport for PolygonInstance { + fn type_support() -> *const rcl::rosidl_message_type_support_t { + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__geometry_msgs__msg__PolygonInstance() + } + } +} + +impl PartialEq for PolygonInstance { + fn eq(&self, other: &Self) -> bool { + unsafe { + geometry_msgs__msg__PolygonInstance__are_equal(self, other) + } + } +} + +impl PartialEq for PolygonInstanceSeq { + fn eq(&self, other: &Self) -> bool { + unsafe { + let msg1 = PolygonInstanceSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + let msg2 = PolygonInstanceSeqRaw{ data: other.data, size: other.size, capacity: other.capacity }; + geometry_msgs__msg__PolygonInstance__Sequence__are_equal(&msg1, &msg2) + } + } +} + +impl PolygonInstance { + pub fn new() -> Option { + let mut msg: Self = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { geometry_msgs__msg__PolygonInstance__init(&mut msg) } { + Some(msg) + } else { + None + } + } +} + +impl Drop for PolygonInstance { + fn drop(&mut self) { + unsafe { geometry_msgs__msg__PolygonInstance__fini(self) }; + } +} + +#[repr(C)] +#[derive(Debug)] +struct PolygonInstanceSeqRaw { + data: *mut PolygonInstance, + size: size_t, + capacity: size_t, +} + +/// Sequence of PolygonInstance. +/// `N` is the maximum number of elements. +/// If `N` is `0`, the size is unlimited. +#[repr(C)] +#[derive(Debug)] +pub struct PolygonInstanceSeq { + data: *mut PolygonInstance, + size: size_t, + capacity: size_t, +} + +impl PolygonInstanceSeq { + /// Create a sequence of. + /// `N` represents the maximum number of elements. + /// If `N` is `0`, the sequence is unlimited. + pub fn new(size: usize) -> Option { + if N != 0 && size > N { + // the size exceeds in the maximum number + return None; + } + let mut msg: PolygonInstanceSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { geometry_msgs__msg__PolygonInstance__Sequence__init(&mut msg, size) } { + Some(Self { data: msg.data, size: msg.size, capacity: msg.capacity }) + } else { + None + } + } + + pub fn null() -> Self { + let msg: PolygonInstanceSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + Self { data: msg.data, size: msg.size, capacity: msg.capacity } + } + + pub fn as_slice(&self) -> &[PolygonInstance] { + if self.data.is_null() { + &[] + } else { + let s = unsafe { std::slice::from_raw_parts(self.data, self.size as _) }; + s + } + } + + pub fn as_slice_mut(&mut self) -> &mut [PolygonInstance] { + if self.data.is_null() { + &mut [] + } else { + let s = unsafe { std::slice::from_raw_parts_mut(self.data, self.size as _) }; + s + } + } + + pub fn iter(&self) -> std::slice::Iter<'_, PolygonInstance> { + self.as_slice().iter() + } + + pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, PolygonInstance> { + self.as_slice_mut().iter_mut() + } + + pub fn len(&self) -> usize { + self.as_slice().len() + } + + pub fn is_empty(&self) -> bool { + self.len() == 0 + } +} + +impl Drop for PolygonInstanceSeq { + fn drop(&mut self) { + let mut msg = PolygonInstanceSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + unsafe { geometry_msgs__msg__PolygonInstance__Sequence__fini(&mut msg) }; + } +} + +unsafe impl Send for PolygonInstanceSeq {} +unsafe impl Sync for PolygonInstanceSeq {} diff --git a/src/custom_interfaces/bindings/geometry_msgs/src/msg/polygon_instance_stamped.rs b/src/custom_interfaces/bindings/geometry_msgs/src/msg/polygon_instance_stamped.rs new file mode 100644 index 00000000..46447ed4 --- /dev/null +++ b/src/custom_interfaces/bindings/geometry_msgs/src/msg/polygon_instance_stamped.rs @@ -0,0 +1,147 @@ +use safe_drive::{msg::TypeSupport, rcl::{self, size_t}}; + +#[repr(C)] +#[derive(Debug)] +pub struct PolygonInstanceStamped { + pub header: std_msgs::msg::Header, + pub polygon: crate::msg::PolygonInstance, +} + +extern "C" { + fn geometry_msgs__msg__PolygonInstanceStamped__init(msg: *mut PolygonInstanceStamped) -> bool; + fn geometry_msgs__msg__PolygonInstanceStamped__fini(msg: *mut PolygonInstanceStamped); + fn geometry_msgs__msg__PolygonInstanceStamped__are_equal(lhs: *const PolygonInstanceStamped, rhs: *const PolygonInstanceStamped) -> bool; + fn geometry_msgs__msg__PolygonInstanceStamped__Sequence__init(msg: *mut PolygonInstanceStampedSeqRaw, size: usize) -> bool; + fn geometry_msgs__msg__PolygonInstanceStamped__Sequence__fini(msg: *mut PolygonInstanceStampedSeqRaw); + fn geometry_msgs__msg__PolygonInstanceStamped__Sequence__are_equal(lhs: *const PolygonInstanceStampedSeqRaw, rhs: *const PolygonInstanceStampedSeqRaw) -> bool; + fn rosidl_typesupport_c__get_message_type_support_handle__geometry_msgs__msg__PolygonInstanceStamped() -> *const rcl::rosidl_message_type_support_t; +} + +impl TypeSupport for PolygonInstanceStamped { + fn type_support() -> *const rcl::rosidl_message_type_support_t { + unsafe { + rosidl_typesupport_c__get_message_type_support_handle__geometry_msgs__msg__PolygonInstanceStamped() + } + } +} + +impl PartialEq for PolygonInstanceStamped { + fn eq(&self, other: &Self) -> bool { + unsafe { + geometry_msgs__msg__PolygonInstanceStamped__are_equal(self, other) + } + } +} + +impl PartialEq for PolygonInstanceStampedSeq { + fn eq(&self, other: &Self) -> bool { + unsafe { + let msg1 = PolygonInstanceStampedSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + let msg2 = PolygonInstanceStampedSeqRaw{ data: other.data, size: other.size, capacity: other.capacity }; + geometry_msgs__msg__PolygonInstanceStamped__Sequence__are_equal(&msg1, &msg2) + } + } +} + +impl PolygonInstanceStamped { + pub fn new() -> Option { + let mut msg: Self = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { geometry_msgs__msg__PolygonInstanceStamped__init(&mut msg) } { + Some(msg) + } else { + None + } + } +} + +impl Drop for PolygonInstanceStamped { + fn drop(&mut self) { + unsafe { geometry_msgs__msg__PolygonInstanceStamped__fini(self) }; + } +} + +#[repr(C)] +#[derive(Debug)] +struct PolygonInstanceStampedSeqRaw { + data: *mut PolygonInstanceStamped, + size: size_t, + capacity: size_t, +} + +/// Sequence of PolygonInstanceStamped. +/// `N` is the maximum number of elements. +/// If `N` is `0`, the size is unlimited. +#[repr(C)] +#[derive(Debug)] +pub struct PolygonInstanceStampedSeq { + data: *mut PolygonInstanceStamped, + size: size_t, + capacity: size_t, +} + +impl PolygonInstanceStampedSeq { + /// Create a sequence of. + /// `N` represents the maximum number of elements. + /// If `N` is `0`, the sequence is unlimited. + pub fn new(size: usize) -> Option { + if N != 0 && size > N { + // the size exceeds in the maximum number + return None; + } + let mut msg: PolygonInstanceStampedSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + if unsafe { geometry_msgs__msg__PolygonInstanceStamped__Sequence__init(&mut msg, size) } { + Some(Self { data: msg.data, size: msg.size, capacity: msg.capacity }) + } else { + None + } + } + + pub fn null() -> Self { + let msg: PolygonInstanceStampedSeqRaw = unsafe { std::mem::MaybeUninit::zeroed().assume_init() }; + Self { data: msg.data, size: msg.size, capacity: msg.capacity } + } + + pub fn as_slice(&self) -> &[PolygonInstanceStamped] { + if self.data.is_null() { + &[] + } else { + let s = unsafe { std::slice::from_raw_parts(self.data, self.size as _) }; + s + } + } + + pub fn as_slice_mut(&mut self) -> &mut [PolygonInstanceStamped] { + if self.data.is_null() { + &mut [] + } else { + let s = unsafe { std::slice::from_raw_parts_mut(self.data, self.size as _) }; + s + } + } + + pub fn iter(&self) -> std::slice::Iter<'_, PolygonInstanceStamped> { + self.as_slice().iter() + } + + pub fn iter_mut(&mut self) -> std::slice::IterMut<'_, PolygonInstanceStamped> { + self.as_slice_mut().iter_mut() + } + + pub fn len(&self) -> usize { + self.as_slice().len() + } + + pub fn is_empty(&self) -> bool { + self.len() == 0 + } +} + +impl Drop for PolygonInstanceStampedSeq { + fn drop(&mut self) { + let mut msg = PolygonInstanceStampedSeqRaw{ data: self.data, size: self.size, capacity: self.capacity }; + unsafe { geometry_msgs__msg__PolygonInstanceStamped__Sequence__fini(&mut msg) }; + } +} + +unsafe impl Send for PolygonInstanceStampedSeq {} +unsafe impl Sync for PolygonInstanceStampedSeq {} diff --git a/src/custom_interfaces/bindings/unique_identifier_msgs/cksum b/src/custom_interfaces/bindings/unique_identifier_msgs/cksum index 739f22bd..0fe9d9c0 100644 --- a/src/custom_interfaces/bindings/unique_identifier_msgs/cksum +++ b/src/custom_interfaces/bindings/unique_identifier_msgs/cksum @@ -1,3 +1,3 @@ 0.2.6 Version("0.4.3") -o19CxSX0E26F8ppaiJ0hAQX9tYAKqJL3LQG+jeb5lMLaMSkGA1x8lEP0ETDmdfHz8F4D83m0utjq884uaLQvFg== \ No newline at end of file +ikBqjA+K9G8yNQh0Mp87YNHtfbgPBNfsKdvv40L4cZPfpEZe4/FDa4Knp76EqfD6mZqtQJPWL5QXROiiduD3og== \ No newline at end of file From 200c12445d706dc6be6509331ac52b857b7bfd87 Mon Sep 17 00:00:00 2001 From: Barrett Date: Mon, 6 Apr 2026 19:29:12 -0500 Subject: [PATCH 11/20] feat(aruco): add preferred encoding to OpenCV conv --- src/aruco/aruco_node/main.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/aruco/aruco_node/main.py b/src/aruco/aruco_node/main.py index e2b7326a..c4590f78 100755 --- a/src/aruco/aruco_node/main.py +++ b/src/aruco/aruco_node/main.py @@ -240,7 +240,7 @@ def _mono_image_callback(self, image: RosImage): recv_time: Time = self.get_clock().now().to_msg() # convert the ROS 2 `Image` into an OpenCV `Mat` - cv_image: cv.Mat = self.bridge.imgmsg_to_cv2(image) # pyright: ignore[reportAssignmentType] + cv_image: cv.Mat = self.bridge.imgmsg_to_cv2(image, desired_encoding="bgr8") # pyright: ignore[reportAssignmentType] # check for markers visible in the image dect_res = self.detect_aruco_markers(cv_image) From 4fb604ee7aad1c9cf9d7ad92b4187adf5b319ee4 Mon Sep 17 00:00:00 2001 From: Barrett Date: Mon, 6 Apr 2026 19:29:50 -0500 Subject: [PATCH 12/20] fix(aruco): tuple order mistake lol --- src/aruco/aruco_node/utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/aruco/aruco_node/utils.py b/src/aruco/aruco_node/utils.py index 4ec83c69..e08f0709 100644 --- a/src/aruco/aruco_node/utils.py +++ b/src/aruco/aruco_node/utils.py @@ -92,4 +92,4 @@ def _calc_object_pose_inner( cv_to_robot @ translation_vector ).flatten() - return (orientation_quaternion, translation_robot) + return (translation_robot, orientation_quaternion) From f180e00f255ea7fcd38507bc91589d71c5466ecb Mon Sep 17 00:00:00 2001 From: Barrett Date: Mon, 6 Apr 2026 19:30:07 -0500 Subject: [PATCH 13/20] feat(aruco): add visualization script --- src/aruco/extra_scripts/aruco_visualizer.py | 230 ++++++++++++++++++++ 1 file changed, 230 insertions(+) create mode 100644 src/aruco/extra_scripts/aruco_visualizer.py diff --git a/src/aruco/extra_scripts/aruco_visualizer.py b/src/aruco/extra_scripts/aruco_visualizer.py new file mode 100644 index 00000000..f6a60160 --- /dev/null +++ b/src/aruco/extra_scripts/aruco_visualizer.py @@ -0,0 +1,230 @@ +#!/usr/bin/env python3 +""" +Standalone ArUco Marker Visualizer + +A simple Python script that subscribes to your camera feed and displays +detected ArUco markers with visual overlays. + +No ROS package setup required - just run it! + +Usage: + python3 visualizer.py + +Options: + --image-topic IMAGE_TOPIC Topic to subscribe to (default: /sensors/mono_image) + --aruco-dict DICT_NAME ArUco dictionary to use (default: 4x4_50) +""" + +import argparse +import sys +from typing import Optional + +import cv2 +import cv2.aruco as aruco +import numpy as np +import rclpy +from cv_bridge import CvBridge +from sensor_msgs.msg import Image as RosImage + + +class ArucoVisualizer: + """Simple standalone ArUco visualizer.""" + + def __init__(self, image_topic: str, aruco_dict_name: str): + """ + Initialize the visualizer. + + Args: + image_topic: ROS topic to subscribe to + aruco_dict_name: ArUco dictionary name (e.g., "4x4_50") + """ + self.image_topic = image_topic + self.bridge = CvBridge() + self.current_image: Optional[np.ndarray] = None + self.running = True + + # Initialize ArUco detector + aruco_dict_map = { + "4x4_50": aruco.DICT_4X4_50, + "4x4_100": aruco.DICT_4X4_100, + "4x4_250": aruco.DICT_4X4_250, + "4x4_1000": aruco.DICT_4X4_1000, + "5x5_50": aruco.DICT_5X5_50, + "5x5_100": aruco.DICT_5X5_100, + "5x5_250": aruco.DICT_5X5_250, + "5x5_1000": aruco.DICT_5X5_1000, + "6x6_50": aruco.DICT_6X6_50, + "6x6_100": aruco.DICT_6X6_100, + "6x6_250": aruco.DICT_6X6_250, + "6x6_1000": aruco.DICT_6X6_1000, + "7x7_50": aruco.DICT_7X7_50, + "7x7_100": aruco.DICT_7X7_100, + "7x7_250": aruco.DICT_7X7_250, + "7x7_1000": aruco.DICT_7X7_1000, + } + + if aruco_dict_name not in aruco_dict_map: + raise ValueError(f"Unknown ArUco dictionary: {aruco_dict_name}") + + aruco_dict = aruco.getPredefinedDictionary(aruco_dict_map[aruco_dict_name]) + detector_params = aruco.DetectorParameters() + self.detector = aruco.ArucoDetector(aruco_dict, detector_params) + + print(f"[INFO] Using ArUco dictionary: {aruco_dict_name}") + print(f"[INFO] Subscribing to: {image_topic}") + print("[INFO] Press 'q' in the window to quit\n") + + def image_callback(self, msg: RosImage): + """Callback when a new image arrives.""" + try: + self.current_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8") + except Exception as e: + print(f"[ERROR] Failed to convert image: {e}") + + def draw_markers(self, frame: np.ndarray) -> tuple[np.ndarray, int]: + """Detect and draw ArUco markers on the frame.""" + display_frame = frame.copy() + + # Detect markers + marker_corners, marker_ids, _rejected = self.detector.detectMarkers(frame) + + num_markers = len(marker_ids) if marker_ids is not None else 0 + + if marker_ids is not None and num_markers > 0: + for i, marker_id in enumerate(marker_ids): + corners = marker_corners[i][0].astype(int) + + # Draw bounding box (green) + cv2.polylines(display_frame, [corners], True, (0, 255, 0), 2) + + # Draw corner circles (blue) + for corner in corners: + cv2.circle(display_frame, tuple(corner), 5, (255, 0, 0), -1) + + # Draw ID at center + center_x = int(np.mean(corners[:, 0])) + center_y = int(np.mean(corners[:, 1])) + + id_text = f"ID: {int(marker_id[0])}" + font = cv2.FONT_HERSHEY_SIMPLEX + font_scale = 0.8 + thickness = 2 + text_size = cv2.getTextSize(id_text, font, font_scale, thickness)[0] + + # Background for text + pt1 = ( + center_x - text_size[0] // 2 - 5, + center_y - text_size[1] // 2 - 5, + ) + pt2 = ( + center_x + text_size[0] // 2 + 5, + center_y + text_size[1] // 2 + 5, + ) + overlay = display_frame.copy() + cv2.rectangle(overlay, pt1, pt2, (0, 0, 0), -1) + cv2.addWeighted(overlay, 0.5, display_frame, 0.5, 0, display_frame) + + # Draw text + cv2.putText( + display_frame, + id_text, + (center_x - text_size[0] // 2, center_y + text_size[1] // 2), + font, + font_scale, + (0, 255, 0), + thickness, + ) + + # Draw stats + stats_text = f"Detected: {num_markers} marker(s)" + overlay = display_frame.copy() + cv2.rectangle(overlay, (5, 5), (280, 40), (0, 0, 0), -1) + cv2.addWeighted(overlay, 0.3, display_frame, 0.7, 0, display_frame) + cv2.putText( + display_frame, + stats_text, + (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, + 0.7, + (0, 255, 0), + 2, + ) + + return display_frame, num_markers + + def run(self): + """Main loop - subscribe and display.""" + rclpy.init() + node = rclpy.create_node("aruco_visualizer") + try: + node.create_subscription(RosImage, self.image_topic, self.image_callback, 1) + + print("[INFO] Waiting for images...\n") + + while self.running: + rclpy.spin_once(node, timeout_sec=0.01) + + if self.current_image is not None: + display_frame, num_markers = self.draw_markers(self.current_image) + cv2.imshow("ArUco Markers", display_frame) + + key = cv2.waitKey(1) + if key == ord("q"): + self.running = False + else: + cv2.waitKey(1) + + except KeyboardInterrupt: + print("\n[INFO] Interrupted") + finally: + cv2.destroyAllWindows() + node.destroy_node() + rclpy.shutdown() + + +def main(): + """Parse args and run.""" + parser = argparse.ArgumentParser(description="Standalone ArUco Marker Visualizer") + + parser.add_argument( + "--image-topic", + default="/sensors/mono_image", + help="ROS topic for camera images (default: /sensors/mono_image)", + ) + + parser.add_argument( + "--aruco-dict", + default="4x4_50", + choices=[ + "4x4_50", + "4x4_100", + "4x4_250", + "4x4_1000", + "5x5_50", + "5x5_100", + "5x5_250", + "5x5_1000", + "6x6_50", + "6x6_100", + "6x6_250", + "6x6_1000", + "7x7_50", + "7x7_100", + "7x7_250", + "7x7_1000", + ], + help="ArUco dictionary (default: 4x4_50)", + ) + + args = parser.parse_args() + + try: + visualizer = ArucoVisualizer(args.image_topic, args.aruco_dict) + visualizer.run() + except Exception as e: + print(f"[ERROR] {e}", file=sys.stderr) + sys.exit(1) + + +if __name__ == "__main__": + main() From 1ce1a8a8a1e6ba50a8fbd6fb8b59f1571b99e98b Mon Sep 17 00:00:00 2001 From: Barrett Date: Mon, 6 Apr 2026 19:31:14 -0500 Subject: [PATCH 14/20] feat: tweak some Nav2/robot_localization params --- src/drive_launcher/params/nav2.yaml | 12 +- .../ekf_filter_node_odom.yaml | 7 +- .../robot_localization/navsat_transform.yaml | 105 +++++++++--------- 3 files changed, 64 insertions(+), 60 deletions(-) diff --git a/src/drive_launcher/params/nav2.yaml b/src/drive_launcher/params/nav2.yaml index 9c11384b..449c2998 100644 --- a/src/drive_launcher/params/nav2.yaml +++ b/src/drive_launcher/params/nav2.yaml @@ -62,11 +62,11 @@ controller_server: debug_trajectory_details: true min_vel_x: 0.35 min_vel_y: 0.0 - max_vel_x: 3.5 + max_vel_x: 2.5 max_vel_y: 0.0 max_vel_theta: 1.0 min_speed_xy: 0.0 - max_speed_xy: 3.5 + max_speed_xy: 2.5 min_speed_theta: 0.0 acc_lim_x: 3.5 acc_lim_y: 0.0 @@ -200,8 +200,8 @@ controller_server: # see: https://docs.nav2.org/tutorials/docs/navigation2_with_gps.html#setup-navigation-system rolling_window: True resolution: 0.25 - width: 8000 - height: 8000 + width: 1000 + height: 1000 # allow higher latency from sensors transform_tolerance: 2.0 @@ -326,8 +326,8 @@ velocity_smoother: smoothing_frequency: 20.0 scale_velocities: False feedback: "OPEN_LOOP" - max_velocity: [3.5, 0.0, 2.0] - min_velocity: [-3.5, 0.0, -2.0] + max_velocity: [2.5, 0.0, 2.0] + min_velocity: [-2.5, 0.0, -2.0] max_accel: [3.5, 0.0, 3.2] max_decel: [-3.5, 0.0, -3.2] odom_topic: "odom" diff --git a/src/drive_launcher/params/robot_localization/ekf_filter_node_odom.yaml b/src/drive_launcher/params/robot_localization/ekf_filter_node_odom.yaml index 4eeb293a..6a28398a 100644 --- a/src/drive_launcher/params/robot_localization/ekf_filter_node_odom.yaml +++ b/src/drive_launcher/params/robot_localization/ekf_filter_node_odom.yaml @@ -28,7 +28,10 @@ debug: false debug_out_file: rl_ekf_filter_node_odom.log - # configure imu for local usage + # configure imu for local usage. + # + # use yaw rate only; direct yaw can freeze heading if orientation output is + # stale while gyro values keep changing. imu0: /unilidar/imu imu0_config: [ @@ -37,7 +40,7 @@ false, false, false, - true, + false, false, false, false, diff --git a/src/drive_launcher/params/robot_localization/navsat_transform.yaml b/src/drive_launcher/params/robot_localization/navsat_transform.yaml index 7edf15c6..1976eac9 100644 --- a/src/drive_launcher/params/robot_localization/navsat_transform.yaml +++ b/src/drive_launcher/params/robot_localization/navsat_transform.yaml @@ -4,60 +4,61 @@ # `global_odom` file's `ekf_filter_node_map` node. /**: - ros__parameters: - qos_overrides: - /sensors/gps: - subscription: - reliability: best_effort - history: keep_last - depth: 10 - durability: volatile - /unilidar/imu: - subscription: - reliability: best_effort - history: keep_last - depth: 50 - durability: volatile - /odometry/filtered: - subscription: - reliability: reliable - history: keep_last - depth: 10 - durability: volatile + ros__parameters: + qos_overrides: + /sensors/gps: + subscription: + reliability: best_effort + history: keep_last + depth: 10 + durability: volatile + /unilidar/imu: + subscription: + reliability: best_effort + history: keep_last + depth: 50 + durability: volatile + /odometry/filtered: + subscription: + reliability: reliable + history: keep_last + depth: 10 + durability: volatile - # timing - frequency: 5.0 - delay: 0.2 + # timing + frequency: 5.0 + delay: 0.2 - # behavior - enable: true - # ...note: this is positive in both Utah and Oklahoma. - # - # Norman, OK is closer to ~64 deg., while Hanksville, UT is around ~10 deg. - # - # make sure to convert to radians when using another value! - # magnetic_declination_radians: 0.1745329 - # ...an offset makes the imu zero (when facing east). - # - # our imu can use `0.0`, since it works like that naturally :D - yaw_offset: 0.0 - zero_altitude: false - use_odometry_yaw: false + # behavior + enable: true + # ...note: this is positive in both Utah and Oklahoma. + # + # Norman, OK is closer to ~64 deg., while Hanksville, UT is around ~10 deg. + # + # make sure to convert to radians when using another value! + # magnetic_declination_radians: 0.1745329 + # ...an offset makes the imu zero (when facing east). + # + # calibrated on rover with Unitree L2 IMU; see scripts/get_navsat_yaw_offset.sh + # yaw_offset: -1.40399 + yaw_offset: 0.0 + zero_altitude: false + use_odometry_yaw: false - # node rules - # - # ... when `true`, this puts the utm frame in the chain. we need it to - # translate coordinates into map-local values - broadcast_cartesian_transform: false - broadcast_cartesian_transform_as_parent_frame: false - publish_filtered_gps: true - wait_for_datum: false + # node rules + # + # ... when `true`, this puts the utm frame in the chain. we need it to + # translate coordinates into map-local values + broadcast_cartesian_transform: false + broadcast_cartesian_transform_as_parent_frame: false + publish_filtered_gps: true + wait_for_datum: false - # frame config - map_frame: map - odom_frame: odom - base_link_frame: base_link - earth_frame: earth + # frame config + map_frame: map + odom_frame: odom + base_link_frame: base_link + earth_frame: earth - # we'll get gps messages with *this* frame_id: - frame_id: gps_link + # we'll get gps messages with *this* frame_id: + frame_id: gps_link From 657890784c4e7f1437920e24bd484d314bf83ff7 Mon Sep 17 00:00:00 2001 From: Barrett Date: Mon, 6 Apr 2026 19:32:23 -0500 Subject: [PATCH 15/20] fix(nav_bringup): dont move when we start up it was causing us to run into things lmao the wheels were way too fast when doing this, and we didn't need it. maybe only do this if `use_sim := True`??? --- src/navigator/navigation_bringup_node/main.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/navigator/navigation_bringup_node/main.py b/src/navigator/navigation_bringup_node/main.py index 1c52ec28..c2ba1557 100644 --- a/src/navigator/navigation_bringup_node/main.py +++ b/src/navigator/navigation_bringup_node/main.py @@ -89,11 +89,12 @@ def _on_map_message(self, map_message: OccupancyGrid): def _on_wheel_speeds_timer_tick(self): # decide which speeds to publish - wheel_speed: Twist = Twist() - wheel_speed.linear.x = MOVEMENT_SPEED + # wheel_speed: Twist = Twist() + # wheel_speed.linear.x = MOVEMENT_SPEED # ...publish them. - self._wheels_publisher.publish(wheel_speed) + # self._wheels_publisher.publish(wheel_speed) + pass def stop_wheels(self): if rclpy.ok(): From 77fda365e7a2b5db5c333e533080e75268f48740 Mon Sep 17 00:00:00 2001 From: Barrett Date: Mon, 6 Apr 2026 19:33:23 -0500 Subject: [PATCH 16/20] fix(nav): startup and other mistakes --- src/navigator/navigator_node/main.py | 22 +++++--- src/navigator/navigator_node/pose.py | 18 +++++-- src/navigator/navigator_node/search.py | 65 +++++++++++++++++++++++ src/navigator/utm_conversion_node/main.py | 4 +- 4 files changed, 96 insertions(+), 13 deletions(-) create mode 100644 src/navigator/navigator_node/search.py diff --git a/src/navigator/navigator_node/main.py b/src/navigator/navigator_node/main.py index 1d60c86f..ad99870b 100644 --- a/src/navigator/navigator_node/main.py +++ b/src/navigator/navigator_node/main.py @@ -743,13 +743,23 @@ def main(args: list[str] | None = None): # Spin the ROS 2 node and run navigation logic concurrently. # - # `asyncio.create_task` requires a running loop, so we enter async context - # with `asyncio.run(...)` first. + # Important: all task creation must happen from inside a running loop. async def _run() -> None: - _ = await asyncio.gather( - ros_spin(navigator_node), - navigator_node.navigator(), - ) + loop = asyncio.get_running_loop() + ros_spin_task = loop.create_task(ros_spin(navigator_node)) + navigator_task = loop.create_task(navigator_node.navigator()) + + try: + _ = await asyncio.gather(ros_spin_task, navigator_task) + finally: + for task in (ros_spin_task, navigator_task): + if not task.done(): + _ = task.cancel() + _ = await asyncio.gather( + ros_spin_task, + navigator_task, + return_exceptions=True, + ) try: asyncio.run(_run()) diff --git a/src/navigator/navigator_node/pose.py b/src/navigator/navigator_node/pose.py index d7ff0952..70230d00 100644 --- a/src/navigator/navigator_node/pose.py +++ b/src/navigator/navigator_node/pose.py @@ -4,7 +4,12 @@ from std_msgs.msg import Header -def geopoint_to_pose(converted_geopoint: Point, time: Time) -> PoseStamped: +def geopoint_to_pose( + converted_geopoint: Point, + time: Time, + *, + frame_id: str = "map", +) -> PoseStamped: """ Converts a GeoPoint-derived offset into a `geometry_msgs::PoseStamped`. @@ -20,19 +25,22 @@ def geopoint_to_pose(converted_geopoint: Point, time: Time) -> PoseStamped: # add the header h: Header = Header() - llogger.debug("adding map...") - h.frame_id = "map" + llogger.debug(f"adding frame id: {frame_id}") + h.frame_id = frame_id llogger.debug("adding time...") h.stamp = time pose.header = h - llogger.debug("map and time added!") + llogger.debug("frame and time added!") # set our position correctly lol llogger.debug("adding position data...") pose.pose.position.x = converted_geopoint.x pose.pose.position.y = converted_geopoint.y pose.pose.position.z = 0.0 - pose.pose.orientation.w = 1.0 # TODO(bray): test this lol + pose.pose.orientation.x = 0.0 + pose.pose.orientation.y = 0.0 + pose.pose.orientation.z = 0.0 + pose.pose.orientation.w = 1.0 llogger.debug("position data added successfully!") return pose diff --git a/src/navigator/navigator_node/search.py b/src/navigator/navigator_node/search.py new file mode 100644 index 00000000..a5f68c61 --- /dev/null +++ b/src/navigator/navigator_node/search.py @@ -0,0 +1,65 @@ +""" +Implementation of the target search algorithm. +""" + +from geographic_msgs.msg import GeoPoint +from geopy.distance import distance +from geopy.point import Point +from loguru import logger as llogger +from rclpy.publisher import Publisher + + +# FIXME: this needs to be a class method. we can't just navigate without `self` :( +async def search_for_target( + lead_coordinates: list[GeoPoint], _wheel_publisher: Publisher +): + """ + Moves the Rover to each potential target location we've collected. The info + obtained from performing this search isn't managed by this function. + + Instead, other ROS 2 nodes will tell the Navigator that it has found + something, in which case it can cancel this async function. + + IMPORTANT: Ensure that you don't re-use this list (or that it has been + deep-copied)! We might modify it. + """ + llogger.debug("starting search for a list of leads...") + + # iterate over each "potential location" (where a target would be located) + for potential_location in lead_coordinates: + llogger.debug( + f"checking location at ({potential_location.latitude} lat, {potential_location.longitude} lon)..." + ) + pass + pass + + +def generate_similar_coordinates( + src: GeoPoint, radius: float, num_points: int +) -> list[GeoPoint]: + """ + Given a coordinate, a radius in meters, and a number of points to return, this function + takes the source coordinate and uses it to generate a list of new coordinates in + a sequential radius around the starting coordinate. + + This functions as a simplistic "search" strategy for the rover, where we can then feed + the generated coordinates as our desired path while we look for a tag or object. + """ + new_points: list[GeoPoint] = [] + + for i in range(num_points): + angle = (360 / num_points) * i # Evenly spaced angles around a circle + + # Calculates a new coordinate that is the given amount of meters away from source + point: Point = distance(meters=radius).destination( + Point(src.latitude, src.longitude), bearing=angle + ) + + # make that into a `GeoPoint` msg + g: GeoPoint = GeoPoint() + g.latitude = point.latitude + g.longitude = point.longitude + + new_points.append(g) + + return new_points diff --git a/src/navigator/utm_conversion_node/main.py b/src/navigator/utm_conversion_node/main.py index 62b7a017..5d09057e 100644 --- a/src/navigator/utm_conversion_node/main.py +++ b/src/navigator/utm_conversion_node/main.py @@ -54,10 +54,10 @@ def __init__(self): GPS_QOS_PROFILE, ) - # also, grab odom stream + # also, grab the map frame's odom stream self._odom_subscriber = self.create_subscription( Odometry, - "/odometry/filtered", + "/ekf_filter_node_map/odometry/filtered", self._on_odom_message, ODOM_QOS_PROFILE, ) From e02d2fffcd9e2648d878977b8cb622ce18b67870 Mon Sep 17 00:00:00 2001 From: Barrett Date: Mon, 6 Apr 2026 19:34:11 -0500 Subject: [PATCH 17/20] feat(lidar): set range minimum as param also, lower it to 1.1m from 1.2m. worked a bit better for us, iirc --- src/soro_lidar/launch/launch.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/soro_lidar/launch/launch.py b/src/soro_lidar/launch/launch.py index 82a55a1e..c0c66006 100644 --- a/src/soro_lidar/launch/launch.py +++ b/src/soro_lidar/launch/launch.py @@ -8,6 +8,7 @@ def generate_launch_description(): launch_rviz = LaunchConfiguration("launch_rviz") + range_min = LaunchConfiguration("range_min") # Run unitree lidar node1 = Node( @@ -20,7 +21,7 @@ def generate_launch_description(): {"work_mode": 0}, {"use_system_timestamp": True}, {"publish_tf": False}, - {"range_min": 1.2}, + {"range_min": range_min}, {"range_max": 100.0}, {"cloud_scan_num": 18}, { @@ -54,6 +55,8 @@ def generate_launch_description(): return LaunchDescription( [ DeclareLaunchArgument("launch_rviz", default_value="false"), + # we won't detect stuff within 1.1 meters + DeclareLaunchArgument("range_min", default_value="1.1"), node1, rviz_node, ] From 37aa947f2ed9ec5b85c50abc546268075f1b345f Mon Sep 17 00:00:00 2001 From: Barrett Date: Mon, 6 Apr 2026 19:34:27 -0500 Subject: [PATCH 18/20] feat(COMMANDS.txt): add list of EQuad test cmds --- COMMANDS.txt | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 COMMANDS.txt diff --git a/COMMANDS.txt b/COMMANDS.txt new file mode 100644 index 00000000..8d5e842c --- /dev/null +++ b/COMMANDS.txt @@ -0,0 +1,25 @@ +# NAVIGATOR: to the right of bay door +ros2 run navigator navigator_node --ros-args -p latitude:=35.2101718 -p longitude:=-97.4414963 -p mode:=0 + +# NAVIGATOR: straight ahead of bay door +ros2 run navigator navigator_node --ros-args -p latitude:=35.2103225 -p longitude:=-97.4411651 -p mode:=0 + +# NAVIGATOR: end of repf closest segment +ros2 run navigator navigator_node --ros-args -p latitude:=35.2105681 -p longitude:=-97.4419394 -p mode:=0 + +# NAVIGATOR: aruco marker placed in equad (2026-02-25 5:09 PM) +ros2 run navigator navigator_node --ros-args -p latitude:=35.2103889 -p longitude:=-97.4418893 -p mode:=0 + +# NAVIGATOR: aruco marker placed in equad (2026-02-25 6:08 PM) +ros2 run navigator navigator_node --ros-args -p latitude:=35.2106322 -p longitude:=-97.4419303 -p mode:=0 + +ros2 launch drive_launcher rover.launch.py use_sim_time:=False + +ros2 launch drive_launcher ebox.launch.py + + +# For RVIZ: +# terminal 1 +Xvfb :99 -screen 0 1920x1080x24 & export DISPLAY=:99 & gnome-session --session=ubuntu & rviz2 +# terminal 2 +x11vnc -display :99 From 87058af8a1cd1d647e8899174e9bf17e86b8271c Mon Sep 17 00:00:00 2001 From: Barrett Date: Mon, 6 Apr 2026 19:35:17 -0500 Subject: [PATCH 19/20] feat(scripts): add some testing scripts they helped save us time. might do so again in the future. note that these are slop-made. analyze and understand the results thoroughly; don't just trust em --- scripts/get_navsat_yaw_offset.sh | 179 +++++++++++++++++++++++++++++++ scripts/verify_rotation.sh | 178 ++++++++++++++++++++++++++++++ 2 files changed, 357 insertions(+) create mode 100644 scripts/get_navsat_yaw_offset.sh create mode 100644 scripts/verify_rotation.sh diff --git a/scripts/get_navsat_yaw_offset.sh b/scripts/get_navsat_yaw_offset.sh new file mode 100644 index 00000000..398713ea --- /dev/null +++ b/scripts/get_navsat_yaw_offset.sh @@ -0,0 +1,179 @@ +#!/usr/bin/env bash +set -euo pipefail + +usage() { + cat <<'EOF' +Usage: + scripts/get_navsat_yaw_offset.sh --heading-deg [options] + +Computes a recommended /navsat_transform_node yaw_offset from live IMU data. + +Heading convention for --heading-deg: + 0 = North, 90 = East, 180 = South, 270 = West. + +Options: + --topic IMU topic to sample (default: /unilidar/imu) + --duration Sampling time (default: 10) + --node Node used by --apply (default: /navsat_transform_node) + --apply Apply computed yaw_offset live with ros2 param set + --no-source Skip sourcing install/setup.bash + -h, --help Show this help +EOF +} + +topic="/unilidar/imu" +duration="10" +node="/navsat_transform_node" +apply_now=0 +auto_source=1 +heading_deg="" + +while [[ $# -gt 0 ]]; do + case "$1" in + --heading-deg) + heading_deg="${2:-}" + shift 2 + ;; + --topic) + topic="${2:-}" + shift 2 + ;; + --duration) + duration="${2:-}" + shift 2 + ;; + --node) + node="${2:-}" + shift 2 + ;; + --apply) + apply_now=1 + shift + ;; + --no-source) + auto_source=0 + shift + ;; + -h | --help) + usage + exit 0 + ;; + *) + echo "Unknown option: $1" >&2 + usage >&2 + exit 2 + ;; + esac +done + +if [[ -z "${heading_deg}" ]]; then + echo "Missing required --heading-deg " >&2 + usage >&2 + exit 2 +fi + +if ! [[ "${heading_deg}" =~ ^-?[0-9]+([.][0-9]+)?$ ]]; then + echo "--heading-deg must be numeric" >&2 + exit 2 +fi + +if ! [[ "${duration}" =~ ^[0-9]+([.][0-9]+)?$ ]]; then + echo "--duration must be numeric" >&2 + exit 2 +fi + +if [[ "${auto_source}" -eq 1 ]]; then + script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + workspace_root="$(cd "${script_dir}/.." && pwd)" + setup_file="${workspace_root}/install/setup.bash" + if [[ -f "${setup_file}" ]]; then + set +u + # shellcheck disable=SC1090 + source "${setup_file}" + set -u + else + echo "warning: ${setup_file} not found, assuming ROS env already sourced" >&2 + fi +fi + +if ! command -v ros2 >/dev/null 2>&1; then + echo "ros2 not found in PATH. Source your ROS environment first." >&2 + exit 1 +fi + +awk_program=' +function wrap_deg(a) { while (a > 180.0) a -= 360.0; while (a <= -180.0) a += 360.0; return a } +function wrap_rad(a) { + pi = atan2(0, -1) + while (a > pi) a -= 2.0 * pi + while (a <= -pi) a += 2.0 * pi + return a +} +BEGIN { + pi = atan2(0, -1) + n = 0 + sum_s = 0.0 + sum_c = 0.0 + sum_wz = 0.0 +} +NF >= 19 { + x = $4 + 0.0 + y = $5 + 0.0 + z = $6 + 0.0 + w = $7 + 0.0 + yaw = atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) + sum_s += sin(yaw) + sum_c += cos(yaw) + sum_wz += ($19 + 0.0) + n++ +} +END { + if (n < 1) { + exit 2 + } + + mean_yaw = atan2(sum_s / n, sum_c / n) + mean_yaw_deg = mean_yaw * 180.0 / pi + expected_yaw_deg = wrap_deg(90.0 - (heading_deg + 0.0)) + offset_deg = wrap_deg(expected_yaw_deg - mean_yaw_deg) + offset_rad = wrap_rad(offset_deg * pi / 180.0) + + R = sqrt((sum_c / n) * (sum_c / n) + (sum_s / n) * (sum_s / n)) + if (R <= 0.0) { + yaw_spread_deg = 180.0 + } else { + yaw_spread_deg = sqrt(-2.0 * log(R)) * 180.0 / pi + } + + printf("samples=%d\n", n) + printf("mean_imu_yaw_deg=%.6f\n", mean_yaw_deg) + printf("yaw_spread_deg=%.6f\n", yaw_spread_deg) + printf("expected_yaw_deg=%.6f\n", expected_yaw_deg) + printf("recommended_yaw_offset_deg=%.6f\n", offset_deg) + printf("recommended_yaw_offset_rad=%.6f\n", offset_rad) + printf("mean_gyro_z_rad_s=%.6f\n", sum_wz / n) +} +' + +if ! stats="$( + { + timeout "${duration}" ros2 topic echo "${topic}" --csv 2>/dev/null || true + } | awk -F, -v heading_deg="${heading_deg}" "${awk_program}" +)"; then + echo "No IMU samples received from ${topic} in ${duration}s." >&2 + exit 1 +fi + +echo "${stats}" +offset_rad="$(awk -F= '/^recommended_yaw_offset_rad=/{print $2}' <<<"${stats}")" + +if [[ -z "${offset_rad}" ]]; then + echo "Failed to compute recommended_yaw_offset_rad." >&2 + exit 1 +fi + +echo "apply_cmd=ros2 param set ${node} yaw_offset ${offset_rad}" + +if [[ "${apply_now}" -eq 1 ]]; then + ros2 param set "${node}" yaw_offset "${offset_rad}" +fi diff --git a/scripts/verify_rotation.sh b/scripts/verify_rotation.sh new file mode 100644 index 00000000..1d107436 --- /dev/null +++ b/scripts/verify_rotation.sh @@ -0,0 +1,178 @@ +#!/usr/bin/env bash +set -euo pipefail + +usage() { + cat <<'EOF' +Usage: + scripts/verify_rotation.sh [options] + +Checks whether rover heading is changing in TF while you rotate in place. +It samples tf2 yaw from odom -> base_link and reports PASS/FAIL. + +Options: + --duration Sampling time (default: 12) + --min-yaw-change-rad Minimum yaw span to pass (default: 0.20) + --from-frame Parent frame (default: odom) + --to-frame Child frame (default: base_link) + --no-source Skip sourcing install/setup.bash + -h, --help Show this help +EOF +} + +duration="12" +min_yaw_change_rad="0.20" +from_frame="odom" +to_frame="base_link" +auto_source=1 + +while [[ $# -gt 0 ]]; do + case "$1" in + --duration) + duration="${2:-}" + shift 2 + ;; + --min-yaw-change-rad) + min_yaw_change_rad="${2:-}" + shift 2 + ;; + --from-frame) + from_frame="${2:-}" + shift 2 + ;; + --to-frame) + to_frame="${2:-}" + shift 2 + ;; + --no-source) + auto_source=0 + shift + ;; + -h | --help) + usage + exit 0 + ;; + *) + echo "Unknown option: $1" >&2 + usage >&2 + exit 2 + ;; + esac +done + +if ! [[ "${duration}" =~ ^[0-9]+([.][0-9]+)?$ ]]; then + echo "--duration must be numeric" >&2 + exit 2 +fi + +if ! [[ "${min_yaw_change_rad}" =~ ^[0-9]+([.][0-9]+)?$ ]]; then + echo "--min-yaw-change-rad must be numeric" >&2 + exit 2 +fi + +if [[ "${auto_source}" -eq 1 ]]; then + script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + workspace_root="$(cd "${script_dir}/.." && pwd)" + setup_file="${workspace_root}/install/setup.bash" + if [[ -f "${setup_file}" ]]; then + set +u + # shellcheck disable=SC1090 + source "${setup_file}" + set -u + else + echo "warning: ${setup_file} not found, assuming ROS env already sourced" >&2 + fi +fi + +if ! command -v ros2 >/dev/null 2>&1; then + echo "ros2 not found in PATH. Source your ROS environment first." >&2 + exit 1 +fi + +if ! command -v timeout >/dev/null 2>&1; then + echo "timeout command not found in PATH." >&2 + exit 1 +fi + +echo "Sampling TF yaw (${from_frame} -> ${to_frame}) for ${duration}s..." +echo "Rotate the rover in place now." + +if ! result="$( + timeout "${duration}" ros2 run tf2_ros tf2_echo "${from_frame}" "${to_frame}" 2>/dev/null \ + | awk -v min_span="${min_yaw_change_rad}" ' + function trim(s) { + gsub(/^[ \t]+|[ \t]+$/, "", s) + return s + } + function abs(v) { + return v < 0 ? -v : v + } + /Rotation: in RPY/ { + if (match($0, /\[[^]]+\]/) == 0) { + next + } + + vec = substr($0, RSTART + 1, RLENGTH - 2) + nsplit = split(vec, parts, ",") + if (nsplit < 3) { + next + } + + yaw = trim(parts[3]) + 0.0 + pi = atan2(0, -1) + + if (n == 0) { + first = yaw + prev = yaw + unwrapped = yaw + min_u = yaw + max_u = yaw + } else { + delta = yaw - prev + while (delta > pi) delta -= 2.0 * pi + while (delta < -pi) delta += 2.0 * pi + unwrapped += delta + prev = yaw + if (unwrapped < min_u) min_u = unwrapped + if (unwrapped > max_u) max_u = unwrapped + } + + last = unwrapped + n++ + } + END { + if (n < 2) { + print "status=FAIL" + print "reason=insufficient_tf_samples" + print "samples=" n + exit 3 + } + + span = max_u - min_u + span_deg = span * 180.0 / atan2(0, -1) + delta = last - first + delta_deg = delta * 180.0 / atan2(0, -1) + + print "samples=" n + printf("yaw_span_rad=%.6f\n", span) + printf("yaw_span_deg=%.2f\n", span_deg) + printf("yaw_delta_rad=%.6f\n", delta) + printf("yaw_delta_deg=%.2f\n", delta_deg) + printf("min_required_span_rad=%.6f\n", min_span + 0.0) + + if (span >= (min_span + 0.0)) { + print "status=PASS" + exit 0 + } + + print "status=FAIL" + print "reason=yaw_span_below_threshold" + exit 4 + } + ' +)"; then + echo "Rotation verification failed." + echo "${result:-no output}" + exit 1 +fi + +echo "${result}" From e511fc417d5ea5b91db80f9471920f20d0dad4c2 Mon Sep 17 00:00:00 2001 From: Barrett Date: Mon, 6 Apr 2026 19:44:30 -0500 Subject: [PATCH 20/20] style: don't care about extra script linting ;D --- pyproject.toml | 1 + 1 file changed, 1 insertion(+) diff --git a/pyproject.toml b/pyproject.toml index 8cba50dc..140069fb 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -31,6 +31,7 @@ exclude = [ "src/**/build/", "**/__pycache__", "**/.*", + "src/aruco/extra_scripts/aruco_visualizer.py", ] [tool.pixi.workspace]