GPS Action Node

src/msgs/CMakeLists.txt

@@ -10,6 +10,8 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(action_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/CANA.msg"
@@ -18,7 +20,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/JointReceiver.msg"
   "msg/SystemInfo.msg"
   "srv/SetController.srv"
-  DEPENDENCIES std_msgs geometry_msgs sensor_msgs
+  "action/NavigateToGPS.action"
+  DEPENDENCIES std_msgs geometry_msgs sensor_msgs builtin_interfaces action_msgs
 )
 
 ament_package()

src/msgs/action/NavigateToGPS.action

@@ -0,0 +1,13 @@
+# Goal: GPS coordinates and optional tolerance
+float64 latitude           # Target latitude in decimal degrees
+float64 longitude          # Target longitude in decimal degrees
+float32 position_tolerance # Position tolerance in meters (0.0 = use Nav2 default)
+---
+# Result: Navigation outcome
+bool success               # Whether the goal was reached successfully
+string message             # Status message
+---
+# Feedback: Navigation progress (relayed from Nav2)
+float32 distance_remaining          # Distance to goal in meters
+builtin_interfaces/Duration estimated_time_remaining  # Estimated time to reach goal
+geometry_msgs/PoseStamped current_pose                # Current robot pose

src/msgs/package.xml

@@ -13,6 +13,8 @@
   <depend>std_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>action_msgs</depend>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
 

src/subsystems/navigation/gps_goal/config/gps_goal_params.yaml

@@ -0,0 +1,7 @@
+gps_goal_server:
+  ros__parameters:
+    origin_lat: 38.42391162772634
+    origin_lon: -110.78490558433397
+    origin_alt: 0.0
+    map_frame: "map"
+    default_position_tolerance: 2.0

src/subsystems/navigation/gps_goal/gps_goal/gps_goal_server.py

