Generalize nav2 tests and add GPS test. (#338)

* Generalize Nav2 tests.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Create simplified nav2_manager using nav2 commander API.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Add GPS navigation and extend UI.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* WIP

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Fix tests.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Switch to using tf2 listener directly.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Apply feedback.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Try spin_once function with zero timeouts.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Apply feedback.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Apply feedback.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

---------

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

Author: Jelmerdw

conftest.py

@@ -80,3 +80,27 @@ def joint_movement_tolerance() -> float:
         float: The tolerance value for joint movements.
     """
     return 0.01
+
+
+@pytest.fixture(scope="session")
+def navigation_distance_tolerance() -> float:
+    """Distance tolerance of testing navigation.
+
+    This is the maximum allowed deviation for navigation during tests.
+
+    Returns:
+        float: The tolerance value for navigation.
+    """
+    return 0.2
+
+
+@pytest.fixture(scope="session")
+def navigation_degree_tolerance() -> float:
+    """Latitude/Longitude degree tolerance of testing navigation.
+
+    This is the maximum allowed deviation for navigation during tests.
+
+    Returns:
+        float: The tolerance value for navigation.
+    """
+    return 2e-6

ros2_ws/src/rcdt_launch/launch/bringup.launch.py

@@ -4,7 +4,7 @@
 
 from launch import LaunchContext, LaunchDescription
 from launch.actions import OpaqueFunction
-from launch_ros.actions import SetParameter
+from launch_ros.actions import Node, SetParameter
 from rcdt_launch.environment_configuration import EnvironmentConfiguration
 from rcdt_launch.predefined_configurations import PredefinedConfigurations
 from rcdt_launch.rviz import Rviz
@@ -79,8 +79,19 @@ def launch_setup(context: LaunchContext) -> list:
         get_file_path("rcdt_panther", ["launch"], "vizanti.launch.py")
     )
 
+    platforms_dict = {
+        platform.namespace: type(platform).__name__
+        for platform in EnvironmentConfiguration.platforms
+    }
+    rcdt_gui = Node(
+        package="rcdt_utilities",
+        executable="rcdt_gui.py",
+        parameters=[{"platforms": str(platforms_dict)}],
+    )
+    start_rcdt_gui = not set(platforms_dict.values()).isdisjoint(["Arm", "Vehicle"])
     return [
         SetParameter(name="use_sim_time", value=EnvironmentConfiguration.simulation),
+        Register.on_start(rcdt_gui, context) if start_rcdt_gui else launch_utils.SKIP,
         Register.group(rviz, context) if use_rviz else launch_utils.SKIP,
         *[Register.on_start(node, context) for node in state_publishers],
         Register.group(gazebo, context)

ros2_ws/src/rcdt_launch/rcdt_launch/platforms/arm.py

@@ -17,11 +17,17 @@
 
 
 class Arm(Platform):
-    """Extension on Platform with arm specific functionalities."""
+    """Extension on Platform with arm specific functionalities.
+
+    Attributes:
+        SUPPORTED_PLATFORMS: The supported platforms.
+    """
+
+    SUPPORTED_PLATFORMS = Literal["franka"]
 
     def __init__(  # noqa: PLR0913
         self,
-        platform: Literal["franka"],
+        platform: SUPPORTED_PLATFORMS,
         position: list,
         orientation: list | None = None,
         namespace: str | None = None,
@@ -34,7 +40,7 @@ def __init__(  # noqa: PLR0913
         """Initialize the Arm platform.
 
         Args:
-            platform (Literal["franka"]): The platform type.
+            platform (SUPPORTED_PLATFORMS): The platform type.
             position (list): The position of the arm.
             orientation (list | None): The initial orientation of the arm.
             namespace (str | None): The namespace of the arm.

ros2_ws/src/rcdt_launch/rcdt_launch/platforms/camera.py

@@ -18,11 +18,17 @@
 
 
 class Camera(Platform):
-    """Extension on Platform with camera specific functionalities."""
+    """Extension on Platform with camera specific functionalities.
+
+    Attributes:
+        SUPPORTED_PLATFORMS: The supported platforms.
+    """
+
+    SUPPORTED_PLATFORMS = Literal["realsense", "zed"]
 
     def __init__(  # noqa: PLR0913
         self,
-        platform: Literal["realsense", "zed"],
+        platform: SUPPORTED_PLATFORMS,
         position: list,
         orientation: list | None = None,
         namespace: str | None = None,
@@ -32,7 +38,7 @@ def __init__(  # noqa: PLR0913
         """Initialize the Camera platform.
 
         Args:
-            platform (Literal["realsense", "zed"]): The platform type.
+            platform (SUPPORTED_PLATFORMS): The platform type.
             position (list): The position of the camera.
             orientation (list | None): The initial orientation of the camera.
             namespace (str | None): The namespace of the camera.

ros2_ws/src/rcdt_launch/rcdt_launch/platforms/gps.py

@@ -18,11 +18,17 @@
 
 
 class GPS(Platform):
-    """Extension on Platform with GPS specific functionalities."""
+    """Extension on Platform with GPS specific functionalities.
+
+    Attributes:
+        SUPPORTED_PLATFORMS: The supported platforms.
+    """
+
+    SUPPORTED_PLATFORMS = Literal["nmea"]
 
     def __init__(  # noqa: PLR0913
         self,
-        platform: Literal["nmea"],
+        platform: SUPPORTED_PLATFORMS,
         position: list,
         orientation: list | None = None,
         namespace: str | None = None,
@@ -32,7 +38,7 @@ def __init__(  # noqa: PLR0913
         """Initialize the GPS platform.
 
         Args:
-            platform (Literal["nmea"]): The platform type.
+            platform (SUPPORTED_PLATFORMS): The platform type.
             position (list): The position of the platform.
             orientation (list | None): The orientation of the platform.
             namespace (str | None): The namespace of the platform.

ros2_ws/src/rcdt_launch/rcdt_launch/platforms/lidar.py

@@ -18,11 +18,17 @@
 
 
 class Lidar(Platform):
-    """Extension on Platform with lidar specific functionalities."""
+    """Extension on Platform with lidar specific functionalities.
+
+    Attributes:
+        SUPPORTED_PLATFORMS: The supported platforms.
+    """
+
+    SUPPORTED_PLATFORMS = Literal["velodyne", "ouster"]
 
     def __init__(  # noqa: PLR0913
         self,
-        platform: Literal["velodyne", "ouster"],
+        platform: SUPPORTED_PLATFORMS,
         position: list,
         orientation: list | None = None,
         namespace: str | None = None,
@@ -32,7 +38,7 @@ def __init__(  # noqa: PLR0913
         """Initialize the Lidar platform.
 
         Args:
-            platform (Literal["velodyne", "ouster"]): The platform type.
+            platform (SUPPORTED_PLATFORMS): The platform type.
             position (list): The position of the lidar.
             orientation (list | None): The initial orientation of the lidar.
             namespace (str | None): The namespace of the lidar.

ros2_ws/src/rcdt_launch/rcdt_launch/platforms/platform.py

@@ -15,11 +15,17 @@
 
 
 class Platform:  # noqa: PLR0904
-    """A class used to dynamically create all the required nodes for a platform."""
+    """A class used to dynamically create all the required nodes for a platform.
+
+    Attributes:
+        SUPPORTED_PLATFORMS: The supported platforms.
+    """
+
+    SUPPORTED_PLATFORMS = Literal["axis"]
 
     def __init__(  # noqa: PLR0913
         self,
-        platform: Literal["axis"],
+        platform: SUPPORTED_PLATFORMS,
         position: list,
         orientation: list | None = None,
         namespace: str | None = None,
@@ -29,7 +35,7 @@ def __init__(  # noqa: PLR0913
         """Initialize a platform instance.
 
         Args:
-            platform (Literal["axis"]): The platform type of the platform.
+            platform (SUPPORTED_PLATFORMS): The platform type of the platform.
             position (list): The initial position of the platform.
             orientation (list | None): The initial orientation of the platform.
             namespace (str | None): The namespace of the platform. If None, a unique namespace will be generated.

ros2_ws/src/rcdt_launch/rcdt_launch/platforms/vehicle.py

@@ -19,11 +19,17 @@
 
 
 class Vehicle(Platform):
-    """Extension on Platform with vehicle specific functionalities."""
+    """Extension on Platform with vehicle specific functionalities.
+
+    Attributes:
+        SUPPORTED_PLATFORMS: The supported platforms.
+    """
+
+    SUPPORTED_PLATFORMS = Literal["panther"]
 
     def __init__(  # noqa: PLR0913
         self,
-        platform: Literal["panther"],
+        platform: SUPPORTED_PLATFORMS,
         position: list,
         orientation: list | None = None,
         namespace: str | None = None,
@@ -38,7 +44,7 @@ def __init__(  # noqa: PLR0913
         """Initialize the Vehicle platform.
 
         Args:
-            platform (Literal["panther"]): The platform type.
+            platform (SUPPORTED_PLATFORMS): The platform type.
             position (list): The position of the vehicle.
             orientation (list | None): The initial orientation of the vehicle.
             namespace (str | None): The namespace of the vehicle.

ros2_ws/src/rcdt_launch/rcdt_launch/predefined_configurations.py

@@ -110,29 +110,29 @@ def config_franka() -> None:  # noqa: D103
 
 @register_configuration("franka_axis")
 def config_franka_axis() -> None:  # noqa: D103
-    EnvironmentConfiguration.world = "table_with_1_brick.sdf"
-    Rviz.add_markers()
-    Rviz.load_robot_state = True
-    Rviz.load_trajectory = True
-    Rviz.load_planning_scene = True
-    arm = Arm("franka", [0, 0, 0], moveit=True)
-    Camera("realsense", [0.05, 0, 0], [0, -90, 180], parent=arm)
-
-
-@register_configuration("franka_double")
-def config_franka_double() -> None:  # noqa: D103
     franka = Arm("franka", [0, 0, 0], [0, 0, 20])
     Platform("axis", [0, 0, 0.1], [0, 20, 0], parent=franka)
 
 
-@register_configuration("franka_planning")
-def config_franka_planning() -> None:  # noqa: D103
+@register_configuration("franka_double")
+def config_franka_double() -> None:  # noqa: D103
     EnvironmentConfiguration.use_joystick = False
     Rviz.load_motion_planning_plugin = True
     Arm("franka", [1.0, 0, 0], gripper=True, moveit=True)
     Arm("franka", [-1.0, 0, 0], gripper=True, moveit=True)
 
 
+@register_configuration("franka_planning")
+def config_franka_planning() -> None:  # noqa: D103
+    EnvironmentConfiguration.world = "table_with_1_brick.sdf"
+    Rviz.add_markers()
+    Rviz.load_robot_state = True
+    Rviz.load_trajectory = True
+    Rviz.load_planning_scene = True
+    arm = Arm("franka", [0, 0, 0], moveit=True)
+    Camera("realsense", [0.05, 0, 0], [0, -90, 180], parent=arm)
+
+
 @register_configuration("franka_realsense")
 def config_franka_realsense() -> None:  # noqa: D103
     arm = Arm("franka", [0, 0, 0], moveit=True)
@@ -175,6 +175,7 @@ def config_panther_zed() -> None:  # noqa: D103
 
 @register_configuration("panther_velodyne")
 def config_panther_velodyne() -> None:  # noqa: D103
+    EnvironmentConfiguration.use_vizanti = True
     panther = Vehicle("panther", [0, 0, 0.2], navigation=True)
     Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
 
@@ -205,6 +206,17 @@ def config_mm_ouster() -> None:  # noqa: D103
     Lidar("ouster", [0.13, -0.13, 0.35], parent=panther)
 
 
+@register_configuration("mm_gps")
+def config_mm_gps() -> None:  # noqa: D103
+    EnvironmentConfiguration.world = "map_5.940906_51.966960"
+    panther = Vehicle(
+        "panther", [0, 0, 0.2], navigation=True, use_gps=True, window_size=50
+    )
+    Arm("franka", [0, 0, 0.14], gripper=True, parent=panther, moveit=True)
+    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+    GPS("nmea", [0, 0, 0.2], parent=panther)
+
+
 @register_configuration("panther_and_franka")
 def config_panther_and_franka() -> None:  # noqa: D103
     Vehicle("panther", [0, -0.5, 0.2])

ros2_ws/src/rcdt_panther/launch/nav2.launch.py

@@ -308,7 +308,6 @@ def launch_setup(context: LaunchContext) -> list:  # noqa: PLR0915
         respawn=use_respawn,
         respawn_delay=2.0,
         parameters=[bt_navigator_params],
-        remappings=[(f"/{namespace_vehicle}/goal_pose", "/goal_pose")],
         namespace=namespace_vehicle,
     )
 
@@ -335,17 +334,20 @@ def launch_setup(context: LaunchContext) -> list:  # noqa: PLR0915
     remappings = []
     if use_gps:
         remappings.append(("/gps/fix", f"/{namespace_gps}/fix"))
+        remappings.append(("/fromLL", f"/{namespace_gps}/fromLL"))
+
     waypoint_follower = Node(
         package="nav2_waypoint_follower",
         executable="waypoint_follower",
         namespace=namespace_vehicle,
         remappings=remappings,
     )
 
-    waypoint_follower_controller = Node(
+    nav2_manager = Node(
         package="rcdt_panther",
-        executable="waypoint_follower_controller.py",
+        executable="nav2_manager.py",
         namespace=namespace_vehicle,
+        remappings=remappings,
     )
 
     pub_topic = (
@@ -370,7 +372,7 @@ def launch_setup(context: LaunchContext) -> list:  # noqa: PLR0915
         Register.on_log(lifecycle_manager, "Managed nodes are active", context)
         if lifecycle_nodes
         else SKIP,
-        Register.on_log(waypoint_follower_controller, "Controller is ready.", context)
+        Register.on_log(nav2_manager, "Controller is ready.", context)
         if use_navigation
         else SKIP,
     ]