[Description] Optimize code generation and enable generation of URDF tests per default

scripts/setup-robot-description.bash

@@ -19,8 +19,9 @@
 usage="setup-robot-description ROBOT_NAME"
 
 # Load Framework defines
-script_own_dir="$( cd "$( dirname "${BASH_SOURCE[0]}" )" > /dev/null && pwd )"
+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
@@ -34,7 +35,7 @@ fi
 PKG_NAME="$(grep -Po '(?<=<name>).*?(?=</name>)' package.xml | sed -e 's/[[:space:]]//g')"
 
 echo ""
-echo -e "${TERMINAL_COLOR_USER_NOTICE}ATTENTION: Setting up description configuration for robot '$ROBOT_NAME' in package '$PKG_NAME' in folder '`pwd`'.${TERMINAL_COLOR_NC}"
+echo -e "${TERMINAL_COLOR_USER_NOTICE}ATTENTION: Setting up description configuration for robot '$ROBOT_NAME' in package '$PKG_NAME' in folder '$(pwd)'.${TERMINAL_COLOR_NC}"
 echo -e "${TERMINAL_COLOR_USER_CONFIRMATION}If correct press <ENTER>, otherwise <CTRL>+C and start the script again from the package folder and/or with correct robot name.${TERMINAL_COLOR_NC}"
 read
 
@@ -74,42 +75,90 @@ mkdir -p rviz
 ROBOT_RVIZ="rviz/${ROBOT_NAME}.rviz"
 cp -n "$ROBOT_DESCRIPTION_TEMPLATES/robot.rviz" $ROBOT_RVIZ
 
-# sed all needed files
-FILES_TO_SED=($ROBOT_URDF_XACRO $ROBOT_MACRO $ROBOT_MACRO_ROS2_CONTROL $VIEW_ROBOT_LAUNCH)
+# 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 $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 dependencies if they not exist
-DEP_PKGS=("xacro" "rviz2" "robot_state_publisher" "joint_state_publisher_gui")
+# 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  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
+  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
 fi
 
 #TODO: Set license
 
-git add .
-git commit -m "RosTeamWS: Description files for $ROBOT_NAME generated."
-
 # Compile and add new package the to the path
 compile_and_source_package $PKG_NAME
 

templates/robot_description/test_urdf_xacro.py

@@ -0,0 +1,79 @@
+# 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.
+#
+# Author: Lukas Sackewitz
+#
+# This file was taken from https://github.com/UniversalRobots/Universal_Robots_ROS2_Description
+
+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()