@@ -0,0 +1,204 @@
+#!/usr/bin/env python3
+import math
+
+import rclpy
+from rclpy.action import ActionServer, GoalResponse, CancelResponse
+from rclpy.action.server import ServerGoalHandle
+from rclpy.node import Node
+from rclpy.action import ActionClient
+from rclpy.callback_groups import ReentrantCallbackGroup
+from action_msgs.msg import GoalStatus
+
+from msgs.action import NavigateToGPS
+from nav2_msgs.action import NavigateToPose
+from geometry_msgs.msg import PoseStamped, Quaternion
+
+
+class GPSGoalServer(Node):
+    def __init__(self):
+        super().__init__('gps_goal_server')
+
+        self.declare_parameter('origin_lat', 38.42391162772634)
+        self.declare_parameter('origin_lon', -110.78490558433397)
+        self.declare_parameter('origin_alt', 0.0)
+        self.declare_parameter('map_frame', 'map')
+        self.declare_parameter('default_position_tolerance', 2.0)
+
+        self.origin_lat = self.get_parameter('origin_lat').value
+        self.origin_lon = self.get_parameter('origin_lon').value
+        self.origin_alt = self.get_parameter('origin_alt').value
+        self.map_frame = self.get_parameter('map_frame').value
+        self.default_tolerance = self.get_parameter('default_position_tolerance').value
+
+        self.get_logger().info(
+            f'GPS Goal Server initialized with ENU origin: '
+            f'({self.origin_lat:.6f}, {self.origin_lon:.6f}, {self.origin_alt:.2f})'
+        )
+
+        self.callback_group = ReentrantCallbackGroup()
+
+        self._action_server = ActionServer(
+            self,
+            NavigateToGPS,
+            'navigate_to_gps',
+            execute_callback=self.execute_callback,
+            goal_callback=self.goal_callback,
+            cancel_callback=self.cancel_callback,
+            callback_group=self.callback_group
+        )
+
+        self._nav2_client = ActionClient(
+            self,
+            NavigateToPose,
+            'navigate_to_pose',
+            callback_group=self.callback_group
+        )
+
+        self.get_logger().info('GPS Goal Action Server ready')
+
+    def goal_callback(self, goal_request):
+        self.get_logger().info(
+            f'Received GPS goal request: lat={goal_request.latitude:.6f}, '
+            f'lon={goal_request.longitude:.6f}, tolerance={goal_request.position_tolerance:.2f}'
+        )
+        return GoalResponse.ACCEPT
+
+    def cancel_callback(self, goal_handle):
+        self.get_logger().info('Received cancel request')
+        return CancelResponse.ACCEPT
+
+    async def execute_callback(self, goal_handle: ServerGoalHandle):
+        """Execute the GPS navigation goal."""
+        self.get_logger().info('Executing GPS navigation goal...')
+
+        lat = goal_handle.request.latitude
+        lon = goal_handle.request.longitude
+        tolerance = goal_handle.request.position_tolerance
+
+        if tolerance <= 0.0:
+            tolerance = self.default_tolerance
+
+        try:
+            x, y, z = self.gps_to_enu(lat, lon, self.origin_alt)
+            self.get_logger().info(
+                f'Converted GPS ({lat:.6f}, {lon:.6f}) to ENU: ({x:.2f}, {y:.2f}, {z:.2f})'
+            )
+        except Exception as e:
+            self.get_logger().error(f'Failed to convert GPS to ENU: {e}')
+            goal_handle.abort()
+            result = NavigateToGPS.Result()
+            result.success = False
+            result.message = f'GPS conversion failed: {e}'
+            return result
+
+        nav2_goal = NavigateToPose.Goal()
+        nav2_goal.pose = PoseStamped()
+        nav2_goal.pose.header.frame_id = self.map_frame
+        nav2_goal.pose.header.stamp = self.get_clock().now().to_msg()
+        nav2_goal.pose.pose.position.x = x
+        nav2_goal.pose.pose.position.y = y
+        nav2_goal.pose.pose.position.z = 0.0
+
+        nav2_goal.pose.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
+
+        if not self._nav2_client.wait_for_server(timeout_sec=5.0):
+            self.get_logger().error('Nav2 action server not available')
+            goal_handle.abort()
+            result = NavigateToGPS.Result()
+            result.success = False
+            result.message = 'Nav2 action server not available'
+            return result
+
+        self.get_logger().info(
+            f'Sending goal to Nav2: x={x:.2f}, y={y:.2f} in frame {self.map_frame}'
+        )
+
+        nav2_goal_future = self._nav2_client.send_goal_async(
+            nav2_goal,
+            feedback_callback=lambda feedback: self._nav2_feedback_callback(feedback, goal_handle)
+        )
+
+        nav2_goal_handle = await nav2_goal_future
+
+        if not nav2_goal_handle.accepted:
+            self.get_logger().error('Nav2 rejected the goal')
+            goal_handle.abort()
+            result = NavigateToGPS.Result()
+            result.success = False
+            result.message = 'Nav2 rejected the goal'
+            return result
+
+        self.get_logger().info('Nav2 accepted the goal, navigating...')
+
+        nav2_result_future = nav2_goal_handle.get_result_async()
+        nav2_result = await nav2_result_future
+
+        if nav2_result.status == GoalStatus.STATUS_SUCCEEDED:
+            self.get_logger().info('Navigation succeeded!')
+            goal_handle.succeed()
+            result = NavigateToGPS.Result()
+            result.success = True
+            result.message = 'Successfully reached GPS goal'
+        elif nav2_result.status == GoalStatus.STATUS_ABORTED:
+            self.get_logger().warn('Navigation aborted')
+            goal_handle.abort()
+            result = NavigateToGPS.Result()
+            result.success = False
+            result.message = 'Navigation aborted by Nav2'
+        elif nav2_result.status == GoalStatus.STATUS_CANCELED:
+            self.get_logger().info('Navigation canceled')
+            goal_handle.canceled()
+            result = NavigateToGPS.Result()
+            result.success = False
+            result.message = 'Navigation canceled'
+        else:
+            self.get_logger().error(f'Navigation failed with status: {nav2_result.status}')
+            goal_handle.abort()
+            result = NavigateToGPS.Result()
+            result.success = False
+            result.message = f'Navigation failed with status {nav2_result.status}'
+
+        return result
+
+    def _nav2_feedback_callback(self, nav2_feedback, gps_goal_handle):
+        feedback = NavigateToGPS.Feedback()
+
+        feedback.distance_remaining = nav2_feedback.feedback.distance_remaining
+        feedback.estimated_time_remaining = nav2_feedback.feedback.estimated_time_remaining
+        feedback.current_pose = nav2_feedback.feedback.current_pose
+
+        gps_goal_handle.publish_feedback(feedback)
+
+    def gps_to_enu(self, lat: float, lon: float, alt: float) -> tuple:
+        lat_rad = math.radians(lat)
+        lon_rad = math.radians(lon)
+        origin_lat_rad = math.radians(self.origin_lat)
+        origin_lon_rad = math.radians(self.origin_lon)
+
+        R = 6378137.0
+
+        lat_center = (lat_rad + origin_lat_rad) / 2.0
+
+        east = R * (lon_rad - origin_lon_rad) * math.cos(lat_center)
+        north = R * (lat_rad - origin_lat_rad)
+        up = alt - self.origin_alt
+
+        return (east, north, up)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    gps_goal_server = GPSGoalServer()
+
+    try:
+        rclpy.spin(gps_goal_server)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        gps_goal_server.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/subsystems/navigation/gps_goal/launch/gps_goal_server.launch.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+"""Launch file for GPS Goal Action Server."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    """Generate launch description for GPS goal server."""
+
+    use_sim_time_arg = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation (Gazebo) clock if true'
+    )
+
+    config_file_arg = DeclareLaunchArgument(
+        'config_file',
+        default_value=PathJoinSubstitution([
+            FindPackageShare('gps_goal'),
+            'config',
+            'gps_goal_params.yaml'
+        ]),
+        description='Path to GPS goal server configuration file'
+    )
+
+    use_sim_time = LaunchConfiguration('use_sim_time')
+    config_file = LaunchConfiguration('config_file')
+
+    gps_goal_server_node = Node(
+        package='gps_goal',
+        executable='gps_goal_server',
+        name='gps_goal_server',
+        output='screen',
+        parameters=[
+            config_file,
+            {'use_sim_time': use_sim_time}
+        ],
+        respawn=True,
+        respawn_delay=2.0,
+    )
+
+    return LaunchDescription([
+        use_sim_time_arg,
+        config_file_arg,
+        gps_goal_server_node,
+    ])

src/subsystems/navigation/gps_goal/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gps_goal</name>
+  <version>0.0.0</version>
+  <description>GPS goal navigation action server for Nav2</description>
+  <maintainer email="mdurrani808@gmail.com">ros</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>msgs</depend>
+  <depend>nav2_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>action_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/subsystems/navigation/gps_goal/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/gps_goal
+[install]
+install_scripts=$base/lib/gps_goal

src/subsystems/navigation/gps_goal/setup.py

@@ -0,0 +1,30 @@
+from setuptools import find_packages, setup
+from glob import glob
+import os
+
+package_name = 'gps_goal'
+
+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')),
+        (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='ros',
+    maintainer_email='mdurrani808@gmail.com',
+    description='GPS goal navigation action server for Nav2',
+    license='MIT',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'gps_goal_server = gps_goal.gps_goal_server:main',
+        ],
+    },
+)