377-remote-monitor-and-control

.gitignore

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

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/alliander_visualization.Dockerfile

@@ -16,6 +16,7 @@ RUN apt update && apt install -y --no-install-recommends \
   ros-$ROS_DISTRO-rqt-tf-tree \
   ros-$ROS_DISTRO-moveit-ros-visualization \
   ros-$ROS_DISTRO-rviz-satellite \
+  ros-$ROS_DISTRO-foxglove-bridge \
   && rm -rf /var/lib/apt/lists/* \
   && apt autoremove -y \
   && apt clean

alliander_visualization/docker-compose.yml

@@ -15,4 +15,5 @@ services:
     volumes:
       - "/tmp/.X11-unix:/tmp/.X11-unix"
       - "/dev:/dev"
+      - "./foxglove.json:/foxglove.json"
     command: ["/bin/bash", "-c", "ros2 launch alliander_visualization visualization.launch.py"]

alliander_visualization/src/alliander_visualization/alliander_visualization/foxglove.py

@@ -0,0 +1,193 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+import json
+
+from alliander_utilities.ros_utils import get_file_path
+
+
+class Foxglove:
+    """A class to dynammically manage the Foxglove layout.
+
+    Attributes:
+        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(
+        get_file_path("alliander_visualization", ["config"], "foxglove_layout.json"),
+        encoding="utf-8",
+    ) as json_file:
+        layout = json.load(json_file)
+
+    @staticmethod
+    def create_layout_file() -> None:
+        """Create the Foxglove layout file."""
+        panels = list(Foxglove.layout["configById"].keys())
+
+        layout = {"first": {}, "second": {}, "direction": "row"}
+        references = [layout]
+
+        if len(panels) == 1:
+            layout = panels.pop()
+
+        places = 2
+        while len(panels) > 0:
+            reference = references[0]
+            direction = "row" if reference["direction"] == "column" else "column"
+            if places < len(panels):
+                if reference["second"] == {}:
+                    reference["second"] = {
+                        "first": {},
+                        "second": {},
+                        "direction": direction,
+                    }
+                    references.append(reference["second"])
+                else:
+                    reference["first"] = {
+                        "first": {},
+                        "second": {},
+                        "direction": direction,
+                    }
+                    references.append(reference["first"])
+                    references.pop(0)
+                places += 1
+            elif reference["second"] == {}:
+                reference["second"] = panels.pop()
+            else:
+                reference["first"] = panels.pop()
+                references.pop(0)
+
+        Foxglove.layout["layout"] = layout
+        with open("/foxglove.json", "w", encoding="utf-8") as outfile:
+            json.dump(Foxglove.layout, outfile)
+
+    @staticmethod
+    def add_platform_model(namespace: str) -> None:
+        """Add a robot model to the Foxglove layout.
+
+        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",
+            "topic": f"/{namespace}/robot_description",
+            "framePrefix": f"{namespace}/",
+        }
+
+    @staticmethod
+    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 platform.
+        """
+        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}/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.
+
+        Args:
+            namespace (str): The namespace of the robot.
+        """
+        Foxglove.layout["configById"]["virtual-joystick.Virtual Joystick"]["topic"] = (
+            f"/{namespace}/cmd_vel"
+        )

alliander_visualization/src/alliander_visualization/alliander_visualization/tool_manager.py

@@ -13,6 +13,7 @@
     VisualizationConfig,
 )
 
+from alliander_visualization.foxglove import Foxglove
 from alliander_visualization.rviz import Rviz
 from alliander_visualization.vizanti import Vizanti
 
@@ -40,6 +41,7 @@ def __init__(self, config: VisualizationConfig, platform_list: PlatformList):
 
         for platform in platform_list.platforms:
             Rviz.add_platform_model(platform.namespace)
+            Foxglove.add_platform_model(platform.namespace)
             match platform.platform_type:
                 case "Arm":
                     self.add_arm(Arm.from_str(platform.to_str()))
@@ -59,6 +61,9 @@ 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()
 
@@ -123,13 +128,21 @@ 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")
+        Foxglove.add_trigger_service("E-Stop Trigger", f"/{ns}/hardware/e_stop_trigger")
+        Foxglove.add_trigger_service("E-Stop Reset", f"/{ns}/hardware/e_stop_reset")
 
         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()
@@ -145,6 +158,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.
@@ -153,6 +169,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:
@@ -168,6 +185,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.
@@ -176,3 +195,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

@@ -0,0 +1,33 @@
+{
+  "configById": {
+    "3D": {
+      "cameraState": {},
+      "followMode": "follow-pose",
+      "followTf": "map",
+      "fixedFrame": "map",
+      "scene": {
+        "transforms": {
+          "visible": false
+        },
+        "backgroundColor": "#454545"
+      },
+      "transforms": {},
+      "topics": {},
+      "layers": {
+        "grid": {
+          "layerId": "foxglove.Grid",
+          "visible": true
+        }
+      },
+      "publish": {
+        "type": "pose",
+        "poseTopic": "/goal_pose"
+      }
+    },
+    "virtual-joystick.Virtual Joystick": {
+      "topic": "/cmd_vel",
+      "messageSchema": "geometry_msgs/msg/TwistStamped"
+    }
+  },
+  "layout": {}
+}

alliander_visualization/src/alliander_visualization/config/foxglove_layout.json.license

@@ -0,0 +1,3 @@
+SPDX-FileCopyrightText: Alliander N. V.
+
+SPDX-License-Identifier: Apache-2.0

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

@@ -1,13 +1,12 @@
 # SPDX-FileCopyrightText: Alliander N. V.
 #
 # SPDX-License-Identifier: Apache-2.0
-
 from alliander_utilities.config_objects import PlatformList, VisualizationConfig
 from alliander_utilities.launch_argument import LaunchArgument
 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
@@ -45,11 +44,25 @@ def launch_setup(context: LaunchContext) -> list:
         parameters=[{"platform_list": platforms.to_str()}],
     )
 
+    foxglove = Node(
+        package="foxglove_bridge",
+        executable="foxglove_bridge",
+        parameters=[
+            {
+                "topic_whitelist": Foxglove.topics,
+                "service_whitelist": Foxglove.services,
+                "param_whitelist": [""],
+                "client_topic_whitelist": [""],
+            }
+        ],
+    )
+
     return [
         SetParameter(name="use_sim_time", value=simulation),
         Register.group(rviz, context) if config.rviz else SKIP,
         Register.group(vizanti, context) if config.vizanti else SKIP,
         Register.on_start(gui, context) if config.gui else SKIP,
+        Register.on_start(foxglove, context),
     ]