375 changes to nav2 planner and controller if required

.gitignore

@@ -28,3 +28,4 @@ clangd/
 # generated compose files
 compose.yml
 compose_pytest.yml
+rviz.yml

alliander_core/src/alliander_utilities/alliander_utilities/adapted_yaml.py

@@ -34,12 +34,12 @@ def get_ros_params(source_file: str) -> tuple[list[str], dict]:
         Args:
             source_file (str): Path to the YAML file.
 
-        Raises:
-            KeyError: If no or multiple 'ros__parameters' keys are found.
-
         Returns:
             tuple[list[str], dict]: A tuple containing the namespace as a list of strings and the
             'ros__parameters' dictionary.
+
+        Raises:
+            KeyError: If no or multiple 'ros__parameters' keys are found.
         """
         params = get_yaml(source_file)
         sub_dicts: list[tuple[list[str], dict]] = [([], params)]

alliander_core/src/alliander_utilities/alliander_utilities/launch_argument.py

@@ -61,11 +61,12 @@ def bool_value(self, context: LaunchContext) -> bool:
         Args:
             context (LaunchContext): The launch context in which to evaluate the argument.
 
+        Returns:
+            bool: The boolean value of the launch argument.
+
         Raises:
             TypeError: If the string value cannot be interpreted as a boolean.
 
-        Returns:
-            bool: The boolean value of the launch argument.
         """
         string_value = self.string_value(context)
         if string_value in {"True", "true"}:
@@ -93,11 +94,12 @@ def float_value(self, context: LaunchContext) -> float:
         Args:
             context (LaunchContext): The launch context in which to evaluate the argument.
 
+        Returns:
+            float: The float value of the launch argument.
+
         Raises:
             RuntimeError: If the float value is outside the specified min or max range.
 
-        Returns:
-            float: The float value of the launch argument.
         """
         string_value = self.string_value(context)
         float_value = float(string_value)

alliander_gazebo/src/alliander_gazebo/launch/gazebo.launch.py

@@ -26,11 +26,12 @@ def get_sdf_file(world: str) -> str:
     Args:
         world (str): The world argument.
 
+    Returns:
+        str: The path to the sdf file.
+
     Raises:
         ValueError: If the SDF file cannot be generated.
 
-    Returns:
-        str: The path to the sdf file.
     """
     if world.startswith("map"):
         try:
@@ -92,11 +93,12 @@ def launch_setup(context: LaunchContext) -> list:
     Args:
         context (LaunchContext): The launch context.
 
+    Returns:
+        list: The actions to start.
+
     Raises:
         ValueError: If the SDF file does not contain a world attribute with a name.
 
