Add navigation data to foxglove.

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

Author: Jelmerdw

alliander_core/src/alliander_utilities/alliander_utilities/config_objects.py

@@ -358,10 +358,12 @@ class VisualizationConfig(Config):
 
     Attributes:
         rviz (bool): Whether to enable RViz visualization.
+        foxglove (bool): Whether to enable Foxglove visualization.
         vizanti (bool): Whether to enable Vizanti visualization.
         gui (bool): Whether to enable GUI.
     """
 
-    rviz: bool = True
+    rviz: bool = False
+    foxglove: bool = True
     vizanti: bool = False
     gui: bool = False

alliander_visualization/src/alliander_visualization/alliander_visualization/foxglove.py

@@ -10,9 +10,13 @@ class Foxglove:
     """A class to dynammically manage the Foxglove layout.
 
     Attributes:
-        layout (dict): The default Foxglove layout.
+        topics (list): The topics to bridge.
+        services (list): The services to bridge.
+        layout (dict): The Foxglove layout.
     """
 
+    topics: list = ["/clock", "/tf", "/tf_static"]
+    services: list = [""]
     layout: dict
 
     with open(
@@ -37,26 +41,26 @@ def create_layout_file() -> None:
             reference = references[0]
             direction = "row" if reference["direction"] == "column" else "column"
             if places < len(panels):
-                if reference["first"] == {}:
-                    reference["first"] = {
+                if reference["second"] == {}:
+                    reference["second"] = {
                         "first": {},
                         "second": {},
                         "direction": direction,
                     }
-                    references.append(reference["first"])
+                    references.append(reference["second"])
                 else:
-                    reference["second"] = {
+                    reference["first"] = {
                         "first": {},
                         "second": {},
                         "direction": direction,
                     }
-                    references.append(reference["second"])
+                    references.append(reference["first"])
                     references.pop(0)
                 places += 1
-            elif reference["first"] == {}:
-                reference["first"] = panels.pop(0)
+            elif reference["second"] == {}:
+                reference["second"] = panels.pop()
             else:
-                reference["second"] = panels.pop(0)
+                reference["first"] = panels.pop()
                 references.pop(0)
 
         Foxglove.layout["layout"] = layout
@@ -70,6 +74,7 @@ def add_platform_model(namespace: str) -> None:
         Args:
             namespace (str): The namespace of the robot.
         """
+        Foxglove.topics.append([f"/{namespace}/robot_description"])
         Foxglove.layout["configById"]["3D"]["layers"][f"urdf-{namespace}"] = {
             "layerId": "foxglove.Urdf",
             "sourceType": "topic",
@@ -78,19 +83,104 @@ def add_platform_model(namespace: str) -> None:
         }
 
     @staticmethod
-    def add_camera(namespace: str) -> None:
+    def add_street_map(namespace: str) -> None:
+        """Add a street map to the Foxglove 3D panel.
+
+        Args:
+            namespace (str): The namespace of the platform.
+        """
+        Foxglove.topics.append(f"/{namespace}/gps/fix")
+        Foxglove.topics.append(f"/{namespace}/gps/filtered")
+        Foxglove.layout["configById"]["3D"]["layers"]["map"] = {
+            "layerId": "foxglove.TiledMap",
+            "visible": True,
+            "serverConfig": "map",
+            "label": "Map",
+        }
+
+    @staticmethod
+    def add_image(namespace: str) -> None:
         """Add a camera feed to the Foxglove layout.
 
         Args:
-            namespace (str): The namespace of the robot.
+            namespace (str): The namespace of the platform.
         """
