Merge pull request #34 from Tigul/urdf-ros

Added parsing of filepaths in URDFs

Author: tomsch420

.github/workflows/ci.yml

@@ -10,38 +10,31 @@ on:
 jobs:
   test:
     runs-on: ubuntu-latest
-    strategy:
-      matrix:
-        python-version: ['3.10']
-
+    container:
+      image: "pycram/semantic_world:jazzy"
     steps:
-    - uses: actions/checkout@v3
-
-    - name: Set up Python ${{ matrix.python-version }}
-      uses: actions/setup-python@v4
+    - uses: actions/checkout@v4
       with:
-        python-version: ${{ matrix.python-version }}
+        path: "ros/src/semantic_world"
+        repository: ${{ github.repository }}
+        ref: ${{ github.ref }}
+        submodules: 'false'
+    - name: Update semantic_world source files
+      run: |
+        rm -rf /opt/ros/overlay_ws/src/semantic_world/* 
+        cd /opt/ros/overlay_ws/src/semantic_world
+        rm -rf .git .github .gitignore .gitmodules .readthedocs.yaml
+        cp -r /__w/${{ github.event.repository.name }}/${{ github.event.repository.name }}/ros/src/semantic_world /opt/ros/overlay_ws/src
 
     - name: Install dependencies
       run: |
         sudo apt-get update
-        sudo apt install graphviz graphviz-dev
-        python -m pip install --upgrade pip
-        pip install virtualenv
-        python -m virtualenv venv
-        source venv/bin/activate
-        pip install -r requirements.txt
-        pip install pytest mypy flake8 black isort
-        pip install .
-        git clone https://github.com/Multiverse-Framework/Multiverse-Parser.git
-        pip install -r ./Multiverse-Parser/requirements.txt
-        pip install -e ./Multiverse-Parser
-        echo "PATH=$PWD/ultiverse-Parser/ext/blender:$PWD/ultiverse-Parser/USD/linux/lib/python:$PWD/ultiverse-Parser/USD/linux/plugin/usd:$PATH" >> $GITHUB_ENV
-        echo "PYTHONPATH=$PWD/ultiverse-Parser/USD/linux/lib/python:$PWD/src" >> $GITHUB_ENV
+        cd /opt/ros/overlay_ws/src/semantic_world
+        source ../semantic_world-venv/bin/activate    
+        pip install -U pip && pip install -r requirements.txt && pip install . && pip install -r doc/requirements.txt
 
     - name: Run tests
       run: |
-        source venv/bin/activate
-        echo "PATH is: $PATH"
-        echo "PYTHONPATH is: $PYTHONPATH"
-        PATH=$PATH PYTHONPATH=$PYTHONPATH pytest -v test/
+        cd /opt/ros/overlay_ws/src/semantic_world
+        source ../semantic_world-venv/bin/activate 
+        pytest -v test/

docker/Dockerfile

@@ -9,12 +9,18 @@ ARG OVERLAY_WS=/opt/ros/overlay_ws
 WORKDIR $OVERLAY_WS/src
 SHELL ["/bin/bash", "-c"]
 
-RUN apt update && apt install python3.12-venv ros-jazzy-xacro python3-vcstool git ros-dev-tools default-jre graphviz graphviz-dev -y
+RUN apt update && apt install python3.12-venv ros-jazzy-xacro python3-vcstool git ros-dev-tools default-jre graphviz graphviz-dev -y && apt-get clean && rm -rf /var/lib/apt/lists/*
 RUN vcs import --input https://raw.githubusercontent.com/cram2/semantic_world/main/rosinstall/semantic_world-https.rosinstall
 RUN source /opt/ros/jazzy/setup.bash && cd $OVERLAY_WS && colcon build --symlink-install
 RUN echo "source $OVERLAY_WS/install/setup.bash" >> ~/.bashrc
 
 RUN python3 -m venv semantic_world-venv --system-site-packages  && source semantic_world-venv/bin/activate && pip install -U pip && pip install -U setuptools && pip install -r $OVERLAY_WS/src/semantic_world/requirements.txt
 
+# Install the Multiverse Parser
+RUN git clone https://github.com/Multiverse-Framework/Multiverse-Parser.git
+RUN source semantic_world-venv/bin/activate && pip install -r ../Multiverse-Parser/requirements.txt && pip install -e ../Multiverse-Parser
+RUN echo "PATH=$OVERLAY_WS/src/Multiverse-Parser/ext/blender:$OVERLAY_WS/src/Multiverse-Parser/USD/linux/lib/python:$OVERLAY_WS/src/Multiverse-Parser/USD/linux/plugin/usd:$PATH" >> $GITHUB_ENV
+RUN echo "PYTHONPATH=$OVERLAY_WS/src/Multiverse-Parser/USD/linux/lib/python:$OVERLAY_WS/src/src" >> $GITHUB_ENV
+
 COPY entrypoint.sh /
 ENTRYPOINT ["bash", "/entrypoint.sh"]

src/semantic_world/adapters/urdf.py

@@ -225,6 +225,28 @@ def parse_geometry(self, geometry: Union[List[urdf.Collision], List[urdf.Visual]
             elif isinstance(geom.geometry, urdf.Mesh):
                 if geom.geometry.filename is None:
                     raise ValueError("Mesh geometry must have a filename.")
-                res.append(Mesh(origin=origin_transform, filename=geom.geometry.filename,
+                res.append(Mesh(origin=origin_transform, filename=self.parse_file_path(geom.geometry.filename),
                                 scale=Scale(*(geom.geometry.scale or (1, 1, 1)))))
         return res
+
+    @staticmethod
+    def parse_file_path(file_path: str) -> str:
+        """
+        Parses a file path which contains a ros package to a path in the local file system.
+        :param file_path: The path to the URDF file.
+        :return: The parsed and processed file path.
+        """
+        if "package://" in file_path:
+            try:
+                from ament_index_python.packages import get_package_share_directory
+            except ImportError:
+                raise ImportError("No ROS install found while the URDF file contains references to ROS packages. ")
+            # Splits the file path at '//' to get the package  and the rest of the path
+            package_split = file_path.split('//')
+            # Splits the path after the // to get the package name and the rest of the path
+            package_name = package_split[1].split('/')[0]
+            package_path = get_package_share_directory(package_name)
+            file_path = file_path.replace("package://" + package_name, package_path)
+        if 'file://' in file_path:
+            file_path = file_path.replace("file://", './')
+        return file_path