-    Returns:
-        list: The actions to start.
     """
     config = SimulatorConfig.from_str(config_arg.string_value(context))
     platforms = PlatformList.from_str(platform_list_arg.string_value(context))

alliander_gazebo/src/alliander_gazebo/worlds/world.sdf

@@ -7,7 +7,7 @@ SPDX-License-Identifier: Apache-2.0
 <sdf version="1.6">
   <world name="generated_world">
     <physics name="1ms" type="ignored">
-      <max_step_size>0.001</max_step_size>
+      <max_step_size>0.004</max_step_size>
       <real_time_factor>1.0</real_time_factor>
     </physics>
 
@@ -65,4 +65,4 @@ SPDX-License-Identifier: Apache-2.0
     </spherical_coordinates>
 
   </world>
-</sdf>
+</sdf>

alliander_gps/alliander_gps.Dockerfile

@@ -25,7 +25,7 @@ RUN /$WORKDIR/colcon_build.sh
 # Install python dependencies:
 WORKDIR $WORKDIR
 COPY pyproject.toml/ /$WORKDIR/pyproject.toml
-RUN uv sync \
+RUN uv sync --group alliander-gps \
   && echo "export PYTHONPATH=\"$(dirname $(dirname $(uv python find)))/lib/python3.12/site-packages:\$PYTHONPATH\"" >> /root/.bashrc \
   && echo "export PATH=\"$(dirname $(dirname $(uv python find)))/bin:\$PATH\"" >> /root/.bashrc
 

alliander_gps/src/alliander_gps/CMakeLists.txt

@@ -11,7 +11,7 @@ find_package(ament_cmake_python REQUIRED)
 
 # Shared folders:
 install(
-  DIRECTORY launch
+  DIRECTORY config launch
   DESTINATION share/${PROJECT_NAME}
 )
 

alliander_gps/src/alliander_gps/config/ekf_global.yaml

@@ -0,0 +1,101 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+# Based on: https://github.com/husarion/husarion_ugv_ros/blob/ros2/husarion_ugv_localization/config/enu_localization_with_gps.yaml
+ekf_global:
+  ros__parameters:
+    frequency: 20.0
+    sensor_timeout: 0.05
+    two_d_mode: true
+
+    transform_time_offset: 0.0
+    transform_timeout: 0.05
+
+    world_frame: map
+    map_frame: map
+    odom_frame: substitute_me
+    base_link_frame: substitute_me
+    publish_tf: true
+    publish_acceleration: false
+
+    odom0: substitute_me
+    odom0_config: [false, false, false,
+                   false, false, false,
+                    true,  true, false,
+                   false, false,  true,
+                   false, false, false]
+    odom0_queue_size: 3
+    odom0_nodelay: true
+    odom0_differential: false
+    odom0_relative: true
+
+    odom1: substitute_me 
+    odom1_config: [true,  true, false,
+                   false, false, false,
+                   false, false, false,
+                   false, false, false,
+                   false, false, false]
+    odom1_queue_size: 2
+    odom1_differential: false
+    odom1_relative: false
+
+    imu0: substitute_me
+    imu0_config: [false, false, false,
+                  false, false,  true,
+                  false, false, false,
+                  false, false,  true,
+                  false, false, false]
+    imu0_queue_size: 3
+    imu0_nodelay: true
+    imu0_differential: false
+    imu0_relative: false
+    imu0_remove_gravitational_acceleration: false
+
+    use_control: true
+    stamped_control: true
+    control_timeout: 0.5
+    control_config: [true, true, false, false, false, true]
+    acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config
+
+    predict_to_current_time: true
+
+    # Selected values ​​experimentally so as to ensure relatively fast convergence (values ​​should be about 10x higher than the sensor variance values)
+    dynamic_process_noise_covariance: true
+    process_noise_covariance: [5e-3,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0, 5e-3,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0, 5e-3,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0, 3e-5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0, 3e-5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0, 3e-5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 2e-5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 2e-5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 2e-5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 2e-4,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 2e-4,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 2e-4,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 5e-5,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 5e-5,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 5e-5]
+
+    initial_state: [0.0,  0.0,  0.0,
+                    0.0,  0.0,  0.0,
+                    0.0,  0.0,  0.0,
+                    0.0,  0.0,  0.0,
+                    0.0,  0.0,  0.0]
+
+    initial_estimate_covariance: [1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  1e5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9]

alliander_gps/src/alliander_gps/config/navsat_transform.yaml

@@ -0,0 +1,16 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+# Based on: https://github.com/husarion/husarion_ugv_ros/blob/ros2/husarion_ugv_localization/config/enu_localization_with_gps.yaml
+navsat_transform:
+  navsat_transform:
+    ros__parameters:
+      delay: 3.0
+      magnetic_declination_radians: 0.0
+      yaw_offset: 0.0
+      zero_altitude: true
+      broadcast_cartesian_transform: false
+      publish_filtered_gps: true
+      use_odometry_yaw: true
+      wait_for_datum: false

alliander_gps/src/alliander_gps/launch/gps.launch.py

@@ -1,8 +1,7 @@
 # SPDX-FileCopyrightText: Alliander N. V.
 #
 # SPDX-License-Identifier: Apache-2.0
-import itertools
-
+from alliander_utilities.adapted_yaml import AdaptedYaml
 from alliander_utilities.config_objects import GPS
 from alliander_utilities.launch_argument import LaunchArgument
 from alliander_utilities.launch_utils import SKIP, state_publisher_node, static_tf_node
@@ -51,20 +50,34 @@ def launch_setup(context: LaunchContext) -> list:
         {"platform_config": gps_config.to_str()},
     )
 
+    ekf_global_params = AdaptedYaml(
+        get_file_path("alliander_gps", ["config"], "ekf_global.yaml"),
+        {
+            "odom_frame": f"{gps_config.parent.namespace}/odom",
+            "base_link_frame": f"{gps_config.parent.namespace}/base_footprint",
+            "odom0": f"/{gps_config.parent.namespace}/odometry/wheels",
+            "odom1": f"/{gps_config.namespace}/odometry/gps",
+            "imu0": f"/{gps_config.parent.namespace}/imu/data",
+        },
+        root_key=gps_config.parent.namespace,
+    )
+    navsat_transform_params = AdaptedYaml(
+        get_file_path("alliander_gps", ["config"], "navsat_transform.yaml"),
+        {},
+        root_key=gps_config.parent.namespace,
+    )
+
     navsat_transform = Node(
         package="robot_localization",
         executable="navsat_transform_node",
+        name="navsat_transform",
         namespace=gps_config.namespace,
-        parameters=[
-            {
-                "publish_filtered_gps": True,
-            }
-        ],
+        parameters=[navsat_transform_params.file],
         remappings=[
             ("imu", f"/{gps_config.parent.namespace}/imu/data"),
             (
                 "odometry/filtered",
-                f"/{gps_config.parent.namespace}/odometry/filtered",
+                f"/{gps_config.parent.namespace}/odometry/global",
             ),
         ],
     )
@@ -75,51 +88,9 @@ def launch_setup(context: LaunchContext) -> list:
         executable="ekf_node",
         name="ekf_global",
         namespace=gps_config.parent.namespace,
-        parameters=[
-            {
-                "two_d_mode": True,
-                "publish_tf": True,
-                "world_frame": "map",
-                "map_frame": "map",
-                "odom_frame": f"{gps_config.parent.namespace}/odom",
-                "base_link_frame": f"{gps_config.parent.namespace}/base_footprint",
-                "odom0": f"/{gps_config.parent.namespace}/odometry/wheels",
-                "odom0_config": list(
-                    itertools.chain.from_iterable(
-                        [
-                            [F, F, F],  # [x_pos, y_pos, z_pos]
-                            [F, F, F],  # [roll, pitch, yaw]
-                            [T, T, T],  # [x_vel, y_vel, z_vel]
-                            [F, F, T],  # [roll_rate, pitch_rate, yaw_rate]
-                            [F, F, F],  # [x_accel, y_accel, z_accel]
-                        ]
-                    )
-                ),
-                "odom1": f"/{gps_config.namespace}/odometry/gps",
-                "odom1_config": list(
-                    itertools.chain.from_iterable(
-                        [
-                            [T, T, F],  # [x_pos, y_pos, z_pos]
-                            [F, F, F],  # [roll, pitch, yaw]
-                            [F, F, F],  # [x_vel, y_vel, z_vel]
-                            [F, F, F],  # [roll_rate, pitch_rate, yaw_rate]
-                            [F, F, F],  # [x_accel, y_accel, z_accel]
-                        ]
-                    )
-                ),
-                "imu0": f"/{gps_config.parent.namespace}/imu/data",
-                "imu0_config": list(
-                    itertools.chain.from_iterable(
-                        [
-                            [F, F, F],  # [x_pos, y_pos, z_pos]
-                            [F, F, T],  # [roll, pitch, yaw]
-                            [F, F, F],  # [x_vel, y_vel, z_vel]
-                            [F, F, F],  # [roll_rate, pitch_rate, yaw_rate]
-                            [F, F, F],  # [x_accel, y_accel, z_accel]
-                        ]
-                    )
-                ),
-            }
+        parameters=[ekf_global_params.file],
+        remappings=[
+            ("odometry/filtered", f"/{gps_config.parent.namespace}/odometry/global"),
         ],
     )
 

alliander_gps/src/alliander_gps/launch/hardware.launch.py

@@ -32,8 +32,8 @@ def launch_setup(context: LaunchContext) -> list:
             {
                 "ip": gps_config.ip_address,
                 "port": 5000,
-                "frame_id": "gps",
-                "tf_prefix": gps_config.parent.namespace,
+                "frame_id": "base_link",
+                "tf_prefix": gps_config.namespace,
             },
         ],
         remappings=[

alliander_nav2/src/alliander_nav2/config/nav2/controller_server.yaml

@@ -7,7 +7,6 @@
 controller_server:
   ros__parameters:
     enable_stamped_cmd_vel: true
-    progress_checker_plugins: ["progress_checker"] # progress_checker_plugin: "progress_checker" For Humble and older
-    progress_checker:
-      plugin: "nav2_controller::SimpleProgressChecker"
-      movement_time_allowance: 40.0
+    PathHandler:
+      prune_distance: 5.5  # at least as large as furthest distance of interest by critic
+