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 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)), } } } 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] 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}" 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) 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) 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() 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 diff --git a/src/drive_launcher/params/nav2.yaml b/src/drive_launcher/params/nav2.yaml index 32a1ec32..449c2998 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.35 min_vel_y: 0.0 - max_vel_x: 0.5 + max_vel_x: 2.5 max_vel_y: 0.0 max_vel_theta: 1.0 min_speed_xy: 0.0 - max_speed_xy: 0.5 + max_speed_xy: 2.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: @@ -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: 1000 + height: 1000 # allow higher latency from sensors transform_tolerance: 2.0 @@ -286,9 +286,10 @@ 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 backup: plugin: "nav2_behaviors/BackUp" backup_speed: 0.3 @@ -299,16 +300,14 @@ 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 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: @@ -327,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: [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" odom_duration: 0.1 deadband_velocity: [0.0, 0.0, 0.0] 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 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(): diff --git a/src/navigator/navigator_node/main.py b/src/navigator/navigator_node/main.py index 0fdb92a6..ad99870b 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 @@ -741,23 +741,38 @@ 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) + # Important: all task creation must happen from inside a running loop. + async def _run() -> None: + 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()) + 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): 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 65a48a0f..5d09057e 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,15 +51,15 @@ def __init__(self): NavSatFix, "/sensors/gps", self._on_filtered_gps_message, - QOS_PROFILE, + 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, - 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 diff --git a/src/soro_lidar/launch/launch.py b/src/soro_lidar/launch/launch.py index 48c91735..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": 0.0}, + {"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, ]