From 23335645c3be1faab6453ba1c00f9c6885ae0543 Mon Sep 17 00:00:00 2001 From: Manuel Muth Date: Wed, 21 Feb 2024 13:23:51 +0100 Subject: [PATCH] [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 --- scripts/setup-robot-description.bash | 84 +++++++++++++--- .../robot_description/append_to_README.md | 19 +++- .../test_robot_description.launch.py | 97 ------------------- .../robot_description/test_urdf_xacro.py | 83 ++++++++++++++++ .../robot_description/view_robot.launch.py | 4 +- .../robot_description/view_robot.launch.xml | 25 ++++- 6 files changed, 194 insertions(+), 118 deletions(-) delete mode 100644 templates/robot_description/test_robot_description.launch.py create mode 100644 templates/robot_description/test_urdf_xacro.py diff --git a/scripts/setup-robot-description.bash b/scripts/setup-robot-description.bash index 79a2ab93..428ff6fc 100755 --- a/scripts/setup-robot-description.bash +++ b/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*" package.xml | tail -n 1) +if [ -z "$PREVIOUS_STRING" ]; then + PREVIOUS_STRING="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 + 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*" package.xml | tail -n 1) +if [ -z "$PREVIOUS_STRING" ]; then + PREVIOUS_STRING="xacro" +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="ament_cmake<\/buildtool_depend>" - sed -i "s/$append_to_string/$append_to_string\\n\\n ${DEP_PKG}<\/exec_depend>/g" package.xml + # Check if the previous string starts with + 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 diff --git a/templates/robot_description/append_to_README.md b/templates/robot_description/append_to_README.md index 2c308af1..8e8c06c2 100644 --- a/templates/robot_description/append_to_README.md +++ b/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. diff --git a/templates/robot_description/test_robot_description.launch.py b/templates/robot_description/test_robot_description.launch.py deleted file mode 100644 index 18a46f5a..00000000 --- a/templates/robot_description/test_robot_description.launch.py +++ /dev/null @@ -1,97 +0,0 @@ -# Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -# -# 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. -# -# -# Author: Dr. Denis -# - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - # Declare arguments - declared_arguments = [] - declared_arguments.append( - DeclareLaunchArgument( - "description_package", - default_value="$PKG_NAME$", - description="Description package of the $ROBOT_NAME$. Usually the argument is not set, \ - it enables use of a custom description.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "prefix", - default_value='""', - 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.", - ) - ) - - # Initialize Arguments - description_package = LaunchConfiguration("description_package") - prefix = LaunchConfiguration("prefix") - - # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", "$ROBOT_NAME$.urdf.xacro"] - ), - " ", - "prefix:=", - prefix, - " ", - ] - ) - - robot_description = {"robot_description": robot_description_content} - - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(description_package), "rviz", "$ROBOT_NAME$.rviz"] - ) - - joint_state_publisher_node = Node( - package="joint_state_publisher_gui", - executable="joint_state_publisher_gui", - ) - robot_state_publisher_node = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - ) - - return LaunchDescription( - declared_arguments - + [ - joint_state_publisher_node, - robot_state_publisher_node, - rviz_node, - ] - ) diff --git a/templates/robot_description/test_urdf_xacro.py b/templates/robot_description/test_urdf_xacro.py new file mode 100644 index 00000000..60552c4f --- /dev/null +++ b/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() diff --git a/templates/robot_description/view_robot.launch.py b/templates/robot_description/view_robot.launch.py index 18a46f5a..d8274a87 100755 --- a/templates/robot_description/view_robot.launch.py +++ b/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 # diff --git a/templates/robot_description/view_robot.launch.xml b/templates/robot_description/view_robot.launch.xml index 533fb4fc..d7076547 100644 --- a/templates/robot_description/view_robot.launch.xml +++ b/templates/robot_description/view_robot.launch.xml @@ -1,12 +1,29 @@ + + + description="Description package of the dte_ea_1000. Usually the argument is not set, it enables use of a custom description."/> + 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."/>