From 069c1aa6dd415c8d3078b6cadbf8cc7b59f115dc Mon Sep 17 00:00:00 2001 From: Zhuen Date: Sun, 28 Jun 2026 20:24:13 -0400 Subject: [PATCH 1/5] added precompiled headers for boosting compile speed and drop cpu time --- src/subjugator/drivers/waterlinked_dvl | 2 +- src/subjugator/mission_planner/CMakeLists.txt | 19 ++++++++++++++++++- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/subjugator/drivers/waterlinked_dvl b/src/subjugator/drivers/waterlinked_dvl index 1b009c4e..02fe8bd5 160000 --- a/src/subjugator/drivers/waterlinked_dvl +++ b/src/subjugator/drivers/waterlinked_dvl @@ -1 +1 @@ -Subproject commit 1b009c4ee776cc0cd0734ddb3caa57a41850b502 +Subproject commit 02fe8bd5f9d8bb1dcd0935a56a27b787ae4c6fb7 diff --git a/src/subjugator/mission_planner/CMakeLists.txt b/src/subjugator/mission_planner/CMakeLists.txt index 6426174e..851bcf4a 100644 --- a/src/subjugator/mission_planner/CMakeLists.txt +++ b/src/subjugator/mission_planner/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.14) +cmake_minimum_required(VERSION 3.16) project(mission_planner) set(CMAKE_CXX_STANDARD 17) @@ -62,6 +62,23 @@ ament_target_dependencies( rcl_interfaces ament_index_cpp) +target_precompile_headers( + operations + PRIVATE + + + + + + + + + + + + + ) + # Node add_executable(mission_planner_node src/mission_planner_node.cpp) target_link_libraries(mission_planner_node operations) From 30b20593206c0e715426314cec51442365a49b88 Mon Sep 17 00:00:00 2001 From: Zhuen Date: Sun, 28 Jun 2026 20:27:24 -0400 Subject: [PATCH 2/5] revert waterlinked_dvl submodule to match main --- src/subjugator/drivers/waterlinked_dvl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/subjugator/drivers/waterlinked_dvl b/src/subjugator/drivers/waterlinked_dvl index 02fe8bd5..1b009c4e 160000 --- a/src/subjugator/drivers/waterlinked_dvl +++ b/src/subjugator/drivers/waterlinked_dvl @@ -1 +1 @@ -Subproject commit 02fe8bd5f9d8bb1dcd0935a56a27b787ae4c6fb7 +Subproject commit 1b009c4ee776cc0cd0734ddb3caa57a41850b502 From a254c4e8e3c937ecb0a8909e377762df5d69021b Mon Sep 17 00:00:00 2001 From: Zhuen Date: Sun, 28 Jun 2026 21:04:17 -0400 Subject: [PATCH 3/5] add pose trajectory script and mock odom for manual PID tuning --- src/subjugator/scripts/mock_odom.py | 110 ++++++++++++++++++ src/subjugator/scripts/move_poses.py | 163 +++++++++++++++++++++++++++ 2 files changed, 273 insertions(+) create mode 100644 src/subjugator/scripts/mock_odom.py create mode 100644 src/subjugator/scripts/move_poses.py diff --git a/src/subjugator/scripts/mock_odom.py b/src/subjugator/scripts/mock_odom.py new file mode 100644 index 00000000..1b15b5e4 --- /dev/null +++ b/src/subjugator/scripts/mock_odom.py @@ -0,0 +1,110 @@ +#!/usr/bin/env python3 + +""" +Simulate sub odometry for testing move_poses.py on a laptop. + +Listens to /goal/trajectory and gradually "moves" a simulated position +toward the goal, publishing the result to /odometry/filtered. + +Usage: + python3 mock_odom.py # default speed 0.3 m/s + python3 mock_odom.py --speed 0.5 # faster simulation + python3 mock_odom.py --start 0,0,-1,0 # custom start pose +""" + +import argparse +import math + +import rclpy +from geometry_msgs.msg import Pose, PoseWithCovariance, PoseWithCovarianceStamped, Quaternion +from nav_msgs.msg import Odometry +from rclpy.node import Node + + +class MockOdom(Node): + def __init__(self, start_pose: Pose, speed: float): + super().__init__("mock_odom") + self.current = start_pose + self.goal: Pose | None = None + self.speed = speed # m/s + + self.odom_pub = self.create_publisher(Odometry, "/odometry/filtered", 10) + self.goal_sub = self.create_subscription( + Pose, "/goal/trajectory", self._goal_cb, 1 + ) + + # update at 20 Hz + self.create_timer(0.05, self._update) + self.get_logger().info(f"Mock odom started speed={speed}m/s") + + def _goal_cb(self, msg: Pose) -> None: + self.goal = msg + p = msg.position + self.get_logger().info(f"New goal → x={p.x:.2f} y={p.y:.2f} z={p.z:.2f}") + + def _update(self) -> None: + if self.goal is not None: + self._step_toward_goal() + self._publish() + + def _step_toward_goal(self) -> None: + dx = self.goal.position.x - self.current.position.x + dy = self.goal.position.y - self.current.position.y + dz = self.goal.position.z - self.current.position.z + dist = math.sqrt(dx * dx + dy * dy + dz * dz) + + if dist < 0.01: + return + + # move one timestep toward goal (dt=0.05s) + step = min(self.speed * 0.05, dist) + scale = step / dist + self.current.position.x += dx * scale + self.current.position.y += dy * scale + self.current.position.z += dz * scale + + def _publish(self) -> None: + msg = Odometry() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "odom" + msg.child_frame_id = "base_link" + msg.pose.pose = self.current + self.odom_pub.publish(msg) + + +def _parse_start(s: str) -> Pose: + parts = s.split(",") + if len(parts) != 4: + raise argparse.ArgumentTypeError(f"Expected x,y,z,yaw_deg — got: {s!r}") + x, y, z, yaw_deg = map(float, parts) + p = Pose() + p.position.x = x + p.position.y = y + p.position.z = z + yaw_rad = math.radians(yaw_deg) + p.orientation.w = math.cos(yaw_rad / 2.0) + p.orientation.z = math.sin(yaw_rad / 2.0) + return p + + +def main() -> None: + parser = argparse.ArgumentParser(description="Simulate sub odometry for testing move_poses.py") + parser.add_argument("--speed", type=float, default=0.3, help="Simulated movement speed in m/s (default: 0.3)") + parser.add_argument("--start", default="0,0,-1,0", metavar="x,y,z,yaw_deg", help="Starting pose (default: 0,0,-1,0)") + + args, _ = parser.parse_known_args() + start_pose = _parse_start(args.start) + + rclpy.init() + node = MockOdom(start_pose, args.speed) + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/subjugator/scripts/move_poses.py b/src/subjugator/scripts/move_poses.py new file mode 100644 index 00000000..6c85c9f9 --- /dev/null +++ b/src/subjugator/scripts/move_poses.py @@ -0,0 +1,163 @@ +#!/usr/bin/env python3 + +""" +Move the sub through a sequence of poses via /goal/trajectory. + +Publishes each pose directly to the PID controller and waits until +the sub reaches it (within tolerance) before advancing to the next. + +Usage: + python3 move_poses.py --poses 0,0,-1,0 2,0,-1,0 2,2,-1,90 + python3 move_poses.py --file poses.yaml + python3 move_poses.py --tolerance 0.3 --timeout 30 --poses 0,0,-1,0 + +Pose format: x,y,z,yaw_deg + +YAML file format: + poses: + - {x: 0.0, y: 0.0, z: -1.0, yaw_deg: 0.0} + - {x: 2.0, y: 0.0, z: -1.0, yaw_deg: 90.0} +""" + +import argparse +import math +import time + +import rclpy +from geometry_msgs.msg import Pose, Quaternion +from nav_msgs.msg import Odometry +from rclpy.node import Node + + +def _yaw_to_quat(yaw_rad: float) -> Quaternion: + q = Quaternion() + q.w = math.cos(yaw_rad / 2.0) + q.x = 0.0 + q.y = 0.0 + q.z = math.sin(yaw_rad / 2.0) + return q + + +def _make_pose(x: float, y: float, z: float, yaw_deg: float) -> Pose: + p = Pose() + p.position.x = x + p.position.y = y + p.position.z = z + p.orientation = _yaw_to_quat(math.radians(yaw_deg)) + return p + + +def _distance(a: Pose, b: Pose) -> float: + dx = a.position.x - b.position.x + dy = a.position.y - b.position.y + dz = a.position.z - b.position.z + return math.sqrt(dx * dx + dy * dy + dz * dz) + + +class MovePoses(Node): + def __init__(self, poses: list[Pose], tolerance: float, timeout: float): + super().__init__("move_poses") + self.poses = poses + self.tolerance = tolerance + self.timeout = timeout + self.current_pose: Pose | None = None + + self.goal_pub = self.create_publisher(Pose, "/goal/trajectory", 1) + self.odom_sub = self.create_subscription( + Odometry, "/odometry/filtered", self._odom_cb, 10 + ) + + def _odom_cb(self, msg: Odometry) -> None: + self.current_pose = msg.pose.pose + + def run(self) -> None: + print("Waiting for odometry...") + while rclpy.ok() and self.current_pose is None: + rclpy.spin_once(self, timeout_sec=0.1) + + print( + f"Moving through {len(self.poses)} pose(s) " + f"[tolerance={self.tolerance}m timeout={self.timeout}s]" + ) + + for i, pose in enumerate(self.poses): + p = pose.position + print(f"\n[{i + 1}/{len(self.poses)}] Goal → x={p.x:.2f} y={p.y:.2f} z={p.z:.2f}") + self.goal_pub.publish(pose) + + start = time.monotonic() + while rclpy.ok(): + rclpy.spin_once(self, timeout_sec=0.05) + dist = _distance(self.current_pose, pose) + elapsed = time.monotonic() - start + + if dist < self.tolerance: + print(f" Reached in {elapsed:.1f}s (dist={dist:.3f}m)") + break + + if elapsed > self.timeout: + print(f" Timeout after {elapsed:.1f}s (dist={dist:.3f}m) — advancing anyway") + break + + print("\nDone.") + + +def _parse_pose(s: str) -> Pose: + parts = s.split(",") + if len(parts) != 4: + raise argparse.ArgumentTypeError(f"Expected x,y,z,yaw_deg — got: {s!r}") + try: + x, y, z, yaw_deg = map(float, parts) + except ValueError as e: + raise argparse.ArgumentTypeError(str(e)) + return _make_pose(x, y, z, yaw_deg) + + +def _load_yaml(path: str) -> list[Pose]: + import yaml + + with open(path) as f: + data = yaml.safe_load(f) + return [ + _make_pose( + float(e["x"]), float(e["y"]), float(e["z"]), float(e.get("yaw_deg", 0.0)) + ) + for e in data["poses"] + ] + + +def main() -> None: + parser = argparse.ArgumentParser( + description="Move sub through a sequence of poses via /goal/trajectory", + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog="Example: python3 move_poses.py --poses 0,0,-1,0 2,0,-1,90", + ) + parser.add_argument( + "--poses", nargs="+", type=_parse_pose, metavar="x,y,z,yaw_deg" + ) + parser.add_argument("--file", metavar="PATH", help="YAML file with pose list") + parser.add_argument("--tolerance", type=float, default=0.3, help="Goal tolerance in meters (default: 0.3)") + parser.add_argument("--timeout", type=float, default=60.0, help="Per-pose timeout in seconds (default: 60)") + + args, _ = parser.parse_known_args() + + if args.poses: + poses = args.poses + elif args.file: + poses = _load_yaml(args.file) + else: + parser.error("Provide --poses or --file") + + rclpy.init() + node = MovePoses(poses, args.tolerance, args.timeout) + try: + node.run() + except KeyboardInterrupt: + print("\nInterrupted.") + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() From 011bc20ab4abba65cfbfc71e61184005831d7426 Mon Sep 17 00:00:00 2001 From: Zhuen Date: Tue, 30 Jun 2026 13:11:13 -0400 Subject: [PATCH 4/5] remove CmakeList changes which supposed to be in precompiled-header branch --- src/subjugator/mission_planner/CMakeLists.txt | 19 +------------------ 1 file changed, 1 insertion(+), 18 deletions(-) diff --git a/src/subjugator/mission_planner/CMakeLists.txt b/src/subjugator/mission_planner/CMakeLists.txt index 851bcf4a..6426174e 100644 --- a/src/subjugator/mission_planner/CMakeLists.txt +++ b/src/subjugator/mission_planner/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.16) +cmake_minimum_required(VERSION 3.14) project(mission_planner) set(CMAKE_CXX_STANDARD 17) @@ -62,23 +62,6 @@ ament_target_dependencies( rcl_interfaces ament_index_cpp) -target_precompile_headers( - operations - PRIVATE - - - - - - - - - - - - - ) - # Node add_executable(mission_planner_node src/mission_planner_node.cpp) target_link_libraries(mission_planner_node operations) From 7cac7939e2d9506fdd81abf50558d58bd920458c Mon Sep 17 00:00:00 2001 From: Zhuen Date: Tue, 30 Jun 2026 13:24:52 -0400 Subject: [PATCH 5/5] fix pre-commit formatting for move_poses and mock_odom --- src/subjugator/scripts/mock_odom.py | 27 +++++++++++++++++---- src/subjugator/scripts/move_poses.py | 36 +++++++++++++++++++++------- 2 files changed, 49 insertions(+), 14 deletions(-) diff --git a/src/subjugator/scripts/mock_odom.py b/src/subjugator/scripts/mock_odom.py index 1b15b5e4..66d5e901 100644 --- a/src/subjugator/scripts/mock_odom.py +++ b/src/subjugator/scripts/mock_odom.py @@ -16,7 +16,9 @@ import math import rclpy -from geometry_msgs.msg import Pose, PoseWithCovariance, PoseWithCovarianceStamped, Quaternion +from geometry_msgs.msg import ( + Pose, +) from nav_msgs.msg import Odometry from rclpy.node import Node @@ -30,7 +32,10 @@ def __init__(self, start_pose: Pose, speed: float): self.odom_pub = self.create_publisher(Odometry, "/odometry/filtered", 10) self.goal_sub = self.create_subscription( - Pose, "/goal/trajectory", self._goal_cb, 1 + Pose, + "/goal/trajectory", + self._goal_cb, + 1, ) # update at 20 Hz @@ -88,9 +93,21 @@ def _parse_start(s: str) -> Pose: def main() -> None: - parser = argparse.ArgumentParser(description="Simulate sub odometry for testing move_poses.py") - parser.add_argument("--speed", type=float, default=0.3, help="Simulated movement speed in m/s (default: 0.3)") - parser.add_argument("--start", default="0,0,-1,0", metavar="x,y,z,yaw_deg", help="Starting pose (default: 0,0,-1,0)") + parser = argparse.ArgumentParser( + description="Simulate sub odometry for testing move_poses.py", + ) + parser.add_argument( + "--speed", + type=float, + default=0.3, + help="Simulated movement speed in m/s (default: 0.3)", + ) + parser.add_argument( + "--start", + default="0,0,-1,0", + metavar="x,y,z,yaw_deg", + help="Starting pose (default: 0,0,-1,0)", + ) args, _ = parser.parse_known_args() start_pose = _parse_start(args.start) diff --git a/src/subjugator/scripts/move_poses.py b/src/subjugator/scripts/move_poses.py index 6c85c9f9..7b116bdb 100644 --- a/src/subjugator/scripts/move_poses.py +++ b/src/subjugator/scripts/move_poses.py @@ -64,7 +64,10 @@ def __init__(self, poses: list[Pose], tolerance: float, timeout: float): self.goal_pub = self.create_publisher(Pose, "/goal/trajectory", 1) self.odom_sub = self.create_subscription( - Odometry, "/odometry/filtered", self._odom_cb, 10 + Odometry, + "/odometry/filtered", + self._odom_cb, + 10, ) def _odom_cb(self, msg: Odometry) -> None: @@ -77,12 +80,14 @@ def run(self) -> None: print( f"Moving through {len(self.poses)} pose(s) " - f"[tolerance={self.tolerance}m timeout={self.timeout}s]" + f"[tolerance={self.tolerance}m timeout={self.timeout}s]", ) for i, pose in enumerate(self.poses): p = pose.position - print(f"\n[{i + 1}/{len(self.poses)}] Goal → x={p.x:.2f} y={p.y:.2f} z={p.z:.2f}") + print( + f"\n[{i + 1}/{len(self.poses)}] Goal → x={p.x:.2f} y={p.y:.2f} z={p.z:.2f}", + ) self.goal_pub.publish(pose) start = time.monotonic() @@ -96,7 +101,9 @@ def run(self) -> None: break if elapsed > self.timeout: - print(f" Timeout after {elapsed:.1f}s (dist={dist:.3f}m) — advancing anyway") + print( + f" Timeout after {elapsed:.1f}s (dist={dist:.3f}m) — advancing anyway", + ) break print("\nDone.") @@ -120,7 +127,10 @@ def _load_yaml(path: str) -> list[Pose]: data = yaml.safe_load(f) return [ _make_pose( - float(e["x"]), float(e["y"]), float(e["z"]), float(e.get("yaw_deg", 0.0)) + float(e["x"]), + float(e["y"]), + float(e["z"]), + float(e.get("yaw_deg", 0.0)), ) for e in data["poses"] ] @@ -132,12 +142,20 @@ def main() -> None: formatter_class=argparse.RawDescriptionHelpFormatter, epilog="Example: python3 move_poses.py --poses 0,0,-1,0 2,0,-1,90", ) + parser.add_argument("--poses", nargs="+", type=_parse_pose, metavar="x,y,z,yaw_deg") + parser.add_argument("--file", metavar="PATH", help="YAML file with pose list") parser.add_argument( - "--poses", nargs="+", type=_parse_pose, metavar="x,y,z,yaw_deg" + "--tolerance", + type=float, + default=0.3, + help="Goal tolerance in meters (default: 0.3)", + ) + parser.add_argument( + "--timeout", + type=float, + default=60.0, + help="Per-pose timeout in seconds (default: 60)", ) - parser.add_argument("--file", metavar="PATH", help="YAML file with pose list") - parser.add_argument("--tolerance", type=float, default=0.3, help="Goal tolerance in meters (default: 0.3)") - parser.add_argument("--timeout", type=float, default=60.0, help="Per-pose timeout in seconds (default: 60)") args, _ = parser.parse_known_args()