diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 83a12abd..df315c9a 100644 --- a/src/msgs/CMakeLists.txt +++ b/src/msgs/CMakeLists.txt @@ -20,8 +20,13 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/JointReceiver.msg" "msg/SystemInfo.msg" "msg/Heading.msg" + "msg/WaypointEntry.msg" + "msg/WaypointManagerState.msg" "srv/SetController.srv" + "srv/AddWaypoint.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 new file mode 100644 index 00000000..5ab20aed --- /dev/null +++ b/src/msgs/action/NavigateToWaypoint.action @@ -0,0 +1,7 @@ +uint32 waypoint_id +--- +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..b66ef5e4 --- /dev/null +++ b/src/msgs/msg/WaypointEntry.msg @@ -0,0 +1,4 @@ +uint32 id +float64 latitude +float64 longitude +builtin_interfaces/Time timestamp diff --git a/src/msgs/msg/WaypointManagerState.msg b/src/msgs/msg/WaypointManagerState.msg new file mode 100644 index 00000000..088b5761 --- /dev/null +++ b/src/msgs/msg/WaypointManagerState.msg @@ -0,0 +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 +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 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/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), 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..94e3bba7 --- /dev/null +++ b/src/subsystems/navigation/waypoint_manager/launch/waypoint_manager.launch.py @@ -0,0 +1,31 @@ +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( + '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'), + 'waypoints_file': LaunchConfiguration('waypoints_file'), + 'load_from_file': LaunchConfiguration('load_from_file'), + }], + 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.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/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..390e669d --- /dev/null +++ b/src/subsystems/navigation/waypoint_manager/waypoint_manager/waypoint_manager_node.py @@ -0,0 +1,362 @@ +#!/usr/bin/env python3 + +import os +import yaml +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 + +from std_msgs.msg import String +from sensor_msgs.msg import NavSatFix + +from msgs.msg import WaypointEntry, WaypointManagerState +from msgs.srv import AddWaypoint, ClearWaypoints +from msgs.action import NavigateToGPS, NavigateToWaypoint + + +class WaypointManagerNode(Node): + def __init__(self): + super().__init__('waypoint_manager') + + # 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 = WaypointManagerState.STATUS_IDLE + self._active_waypoint_id = 0 # 0 = none + self._current_nav_goal_handle = None + + self._cb_group = ReentrantCallbackGroup() + + 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') + 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( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + ) + self._nav_mode_pub = self.create_publisher(String, nav_mode_topic, nav_mode_qos) + + self._state_pub = self.create_publisher( + WaypointManagerState, '/waypoint_manager/state', 10 + ) + self.create_timer(0.5, self._publish_state) + + self.create_subscription(NavSatFix, gps_topic, self._gps_cb, 10) + self.get_logger().info(f'Subscribed to {gps_topic}') + + self.create_subscription( + GoalStatusArray, + nav2_status_topic, + self._nav2_status_cb, + 10, + ) + 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( + 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, + ) + + self._gps_client = ActionClient( + self, + NavigateToGPS, + navigate_to_gps_action, + callback_group=self._cb_group, + ) + + 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): + 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): + 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.get_logger().info('Nav2 goal succeeded — storing GPS waypoint') + self._store_current_gps() + self._seen_goal_statuses[uid] = curr + + def _store_current_gps(self): + if self._latest_gps is None: + self.get_logger().warn('No GPS fix available; waypoint not stored') + return + gps = self._latest_gps + self._wp_counter += 1 + wp_id = self._wp_counter + now = self.get_clock().now().to_msg() + self._waypoints[wp_id] = { + 'id': wp_id, + 'latitude': gps.latitude, + 'longitude': gps.longitude, + '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 ─────────────────────────────────────────────────────────────── + + def _dict_to_msg(self, d: dict) -> WaypointEntry: + msg = WaypointEntry() + msg.id = d['id'] + msg.latitude = float(d['latitude']) + msg.longitude = float(d['longitude']) + 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_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() + self._state_pub.publish(msg) + + # ── 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)') + return response + + # ── Action callbacks ────────────────────────────────────────────────────── + + def _goal_callback(self, goal_request): + if self._nav_status == WaypointManagerState.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 — 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): + wp_id = goal_handle.request.waypoint_id + result = NavigateToWaypoint.Result() + + 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 = WaypointManagerState.STATUS_NAVIGATING + self._nav_mode_pub.publish(String(data='GNSS')) + + 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() + feedback.status = 'navigating' + feedback.distance_remaining = fb.feedback.distance_remaining + goal_handle.publish_feedback(feedback) + + 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 = WaypointManagerState.STATUS_FAILED + self._active_waypoint_id = 0 + goal_handle.abort() + result.success = False + result.message = 'navigate_to_gps server not available' + return result + + 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 navigate_to_gps server') + self._nav_status = WaypointManagerState.STATUS_FAILED + self._active_waypoint_id = 0 + goal_handle.abort() + result.success = False + result.message = 'Goal rejected by navigate_to_gps server' + return result + + 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 = WaypointManagerState.STATUS_CANCELED + self._active_waypoint_id = 0 + goal_handle.canceled() + result.success = False + result.message = 'Canceled' + return result + + if nav_result.status == GoalStatus.STATUS_SUCCEEDED: + 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 = WaypointManagerState.STATUS_CANCELED + goal_handle.canceled() + result.success = False + result.message = 'Navigation canceled' + else: + self._nav_status = WaypointManagerState.STATUS_FAILED + self.get_logger().warn( + 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_waypoint_id = 0 + 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()