From 7f4cf08cb5568916dd9accbc626a63a4f4968ec3 Mon Sep 17 00:00:00 2001
From: dominikn <dominik.nwk@gmail.com>
Date: Thu, 13 Jun 2024 17:34:46 +0000
Subject: [PATCH] added cyclonedds

---
 snap/local/ros_common/configure_hook_ros.sh | 64 +++++++++++++++++++--
 snap/local/ros_common/ros_setup.sh          | 21 +------
 snap/local/ros_common/udp-lo-cyclone.xml    | 16 ++++++
 snap/local/ros_common/utils.sh              |  3 +-
 snap/snapcraft.yaml                         |  3 +-
 5 files changed, 80 insertions(+), 27 deletions(-)
 create mode 100644 snap/local/ros_common/udp-lo-cyclone.xml

diff --git a/snap/local/ros_common/configure_hook_ros.sh b/snap/local/ros_common/configure_hook_ros.sh
index 7904d0e..b24de38 100755
--- a/snap/local/ros_common/configure_hook_ros.sh
+++ b/snap/local/ros_common/configure_hook_ros.sh
@@ -8,6 +8,43 @@
 # Define a function to log and echo messages
 source $SNAP/usr/bin/utils.sh
 
+# Function to check the type of the provided XML file
+check_xml_profile_type() {
+    local xml_file="$1"
+
+    if [[ ! -f "$xml_file" ]]; then
+        log_and_echo "File '$xml_file' does not exist."
+        return 1
+    fi
+
+    local root_element
+    local namespace
+
+    # Extract the root element
+    root_element=$(yq '. | keys | .[1]' "$xml_file")
+
+    # Extract the namespace based on the root element
+    if [[ "$root_element" == "CycloneDDS" ]]; then
+        namespace=$(yq .CycloneDDS."+@xmlns" "$xml_file")
+    elif [[ "$root_element" == "profiles" ]]; then
+        namespace=$(yq .profiles."+@xmlns" "$xml_file")
+    else
+        namespace="unknown"
+    fi
+
+    # Remove quotes from the extracted values
+    root_element=${root_element//\"/}
+    namespace=${namespace//\"/}
+
+    if [[ "$root_element" == "profiles" ]] && [[ "$namespace" == "http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles" ]]; then
+        echo "rmw_fastrtps_cpp"
+    elif [[ "$root_element" == "CycloneDDS" ]] && [[ "$namespace" == "https://cdds.io/config" ]]; then
+        echo "rmw_cyclonedds_cpp"
+    else
+        exit 1
+    fi
+}
+
 # Make sure ROS_LOCALHOST_ONLY is valid
 OPT="ros-localhost-only"
 ROS_LOCALHOST_ONLY="$(snapctl get ${OPT})"
@@ -36,14 +73,14 @@ OPT="transport"
 TRANSPORT_SETTING="$(snapctl get ${OPT})"
 
 # Only exit with status 1 if conditions are not met
-if [ "$TRANSPORT_SETTING" != "builtin" ] && [ ! -f "${SNAP_COMMON}/${TRANSPORT_SETTING}.xml" ]; then
+if [ "$TRANSPORT_SETTING" != "rmw_fastrtps_cpp" ] && [ "$TRANSPORT_SETTING" != "rmw_cyclonedds_cpp" ] && [ ! -f "${SNAP_COMMON}/${TRANSPORT_SETTING}.xml" ]; then
   log_and_echo "'${SNAP_COMMON}/${TRANSPORT_SETTING}.xml' does not exist."
   exit 1
 fi
 
-if [ "$TRANSPORT_SETTING" = "builtin" ] || [ "$TRANSPORT_SETTING" = "shm" ]; then
+if [ "$TRANSPORT_SETTING" = "rmw_fastrtps_cpp" ] || [ "$TRANSPORT_SETTING" = "shm" ]; then
   if ! snapctl is-connected shm-plug; then
-    log_and_echo "to use 'builtin' and 'shm' tranport shm-plug need to be connected, please run:"
+    log_and_echo "to use 'rmw_fastrtps_cpp' and 'shm' tranport shm-plug need to be connected, please run:"
     log_and_echo "sudo snap connect ${SNAP_NAME}:shm-plug ${SNAP_NAME}:shm-slot"
     exit 1
   fi
@@ -78,8 +115,25 @@ else
   echo "unset ROS_NAMESPACE" >> "${ROS_ENV_FILE}"
 fi
 
-if [ "$TRANSPORT_SETTING" != "builtin" ]; then
-  echo "export FASTRTPS_DEFAULT_PROFILES_FILE=${SNAP_COMMON}/${TRANSPORT_SETTING}.xml" >> "${ROS_ENV_FILE}"
+# Check the transport setting and export the appropriate environment variable
+if [ "$TRANSPORT_SETTING" != "rmw_fastrtps_cpp" ] && [ "$TRANSPORT_SETTING" != "rmw_cyclonedds_cpp" ]; then
+    profile_type=$(check_xml_profile_type "${SNAP_COMMON}/${TRANSPORT_SETTING}.xml")
+    if [[ "$profile_type" == "rmw_fastrtps_cpp" ]]; then
+        echo "unset CYCLONEDDS_URI" >> "${ROS_ENV_FILE}"
+        echo "export RMW_IMPLEMENTATION=${profile_type}" >> "${ROS_ENV_FILE}"
+        echo "export FASTRTPS_DEFAULT_PROFILES_FILE=${SNAP_COMMON}/${TRANSPORT_SETTING}.xml" >> "${ROS_ENV_FILE}"
+    elif [[ "$profile_type" == "rmw_cyclonedds_cpp" ]]; then
+        echo "unset FASTRTPS_DEFAULT_PROFILES_FILE" >> "${ROS_ENV_FILE}"
+        echo "export RMW_IMPLEMENTATION=${profile_type}" >> "${ROS_ENV_FILE}"
+        echo "export CYCLONEDDS_URI=file://${SNAP_COMMON}/${TRANSPORT_SETTING}.xml" >> "${ROS_ENV_FILE}"
+    else
+        log_and_echo "'${TRANSPORT_SETTING}' is not a supported value for '${OPT}'. Possible values are: rmw_fastrtps_cpp, rmw_cyclonedds_cpp, or a valid profile XML file."
+        exit 1
+    fi
+elif [ "$TRANSPORT_SETTING" == "rmw_fastrtps_cpp" ] || [ "$TRANSPORT_SETTING" == "rmw_cyclonedds_cpp" ]; then
+  echo "unset CYCLONEDDS_URI" >> "${ROS_ENV_FILE}"
+  echo "unset FASTRTPS_DEFAULT_PROFILES_FILE" >> "${ROS_ENV_FILE}"
+  echo "export RMW_IMPLEMENTATION=${TRANSPORT_SETTING}" >> "${ROS_ENV_FILE}"
 fi
 
 # Define the path for the manage_ros_env.sh script
diff --git a/snap/local/ros_common/ros_setup.sh b/snap/local/ros_common/ros_setup.sh
index f3a290f..2fffe43 100755
--- a/snap/local/ros_common/ros_setup.sh
+++ b/snap/local/ros_common/ros_setup.sh
@@ -1,24 +1,5 @@
 #!/bin/bash -e
 
-source $SNAP/usr/bin/utils.sh
-
-TRANSPORT="$(snapctl get transport)"
-# watch the log with: "journalctl -t ${SNAP_NAME}"
-log "transport: ${TRANSPORT}"
-
-if [ "$TRANSPORT" = "builtin" ]; then
-  log "using builtin transport setting"
-else
-  PROFILE_FILE="${SNAP_COMMON}/${TRANSPORT}.xml"
-  export FASTRTPS_DEFAULT_PROFILES_FILE=$PROFILE_FILE
-  log "using ${PROFILE_FILE} transport setting"
-  log "$(cat $FASTRTPS_DEFAULT_PROFILES_FILE)"
-fi
-
-export ROS_LOCALHOST_ONLY="$(snapctl get ros-localhost-only)"
-export ROS_DOMAIN_ID="$(snapctl get ros-domain-id)"
-
-log "ROS_LOCAHOST_ONLY=${ROS_LOCALHOST_ONLY}"
-log "ROS_DOMAIN_ID=${ROS_DOMAIN_ID}"
+source $SNAP_COMMON/ros.env
 
 exec $@
\ No newline at end of file
diff --git a/snap/local/ros_common/udp-lo-cyclone.xml b/snap/local/ros_common/udp-lo-cyclone.xml
new file mode 100644
index 0000000..6cdc91c
--- /dev/null
+++ b/snap/local/ros_common/udp-lo-cyclone.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<CycloneDDS xmlns="https://cdds.io/config" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="https://cdds.io/config https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclonedds.xsd">
+    <Domain id="any">
+        <General>
+            <NetworkInterfaceAddress>lo</NetworkInterfaceAddress>
+            <AllowMulticast>false</AllowMulticast>
+        </General>
+        <Discovery>
+            <ParticipantIndex>auto</ParticipantIndex>
+            <MaxAutoParticipantIndex>30</MaxAutoParticipantIndex>
+            <Peers>
+                <Peer address="localhost"/>
+            </Peers>
+        </Discovery>
+    </Domain>
+</CycloneDDS>
\ No newline at end of file
diff --git a/snap/local/ros_common/utils.sh b/snap/local/ros_common/utils.sh
index d8144e0..7c3223a 100755
--- a/snap/local/ros_common/utils.sh
+++ b/snap/local/ros_common/utils.sh
@@ -19,4 +19,5 @@ log() {
 
 is_integer() {
   expr "$1" : '-\?[0-9][0-9]*$' >/dev/null 2>&1
-}
\ No newline at end of file
+}
+
diff --git a/snap/snapcraft.yaml b/snap/snapcraft.yaml
index cb4b7e5..6c3550f 100644
--- a/snap/snapcraft.yaml
+++ b/snap/snapcraft.yaml
@@ -37,7 +37,7 @@ description: |
 
   The `webui` parameter is a dictionary containing the following keys for the Web UI:
 
-  * `webui.layout`: `sensors` - Specifies the layout for the Web UI. Available `*.json` layout files can be found in the `/var/snap/rosbot-xl/common` directory (custom layouts can also be created here).
+  * `webui.layout`: `default` - Specifies the layout for the Web UI. Available `*.json` layout files can be found in the `/var/snap/rosbot-xl/common` directory (custom layouts can also be created here).
   * `webui.port`: `8080` - Specifies the port for the built-in web server hosting the Web UI.
 
   To set parameters, use the snap set command, e.g.,
@@ -217,6 +217,7 @@ parts:
       - stm32flash
       - libusb-1.0-0
       - usbutils
+      - ros-humble-rmw-cyclonedds-cpp
     override-pull: |
       craftctl default