Change RewrittenYaml to AdaptedYaml and improve launch logic. (#349)

* Change RewrittenYaml to AdaptedYaml.

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

* Fix global costmap and improve launch file logic.

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

---------

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

Author: Jelmerdw

ros2_ws/src/rcdt_launch/rcdt_launch/predefined_configurations.py

@@ -66,6 +66,7 @@ def wrapper(fn: ConfigurationFunction) -> ConfigurationFunction:
     return wrapper
 
 
+# Sensors:
 @register_configuration("")
 def config_empty() -> None:  # noqa: D103
     pass
@@ -101,17 +102,18 @@ def config_zed() -> None:  # noqa: D103
     Camera("zed", [0, 0, 0.5], namespace="zed")
 
 
+# Franka:
 @register_configuration("franka")
 def config_franka() -> None:  # noqa: D103
     if not EnvironmentConfiguration.simulation:
         Rviz.load_motion_planning_plugin = True
     Arm("franka", [0, 0, 0], gripper=True, moveit=True, ip_address="172.16.0.2")
 
 
-@register_configuration("franka_axis")
-def config_franka_axis() -> 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_realsense")
+def config_franka_realsense() -> None:  # noqa: D103
+    arm = Arm("franka", [0, 0, 0], moveit=True)
+    Camera("realsense", [0.05, 0, 0], [0, -90, 180], parent=arm)
 
 
 @register_configuration("franka_double")
@@ -133,34 +135,12 @@ def config_franka_planning() -> None:  # noqa: D103
     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)
-    Camera("realsense", [0.05, 0, 0], [0, -90, 180], parent=arm)
-
-
+# Panther:
 @register_configuration("panther")
 def config_panther() -> None:  # noqa: D103
     Vehicle("panther", [0, 0, 0.2], namespace="panther")
 
 
-@register_configuration("panther_axis")
-def config_panther_axis() -> None:  # noqa: D103
-    vehicle = Vehicle("panther", [0, 0, 0.2], orientation=[0, 0, 90])
-    Platform("axis", [0, 0, 0.2], [20, 0, 0], parent=vehicle)
-
-
-@register_configuration("panther_gps")
-def config_panther_gps() -> None:  # noqa: D103
-    EnvironmentConfiguration.world = "map_5.940906_51.966960"
-    EnvironmentConfiguration.use_vizanti = True
-    panther = Vehicle(
-        "panther", [0, 0, 0.2], navigation=True, use_gps=True, window_size=50
-    )
-    GPS("nmea", [0, 0, 0.2], parent=panther)
-    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
-
-
 @register_configuration("panther_realsense")
 def config_panther_realsense() -> None:  # noqa: D103
     panther = Vehicle("panther", [0, 0, 0.2])
@@ -175,17 +155,46 @@ 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)
+    panther = Vehicle("panther", [0, 0, 0.2])
     Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
 
 
 @register_configuration("panther_ouster")
 def config_panther_ouster() -> None:  # noqa: D103
-    panther = Vehicle("panther", [0, 0, 0.2], navigation=True)
+    panther = Vehicle("panther", [0, 0, 0.2])
     Lidar("ouster", [0.13, -0.13, 0.35], parent=panther)
 
 
+@register_configuration("panther_collision_monitor")
+def config_panther_collision_monitor() -> None:  # noqa: D103
+    panther = Vehicle("panther", [0, 0, 0.2], collision_monitor=True)
+    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+
+
+@register_configuration("panther_slam")
+def config_panther_slam() -> None:  # noqa: D103
+    panther = Vehicle("panther", [0, 0, 0.2], slam=True)
+    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+
+
+@register_configuration("panther_lidar_navigation")
+def config_panther_lidar_navigation() -> None:  # noqa: D103
+    panther = Vehicle("panther", [0, 0, 0.2], navigation=True)
+    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+
+
+@register_configuration("panther_gps_navigation")
+def config_panther_gps() -> None:  # noqa: D103
+    EnvironmentConfiguration.world = "map_5.940906_51.966960"
+    EnvironmentConfiguration.use_vizanti = True
+    panther = Vehicle(
+        "panther", [0, 0, 0.2], navigation=True, use_gps=True, window_size=50
+    )
+    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+    GPS("nmea", [0, 0, 0.2], parent=panther)
+
+
+# Mobile Manipulators:
 @register_configuration("mm")
 def config_mm() -> None:  # noqa: D103
     panther = Vehicle("panther", [0, 0, 0.2])
@@ -217,6 +226,7 @@ def config_mm_gps() -> None:  # noqa: D103
     GPS("nmea", [0, 0, 0.2], parent=panther)
 
 
+# Multiple non-connected platforms:
 @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/config/nav2/global_costmap.yaml

@@ -4,36 +4,39 @@
 
 # Based on: https://docs.nav2.org/configuration/packages/configuring-costmaps.html
 
-always_send_full_costmap: true
-rolling_window: substitute_me
-width: substitute_me
-height: substitute_me
-update_frequency: 30.0
-publish_frequency: 30.0
-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"]
-static_layer:
-  plugin: "nav2_costmap_2d::StaticLayer"
-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: 1.0
+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
+      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"]
+      static_layer:
+        plugin: "nav2_costmap_2d::StaticLayer"
+      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: 1.0

ros2_ws/src/rcdt_panther/config/nav2/local_costmap.yaml

@@ -4,13 +4,16 @@
 
 # Based on: https://docs.nav2.org/configuration/packages/configuring-costmaps.html
 
-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"
-obstacle_layer:
-  plugin: "nav2_costmap_2d::ObstacleLayer"
-inflation_layer:
-  plugin: "nav2_costmap_2d::InflationLayer"
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      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"
+      obstacle_layer:
+        plugin: "nav2_costmap_2d::ObstacleLayer"
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"