[Description] Optimize code generation and enable generation of URDF …

…tests per default (#172)

* add urdf test in Python with reference to the original code
---------

Co-authored-by: Dr. Denis <[email protected]>

scripts/setup-robot-description.bash

@@ -21,6 +21,7 @@ usage="setup-robot-description ROBOT_NAME LAUNCH_FILE_TYPE"
 # Load Framework defines
 script_own_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
 source $script_own_dir/../setup.bash
+
 check_and_set_ros_distro_and_version ${ROS_DISTRO}
 
 ROBOT_NAME=$1
@@ -108,30 +109,89 @@ mkdir -p rviz
 ROBOT_RVIZ="rviz/${ROBOT_NAME}.rviz"
 cp -n "$ROBOT_DESCRIPTION_TEMPLATES/robot.rviz" $ROBOT_RVIZ
 
-# Add dependencies if they not exist
-DEP_PKGS=("xacro" "rviz2" "robot_state_publisher" "joint_state_publisher_gui")
+# copy test files
+mkdir -p test
+ROBOT_TEST_FILE="test/${ROBOT_NAME}_test_urdf_xacro.py"
+cp -n "${ROBOT_DESCRIPTION_TEMPLATES}/test_urdf_xacro.py" $ROBOT_TEST_FILE
+
+# sed all needed files
+FILES_TO_SED=($ROBOT_URDF_XACRO $ROBOT_MACRO $ROBOT_MACRO_ROS2_CONTROL $VIEW_ROBOT_LAUNCH_XML $ROBOT_TEST_FILE)
+for SED_FILE in "${FILES_TO_SED[@]}"; do
+  sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" $SED_FILE
+  sed -i "s/\\\$ROBOT_NAME\\\$/${ROBOT_NAME}/g" $SED_FILE
+done
+
+# Add all the exec depend packages
+DEPENDENCIES=("joint_state_publisher_gui" "robot_state_publisher" "rviz2" "xacro")
+# append to last exec_depend or if non is found to buildtool_depend
+PREVIOUS_STRING=$(grep -E "^\s*<exec_depend>" package.xml | tail -n 1)
+if [ -z "$PREVIOUS_STRING" ]; then
+  PREVIOUS_STRING="<buildtool_depend>ament_cmake<\/buildtool_depend>"
+fi
+DEPEND_TAG="exec_depend"
+for DEP_PKG in "${DEPENDENCIES[@]}"; do
+  # Check if the current package is already listed in the package.xml file
+  if grep -q "<$DEPEND_TAG>${DEP_PKG}<\/$DEPEND_TAG>" package.xml; then
+    echo "'$DEP_PKG' is already listed in package.xml"
+  else
+    # Check if the previous string starts with <buildtool_depend>
+    if [[ ! "$PREVIOUS_STRING" =~ ^\s*\<$DEPEND_TAG.* ]]; then
+      # If the previous string starts with different tag, add 2 newline characters after it to start new block
+      newline="\n\n"
+    else
+      # otherwise add only one newline character so it's in the same block
+      newline="\n"
+    fi
+    sed -i "s#$PREVIOUS_STRING#$PREVIOUS_STRING${newline}  <$DEPEND_TAG>${DEP_PKG}<\/$DEPEND_TAG>#g" package.xml
+    PREVIOUS_STRING="<$DEPEND_TAG>${DEP_PKG}<\/$DEPEND_TAG>"
+  fi
+done
 
-for DEP_PKG in "${DEP_PKGS[@]}"; do
-  if $(grep -q $DEP_PKG package.xml); then
+# Add all the test depend packages
+DEPENDENCIES=("ament_cmake_pytest" "launch_testing_ament_cmake" "launch_testing_ros" "liburdfdom-tools" "xacro")
+# append to last exec_depend or if non is found to buildtool_depend
+PREVIOUS_STRING=$(grep -E "^\s*<test_depend>" package.xml | tail -n 1)
+if [ -z "$PREVIOUS_STRING" ]; then
+  PREVIOUS_STRING="<exec_depend>xacro</exec_depend>"
+fi
+DEPEND_TAG="test_depend"
+for DEP_PKG in "${DEPENDENCIES[@]}"; do
+  # Check if the current package is already listed in the package.xml file
+  if grep -q "<$DEPEND_TAG>${DEP_PKG}<\/$DEPEND_TAG>" package.xml; then
     echo "'$DEP_PKG' is already listed in package.xml"
   else
-    append_to_string="<buildtool_depend>ament_cmake<\/buildtool_depend>"
-    sed -i "s/$append_to_string/$append_to_string\\n\\n  <exec_depend>${DEP_PKG}<\/exec_depend>/g" package.xml
+    # Check if the previous string starts with <buildtool_depend>
+    if [[ ! "$PREVIOUS_STRING" =~ ^\s*\<$DEPEND_TAG.* ]]; then
+      # If the previous string starts with different tag, add 2 newline characters after it to start new block
+      newline="\n\n"
+    else
+      # otherwise add only one newline character so it's in the same block
+      newline="\n"
+    fi
+    sed -i "s#$PREVIOUS_STRING#$PREVIOUS_STRING${newline}  <$DEPEND_TAG>${DEP_PKG}<\/$DEPEND_TAG>#g" package.xml
+    PREVIOUS_STRING="<$DEPEND_TAG>${DEP_PKG}<\/$DEPEND_TAG>"
   fi
 done
 
+# Update the CMakeLists.txt
 # Add install paths of the files
 preppend_to_string="if(BUILD_TESTING)"
-sed -i "s/$preppend_to_string/install\(\\n  DIRECTORY config launch meshes rviz urdf\\n  DESTINATION share\/\$\{PROJECT_NAME\}\\n\)\\n\\n$preppend_to_string/g" CMakeLists.txt
+sed -i "s/$preppend_to_string/install\(\\n  DIRECTORY config launch meshes rviz urdf test\\n  DESTINATION share\/\$\{PROJECT_NAME\}\\n\)\\n\\n$preppend_to_string/g" CMakeLists.txt
+# Add the test
+lines_to_append="  find_package(ament_cmake_pytest REQUIRED)\n\n  ament_add_pytest_test(test_${ROBOT_NAME}_urdf_xacro ${ROBOT_TEST_FILE})"
+# Define the search pattern
+pattern='if(BUILD_TESTING)'
+# Use sed to find the pattern and append the lines after it in CMakeLists.txt
+sed -i "/$pattern/a$lines_to_append" CMakeLists.txt
 
 # extend README with general instructions
-if [ -f README.md ]; then
-  cat $ROBOT_DESCRIPTION_TEMPLATES/append_to_README.md >>README.md
-  sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" README.md
-  sed -i "s/\\\$ROBOT_NAME\\\$/${ROBOT_NAME}/g" README.md
+if [ ! -f README.md ]; then
+  echo "${PKG_NAME}\n\n" > README.md
 fi
 
-#TODO: Set license
+cat $ROBOT_DESCRIPTION_TEMPLATES/append_to_README.md >> README.md
+sed -i "s/\\\$PKG_NAME\\\$/${PKG_NAME}/g" README.md
+sed -i "s/\\\$ROBOT_NAME\\\$/${ROBOT_NAME}/g" README.md
 
 # Compile and add new package the to the path
 compile_and_source_package $PKG_NAME

templates/robot_description/append_to_README.md

@@ -1,4 +1,7 @@
 
+Descriptions, meshes, and visualization files for the robots and environments. Corresponding launch and test files are also stored here.
+
+The structure and files in this package are generated using [RosTeamWorkspace script setup-robot-description](https://rtw.stoglrobotics.de/master/use-cases/ros_packages/setup_robot_description_package.html). You can use the same script to generate initial files for other robots.
 
 ## General details about robot description packages
 
@@ -40,10 +43,18 @@ The general package structure is the following:
 1. Go to the root of your workspace folder (there where `src`, `build`, `install` and `log` files are).
 2. Install the package by calling `colcon build --symlink-install --packages-select $PKG_NAME$`
 3. (Re-)Source environment `source install/setup.bash`
-4. Launch description test:
-   ```
-   ros2 launch $PKG_NAME$ test_$ROBOT_NAME$_description.launch.py
-   ```
+
+
+> **NOTE:** If you use [RosTeamWorkspace (RTW)](https://rtw.stoglrobotics.de) than instead of the previous three steps, use `cb $PKG_NAME$` command.
+
+Now, launch description test:
+```
+ros2 launch $PKG_NAME$ view_$ROBOT_NAME$.launch.xml
+```
+or
+```
+ros2 launch $PKG_NAME$ view_$ROBOT_NAME$.launch.py
+```
 
 If there are no issues with the description, two windows are opened: `rviz2` and `Joint State Publisher`.
 Rviz2 visualizes the robot's state and Joint state Publisher to changes joint values using sliders or generates random but valid configurations.

templates/robot_description/test_urdf_xacro.py

@@ -0,0 +1,83 @@
+# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
+# Copyright (c) 2022 FZI Forschungszentrum Informatik
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the FZI Forschungszentrum Informatik nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+#
+# Source of this file is https://github.com/StoglRobotics/ros_team_workspace repository.
+# Modified from tests in https://github.com/UniversalRobots/Universal_Robots_ROS2_Description
+#
+# Author: Lukas Sackewitz
+# Author (template): Manuel Muth
+
+import os
+import shutil
+import subprocess
+import tempfile
+
+from ament_index_python.packages import get_package_share_directory
+
+
+def test_urdf_xacro():
+    # General Arguments
+    description_package = "$PKG_NAME$"
+    description_file = "$ROBOT_NAME$.urdf.xacro"
+
+    description_file_path = os.path.join(
+        get_package_share_directory(description_package), "urdf", description_file
+    )
+
+    (_, tmp_urdf_output_file) = tempfile.mkstemp(suffix=".urdf")
+
+    # Compose `xacro` and `check_urdf` command
+    xacro_command = (
+        f"{shutil.which('xacro')}" f" {description_file_path}" f" > {tmp_urdf_output_file}"
+    )
+    check_urdf_command = f"{shutil.which('check_urdf')} {tmp_urdf_output_file}"
+
+    # Try to call processes but finally remove the temp file
+    try:
+        xacro_process = subprocess.run(
+            xacro_command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True
+        )
+
+        assert xacro_process.returncode == 0, " --- XACRO command failed ---"
+
+        check_urdf_process = subprocess.run(
+            check_urdf_command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True
+        )
+
+        assert (
+            check_urdf_process.returncode == 0
+        ), "\n --- URDF check failed! --- \nYour xacro does not unfold into a proper urdf robot description. Please check your xacro file."
+
+    finally:
+        os.remove(tmp_urdf_output_file)
+
+
+if __name__ == "__main__":
+    test_urdf_xacro()

templates/robot_description/view_robot.launch.py

@@ -1,4 +1,4 @@
-# Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template)
+# Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt)
 #
 # Licensed under the Apache License, Version 2.0 (the "License");
 # you may not use this file except in compliance with the License.
@@ -11,7 +11,9 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
+
 #
+# Source of this file is https://github.com/StoglRobotics/ros_team_workspace repository.
 #
 # Author: Dr. Denis
 #

templates/robot_description/view_robot.launch.xml

@@ -1,12 +1,29 @@
+<!--
+Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt)
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+
+Source of this file are templates in https://github.com/StoglRobotics/ros_team_workspace repository.
+-->
+
 <launch>
   <arg name="description_package"
        default="$PKG_NAME$"
-       description="Description package of the $ROBOT_NAME$. Usually the argument is not set,
-        it enables use of a custom description."/>
+       description="Description package of the dte_ea_1000. Usually the argument is not set, it enables use of a custom description."/>
   <arg name="prefix"
        default=""
-       description="Prefix of the joint names, useful for multi-robot setup. If changed than also joint
-        names in the controllers' configuration have to be updated."/>
+       description="Prefix of the joint names, useful for multi-robot setup. If changed than also joint names in the controllers' configuration have to be updated."/>
 
   <let name="robot_description_content" value="$(command '$(find-exec xacro) $(find-pkg-share $(var description_package))/urdf/$ROBOT_NAME$.urdf.xacro prefix:=$(var prefix)')" />