-        Foxglove.layout["configById"][f"Image_{namespace}"] = {
+        Foxglove.topics.append(f"/{namespace}/color/image_raw")
+        Foxglove.topics.append(f"/{namespace}/color/camera_info")
+        Foxglove.layout["configById"][f"Image!{namespace}"] = {
             "imageMode": {
-                "imageTopic": f"/{namespace}/zed/color/image_raw",
-                "calibrationTopic": f"/{namespace}/zed/color/camera_info",
+                "imageTopic": f"/{namespace}/color/image_raw",
+                "calibrationTopic": f"/{namespace}/color/camera_info",
             }
         }
 
+    @staticmethod
+    def add_pointcloud(namespace: str) -> None:
+        """Add a pointcloud to the Foxglove 3D panel.
+
+        Args:
+            namespace (str): The namespace of the platform.
+        """
+        Foxglove.topics.append(f"/{namespace}/scan/points")
+        Foxglove.layout["configById"]["3D"]["topics"][f"/{namespace}/scan/points"] = {
+            "visible": True,
+            "colorMode": "colormap",
+            "colorMap": "rainbow",
+            "colorField": "intensity",
+        }
+
+    @staticmethod
+    def add_map(topic: str) -> None:
+        """Add a map to the Foxglove 3D panel.
+
+        Args:
+            topic (str): The topic of the costmap.
+        """
+        Foxglove.topics.append(topic)
+        Foxglove.layout["configById"]["3D"]["topics"][topic] = {
+            "visible": True,
+            "colorMode": "costmap",
+        }
+
+    @staticmethod
+    def add_path(topic: str) -> None:
+        """Add a path to the Foxglove 3D panel.
+
+        Args:
+            topic (str): The topic of the path.
+        """
+        Foxglove.topics.append(topic)
+        Foxglove.layout["configById"]["3D"]["topics"][topic] = {
+            "visible": True,
+            "lineWidth": 0.03,
+            "gradient": ["#00ff00ff", "#00ff00ff"],
+        }
+
+    @staticmethod
+    def add_polygon(topic: str) -> None:
+        """Add a polygon to the Foxglove 3D panel.
+
+        Args:
+            topic (str): The topic of the polygon.
+        """
+        Foxglove.topics.append(topic)
+
+    @staticmethod
+    def add_trigger_service(name: str, service: str) -> None:
+        """Add a service call panel to the Foxglove layout.
+
+        Args:
+            name (str): The name of the button.
+            service (str): The service to call when the button is pressed.
+        """
+        Foxglove.services.append(service)
+        Foxglove.layout["configById"][f"CallService!{name}"] = {
+            "serviceName": service,
+            "foxglovePanelTitle": service,
+            "editingMode": False,
+            "buttonText": name,
+        }
+
     @staticmethod
     def add_joystick(namespace: str) -> None:
         """Add a virtual joystick to the Foxglove layout.

alliander_visualization/src/alliander_visualization/alliander_visualization/tool_manager.py

@@ -51,7 +51,6 @@ def __init__(self, config: VisualizationConfig, platform_list: PlatformList):
                     self.add_lidar(Lidar.from_str(platform.to_str()))
                 case "Camera":
                     self.add_depth_camera(Camera.from_str(platform.to_str()))
-                    Foxglove.add_camera(platform.namespace)
                 case "GPS":
                     self.add_gps(GPS.from_str(platform.to_str()))
                 case _:
@@ -62,11 +61,12 @@ def __init__(self, config: VisualizationConfig, platform_list: PlatformList):
         if config.rviz:
             Rviz.create_rviz_file()
 
+        if config.foxglove:
+            Foxglove.create_layout_file()
+
         if config.vizanti:
             Vizanti.create_config_file()
 
-        Foxglove.create_layout_file()
-
     # TODO: refactor this
     @staticmethod
     def add_description(
@@ -128,13 +128,19 @@ def add_vehicle(platform: Vehicle) -> None:
         ns = platform.namespace
         nav2 = platform.nav2_config
         Vizanti.add_platform_model(ns)
+        Foxglove.topics.append(f"/{ns}/cmd_vel")
 
         if (nav2.navigation or nav2.slam) and not nav2.gps:
             Rviz.add_map(f"/{ns}/map")
 
         if nav2.navigation:
             Rviz.add_map(f"/{ns}/global_costmap/costmap")
             Rviz.add_path(f"/{ns}/plan")
+
+            Foxglove.add_map(f"/{ns}/global_costmap/costmap")
+            Foxglove.add_path(f"/{ns}/plan")
+            Foxglove.add_trigger_service("Stop", f"/{ns}/nav2_manager/stop")
+
             Vizanti.add_button("Stop", f"/{ns}/waypoint_follower_controller/stop")
             Vizanti.add_initial_pose()
             Vizanti.add_goal_pose()
@@ -150,6 +156,9 @@ def add_vehicle(platform: Vehicle) -> None:
             Rviz.add_polygon(f"/{ns}/polygon_slower")
             Rviz.add_polygon(f"/{ns}/velocity_polygon_stop")
 
+            Foxglove.add_polygon(f"/{ns}/polygon_slower")
+            Foxglove.add_polygon(f"/{ns}/velocity_polygon_stop")
+
     @staticmethod
     def add_lidar(platform: Lidar) -> None:
         """Add lidar configurations to RViz and Vizanti.
@@ -158,6 +167,7 @@ def add_lidar(platform: Lidar) -> None:
             platform (Lidar): The lidar platform configuration.
         """
         Rviz.add_laser_scan(platform.namespace)
+        Foxglove.add_pointcloud(platform.namespace)
 
     @staticmethod
     def add_depth_camera(platform: Camera) -> None:
@@ -173,6 +183,8 @@ def add_depth_camera(platform: Camera) -> None:
             f"/{platform.namespace}/depth/image_rect_raw",
         )
 
+        Foxglove.add_image(platform.namespace)
+
     @staticmethod
     def add_gps(platform: GPS) -> None:
         """Add GPS configurations to RViz and Vizanti.
@@ -181,3 +193,4 @@ def add_gps(platform: GPS) -> None:
             platform (GPS): The GPS platform configuration.
         """
         Rviz.add_satellite(f"/{platform.namespace}/gps/fix")
+        Foxglove.add_street_map(platform.namespace)

alliander_visualization/src/alliander_visualization/config/foxglove_layout.json

@@ -8,7 +8,8 @@
       "scene": {
         "transforms": {
           "visible": false
-        }
+        },
+        "backgroundColor": "#454545"
       },
       "transforms": {},
       "topics": {},

alliander_visualization/src/alliander_visualization/launch/visualization.launch.py

@@ -6,7 +6,7 @@
 from alliander_utilities.launch_utils import SKIP
 from alliander_utilities.register import Register, RegisteredLaunchDescription
 from alliander_utilities.ros_utils import get_file_path
-from alliander_visualization.tool_manager import ApplyConfigurations
+from alliander_visualization.tool_manager import ApplyConfigurations, Foxglove
 from launch import LaunchContext, LaunchDescription
 from launch.actions import OpaqueFunction
 from launch_ros.actions import Node, SetParameter
@@ -44,24 +44,13 @@ def launch_setup(context: LaunchContext) -> list:
         parameters=[{"platform_list": platforms.to_str()}],
     )
 
-    topic_whitelist = ["/clock", "/tf", "/tf_static"]
-    for platform in platforms.platforms:
-        topic_whitelist.append([f"/{platform.namespace}/robot_description"])
-        match platform.platform_type:
-            case "Vehicle":
-                topic_whitelist.append(f"/{platform.namespace}/cmd_vel")
-            case "Camera":
-                topic_whitelist.append(f"/{platform.namespace}/color/image_raw")
-            case "Lidar":
-                topic_whitelist.append(f"/{platform.namespace}/scan/points")
-
     foxglove = Node(
         package="foxglove_bridge",
         executable="foxglove_bridge",
         parameters=[
             {
-                "topic_whitelist": topic_whitelist,
-                "service_whitelist": [""],
+                "topic_whitelist": Foxglove.topics,
+                "service_whitelist": Foxglove.services,
                 "param_whitelist": [""],
                 "client_topic_whitelist": [""],
             }