diff --git a/src/msgs/CMakeLists.txt b/src/msgs/CMakeLists.txt index 7630b6c..2b9df55 100644 --- a/src/msgs/CMakeLists.txt +++ b/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() diff --git a/src/msgs/action/NavigateToGPS.action b/src/msgs/action/NavigateToGPS.action new file mode 100644 index 0000000..7ef36aa --- /dev/null +++ b/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 diff --git a/src/msgs/package.xml b/src/msgs/package.xml index ee6e6be..24f1e0f 100644 --- a/src/msgs/package.xml +++ b/src/msgs/package.xml @@ -13,6 +13,8 @@ std_msgs geometry_msgs sensor_msgs + builtin_interfaces + action_msgs rosidl_default_runtime diff --git a/src/subsystems/navigation/gps_goal/config/gps_goal_params.yaml b/src/subsystems/navigation/gps_goal/config/gps_goal_params.yaml new file mode 100644 index 0000000..719a268 --- /dev/null +++ b/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 diff --git a/src/subsystems/navigation/gps_goal/gps_goal/__init__.py b/src/subsystems/navigation/gps_goal/gps_goal/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/subsystems/navigation/gps_goal/gps_goal/gps_goal_server.py b/src/subsystems/navigation/gps_goal/gps_goal/gps_goal_server.py new file mode 100644 index 0000000..3b44a60 --- /dev/null +++ b/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() diff --git a/src/subsystems/navigation/gps_goal/launch/gps_goal_server.launch.py b/src/subsystems/navigation/gps_goal/launch/gps_goal_server.launch.py new file mode 100644 index 0000000..81c1a5c --- /dev/null +++ b/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, + ]) diff --git a/src/subsystems/navigation/gps_goal/package.xml b/src/subsystems/navigation/gps_goal/package.xml new file mode 100644 index 0000000..10da7b7 --- /dev/null +++ b/src/subsystems/navigation/gps_goal/package.xml @@ -0,0 +1,27 @@ + + + + gps_goal + 0.0.0 + GPS goal navigation action server for Nav2 + ros + MIT + + rclpy + msgs + nav2_msgs + sensor_msgs + geometry_msgs + tf2_ros + tf2_geometry_msgs + action_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/subsystems/navigation/gps_goal/resource/gps_goal b/src/subsystems/navigation/gps_goal/resource/gps_goal new file mode 100644 index 0000000..e69de29 diff --git a/src/subsystems/navigation/gps_goal/setup.cfg b/src/subsystems/navigation/gps_goal/setup.cfg new file mode 100644 index 0000000..d7c30cf --- /dev/null +++ b/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 diff --git a/src/subsystems/navigation/gps_goal/setup.py b/src/subsystems/navigation/gps_goal/setup.py new file mode 100644 index 0000000..264ff97 --- /dev/null +++ b/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', + ], + }, +)