Feat: completed waypoint manager file

navigation/waypoint_manager/CMakeLists.txt

@@ -5,7 +5,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-list(APPEND CMAKE_PREFIX_PATH "/ros2_ws/install")
+#list(APPEND CMAKE_PREFIX_PATH "/ros2_ws/install")
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
@@ -16,9 +16,9 @@ find_package(nav_msgs REQUIRED)
 
 #find vortex_msgs package 
 find_package(vortex_msgs REQUIRED)
-if (NOT vortex_msgs_FOUND)
-  message(FATAL_ERROR "vortex_msgs ppacket not found")
-endif()
+#if (NOT vortex_msgs_FOUND)
+ # message(FATAL_ERROR "vortex_msgs ppacket not found")
+#endif()
 
 ament_python_install_package(scripts)
 
@@ -33,15 +33,18 @@ install(PROGRAMS
 )
 
 if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
+find_package(ament_cmake_pytest REQUIRED)
+set(_pytest_tests
+  test/test_waypoint_manager.py
+)
+foreach(_test_path ${_pytest_tests})
+  get_filename_component(_test_name ${_test_path} NAME_WE)
+  ament_add_pytest_test(${_test_name} ${_test_path}
+    APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
+    TIMEOUT 60
+    WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+  )
+endforeach()
 endif()
 
 ament_package()

navigation/waypoint_manager/launch/waypoint_manager.launch.py

@@ -10,8 +10,7 @@ def generate_launch_description():
         package='waypoint_manager',
         executable='waypoint_manager.py',
         name='waypoint_manager',
-        output='screen',
-        parameters=[os.path.join(get_package_share_directory('waypoint_manager'), 'config/param_waypoint_manager.yaml')]
+        output='screen'
     )
 
     return LaunchDescription([

navigation/waypoint_manager/package.xml

@@ -17,6 +17,7 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

navigation/waypoint_manager/scripts/waypoint_manager.py

@@ -17,27 +17,43 @@ class WaypointManager(Node):
     Subscribes to:
     Publishes to:
     """
-
+    
     def __init__(self):
-        super().__init("WaypointManager")
+        """
+        Initializes WaypointManager class.
+
+        Initializes node, sets up action client, services, and publishers.
+        """
+        super().__init__("WaypointManager")
 
         self.waypoint_list = []
 
         ## Action client
-        self.action_client = ActionClient(LosPathFollowing, 'LosPathFollowing')
-        while not self.action_client.wait_for_service(timeout_sec=1.0):
-            self.get_logger().info('Action server not available, waiting again...')
+        self._action_client = ActionClient(self, LosPathFollowing, 'LosPathFollowing')
+        #while not self._action_client.wait_for_server(timeout_sec=1.0):
+        #   self.get_logger().info('Action server not available, waiting again...')
 
         # Services
+        self.get_logger().info('kommet til services')
         self.add_waypoint_service = self.create_service(Waypoint, 'add_waypoint', self.add_waypoint_to_list)
+        self.get_logger().info('kommet til etter at man har laget add_waypoint_services')
         self.remove_waypoint_service = self.create_service(Waypoint, 'remove_waypoint', self.remove_waypoint_from_list)
 
         # nav_msgs Path to visualize LOS in Rviz
         self.path_pub = self.create_publisher(Path, 'los_path', 10)
         self.path = Path()
         self.path.header.frame_id = 'world'
-        
+
     def add_waypoint_to_list(self, req):
+        """
+        Adds a waypoint to the waypoint list.
+
+        Args:
+            req (Waypoint.Request): Request containing the waypoint to be added.
+
+        Returns:
+            Waypoint.Response: True if waypoint is added successfully.
+        """
         self.waypoint_list.append(req.waypoint)
         self.get_logger().info("add waypoint to waypoint_list")
         newpose = PoseStamped()
@@ -48,6 +64,12 @@ def add_waypoint_to_list(self, req):
         return Waypoint.Response(True)
 
     def remove_waypoint_from_list(self, req):
+        """
+        Removes a waypoint from the waypoint list.
+
+        Args:
+            req (Waypoint.Request): Request containing the waypoint to be removed.
+        """
         self.waypoint_list.remove(req)
         self.get_logger().info("remove waypoint from waypoint_list")
         self.path.poses.reverse()
@@ -56,6 +78,9 @@ def remove_waypoint_from_list(self, req):
         self.path_pub.publish(self.path)
 
     def spin(self):
+        """
+        Spins the node to process added waypoints and send them to the action server.
+        """
         index_waypoint_k = 0
         while rclpy.ok():
             if len(self.waypoint_list) >= 2 and index_waypoint_k < len(self.waypoint_list) - 1:

navigation/waypoint_manager/test/test_waypoint_manager.py

@@ -0,0 +1,37 @@
+import rclpy
+from scripts.waypoint_manager import WaypointManager
+from vortex_msgs.srv import Waypoint
+import time
+
+class TestWaypointManager:
+    def send_request(self, x, y):
+        self.request.waypoint.x = x
+        self.request.waypoint.y = y
+        future = self.client.call_async(self.request)
+        future.add_done_callback(self.callback)
+
+    def callback(self, future):
+        try:
+            response = future.result()
+            if response.success:
+                self.get_logger().info('Waypoint added successfully')
+            else:
+                self.get_logger().warn('Failed to add waypoint')
+        except Exception as e:
+            self.get_logger().error(f'Service call failed: {str(e)}')
+
+    def test_add_waypoint_manager(self):
+        rclpy.init()
+        waypoint_manager = WaypointManager()
+        self.client = self.create_client(Waypoint, 'add_waypoint_to_list')
+        while not self.client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('Service not available, waiting again...')
+        self.request = Waypoint.Request()
+
+        self.client.send_request(x=10, y=5)
+
+        # Add a delay of 1 second (adjust as needed)
+        time.sleep(5)
+
+        assert waypoint_manager.waypoint_list.__contains__(x=10, y=5)
+        rclpy.shutdown()