Feature/waypoints nav

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
 )
 

src/msgs/action/NavigateToWaypoint.action

@@ -0,0 +1,7 @@
+uint32 waypoint_id
+---
+bool success
+string message
+---
+string status
+float32 distance_remaining

src/msgs/msg/WaypointEntry.msg

@@ -0,0 +1,4 @@
+uint32 id
+float64 latitude
+float64 longitude
+builtin_interfaces/Time timestamp

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

src/msgs/srv/AddWaypoint.srv

@@ -0,0 +1,6 @@
+float64 latitude
+float64 longitude
+---
+bool success
+string message
+uint32 assigned_id

src/msgs/srv/ClearWaypoints.srv

@@ -0,0 +1,3 @@
+---
+bool success
+string message

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),

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',
+        ),
+    ])

src/subsystems/navigation/waypoint_manager/package.xml

@@ -0,0 +1,28 @@
+<?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>waypoint_manager</name>
+  <version>0.0.0</version>
+  <description>Waypoint management and navigation node for simulation</description>
+  <maintainer email="abhinav.kota06@gmail.com">abhinavkota06</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>msgs</depend>
+  <depend>nav2_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>action_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>ament_index_python</depend>
+  <depend>python3-yaml</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/waypoint_manager/setup.cfg

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

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',
+        ],
+    },
+)