From b25601639a256bd0f75c28ab17a9154f5f26aa54 Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Thu, 30 Apr 2026 14:18:00 -0400 Subject: [PATCH 1/6] Waypoint package draft --- src/msgs/action/NavigateToWaypoint.action | 14 + src/msgs/msg/WaypointEntry.msg | 7 + src/msgs/msg/WaypointManagerState.msg | 4 + src/msgs/srv/GetWaypoint.srv | 4 + src/msgs/srv/ListWaypoints.srv | 3 + .../launch/waypoint_manager.launch.py | 21 ++ .../navigation/waypoint_manager/package.xml | 28 ++ .../resource/waypoint_manager | 0 .../navigation/waypoint_manager/setup.py | 29 ++ .../waypoint_manager/__init__.py | 0 .../waypoint_manager/waypoint_manager_node.py | 324 ++++++++++++++++++ 11 files changed, 434 insertions(+) create mode 100644 src/msgs/action/NavigateToWaypoint.action create mode 100644 src/msgs/msg/WaypointEntry.msg create mode 100644 src/msgs/msg/WaypointManagerState.msg create mode 100644 src/msgs/srv/GetWaypoint.srv create mode 100644 src/msgs/srv/ListWaypoints.srv create mode 100644 src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py create mode 100644 src/subsystems/navigation/waypoint_manager/package.xml create mode 100644 src/subsystems/navigation/waypoint_manager/resource/waypoint_manager create mode 100644 src/subsystems/navigation/waypoint_manager/setup.py create mode 100644 src/subsystems/navigation/waypoint_manager/waypoint_manager/__init__.py create mode 100644 src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py diff --git a/src/msgs/action/NavigateToWaypoint.action b/src/msgs/action/NavigateToWaypoint.action new file mode 100644 index 00000000..3f9eee54 --- /dev/null +++ b/src/msgs/action/NavigateToWaypoint.action @@ -0,0 +1,14 @@ +bool use_gps +float64 x +float64 y +float64 yaw +float64 latitude +float64 longitude +float32 position_tolerance +string name +--- +bool success +string message +--- +string status +float32 distance_remaining diff --git a/src/msgs/msg/WaypointEntry.msg b/src/msgs/msg/WaypointEntry.msg new file mode 100644 index 00000000..70013834 --- /dev/null +++ b/src/msgs/msg/WaypointEntry.msg @@ -0,0 +1,7 @@ +string id +float64 x +float64 y +float64 yaw +string description +bool visited +builtin_interfaces/Time timestamp diff --git a/src/msgs/msg/WaypointManagerState.msg b/src/msgs/msg/WaypointManagerState.msg new file mode 100644 index 00000000..b0fab743 --- /dev/null +++ b/src/msgs/msg/WaypointManagerState.msg @@ -0,0 +1,4 @@ +string active_waypoint_id +string nav_status +msgs/WaypointEntry[] waypoints +builtin_interfaces/Time last_update diff --git a/src/msgs/srv/GetWaypoint.srv b/src/msgs/srv/GetWaypoint.srv new file mode 100644 index 00000000..56cce69a --- /dev/null +++ b/src/msgs/srv/GetWaypoint.srv @@ -0,0 +1,4 @@ +string id +--- +bool found +msgs/WaypointEntry waypoint diff --git a/src/msgs/srv/ListWaypoints.srv b/src/msgs/srv/ListWaypoints.srv new file mode 100644 index 00000000..03060e47 --- /dev/null +++ b/src/msgs/srv/ListWaypoints.srv @@ -0,0 +1,3 @@ + +--- +msgs/WaypointEntry[] waypoints diff --git a/src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py b/src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py new file mode 100644 index 00000000..8efc75e4 --- /dev/null +++ b/src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py @@ -0,0 +1,21 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('use_sim_time', default_value='false'), + DeclareLaunchArgument('map_frame', default_value='map'), + Node( + package='waypoint_manager', + executable='waypoint_manager_node', + name='waypoint_manager', + parameters=[{ + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'map_frame': LaunchConfiguration('map_frame'), + }], + output='screen', + ), + ]) diff --git a/src/subsystems/navigation/waypoint_manager/package.xml b/src/subsystems/navigation/waypoint_manager/package.xml new file mode 100644 index 00000000..c5fbd6ef --- /dev/null +++ b/src/subsystems/navigation/waypoint_manager/package.xml @@ -0,0 +1,28 @@ + + + + waypoint_manager + 0.0.0 + Waypoint management and navigation node for simulation + abhinavkota06 + MIT + + rclpy + msgs + nav2_msgs + geometry_msgs + std_msgs + action_msgs + tf2_ros + ament_index_python + python3-yaml + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/subsystems/navigation/waypoint_manager/resource/waypoint_manager b/src/subsystems/navigation/waypoint_manager/resource/waypoint_manager new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/navigation/waypoint_manager/setup.py b/src/subsystems/navigation/waypoint_manager/setup.py new file mode 100644 index 00000000..a71ee970 --- /dev/null +++ b/src/subsystems/navigation/waypoint_manager/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup +from glob import glob +import os + +package_name = 'waypoint_manager' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='abhinavkota06', + maintainer_email='abhinav.kota06@gmail.com', + description='Waypoint management and navigation node for simulation', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'waypoint_manager_node = waypoint_manager.waypoint_manager_node:main', + ], + }, +) diff --git a/src/subsystems/navigation/waypoint_manager/waypoint_manager/__init__.py b/src/subsystems/navigation/waypoint_manager/waypoint_manager/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py b/src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py new file mode 100644 index 00000000..f290efae --- /dev/null +++ b/src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py @@ -0,0 +1,324 @@ +#!/usr/bin/env python3 +import math + +import rclpy +from rclpy.node import Node +from rclpy.action import ActionServer, ActionClient, GoalResponse, CancelResponse +from rclpy.action.server import ServerGoalHandle +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy +from action_msgs.msg import GoalStatus, GoalStatusArray +import tf2_ros +from tf2_ros import LookupException, ConnectivityException, ExtrapolationException + +from std_msgs.msg import String +from geometry_msgs.msg import PoseStamped, Quaternion + +from nav2_msgs.action import NavigateToPose + +from msgs.msg import WaypointEntry, WaypointManagerState +from msgs.srv import ListWaypoints, GetWaypoint +from msgs.action import NavigateToGPS, NavigateToWaypoint + + +class WaypointManagerNode(Node): + def __init__(self): + super().__init__('waypoint_manager') + + self.declare_parameter('map_frame', 'map') + self.declare_parameter('robot_frame', 'base_footprint') + self._map_frame = self.get_parameter('map_frame').value + self._robot_frame = self.get_parameter('robot_frame').value + + # In-memory store: only locations where Nav2 reported STATUS_SUCCEEDED + self._waypoints: dict[str, dict] = {} + self._wp_counter = 0 + + # Track nav2 goal statuses to detect SUCCEEDED transitions + self._seen_goal_statuses: dict[bytes, int] = {} + + self._nav_status = 'idle' + self._active_nav_label = '' + + self._cb_group = ReentrantCallbackGroup() + + # TF listener for pose lookup on nav success + self._tf_buffer = tf2_ros.Buffer() + self._tf_listener = tf2_ros.TransformListener(self._tf_buffer, self) + + # /nav_mode publisher — QoS must match NavSelector (transient_local, reliable, keep_last(1)) + nav_mode_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + ) + self._nav_mode_pub = self.create_publisher(String, '/nav_mode', nav_mode_qos) + + # State broadcast at 2 Hz + self._state_pub = self.create_publisher( + WaypointManagerState, '/waypoint_manager/state', 10 + ) + self.create_timer(0.5, self._publish_state) + + # Watch Nav2 goal statuses — store waypoint whenever navigate_to_pose succeeds + self.create_subscription( + GoalStatusArray, + '/navigate_to_pose/_action/status', + self._nav2_status_cb, + 10, + ) + + # Query services + self.create_service(ListWaypoints, '~/list_waypoints', self._list_waypoints_cb) + self.create_service(GetWaypoint, '~/get_waypoint', self._get_waypoint_cb) + + # NavigateToWaypoint action server — command interface only, does not store waypoints + self._nav_action_server = ActionServer( + self, + NavigateToWaypoint, + '~/navigate_to_waypoint', + execute_callback=self._navigate_execute_cb, + goal_callback=self._goal_callback, + cancel_callback=self._cancel_callback, + callback_group=self._cb_group, + ) + + # Nav2 NavigateToPose action client (map-frame goals) + self._nav2_client = ActionClient( + self, + NavigateToPose, + 'navigate_to_pose', + callback_group=self._cb_group, + ) + + # GPS action client (lat/lon goals — requires gps_goal server to be running) + self._gps_client = ActionClient( + self, + NavigateToGPS, + 'navigate_to_gps', + callback_group=self._cb_group, + ) + + self.get_logger().info('WaypointManager ready') + + # ── Nav2 status watcher ─────────────────────────────────────────────────── + + def _nav2_status_cb(self, msg: GoalStatusArray): + for entry in msg.status_list: + uid = bytes(entry.goal_info.goal_id.uuid) + prev = self._seen_goal_statuses.get(uid) + curr = entry.status + + if prev != GoalStatus.STATUS_SUCCEEDED and curr == GoalStatus.STATUS_SUCCEEDED: + self._store_current_pose() + + self._seen_goal_statuses[uid] = curr + + def _store_current_pose(self): + try: + t = self._tf_buffer.lookup_transform( + self._map_frame, self._robot_frame, rclpy.time.Time() + ) + except (LookupException, ConnectivityException, ExtrapolationException) as e: + self.get_logger().warn(f'TF lookup failed on nav success, waypoint not stored: {e}') + return + + x = t.transform.translation.x + y = t.transform.translation.y + q = t.transform.rotation + yaw = math.atan2( + 2.0 * (q.w * q.z + q.x * q.y), + 1.0 - 2.0 * (q.y * q.y + q.z * q.z), + ) + + self._wp_counter += 1 + wp_id = f'wp_{self._wp_counter}' + now = self.get_clock().now().to_msg() + self._waypoints[wp_id] = { + 'id': wp_id, + 'x': x, + 'y': y, + 'yaw': yaw, + 'description': '', + 'timestamp': {'sec': now.sec, 'nanosec': now.nanosec}, + } + self.get_logger().info( + f'Nav2 goal succeeded — stored "{wp_id}" at ({x:.2f}, {y:.2f})' + ) + + # ── Helpers ─────────────────────────────────────────────────────────────── + + def _dict_to_msg(self, d: dict) -> WaypointEntry: + msg = WaypointEntry() + msg.id = d['id'] + msg.x = float(d['x']) + msg.y = float(d['y']) + msg.yaw = float(d['yaw']) + msg.description = d.get('description', '') + msg.visited = True + ts = d.get('timestamp', {}) + msg.timestamp.sec = int(ts.get('sec', 0)) + msg.timestamp.nanosec = int(ts.get('nanosec', 0)) + return msg + + # ── State broadcast ─────────────────────────────────────────────────────── + + def _publish_state(self): + msg = WaypointManagerState() + msg.active_waypoint_id = self._active_nav_label + msg.nav_status = self._nav_status + msg.waypoints = [self._dict_to_msg(d) for d in self._waypoints.values()] + msg.last_update = self.get_clock().now().to_msg() + self._state_pub.publish(msg) + + # ── Service callbacks ───────────────────────────────────────────────────── + + def _list_waypoints_cb(self, request, response): + response.waypoints = [self._dict_to_msg(d) for d in self._waypoints.values()] + return response + + def _get_waypoint_cb(self, request, response): + if request.id not in self._waypoints: + response.found = False + return response + response.found = True + response.waypoint = self._dict_to_msg(self._waypoints[request.id]) + return response + + # ── Action callbacks ────────────────────────────────────────────────────── + + def _goal_callback(self, goal_request): + if self._nav_status == 'navigating': + self.get_logger().warn('Goal rejected: navigation already in progress') + return GoalResponse.REJECT + return GoalResponse.ACCEPT + + def _cancel_callback(self, goal_handle): + self.get_logger().info('Cancel requested for active navigation') + return CancelResponse.ACCEPT + + async def _navigate_execute_cb(self, goal_handle: ServerGoalHandle): + req = goal_handle.request + label = req.name or 'unnamed' + + self._active_nav_label = label + self._nav_status = 'navigating' + + self._nav_mode_pub.publish(String(data='GNSS')) + + result = NavigateToWaypoint.Result() + + def _feedback_cb(fb): + feedback = NavigateToWaypoint.Feedback() + feedback.status = 'navigating' + feedback.distance_remaining = fb.feedback.distance_remaining + goal_handle.publish_feedback(feedback) + + if req.use_gps: + self.get_logger().info( + f'Navigating to "{label}" via GPS ' + f'(lat={req.latitude:.6f}, lon={req.longitude:.6f})' + ) + if not self._gps_client.wait_for_server(timeout_sec=5.0): + self.get_logger().error('navigate_to_gps server not available') + self._nav_status = 'failed' + self._active_nav_label = '' + goal_handle.abort() + result.success = False + result.message = 'navigate_to_gps server not available' + return result + + gps_goal = NavigateToGPS.Goal() + gps_goal.latitude = req.latitude + gps_goal.longitude = req.longitude + gps_goal.position_tolerance = req.position_tolerance + + send_future = self._gps_client.send_goal_async(gps_goal, feedback_callback=_feedback_cb) + else: + self.get_logger().info( + f'Navigating to "{label}" via map frame ' + f'({req.x:.2f}, {req.y:.2f}), yaw={req.yaw:.2f}' + ) + if not self._nav2_client.wait_for_server(timeout_sec=5.0): + self.get_logger().error('navigate_to_pose server not available') + self._nav_status = 'failed' + self._active_nav_label = '' + goal_handle.abort() + result.success = False + result.message = 'navigate_to_pose server not available' + return result + + pose_goal = NavigateToPose.Goal() + pose_goal.pose = PoseStamped() + pose_goal.pose.header.frame_id = self._map_frame + pose_goal.pose.header.stamp = self.get_clock().now().to_msg() + pose_goal.pose.pose.position.x = req.x + pose_goal.pose.pose.position.y = req.y + pose_goal.pose.pose.position.z = 0.0 + pose_goal.pose.pose.orientation = Quaternion( + x=0.0, + y=0.0, + z=math.sin(req.yaw / 2.0), + w=math.cos(req.yaw / 2.0), + ) + + send_future = self._nav2_client.send_goal_async(pose_goal, feedback_callback=_feedback_cb) + + nav_goal_handle = await send_future + + if not nav_goal_handle.accepted: + self.get_logger().error('Goal rejected by navigation server') + self._nav_status = 'failed' + self._active_nav_label = '' + goal_handle.abort() + result.success = False + result.message = 'Goal rejected by navigation server' + return result + + nav_result = await nav_goal_handle.get_result_async() + + if goal_handle.is_cancel_requested: + self._nav_status = 'idle' + self._active_nav_label = '' + goal_handle.canceled() + result.success = False + result.message = 'Canceled' + return result + + if nav_result.status == GoalStatus.STATUS_SUCCEEDED: + self._nav_status = 'success' + goal_handle.succeed() + result.success = True + result.message = f'Reached "{label}"' + elif nav_result.status == GoalStatus.STATUS_CANCELED: + self._nav_status = 'idle' + goal_handle.canceled() + result.success = False + result.message = 'Navigation canceled' + else: + self._nav_status = 'failed' + self.get_logger().warn( + f'Navigation to "{label}" failed (status={nav_result.status})' + ) + goal_handle.abort() + result.success = False + result.message = f'Navigation failed (status={nav_result.status})' + + self._active_nav_label = '' + return result + + +def main(args=None): + rclpy.init(args=args) + node = WaypointManagerNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() From 0c497dcad3794d5fa74427f9ded22db5b87967e9 Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Sun, 3 May 2026 12:23:24 -0400 Subject: [PATCH 2/6] Working version of waypoints --- .../launch/waypoint_manager.launch.py | 1 - .../navigation/waypoint_manager/setup.cfg | 4 + .../waypoint_manager/waypoint_manager_node.py | 242 ++++++++---------- 3 files changed, 105 insertions(+), 142 deletions(-) create mode 100644 src/subsystems/navigation/waypoint_manager/setup.cfg diff --git a/src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py b/src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py index 8efc75e4..085c11dd 100644 --- a/src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py +++ b/src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py @@ -7,7 +7,6 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('use_sim_time', default_value='false'), - DeclareLaunchArgument('map_frame', default_value='map'), Node( package='waypoint_manager', executable='waypoint_manager_node', diff --git a/src/subsystems/navigation/waypoint_manager/setup.cfg b/src/subsystems/navigation/waypoint_manager/setup.cfg new file mode 100644 index 00000000..753f219e --- /dev/null +++ b/src/subsystems/navigation/waypoint_manager/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/waypoint_manager +[install] +install_scripts=$base/lib/waypoint_manager diff --git a/src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py b/src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py index f290efae..e1017362 100644 --- a/src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py +++ b/src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -import math import rclpy from rclpy.node import Node @@ -8,16 +7,12 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy from action_msgs.msg import GoalStatus, GoalStatusArray -import tf2_ros -from tf2_ros import LookupException, ConnectivityException, ExtrapolationException from std_msgs.msg import String -from geometry_msgs.msg import PoseStamped, Quaternion - -from nav2_msgs.action import NavigateToPose +from sensor_msgs.msg import NavSatFix from msgs.msg import WaypointEntry, WaypointManagerState -from msgs.srv import ListWaypoints, GetWaypoint +from msgs.srv import ClearWaypoints from msgs.action import NavigateToGPS, NavigateToWaypoint @@ -25,54 +20,55 @@ class WaypointManagerNode(Node): def __init__(self): super().__init__('waypoint_manager') - self.declare_parameter('map_frame', 'map') - self.declare_parameter('robot_frame', 'base_footprint') - self._map_frame = self.get_parameter('map_frame').value - self._robot_frame = self.get_parameter('robot_frame').value - - # In-memory store: only locations where Nav2 reported STATUS_SUCCEEDED - self._waypoints: dict[str, dict] = {} + # In-memory store: uint32 id → {id, latitude, longitude, altitude, timestamp} + self._waypoints: dict[int, dict] = {} self._wp_counter = 0 - # Track nav2 goal statuses to detect SUCCEEDED transitions + self._latest_gps: NavSatFix | None = None self._seen_goal_statuses: dict[bytes, int] = {} self._nav_status = 'idle' - self._active_nav_label = '' + self._active_waypoint_id = 0 # 0 = none + self._current_nav_goal_handle = None self._cb_group = ReentrantCallbackGroup() - # TF listener for pose lookup on nav success - self._tf_buffer = tf2_ros.Buffer() - self._tf_listener = tf2_ros.TransformListener(self._tf_buffer, self) + self.declare_parameter('gps_topic', '/gps/fix') + self.declare_parameter('nav_mode_topic', '/nav_mode') + self.declare_parameter('nav2_status_topic', '/navigate_to_pose/_action/status') + self.declare_parameter('navigate_to_gps_action', 'navigate_to_gps') + + gps_topic = self.get_parameter('gps_topic').value + nav_mode_topic = self.get_parameter('nav_mode_topic').value + nav2_status_topic = self.get_parameter('nav2_status_topic').value + navigate_to_gps_action = self.get_parameter('navigate_to_gps_action').value - # /nav_mode publisher — QoS must match NavSelector (transient_local, reliable, keep_last(1)) + # QoS must match NavSelector (transient_local, reliable, depth=1) nav_mode_qos = QoSProfile( depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, reliability=ReliabilityPolicy.RELIABLE, ) - self._nav_mode_pub = self.create_publisher(String, '/nav_mode', nav_mode_qos) + self._nav_mode_pub = self.create_publisher(String, nav_mode_topic, nav_mode_qos) - # State broadcast at 2 Hz self._state_pub = self.create_publisher( WaypointManagerState, '/waypoint_manager/state', 10 ) self.create_timer(0.5, self._publish_state) - # Watch Nav2 goal statuses — store waypoint whenever navigate_to_pose succeeds + self.create_subscription(NavSatFix, gps_topic, self._gps_cb, 10) + self.get_logger().info(f'Subscribed to {gps_topic}') + self.create_subscription( GoalStatusArray, - '/navigate_to_pose/_action/status', + nav2_status_topic, self._nav2_status_cb, 10, ) + self.get_logger().info(f'Subscribed to {nav2_status_topic}') - # Query services - self.create_service(ListWaypoints, '~/list_waypoints', self._list_waypoints_cb) - self.create_service(GetWaypoint, '~/get_waypoint', self._get_waypoint_cb) + self.create_service(ClearWaypoints, '~/clear_waypoints', self._clear_waypoints_cb) - # NavigateToWaypoint action server — command interface only, does not store waypoints self._nav_action_server = ActionServer( self, NavigateToWaypoint, @@ -83,24 +79,25 @@ def __init__(self): callback_group=self._cb_group, ) - # Nav2 NavigateToPose action client (map-frame goals) - self._nav2_client = ActionClient( - self, - NavigateToPose, - 'navigate_to_pose', - callback_group=self._cb_group, - ) - - # GPS action client (lat/lon goals — requires gps_goal server to be running) self._gps_client = ActionClient( self, NavigateToGPS, - 'navigate_to_gps', + navigate_to_gps_action, callback_group=self._cb_group, ) self.get_logger().info('WaypointManager ready') + # ── GPS fix cache ───────────────────────────────────────────────────────── + + def _gps_cb(self, msg: NavSatFix): + if self._latest_gps is None: + self.get_logger().info( + f'First GPS fix received: ({msg.latitude:.6f}, {msg.longitude:.6f}), ' + f'status={msg.status.status}' + ) + self._latest_gps = msg + # ── Nav2 status watcher ─────────────────────────────────────────────────── def _nav2_status_cb(self, msg: GoalStatusArray): @@ -108,42 +105,29 @@ def _nav2_status_cb(self, msg: GoalStatusArray): uid = bytes(entry.goal_info.goal_id.uuid) prev = self._seen_goal_statuses.get(uid) curr = entry.status - if prev != GoalStatus.STATUS_SUCCEEDED and curr == GoalStatus.STATUS_SUCCEEDED: - self._store_current_pose() - + self.get_logger().info('Nav2 goal succeeded — storing GPS waypoint') + self._store_current_gps() self._seen_goal_statuses[uid] = curr - def _store_current_pose(self): - try: - t = self._tf_buffer.lookup_transform( - self._map_frame, self._robot_frame, rclpy.time.Time() - ) - except (LookupException, ConnectivityException, ExtrapolationException) as e: - self.get_logger().warn(f'TF lookup failed on nav success, waypoint not stored: {e}') + def _store_current_gps(self): + if self._latest_gps is None: + self.get_logger().warn('No GPS fix available; waypoint not stored') return - - x = t.transform.translation.x - y = t.transform.translation.y - q = t.transform.rotation - yaw = math.atan2( - 2.0 * (q.w * q.z + q.x * q.y), - 1.0 - 2.0 * (q.y * q.y + q.z * q.z), - ) - + gps = self._latest_gps self._wp_counter += 1 - wp_id = f'wp_{self._wp_counter}' + wp_id = self._wp_counter now = self.get_clock().now().to_msg() self._waypoints[wp_id] = { 'id': wp_id, - 'x': x, - 'y': y, - 'yaw': yaw, - 'description': '', + 'latitude': gps.latitude, + 'longitude': gps.longitude, + 'altitude': gps.altitude, 'timestamp': {'sec': now.sec, 'nanosec': now.nanosec}, } self.get_logger().info( - f'Nav2 goal succeeded — stored "{wp_id}" at ({x:.2f}, {y:.2f})' + f'Nav2 goal succeeded — stored waypoint {wp_id} at ' + f'({gps.latitude:.6f}, {gps.longitude:.6f})' ) # ── Helpers ─────────────────────────────────────────────────────────────── @@ -151,11 +135,9 @@ def _store_current_pose(self): def _dict_to_msg(self, d: dict) -> WaypointEntry: msg = WaypointEntry() msg.id = d['id'] - msg.x = float(d['x']) - msg.y = float(d['y']) - msg.yaw = float(d['yaw']) - msg.description = d.get('description', '') - msg.visited = True + msg.latitude = float(d['latitude']) + msg.longitude = float(d['longitude']) + msg.altitude = float(d['altitude']) ts = d.get('timestamp', {}) msg.timestamp.sec = int(ts.get('sec', 0)) msg.timestamp.nanosec = int(ts.get('nanosec', 0)) @@ -165,7 +147,7 @@ def _dict_to_msg(self, d: dict) -> WaypointEntry: def _publish_state(self): msg = WaypointManagerState() - msg.active_waypoint_id = self._active_nav_label + msg.active_waypoint_id = self._active_waypoint_id msg.nav_status = self._nav_status msg.waypoints = [self._dict_to_msg(d) for d in self._waypoints.values()] msg.last_update = self.get_clock().now().to_msg() @@ -173,16 +155,13 @@ def _publish_state(self): # ── Service callbacks ───────────────────────────────────────────────────── - def _list_waypoints_cb(self, request, response): - response.waypoints = [self._dict_to_msg(d) for d in self._waypoints.values()] - return response - - def _get_waypoint_cb(self, request, response): - if request.id not in self._waypoints: - response.found = False - return response - response.found = True - response.waypoint = self._dict_to_msg(self._waypoints[request.id]) + def _clear_waypoints_cb(self, request, response): + count = len(self._waypoints) + self._waypoints.clear() + self._wp_counter = 0 + response.success = True + response.message = f'Cleared {count} waypoint(s)' + self.get_logger().info(f'Cleared {count} waypoint(s)') return response # ── Action callbacks ────────────────────────────────────────────────────── @@ -194,19 +173,31 @@ def _goal_callback(self, goal_request): return GoalResponse.ACCEPT def _cancel_callback(self, goal_handle): - self.get_logger().info('Cancel requested for active navigation') + self.get_logger().info('Cancel requested — forwarding to navigate_to_gps') + if self._current_nav_goal_handle is not None: + self._current_nav_goal_handle.cancel_goal_async() return CancelResponse.ACCEPT async def _navigate_execute_cb(self, goal_handle: ServerGoalHandle): - req = goal_handle.request - label = req.name or 'unnamed' + wp_id = goal_handle.request.waypoint_id + result = NavigateToWaypoint.Result() - self._active_nav_label = label - self._nav_status = 'navigating' + if wp_id not in self._waypoints: + self.get_logger().error(f'Waypoint {wp_id} not found') + goal_handle.abort() + result.success = False + result.message = f'Waypoint {wp_id} not found' + return result + wp = self._waypoints[wp_id] + self._active_waypoint_id = wp_id + self._nav_status = 'navigating' self._nav_mode_pub.publish(String(data='GNSS')) - result = NavigateToWaypoint.Result() + self.get_logger().info( + f'Navigating to waypoint {wp_id} ' + f'(lat={wp["latitude"]:.6f}, lon={wp["longitude"]:.6f})' + ) def _feedback_cb(fb): feedback = NavigateToWaypoint.Feedback() @@ -214,72 +205,41 @@ def _feedback_cb(fb): feedback.distance_remaining = fb.feedback.distance_remaining goal_handle.publish_feedback(feedback) - if req.use_gps: - self.get_logger().info( - f'Navigating to "{label}" via GPS ' - f'(lat={req.latitude:.6f}, lon={req.longitude:.6f})' - ) - if not self._gps_client.wait_for_server(timeout_sec=5.0): - self.get_logger().error('navigate_to_gps server not available') - self._nav_status = 'failed' - self._active_nav_label = '' - goal_handle.abort() - result.success = False - result.message = 'navigate_to_gps server not available' - return result - - gps_goal = NavigateToGPS.Goal() - gps_goal.latitude = req.latitude - gps_goal.longitude = req.longitude - gps_goal.position_tolerance = req.position_tolerance - - send_future = self._gps_client.send_goal_async(gps_goal, feedback_callback=_feedback_cb) - else: - self.get_logger().info( - f'Navigating to "{label}" via map frame ' - f'({req.x:.2f}, {req.y:.2f}), yaw={req.yaw:.2f}' - ) - if not self._nav2_client.wait_for_server(timeout_sec=5.0): - self.get_logger().error('navigate_to_pose server not available') - self._nav_status = 'failed' - self._active_nav_label = '' - goal_handle.abort() - result.success = False - result.message = 'navigate_to_pose server not available' - return result - - pose_goal = NavigateToPose.Goal() - pose_goal.pose = PoseStamped() - pose_goal.pose.header.frame_id = self._map_frame - pose_goal.pose.header.stamp = self.get_clock().now().to_msg() - pose_goal.pose.pose.position.x = req.x - pose_goal.pose.pose.position.y = req.y - pose_goal.pose.pose.position.z = 0.0 - pose_goal.pose.pose.orientation = Quaternion( - x=0.0, - y=0.0, - z=math.sin(req.yaw / 2.0), - w=math.cos(req.yaw / 2.0), - ) + if not self._gps_client.wait_for_server(timeout_sec=5.0): + self.get_logger().error('navigate_to_gps server not available') + self._nav_status = 'failed' + self._active_waypoint_id = 0 + goal_handle.abort() + result.success = False + result.message = 'navigate_to_gps server not available' + return result - send_future = self._nav2_client.send_goal_async(pose_goal, feedback_callback=_feedback_cb) + gps_goal = NavigateToGPS.Goal() + gps_goal.latitude = wp['latitude'] + gps_goal.longitude = wp['longitude'] + gps_goal.position_tolerance = 0.0 # use gps_goal server default + send_future = self._gps_client.send_goal_async(gps_goal, feedback_callback=_feedback_cb) nav_goal_handle = await send_future if not nav_goal_handle.accepted: - self.get_logger().error('Goal rejected by navigation server') + self.get_logger().error('Goal rejected by navigate_to_gps server') self._nav_status = 'failed' - self._active_nav_label = '' + self._active_waypoint_id = 0 goal_handle.abort() result.success = False - result.message = 'Goal rejected by navigation server' + result.message = 'Goal rejected by navigate_to_gps server' return result - nav_result = await nav_goal_handle.get_result_async() + self._current_nav_goal_handle = nav_goal_handle + try: + nav_result = await nav_goal_handle.get_result_async() + finally: + self._current_nav_goal_handle = None if goal_handle.is_cancel_requested: self._nav_status = 'idle' - self._active_nav_label = '' + self._active_waypoint_id = 0 goal_handle.canceled() result.success = False result.message = 'Canceled' @@ -289,7 +249,7 @@ def _feedback_cb(fb): self._nav_status = 'success' goal_handle.succeed() result.success = True - result.message = f'Reached "{label}"' + result.message = f'Reached waypoint {wp_id}' elif nav_result.status == GoalStatus.STATUS_CANCELED: self._nav_status = 'idle' goal_handle.canceled() @@ -298,13 +258,13 @@ def _feedback_cb(fb): else: self._nav_status = 'failed' self.get_logger().warn( - f'Navigation to "{label}" failed (status={nav_result.status})' + f'Navigation to waypoint {wp_id} failed (status={nav_result.status})' ) goal_handle.abort() result.success = False result.message = f'Navigation failed (status={nav_result.status})' - self._active_nav_label = '' + self._active_waypoint_id = 0 return result From d551c6df0c10e33b4dd8f9fcab4ed32294662e91 Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Sun, 3 May 2026 12:24:12 -0400 Subject: [PATCH 3/6] Added msg, srv, and actions for waypoints --- src/msgs/CMakeLists.txt | 4 ++++ src/msgs/action/NavigateToWaypoint.action | 9 +-------- src/msgs/msg/WaypointEntry.msg | 10 ++++------ src/msgs/msg/WaypointManagerState.msg | 4 ++-- src/msgs/srv/ClearWaypoints.srv | 3 +++ src/msgs/srv/GetWaypoint.srv | 4 ---- src/msgs/srv/ListWaypoints.srv | 3 --- 7 files changed, 14 insertions(+), 23 deletions(-) create mode 100644 src/msgs/srv/ClearWaypoints.srv delete mode 100644 src/msgs/srv/GetWaypoint.srv delete mode 100644 src/msgs/srv/ListWaypoints.srv diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 83a12abd..32c4bf8f 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -20,8 +20,12 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/JointReceiver.msg" "msg/SystemInfo.msg" "msg/Heading.msg" + "msg/WaypointEntry.msg" + "msg/WaypointManagerState.msg" "srv/SetController.srv" + "srv/ClearWaypoints.srv" "action/NavigateToGPS.action" + "action/NavigateToWaypoint.action" DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces action_msgs ) diff --git a/src/msgs/action/NavigateToWaypoint.action b/src/msgs/action/NavigateToWaypoint.action index 3f9eee54..5ab20aed 100644 --- a/src/msgs/action/NavigateToWaypoint.action +++ b/src/msgs/action/NavigateToWaypoint.action @@ -1,11 +1,4 @@ -bool use_gps -float64 x -float64 y -float64 yaw -float64 latitude -float64 longitude -float32 position_tolerance -string name +uint32 waypoint_id --- bool success string message diff --git a/src/msgs/msg/WaypointEntry.msg b/src/msgs/msg/WaypointEntry.msg index 70013834..6e39bd0f 100644 --- a/src/msgs/msg/WaypointEntry.msg +++ b/src/msgs/msg/WaypointEntry.msg @@ -1,7 +1,5 @@ -string id -float64 x -float64 y -float64 yaw -string description -bool visited +uint32 id +float64 latitude +float64 longitude +float64 altitude builtin_interfaces/Time timestamp diff --git a/src/msgs/msg/WaypointManagerState.msg b/src/msgs/msg/WaypointManagerState.msg index b0fab743..94656069 100644 --- a/src/msgs/msg/WaypointManagerState.msg +++ b/src/msgs/msg/WaypointManagerState.msg @@ -1,4 +1,4 @@ -string active_waypoint_id +uint32 active_waypoint_id string nav_status -msgs/WaypointEntry[] waypoints +WaypointEntry[] waypoints builtin_interfaces/Time last_update diff --git a/src/msgs/srv/ClearWaypoints.srv b/src/msgs/srv/ClearWaypoints.srv new file mode 100644 index 00000000..963bf4cd --- /dev/null +++ b/src/msgs/srv/ClearWaypoints.srv @@ -0,0 +1,3 @@ +--- +bool success +string message diff --git a/src/msgs/srv/GetWaypoint.srv b/src/msgs/srv/GetWaypoint.srv deleted file mode 100644 index 56cce69a..00000000 --- a/src/msgs/srv/GetWaypoint.srv +++ /dev/null @@ -1,4 +0,0 @@ -string id ---- -bool found -msgs/WaypointEntry waypoint diff --git a/src/msgs/srv/ListWaypoints.srv b/src/msgs/srv/ListWaypoints.srv deleted file mode 100644 index 03060e47..00000000 --- a/src/msgs/srv/ListWaypoints.srv +++ /dev/null @@ -1,3 +0,0 @@ - ---- -msgs/WaypointEntry[] waypoints From 991a5bc4f69786538d4eed4c2954fd97a65747ab Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Sun, 3 May 2026 12:24:38 -0400 Subject: [PATCH 4/6] Added waypoint manager to navigation launch --- .../launch/navigation.launch.py | 37 ++++++++++++------- 1 file changed, 23 insertions(+), 14 deletions(-) diff --git a/src/subsystems/navigation/athena_planner/launch/navigation.launch.py b/src/subsystems/navigation/athena_planner/launch/navigation.launch.py index e4a4a8ee..89017836 100644 --- a/src/subsystems/navigation/athena_planner/launch/navigation.launch.py +++ b/src/subsystems/navigation/athena_planner/launch/navigation.launch.py @@ -21,14 +21,17 @@ def generate_launch_description(): localizer_launch_file = os.path.join(localizer_share, 'launch', 'localizer.launch.py') zed_tf_publisher_launch_file = os.path.join(localizer_share, 'launch', 'zed_tf_publisher.launch.py') - # gps_goal_share = get_package_share_directory('gps_goal') - # gps_goal_launch_file = os.path.join(gps_goal_share, 'launch', 'gps_goal_server.launch.py') + gps_goal_share = get_package_share_directory('gps_goal') + gps_goal_launch_file = os.path.join(gps_goal_share, 'launch', 'gps_goal_server.launch.py') sensors_share = get_package_share_directory('athena_sensors') sensors_launch_file = os.path.join(sensors_share, 'launch', 'sensors.launch.py') - # aruco_bt_share = get_package_share_directory('aruco_bt') - # aruco_launch_file = os.path.join(aruco_bt_share, 'launch', 'aruco.launch.py') + aruco_bt_share = get_package_share_directory('aruco_bt') + aruco_launch_file = os.path.join(aruco_bt_share, 'launch', 'aruco.launch.py') + + waypoint_share = get_package_share_directory('waypoint_manager') + waypoint_launch_file = os.path.join(waypoint_share, 'launch', 'waypoint_manager.launch.py') default_params = PathJoinSubstitution([ FindPackageShare('athena_planner'), 'config', 'nav2_params.yaml' @@ -91,15 +94,20 @@ def generate_launch_description(): }.items(), ) - # aruco_launch = IncludeLaunchDescription( - # PythonLaunchDescriptionSource(aruco_launch_file), - # launch_arguments={'use_sim_time': sim, 'marker_size': '0.20'}.items() - # ) + aruco_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(aruco_launch_file), + launch_arguments={'use_sim_time': sim, 'marker_size': '0.20'}.items() + ) - # gps_goal_launch = IncludeLaunchDescription( - # PythonLaunchDescriptionSource(gps_goal_launch_file), - # launch_arguments={'use_sim_time': sim}.items(), - # ) + waypoint_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(waypoint_launch_file), + launch_arguments={'use_sim_time': sim}.items() + ) + + gps_goal_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(gps_goal_launch_file), + launch_arguments={'use_sim_time': sim}.items(), + ) point_cloud_filterer_sim = Node( package='point_cloud_filterer', @@ -170,10 +178,11 @@ def generate_launch_description(): localizer_launch, zed_tf_publisher_launch, sensors_launch, - # aruco_launch, + aruco_launch, + waypoint_launch, point_cloud_filterer_sim, point_cloud_relay, - # gps_goal_launch, + gps_goal_launch, IncludeLaunchDescription( PythonLaunchDescriptionSource(nav2_nav), From b97550eb2fc64b5b280b68c307e32f780fcd6add Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Sat, 9 May 2026 18:03:16 -0400 Subject: [PATCH 5/6] Removed altitude, added new service for manually adding waypoints --- src/msgs/CMakeLists.txt | 1 + src/msgs/msg/WaypointEntry.msg | 1 - src/msgs/msg/WaypointManagerState.msg | 8 +++++++- src/msgs/srv/AddWaypoint.srv | 6 ++++++ 4 files changed, 14 insertions(+), 2 deletions(-) create mode 100644 src/msgs/srv/AddWaypoint.srv diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 32c4bf8f..df315c9a 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -23,6 +23,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/WaypointEntry.msg" "msg/WaypointManagerState.msg" "srv/SetController.srv" + "srv/AddWaypoint.srv" "srv/ClearWaypoints.srv" "action/NavigateToGPS.action" "action/NavigateToWaypoint.action" diff --git a/src/msgs/msg/WaypointEntry.msg b/src/msgs/msg/WaypointEntry.msg index 6e39bd0f..b66ef5e4 100644 --- a/src/msgs/msg/WaypointEntry.msg +++ b/src/msgs/msg/WaypointEntry.msg @@ -1,5 +1,4 @@ uint32 id float64 latitude float64 longitude -float64 altitude builtin_interfaces/Time timestamp diff --git a/src/msgs/msg/WaypointManagerState.msg b/src/msgs/msg/WaypointManagerState.msg index 94656069..088b5761 100644 --- a/src/msgs/msg/WaypointManagerState.msg +++ b/src/msgs/msg/WaypointManagerState.msg @@ -1,4 +1,10 @@ +uint8 STATUS_IDLE = 0 +uint8 STATUS_NAVIGATING = 1 +uint8 STATUS_SUCCEEDED = 2 +uint8 STATUS_CANCELED = 3 +uint8 STATUS_FAILED = 4 + uint32 active_waypoint_id -string nav_status +uint8 nav_status WaypointEntry[] waypoints builtin_interfaces/Time last_update diff --git a/src/msgs/srv/AddWaypoint.srv b/src/msgs/srv/AddWaypoint.srv new file mode 100644 index 00000000..3ad73166 --- /dev/null +++ b/src/msgs/srv/AddWaypoint.srv @@ -0,0 +1,6 @@ +float64 latitude +float64 longitude +--- +bool success +string message +uint32 assigned_id From c4a9fbafc371b7eb27218ee5fde02098a88fd966 Mon Sep 17 00:00:00 2001 From: Abhinav Kota Date: Sat, 9 May 2026 18:04:17 -0400 Subject: [PATCH 6/6] Added functionality for saving/loading waypoints to/from a file for persistence --- .../launch/waypoint_manager.launch.py | 13 ++- .../waypoint_manager/waypoint_manager_node.py | 106 +++++++++++++++--- 2 files changed, 104 insertions(+), 15 deletions(-) diff --git a/src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py b/src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py index 085c11dd..94e3bba7 100644 --- a/src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py +++ b/src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py @@ -7,13 +7,24 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('use_sim_time', default_value='false'), + DeclareLaunchArgument( + 'waypoints_file', + default_value='~/.ros/waypoints.yaml', + description='Path to YAML file for waypoint persistence' + ), + DeclareLaunchArgument( + 'load_from_file', + default_value='true', + description='Load saved waypoints from file on startup' + ), Node( package='waypoint_manager', executable='waypoint_manager_node', name='waypoint_manager', parameters=[{ 'use_sim_time': LaunchConfiguration('use_sim_time'), - 'map_frame': LaunchConfiguration('map_frame'), + 'waypoints_file': LaunchConfiguration('waypoints_file'), + 'load_from_file': LaunchConfiguration('load_from_file'), }], output='screen', ), diff --git a/src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py b/src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py index e1017362..390e669d 100644 --- a/src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py +++ b/src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py @@ -1,5 +1,7 @@ #!/usr/bin/env python3 +import os +import yaml import rclpy from rclpy.node import Node from rclpy.action import ActionServer, ActionClient, GoalResponse, CancelResponse @@ -12,7 +14,7 @@ from sensor_msgs.msg import NavSatFix from msgs.msg import WaypointEntry, WaypointManagerState -from msgs.srv import ClearWaypoints +from msgs.srv import AddWaypoint, ClearWaypoints from msgs.action import NavigateToGPS, NavigateToWaypoint @@ -20,14 +22,14 @@ class WaypointManagerNode(Node): def __init__(self): super().__init__('waypoint_manager') - # In-memory store: uint32 id → {id, latitude, longitude, altitude, timestamp} + # In-memory store: uint32 id → {id, latitude, longitude, timestamp} self._waypoints: dict[int, dict] = {} self._wp_counter = 0 self._latest_gps: NavSatFix | None = None self._seen_goal_statuses: dict[bytes, int] = {} - self._nav_status = 'idle' + self._nav_status = WaypointManagerState.STATUS_IDLE self._active_waypoint_id = 0 # 0 = none self._current_nav_goal_handle = None @@ -37,11 +39,19 @@ def __init__(self): self.declare_parameter('nav_mode_topic', '/nav_mode') self.declare_parameter('nav2_status_topic', '/navigate_to_pose/_action/status') self.declare_parameter('navigate_to_gps_action', 'navigate_to_gps') + self.declare_parameter('waypoints_file', os.path.expanduser('~/.ros/waypoints.yaml')) + self.declare_parameter('load_from_file', True) gps_topic = self.get_parameter('gps_topic').value nav_mode_topic = self.get_parameter('nav_mode_topic').value nav2_status_topic = self.get_parameter('nav2_status_topic').value navigate_to_gps_action = self.get_parameter('navigate_to_gps_action').value + self._waypoints_file = os.path.expanduser( + self.get_parameter('waypoints_file').value + ) + + if self.get_parameter('load_from_file').value: + self._load_waypoints() # QoS must match NavSelector (transient_local, reliable, depth=1) nav_mode_qos = QoSProfile( @@ -67,6 +77,7 @@ def __init__(self): ) self.get_logger().info(f'Subscribed to {nav2_status_topic}') + self.create_service(AddWaypoint, '~/add_waypoint', self._add_waypoint_cb) self.create_service(ClearWaypoints, '~/clear_waypoints', self._clear_waypoints_cb) self._nav_action_server = ActionServer( @@ -88,6 +99,53 @@ def __init__(self): self.get_logger().info('WaypointManager ready') + # ── File persistence ────────────────────────────────────────────────────── + + def _load_waypoints(self): + if not os.path.exists(self._waypoints_file): + self.get_logger().info(f'No waypoints file at {self._waypoints_file}, starting empty') + return + try: + with open(self._waypoints_file, 'r') as f: + data = yaml.safe_load(f) + if not data: + return + self._wp_counter = int(data.get('counter', 0)) + for wp_id, d in data.get('waypoints', {}).items(): + wp_id = int(wp_id) + self._waypoints[wp_id] = { + 'id': wp_id, + 'latitude': float(d['latitude']), + 'longitude': float(d['longitude']), + 'timestamp': d.get('timestamp', {'sec': 0, 'nanosec': 0}), + } + self.get_logger().info( + f'Loaded {len(self._waypoints)} waypoint(s) from {self._waypoints_file}' + ) + except Exception as e: + self.get_logger().error(f'Failed to load waypoints file: {e}') + + def _save_waypoints(self): + data = { + 'counter': self._wp_counter, + 'waypoints': { + wp_id: { + 'latitude': d['latitude'], + 'longitude': d['longitude'], + 'timestamp': d['timestamp'], + } + for wp_id, d in self._waypoints.items() + }, + } + tmp = self._waypoints_file + '.tmp' + try: + os.makedirs(os.path.dirname(self._waypoints_file), exist_ok=True) + with open(tmp, 'w') as f: + yaml.dump(data, f) + os.replace(tmp, self._waypoints_file) + except Exception as e: + self.get_logger().error(f'Failed to save waypoints file: {e}') + # ── GPS fix cache ───────────────────────────────────────────────────────── def _gps_cb(self, msg: NavSatFix): @@ -122,13 +180,13 @@ def _store_current_gps(self): 'id': wp_id, 'latitude': gps.latitude, 'longitude': gps.longitude, - 'altitude': gps.altitude, 'timestamp': {'sec': now.sec, 'nanosec': now.nanosec}, } self.get_logger().info( f'Nav2 goal succeeded — stored waypoint {wp_id} at ' f'({gps.latitude:.6f}, {gps.longitude:.6f})' ) + self._save_waypoints() # ── Helpers ─────────────────────────────────────────────────────────────── @@ -137,7 +195,6 @@ def _dict_to_msg(self, d: dict) -> WaypointEntry: msg.id = d['id'] msg.latitude = float(d['latitude']) msg.longitude = float(d['longitude']) - msg.altitude = float(d['altitude']) ts = d.get('timestamp', {}) msg.timestamp.sec = int(ts.get('sec', 0)) msg.timestamp.nanosec = int(ts.get('nanosec', 0)) @@ -153,12 +210,33 @@ def _publish_state(self): msg.last_update = self.get_clock().now().to_msg() self._state_pub.publish(msg) - # ── Service callbacks ───────────────────────────────────────────────────── + # ── Service callbacks ────────────────────────────────────────────────────── + + def _add_waypoint_cb(self, request, response): + now = self.get_clock().now().to_msg() + self._wp_counter += 1 + wp_id = self._wp_counter + self._waypoints[wp_id] = { + 'id': wp_id, + 'latitude': request.latitude, + 'longitude': request.longitude, + 'timestamp': {'sec': now.sec, 'nanosec': now.nanosec}, + } + self._save_waypoints() + self.get_logger().info( + f'Manually added waypoint {wp_id} at ' + f'({request.latitude:.6f}, {request.longitude:.6f})' + ) + response.success = True + response.message = f'Added waypoint {wp_id}' + response.assigned_id = wp_id + return response def _clear_waypoints_cb(self, request, response): count = len(self._waypoints) self._waypoints.clear() self._wp_counter = 0 + self._save_waypoints() response.success = True response.message = f'Cleared {count} waypoint(s)' self.get_logger().info(f'Cleared {count} waypoint(s)') @@ -167,7 +245,7 @@ def _clear_waypoints_cb(self, request, response): # ── Action callbacks ────────────────────────────────────────────────────── def _goal_callback(self, goal_request): - if self._nav_status == 'navigating': + if self._nav_status == WaypointManagerState.STATUS_NAVIGATING: self.get_logger().warn('Goal rejected: navigation already in progress') return GoalResponse.REJECT return GoalResponse.ACCEPT @@ -191,7 +269,7 @@ async def _navigate_execute_cb(self, goal_handle: ServerGoalHandle): wp = self._waypoints[wp_id] self._active_waypoint_id = wp_id - self._nav_status = 'navigating' + self._nav_status = WaypointManagerState.STATUS_NAVIGATING self._nav_mode_pub.publish(String(data='GNSS')) self.get_logger().info( @@ -207,7 +285,7 @@ def _feedback_cb(fb): if not self._gps_client.wait_for_server(timeout_sec=5.0): self.get_logger().error('navigate_to_gps server not available') - self._nav_status = 'failed' + self._nav_status = WaypointManagerState.STATUS_FAILED self._active_waypoint_id = 0 goal_handle.abort() result.success = False @@ -224,7 +302,7 @@ def _feedback_cb(fb): if not nav_goal_handle.accepted: self.get_logger().error('Goal rejected by navigate_to_gps server') - self._nav_status = 'failed' + self._nav_status = WaypointManagerState.STATUS_FAILED self._active_waypoint_id = 0 goal_handle.abort() result.success = False @@ -238,7 +316,7 @@ def _feedback_cb(fb): self._current_nav_goal_handle = None if goal_handle.is_cancel_requested: - self._nav_status = 'idle' + self._nav_status = WaypointManagerState.STATUS_CANCELED self._active_waypoint_id = 0 goal_handle.canceled() result.success = False @@ -246,17 +324,17 @@ def _feedback_cb(fb): return result if nav_result.status == GoalStatus.STATUS_SUCCEEDED: - self._nav_status = 'success' + self._nav_status = WaypointManagerState.STATUS_SUCCEEDED goal_handle.succeed() result.success = True result.message = f'Reached waypoint {wp_id}' elif nav_result.status == GoalStatus.STATUS_CANCELED: - self._nav_status = 'idle' + self._nav_status = WaypointManagerState.STATUS_CANCELED goal_handle.canceled() result.success = False result.message = 'Navigation canceled' else: - self._nav_status = 'failed' + self._nav_status = WaypointManagerState.STATUS_FAILED self.get_logger().warn( f'Navigation to waypoint {wp_id} failed (status={nav_result.status})' )