diff --git a/.gitignore b/.gitignore index 992504cf..aebb5ba7 100644 --- a/.gitignore +++ b/.gitignore @@ -28,3 +28,4 @@ clangd/ # generated compose files compose.yml compose_pytest.yml +rviz.yml diff --git a/alliander_core/src/alliander_utilities/alliander_utilities/adapted_yaml.py b/alliander_core/src/alliander_utilities/alliander_utilities/adapted_yaml.py index fd258fd9..57fc5725 100644 --- a/alliander_core/src/alliander_utilities/alliander_utilities/adapted_yaml.py +++ b/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)] diff --git a/alliander_core/src/alliander_utilities/alliander_utilities/launch_argument.py b/alliander_core/src/alliander_utilities/alliander_utilities/launch_argument.py index 670810fc..a47fd663 100644 --- a/alliander_core/src/alliander_utilities/alliander_utilities/launch_argument.py +++ b/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) diff --git a/alliander_gazebo/src/alliander_gazebo/launch/gazebo.launch.py b/alliander_gazebo/src/alliander_gazebo/launch/gazebo.launch.py index 12fd5abb..186a876b 100644 --- a/alliander_gazebo/src/alliander_gazebo/launch/gazebo.launch.py +++ b/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)) diff --git a/alliander_gazebo/src/alliander_gazebo/worlds/world.sdf b/alliander_gazebo/src/alliander_gazebo/worlds/world.sdf index d13d1212..16c8b18a 100644 --- a/alliander_gazebo/src/alliander_gazebo/worlds/world.sdf +++ b/alliander_gazebo/src/alliander_gazebo/worlds/world.sdf @@ -7,7 +7,7 @@ SPDX-License-Identifier: Apache-2.0 - 0.001 + 0.004 1.0 @@ -65,4 +65,4 @@ SPDX-License-Identifier: Apache-2.0 - \ No newline at end of file + diff --git a/alliander_gps/alliander_gps.Dockerfile b/alliander_gps/alliander_gps.Dockerfile index dae50858..dfbba64d 100644 --- a/alliander_gps/alliander_gps.Dockerfile +++ b/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 diff --git a/alliander_gps/src/alliander_gps/CMakeLists.txt b/alliander_gps/src/alliander_gps/CMakeLists.txt index ab6492d5..142cbf15 100644 --- a/alliander_gps/src/alliander_gps/CMakeLists.txt +++ b/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} ) diff --git a/alliander_gps/src/alliander_gps/config/ekf_global.yaml b/alliander_gps/src/alliander_gps/config/ekf_global.yaml new file mode 100644 index 00000000..4ef9cb6e --- /dev/null +++ b/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] diff --git a/alliander_gps/src/alliander_gps/config/navsat_transform.yaml b/alliander_gps/src/alliander_gps/config/navsat_transform.yaml new file mode 100644 index 00000000..6a0814b1 --- /dev/null +++ b/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 diff --git a/alliander_gps/src/alliander_gps/launch/gps.launch.py b/alliander_gps/src/alliander_gps/launch/gps.launch.py index e0e5ce3c..552c5a74 100644 --- a/alliander_gps/src/alliander_gps/launch/gps.launch.py +++ b/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"), ], ) diff --git a/alliander_gps/src/alliander_gps/launch/hardware.launch.py b/alliander_gps/src/alliander_gps/launch/hardware.launch.py index e8d8892e..04a5ceb9 100644 --- a/alliander_gps/src/alliander_gps/launch/hardware.launch.py +++ b/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=[ diff --git a/alliander_nav2/src/alliander_nav2/config/nav2/controller_server.yaml b/alliander_nav2/src/alliander_nav2/config/nav2/controller_server.yaml index 5fc7ed8c..1f5a54b9 100644 --- a/alliander_nav2/src/alliander_nav2/config/nav2/controller_server.yaml +++ b/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 + diff --git a/alliander_nav2/src/alliander_nav2/config/nav2/controllers/mppi.yaml b/alliander_nav2/src/alliander_nav2/config/nav2/controllers/mppi.yaml index c5e5f2c8..1ec0e66e 100644 --- a/alliander_nav2/src/alliander_nav2/config/nav2/controllers/mppi.yaml +++ b/alliander_nav2/src/alliander_nav2/config/nav2/controllers/mppi.yaml @@ -8,28 +8,23 @@ controller_server: ros__parameters: FollowPath: plugin: "nav2_mppi_controller::MPPIController" + motion_model: "DiffDrive" + visualize: true # computationally expensive, set to false on robot time_steps: 56 - model_dt: 0.05 + model_dt: 0.05 # set this to 1 / controller_frequency batch_size: 2000 - vx_std: 0.2 - vy_std: 0.2 - wz_std: 0.4 - vx_max: 0.5 - vx_min: -0.35 - vy_max: 0.5 - wz_max: 1.9 - ax_max: 3.0 - ax_min: -3.0 - ay_min: -3.0 - ay_max: 3.0 - az_max: 3.5 + vx_std: 0.35 + wz_std: 0.6 + vx_max: 2.0 + vx_min: -1.0 + wz_max: 3.14 + ax_max: 10.0 + ax_min: -10.0 + az_max: 10.0 iteration_count: 1 - prune_distance: 1.7 transform_tolerance: 0.1 temperature: 0.3 gamma: 0.015 - motion_model: "DiffDrive" - visualize: false reset_period: 1.0 # (only in Humble) regenerate_noises: false TrajectoryVisualizer: @@ -39,8 +34,6 @@ controller_server: plugin: "mppi::DefaultOptimalTrajectoryValidator" collision_lookahead_time: 2.0 consider_footprint: false - AckermannConstraints: - min_turning_r: 0.2 critics: [ "ConstraintCritic", @@ -74,14 +67,12 @@ controller_server: ObstaclesCritic: enabled: true cost_power: 1 - repulsion_weight: 1.5 + repulsion_weight: 1.5 # set to inflation_radius - min_dist_to_obstacle critical_weight: 20.0 consider_footprint: false collision_cost: 10000.0 collision_margin_distance: 0.1 near_goal_distance: 0.5 - inflation_radius: 0.55 # (only in Humble) - cost_scaling_factor: 10.0 # (only in Humble) CostCritic: enabled: true cost_power: 1 @@ -94,7 +85,7 @@ controller_server: PathAlignCritic: enabled: true cost_power: 1 - cost_weight: 14.0 + cost_weight: 5.0 max_path_occupancy_ratio: 0.05 trajectory_point_step: 4 threshold_to_consider: 0.5 diff --git a/alliander_nav2/src/alliander_nav2/config/nav2/global_costmap.yaml b/alliander_nav2/src/alliander_nav2/config/nav2/global_costmap.yaml index 01a98003..451ab5ac 100644 --- a/alliander_nav2/src/alliander_nav2/config/nav2/global_costmap.yaml +++ b/alliander_nav2/src/alliander_nav2/config/nav2/global_costmap.yaml @@ -7,12 +7,12 @@ global_costmap: global_costmap: ros__parameters: - always_send_full_costmap: true rolling_window: substitute_me width: substitute_me height: substitute_me - update_frequency: 30.0 - publish_frequency: 30.0 + update_frequency: 20.0 + publish_frequency: 20.0 + always_send_full_costmap: true robot_base_frame: substitute_me footprint: "[[0.4045, 0.424], [0.4045, -0.424], [-0.4045, -0.424], [-0.4045, 0.424]]" plugins: ["static_layer", "obstacle_layer", "inflation_layer"] @@ -39,4 +39,5 @@ global_costmap: inf_is_valid: false inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - inflation_radius: 1.0 + inflation_radius: 1.5 + cost_scaling_factor: 2.0 diff --git a/alliander_nav2/src/alliander_nav2/config/nav2/local_costmap.yaml b/alliander_nav2/src/alliander_nav2/config/nav2/local_costmap.yaml index 39d63227..66507fce 100644 --- a/alliander_nav2/src/alliander_nav2/config/nav2/local_costmap.yaml +++ b/alliander_nav2/src/alliander_nav2/config/nav2/local_costmap.yaml @@ -10,10 +10,32 @@ local_costmap: global_frame: substitute_me robot_base_frame: substitute_me rolling_window: substitute_me - plugins: ["static_layer", "obstacle_layer", "inflation_layer"] - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" + update_frequency: 10.0 + publish_frequency: 5.0 + width: substitute_me + height: substitute_me + footprint: "[[0.4045, 0.424], [0.4045, -0.424], [-0.4045, -0.424], [-0.4045, 0.424]]" + plugins: ["obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + footprint_clearing_enabled: true + max_obstacle_height: 2.0 + combination_method: 1 + scan: + topic: substitute_me + obstacle_max_range: substitute_me + obstacle_min_range: 0.0 + raytrace_max_range: substitute_me + raytrace_min_range: 0.0 + max_obstacle_height: 2.0 + min_obstacle_height: 0.0 + clearing: True + marking: True + data_type: "LaserScan" + inf_is_valid: false inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" + inflation_radius: substitute_me + cost_scaling_factor: 1.0 diff --git a/alliander_nav2/src/alliander_nav2/config/nav2/planner_server.yaml b/alliander_nav2/src/alliander_nav2/config/nav2/planner_server.yaml index 684aa652..cbe41b1f 100644 --- a/alliander_nav2/src/alliander_nav2/config/nav2/planner_server.yaml +++ b/alliander_nav2/src/alliander_nav2/config/nav2/planner_server.yaml @@ -7,7 +7,7 @@ planner_server: ros__parameters: planner_plugins: ["GridBased"] + allow_partial_planning: True GridBased: - plugin: "nav2_smac_planner::SmacPlannerHybrid" - minimum_turning_radius: 0.0 + plugin: "nav2_smac_planner::SmacPlanner2D" diff --git a/alliander_nav2/src/alliander_nav2/launch/nav2.launch.py b/alliander_nav2/src/alliander_nav2/launch/nav2.launch.py index 1696cba4..801b94d9 100644 --- a/alliander_nav2/src/alliander_nav2/launch/nav2.launch.py +++ b/alliander_nav2/src/alliander_nav2/launch/nav2.launch.py @@ -101,7 +101,19 @@ def launch_setup(context: LaunchContext) -> list: # noqa: PLR0915 "global_frame": f"{namespace_vehicle}/odom", "robot_base_frame": f"{namespace_vehicle}/base_footprint", "rolling_window": nav2.gps, + "width": 10, + "height": 10, "plugins": plugins, + "obstacle_layer": { + "scan": { + "topic": f"/{namespace_lidar}/scan", + "obstacle_max_range": 10.0, + "raytrace_max_range": 10.0, + } + }, + "inflation_layer": { + "inflation_radius": float(5), # width / 2 + }, }, root_key=namespace_vehicle, ) @@ -127,7 +139,7 @@ def launch_setup(context: LaunchContext) -> list: # noqa: PLR0915 controller_server_params = AdaptedYaml( get_file_path("alliander_nav2", ["config", "nav2"], "controller_server.yaml"), - {"odom_topic": f"/{namespace_vehicle}/odom"}, + {"odom_topic": f"/{namespace_vehicle}/odometry/filtered"}, root_key=namespace_vehicle, ) @@ -157,7 +169,7 @@ def launch_setup(context: LaunchContext) -> list: # noqa: PLR0915 "alliander_nav2", ["config", "nav2"], "behavior_tree.xml" ), "robot_base_frame": f"{namespace_vehicle}/base_footprint", - "odom_topic": f"/{namespace_vehicle}/odom", + "odom_topic": f"/{namespace_vehicle}/odometry/filtered", }, root_key=namespace_vehicle, ) diff --git a/alliander_nav2/src/alliander_nav2/test/test_gps_waypoint_follower.py b/alliander_nav2/src/alliander_nav2/test/test_gps_waypoint_follower.py new file mode 100755 index 00000000..0b2ead4e --- /dev/null +++ b/alliander_nav2/src/alliander_nav2/test/test_gps_waypoint_follower.py @@ -0,0 +1,186 @@ +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 +import math +from dataclasses import dataclass +from enum import Enum, auto + +import rclpy +from geographic_msgs.msg import GeoPose +from geometry_msgs.msg import Quaternion +from nav2_msgs.action import FollowGPSWaypoints +from nav2_msgs.action._follow_gps_waypoints import FollowGPSWaypoints_FeedbackMessage +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.task import Future + + +@dataclass(frozen=True) +class GPSWaypoint: + """Dataclass containing lat/lon and optional yaw orientation. + + Attributes: + latitude (float): latitude of GPS waypoint. + longitude (float): longitude of GPS waypoint. + yaw (float): yaw of GPS waypoint. + """ + + latitude: float + longitude: float + yaw: float = 0.0 + + +class Route(Enum): + """Enum containing predefined routes for GPS Waypoint Follower. + + Attributes: + KB_TRUCK_PARKING_SPACE: Route nearby Roboticalab entrance. + SIM_TIGHT_ALLEYS: Simulation route in Arnhem, with buildings close together. + """ + + KB_TRUCK_PARKING_SPACE = auto() + SIM_TIGHT_ALLEYS = auto() + + +ROUTES: dict[Route, list[GPSWaypoint]] = { + Route.KB_TRUCK_PARKING_SPACE: [ + GPSWaypoint(51.966663, 5.940867), + GPSWaypoint(51.966511, 5.940912), + GPSWaypoint(51.966512, 5.940945), + GPSWaypoint(51.966661, 5.940892, yaw=math.pi / 4), + ], + Route.SIM_TIGHT_ALLEYS: [ + GPSWaypoint(51.977291, 5.954022, yaw=-math.pi), + GPSWaypoint(51.977251, 5.954025, yaw=-math.pi / 4), + GPSWaypoint(51.977213, 5.954037, yaw=-3 * math.pi / 4), + GPSWaypoint(51.977203, 5.954130, yaw=math.pi / 6), + GPSWaypoint(51.977226, 5.954096, yaw=-5 * math.pi / 6), + GPSWaypoint(51.977172, 5.954103, yaw=-3 * math.pi / 4), + GPSWaypoint(51.977172, 5.954198, yaw=3 * math.pi / 4), + GPSWaypoint(51.977209, 5.954075, yaw=7 * math.pi / 4), + GPSWaypoint(51.977229, 5.953975, yaw=math.pi / 2), + ], +} + + +class GPSWaypointFollower(Node): + """Class that sends predefined routes to nav2's GPS waypoint follower action server.""" + + def __init__(self): + """Sets up the action client.""" + super().__init__("test_gps_waypoints") + self.ac = ActionClient( + self, FollowGPSWaypoints, "/panther/follow_gps_waypoints" + ) + self.current_wp = -1 + self.done = False + + def send_goal(self, route: Route) -> None: + """Sends a Route to the nav2 action server. + + Args: + route (Route): predefined set of waypoints to send. + """ + self.ac.wait_for_server() + + waypoints = ROUTES[route] + goal_msg = FollowGPSWaypoints.Goal() + goal_msg.number_of_loops = 5 + goal_msg.gps_poses = [self._to_gps_pose(wp) for wp in waypoints] + + self._send_goal_future = self.ac.send_goal_async( + goal_msg, feedback_callback=self.cb_feedback + ) + self._send_goal_future.add_done_callback(self.cb_goal_response) + + def cb_goal_response(self, future: Future) -> None: + """Callback to indicate whether goal is accepted or not. + + Args: + future (Future): future containing the goal response. + """ + gh = future.result() + if gh is None: + self.get_logger().info("Received None goal response.") + return + + if not gh.accepted: + self.get_logger().info("Goal rejected.") + return + + self.get_logger().info("Goal accepted!") + self._get_result_future = gh.get_result_async() + self._get_result_future.add_done_callback(self.cb_result) + + def cb_feedback(self, feedback_msg: FollowGPSWaypoints_FeedbackMessage) -> None: + """Callback for feedback from the action server. + + Args: + feedback_msg (FollowGPSWaypoints_FeedbackMessage): feedback message containing current waypoint being followed. + """ + current_wp = feedback_msg.feedback.current_waypoint + if current_wp != self.current_wp: + self.get_logger().info(f"Now navigating to waypoint {current_wp}.") + self.current_wp = current_wp + + def cb_result(self, future: Future) -> None: + """Callback for result from the action server. + + Args: + future (Future): future containing result of GPS waypoint following. + """ + result_msg = future.result() + if result_msg is None: + self.get_logger().info("No result received.") + return + result = result_msg.result + + self.get_logger().info( + f"Completed waypoints.\ + \nMissed waypoints: {result.missed_waypoints} \ + \nError msg: {result.error_msg}" + ) + self.done = True + + @staticmethod + def _to_gps_pose(wp: GPSWaypoint) -> GeoPose: + pose = GeoPose() + pose.position.latitude = wp.latitude + pose.position.longitude = wp.longitude + pose.orientation = GPSWaypointFollower._yaw_to_quaternion(wp.yaw) + return pose + + @staticmethod + def _yaw_to_quaternion(yaw: float) -> Quaternion: + q = Quaternion() + q.z = math.sin(yaw / 2.0) + q.w = math.cos(yaw / 2.0) + return q + + +if __name__ == "__main__": + route_input = input( + f"\nGPS Waypoint Follower test script. \ + \nChoose one of the following routes: \ + \n{[f'{i}: {k.name}' for i, k in enumerate(ROUTES)]} \ + \n\nInput: " + ) + match route_input: + case "KB_TRUCK_PARKING_SPACE" | "0": + route = Route.KB_TRUCK_PARKING_SPACE + case "SIM_TIGHT_ALLEYS" | "1": + route = Route.SIM_TIGHT_ALLEYS + case _: + print("No valid route given. Defaulting to SIM_TIGHT_ALLEYS.") + route = Route.SIM_TIGHT_ALLEYS + print(f"Sending route {route.name} ({len(ROUTES[route])} waypoints).") + + rclpy.init() + gps_waypoint_follower = GPSWaypointFollower() + + future = gps_waypoint_follower.send_goal(route) + while not gps_waypoint_follower.done: + rclpy.spin_once(gps_waypoint_follower) + + gps_waypoint_follower.destroy_node() + rclpy.shutdown() diff --git a/alliander_ouster/src/alliander_ouster/launch/ouster.launch.py b/alliander_ouster/src/alliander_ouster/launch/ouster.launch.py index 8d02c133..94942837 100644 --- a/alliander_ouster/src/alliander_ouster/launch/ouster.launch.py +++ b/alliander_ouster/src/alliander_ouster/launch/ouster.launch.py @@ -47,7 +47,6 @@ def launch_setup(context: LaunchContext) -> list: {"platform_config": lidar_config.to_str()}, ) - target_frame = "" pointcloud_to_laserscan_node = Node( package="pointcloud_to_laserscan", executable="pointcloud_to_laserscan_node", @@ -57,7 +56,6 @@ def launch_setup(context: LaunchContext) -> list: ], parameters=[ { - "target_frame": target_frame, "min_height": 0.1, "max_height": 2.0, "range_min": 0.05, diff --git a/alliander_velodyne/src/alliander_velodyne/launch/velodyne.launch.py b/alliander_velodyne/src/alliander_velodyne/launch/velodyne.launch.py index c262cd38..68c8b54c 100644 --- a/alliander_velodyne/src/alliander_velodyne/launch/velodyne.launch.py +++ b/alliander_velodyne/src/alliander_velodyne/launch/velodyne.launch.py @@ -47,7 +47,6 @@ def launch_setup(context: LaunchContext) -> list: {"platform_config": lidar_config.to_str()}, ) - target_frame = "" pointcloud_to_laserscan_node = Node( package="pointcloud_to_laserscan", executable="pointcloud_to_laserscan_node", @@ -57,7 +56,6 @@ def launch_setup(context: LaunchContext) -> list: ], parameters=[ { - "target_frame": target_frame, "min_height": 0.1, "max_height": 2.0, "range_min": 0.05, diff --git a/alliander_visualization/src/alliander_visualization/alliander_visualization/rviz.py b/alliander_visualization/src/alliander_visualization/alliander_visualization/rviz.py index ae544add..1f416bd8 100644 --- a/alliander_visualization/src/alliander_visualization/alliander_visualization/rviz.py +++ b/alliander_visualization/src/alliander_visualization/alliander_visualization/rviz.py @@ -1,12 +1,11 @@ # SPDX-FileCopyrightText: Alliander N. V. # # SPDX-License-Identifier: Apache-2.0 - import yaml from alliander_utilities.ros_utils import get_file_path, get_yaml -class Rviz: +class Rviz: # noqa PLR0904 """A class to dynammically manage the RViz configuration. Attributes: @@ -119,6 +118,9 @@ def add_point_cloud(namespace: str) -> None: "Class": "rviz_default_plugins/PointCloud2", "Topic": {"Value": f"/{namespace}/scan/points"}, "Name": namespace, + "Use rainbow": False, + "Min Color": "170; 0; 255", + "Alpha": 0.3, } ) @@ -140,7 +142,30 @@ def add_laser_scan(namespace: str) -> None: "Name": namespace, "Color Transformer": "FlatColor", "Color": "255; 0; 0", - "Size (m)": 0.02, + "Style": "Spheres", + "Size (m)": 0.03, + } + ) + + @staticmethod + def add_vehicle_trajectory(topic: str) -> None: + """Add a controller trajectory to the RViz configuration. + + Args: + topic (str): The topic of the trajectory. + """ + Rviz.displays.append( + { + "Enabled": True, + "Class": "rviz_default_plugins/Path", + "Name": topic, + "Topic": {"Value": topic}, + "Pose Style": "Arrows", + "Pose Color": "255; 85; 255", + "Shaft Length": 0.3, + "Head Length": 0.2, + "Shaft Diameter": 0.05, + "Head Diameter": 0.1, } ) @@ -201,7 +226,7 @@ def add_robot_state(namespace: str) -> None: ) @staticmethod - def add_trajectory(namespace: str) -> None: + def add_arm_trajectory(namespace: str) -> None: """Add the trajectory display to the RViz configuration. Args: @@ -219,13 +244,14 @@ def add_trajectory(namespace: str) -> None: ) @staticmethod - def add_map(topic: str) -> None: + def add_map(topic: str, color_scheme: str = "costmap", alpha: float = 0.7) -> None: """Add a map to the RViz configuration. Args: topic (str): The topic of the map. + color_scheme (str): The color scheme of the map. + alpha (float): The transparency level of the map. """ - color_scheme = "costmap" if "costmap" in topic else "map" Rviz.displays.append( { "Enabled": True, @@ -233,6 +259,7 @@ def add_map(topic: str) -> None: "Name": topic, "Topic": {"Value": topic}, "Color Scheme": color_scheme, + "Alpha": alpha, } ) @@ -252,6 +279,23 @@ def add_path(topic: str) -> None: } ) + @staticmethod + def add_odometry(topic: str) -> None: + """Add an odometry marker to the RViz configuration. + + Args: + topic (str): The odometry topic. + """ + Rviz.displays.append( + { + "Enabled": True, + "Class": "rviz_default_plugins/Odometry", + "Name": topic, + "Topic": {"Value": topic}, + "Keep": 1, + } + ) + @staticmethod def add_polygon(topic: str) -> None: """Add a polygon to the RViz configuration. @@ -297,12 +341,13 @@ def add_satellite(topic: str) -> None: "Enabled": True, "Class": "rviz_satellite/AerialMap", "Name": topic, - "Object URI": "https://tile.openstreetmap.org/{z}/{x}/{y}.png", + "Object URI": "https://mt1.google.com/vt/lyrs=s&x={x}&y={y}&z={z}", "Topic": { "Value": topic, }, "Value": True, "Zoom": 19, + "Draw Behind": True, } ) diff --git a/alliander_visualization/src/alliander_visualization/alliander_visualization/tool_manager.py b/alliander_visualization/src/alliander_visualization/alliander_visualization/tool_manager.py index 49100f4e..ab3641c9 100644 --- a/alliander_visualization/src/alliander_visualization/alliander_visualization/tool_manager.py +++ b/alliander_visualization/src/alliander_visualization/alliander_visualization/tool_manager.py @@ -108,7 +108,7 @@ def add_arm(platform: Arm) -> None: ApplyConfigurations.add_description(ns, kinematic=True) Rviz.add_planning_scene(ns) Rviz.add_robot_state(ns) - Rviz.add_trajectory(ns) + Rviz.add_arm_trajectory(ns) Rviz.add_markers() if platform.moveit_config.load_rviz_motion_planning_plugin: Rviz.add_motion_planning_plugin(ns) @@ -125,11 +125,14 @@ def add_vehicle(platform: Vehicle) -> None: Vizanti.add_platform_model(ns) if (nav2.navigation or nav2.slam) and not nav2.gps: - Rviz.add_map(f"/{ns}/map") + Rviz.add_map(f"/{ns}/map", "map") if nav2.navigation: Rviz.add_map(f"/{ns}/global_costmap/costmap") + Rviz.add_map(f"/{ns}/local_costmap/costmap", "map", 0.3) + Rviz.add_odometry(f"/{ns}/odometry/global") Rviz.add_path(f"/{ns}/plan") + Rviz.add_vehicle_trajectory(f"/{ns}/optimal_trajectory") Vizanti.add_button("Stop", f"/{ns}/waypoint_follower_controller/stop") Vizanti.add_initial_pose() Vizanti.add_goal_pose() @@ -153,6 +156,7 @@ def add_lidar(platform: Lidar) -> None: platform (Lidar): The lidar platform configuration. """ Rviz.add_laser_scan(platform.namespace) + Rviz.add_point_cloud(platform.namespace) @staticmethod def add_depth_camera(platform: Camera) -> None: diff --git a/predefined_configurations.py b/predefined_configurations.py index 5f0233a2..b1bfbc99 100644 --- a/predefined_configurations.py +++ b/predefined_configurations.py @@ -142,7 +142,7 @@ def config_panther(self) -> None: # noqa: D102 @register_configuration("panther_realsense") def config_panther_realsense(self) -> None: # noqa: D102 vehicle = Vehicle("panther", (0, 0, 0.2)) - camera = Camera("realsense", (0, 0, 0.2)) + camera = Camera("realsense", (0.18, 0, 0.2)) link(vehicle, camera) self.plat_conf.platforms = [vehicle, camera] @@ -158,7 +158,12 @@ def config_panther_zed(self) -> None: # noqa: D102 @register_configuration("panther_velodyne") def config_panther_velodyne(self) -> None: # noqa: D102 vehicle = Vehicle("panther", (0, 0, 0.2)) - lidar = Lidar("velodyne", (0.13, -0.13, 0.35), ip_address="10.15.20.5") + lidar = Lidar( + "velodyne", + position=(0.125, 0.185, 0.20), + orientation=(0.0, 0.0, 45.0), + ip_address="10.15.20.5", + ) link(vehicle, lidar) self.plat_conf.platforms = [vehicle, lidar] @@ -174,7 +179,7 @@ def config_panther_ouster(self) -> None: # noqa: D102 @register_configuration("panther_gps") def config_panther_gps(self) -> None: # noqa: D102 vehicle = Vehicle("panther", (0, 0, 0.2)) - gps = GPS("gps", (0, 0, 0.2)) + gps = GPS("gps", position=(-0.08, -0.25, 0.2), orientation=(0, 0, -90)) link(vehicle, gps) self.plat_conf.platforms = [vehicle, gps] @@ -184,7 +189,12 @@ def config_panther_gps(self) -> None: # noqa: D102 def config_panther_collision_monitor(self) -> None: # noqa: D102 vehicle = Vehicle("panther", (0, 0, 0.2)) vehicle.nav2_config.collision_monitor = True - lidar = Lidar("velodyne", (0.13, -0.13, 0.35)) + lidar = Lidar( + "velodyne", + position=(0.125, 0.185, 0.20), + orientation=(0.0, 0.0, 45.0), + ip_address="10.15.20.5", + ) link(vehicle, lidar) self.plat_conf.platforms = [vehicle, lidar] @@ -193,7 +203,12 @@ def config_panther_collision_monitor(self) -> None: # noqa: D102 def config_panther_slam(self) -> None: # noqa: D102 vehicle = Vehicle("panther", (0, 0, 0.2)) vehicle.nav2_config.slam = True - lidar = Lidar("velodyne", (0.13, -0.13, 0.35)) + lidar = Lidar( + "velodyne", + position=(0.125, 0.185, 0.20), + orientation=(0.0, 0.0, 45.0), + ip_address="10.15.20.5", + ) link(vehicle, lidar) self.plat_conf.platforms = [vehicle, lidar] @@ -202,7 +217,12 @@ def config_panther_slam(self) -> None: # noqa: D102 def config_panther_lidar_navigation(self) -> None: # noqa: D102 vehicle = Vehicle("panther", (0, 0, 0.2)) vehicle.nav2_config.navigation = True - lidar = Lidar("velodyne", (0.13, -0.13, 0.35)) + lidar = Lidar( + "velodyne", + position=(0.125, 0.185, 0.20), + orientation=(0.0, 0.0, 45.0), + ip_address="10.15.20.5", + ) link(vehicle, lidar) self.plat_conf.platforms = [vehicle, lidar] @@ -211,17 +231,25 @@ def config_panther_lidar_navigation(self) -> None: # noqa: D102 @register_configuration("panther_gps_navigation") def config_panther_gps_navigation(self) -> None: # noqa: D102 vehicle = Vehicle("panther", (0, 0, 0.2)) + vehicle.nav2_config.controller = "mppi" vehicle.nav2_config.navigation = True vehicle.nav2_config.gps = True vehicle.nav2_config.window_size = 50 - lidar = Lidar("velodyne", (0.13, -0.13, 0.35)) - gps = GPS("gps", (0, 0, 0.2)) + lidar = Lidar( + "velodyne", + position=(0.125, 0.185, 0.2), + orientation=(0, 0, 45), + ip_address="10.15.20.5", + ) + gps = GPS("gps", position=(-0.08, -0.25, 0.2), orientation=(0, 0, -90)) + camera = Camera("realsense", (0.18, 0, 0.2)) link(vehicle, lidar) link(vehicle, gps) - self.plat_conf.platforms = [vehicle, lidar, gps] + link(vehicle, camera) + self.plat_conf.platforms = [vehicle, lidar, gps, camera] self.viz_conf.gui = True - self.sim_conf.world = "map_5.940906_51.966960" + self.sim_conf.world = "map_5.954036_51.977320" # Lynx: @register_configuration("lynx") @@ -250,7 +278,12 @@ def config_mm_velodyne(self) -> None: # noqa: D102 vehicle = Vehicle("panther", (0, 0, 0.2)) vehicle.nav2_config.navigation = True arm = Arm("franka", (0, 0, 0.14), gripper=True, moveit=True) - lidar = Lidar("velodyne", (0.13, -0.13, 0.35)) + lidar = Lidar( + "velodyne", + position=(0.125, 0.185, 0.20), + orientation=(0.0, 0.0, 45.0), + ip_address="10.15.20.5", + ) link(vehicle, arm) link(vehicle, lidar) diff --git a/pyproject.toml b/pyproject.toml index e7c4f6d6..d1450425 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -40,6 +40,9 @@ alliander-gazebo = [ "scipy>=1.17.0", "xmltodict>=1.0.2", ] +alliander-gps = [ + "pydantic>=2.12.5", +] alliander-moveit = [ "xmltodict>=1.0.2", ] diff --git a/start.py b/start.py index 7ded2781..e1fda06f 100755 --- a/start.py +++ b/start.py @@ -68,6 +68,7 @@ def __init__(self) -> None: self.dev = False self.gazebo_ui = False self.joystick = False + self.rviz_yaml = False @staticmethod def get_src_mounts(package: str) -> list[str]: @@ -125,11 +126,12 @@ def get_service_config( platform (Platform | None): platform to get config for, or None if not a platform. arguments (str): additional arguments for pytest. + Returns: + tuple[str, str, dict]: tuple consisting of the package, the config for compose, and additional config. + Raises: ValueError: if platform is not provided while a platform is needed. - Returns: - tuple[str, str, dict]: tuple consisting of the package, the config for compose, and additional config. """ needs_platform = service_type in {"platform", "moveit", "nav2"} if needs_platform and platform is None: @@ -378,6 +380,11 @@ def create_compose( # noqa: PLR0912 } self.write_compose(output_file, content) + + if self.rviz_yaml: + rviz_content = {"services": {}} + self.add_service(rviz_content, "visualization") + self.write_compose("rviz.yml", rviz_content) return list(services.keys()) def run_compose(self) -> int: @@ -489,6 +496,20 @@ def run_compose(self) -> int: help="Add this flag to enable joystick control for arm and/or vehicle platforms.", ) + parser.add_argument( + "--rviz", + required=False, + action="store_true", + help="Add this flag to createan additional Rviz config in rviz.yml. You still need to specify platforms.", + ) + + parser.add_argument( + "--no-run", + required=False, + action="store_true", + help="Add this flag if you only want to create the YML files, but not run them.", + ) + # Parse arguments: args = parser.parse_args() compose = Compose() @@ -502,6 +523,7 @@ def run_compose(self) -> int: compose.simulator = not args.hardware compose.visualization = args.visualization compose.joystick = args.joystick + compose.rviz_yaml = args.rviz compose.mode = "configuration" elif isinstance(args.pytest, list): arguments = " " + " ".join(args.pytest) @@ -525,5 +547,6 @@ def run_compose(self) -> int: compose.create_compose(arguments=arguments) # Spin up containers: - ret = compose.run_compose() - sys.exit(ret) + if not args.no_run: + ret = compose.run_compose() + sys.exit(ret) diff --git a/uv.lock b/uv.lock index 88614dcc..df9db0cf 100644 --- a/uv.lock +++ b/uv.lock @@ -97,6 +97,9 @@ alliander-gazebo = [ { name = "scipy" }, { name = "xmltodict" }, ] +alliander-gps = [ + { name = "pydantic" }, +] alliander-moveit = [ { name = "xmltodict" }, ] @@ -152,6 +155,7 @@ alliander-gazebo = [ { name = "scipy", specifier = ">=1.17.0" }, { name = "xmltodict", specifier = ">=1.0.2" }, ] +alliander-gps = [{ name = "pydantic", specifier = ">=2.12.5" }] alliander-moveit = [{ name = "xmltodict", specifier = ">=1.0.2" }] alliander-nav2 = [ { name = "numpy", specifier = ">=2.4.1" },