From b90f9158ac3cfe6ec72500f2bc740246fd7511d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Darko=20Luki=C4=87?= Date: Tue, 30 Jul 2024 12:56:02 +0200 Subject: [PATCH] Prepare packages for new release (#938) * Add development container * fix executables * fix webots controller * fix error details * fix urdf * fix tiago * fix * add jazzy ci * fix * remove iron support * delete hotfix * test ci * print only stderr * use load_and_initialize_components * fix resourcemanager * fix lint * fix * add more options to the container * add WebotsResourceManager * cleanup * fix multirobot example * format * ci * add changelog * ci * ci * ci * ci --------- Co-authored-by: Olivier Michel --- .github/workflows/test.yml | 2 +- .github/workflows/test_develop.yml | 2 +- docker/Dockerfile | 42 ++++++++++++++ docker/Makefile | 44 ++++++++++++++ docker/README.md | 22 +++++++ docker/bashrc | 8 +++ docker/webots.conf | 17 ++++++ scripts/ci_after_init.bash | 41 ++++++------- scripts/ci_before_init_embed.bash | 3 +- webots_ros2/CHANGELOG.rst | 8 +++ webots_ros2_control/CHANGELOG.rst | 4 ++ webots_ros2_control/CMakeLists.txt | 4 +- webots_ros2_control/src/Ros2Control.cpp | 57 ++++++++++++++++++- webots_ros2_driver/CHANGELOG.rst | 5 ++ webots_ros2_driver/CMakeLists.txt | 8 +-- webots_ros2_driver/src/WebotsNode.cpp | 21 ++++++- .../lib/controller/python/vehicle/car.py | 2 +- .../lib/controller/python/vehicle/driver.py | 2 +- .../webots/resources/version.txt | 2 +- .../webots/src/controller/java/Makefile | 2 +- .../webots/src/controller/java/controller.i | 10 +--- .../webots_ros2_driver/ros2_supervisor.py | 0 .../webots_ros2_driver/urdf_spawner.py | 0 .../webots_ros2_driver/utils.py | 2 - .../wait_for_controller_connection.py | 2 - .../webots_ros2_driver/webots_controller.py | 2 - .../webots_ros2_driver/webots_launcher.py | 2 - webots_ros2_epuck/launch/robot_launch.py | 2 +- .../launch/robot_tools_launch.py | 3 +- webots_ros2_tests/test/test_system_epuck.py | 2 +- .../test/test_system_epuck_with_tools.py | 4 +- webots_ros2_tests/test/test_system_tiago.py | 4 +- .../test/test_system_turtlebot.py | 2 +- ...st_system_turtlebot_tutorial_navigation.py | 2 +- .../test_system_turtlebot_tutorial_slam.py | 2 +- webots_ros2_tiago/launch/robot_launch.py | 5 +- webots_ros2_turtlebot/launch/robot_launch.py | 2 +- webots_ros2_universal_robot/CHANGELOG.rst | 4 ++ .../launch/multirobot_launch.py | 29 +++++++++- 39 files changed, 308 insertions(+), 67 deletions(-) create mode 100644 docker/Dockerfile create mode 100644 docker/Makefile create mode 100644 docker/README.md create mode 100644 docker/bashrc create mode 100644 docker/webots.conf mode change 100644 => 100755 webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py mode change 100644 => 100755 webots_ros2_driver/webots_ros2_driver/urdf_spawner.py diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 2f02bc89e..6cd8b0790 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -16,7 +16,7 @@ jobs: fail-fast: false matrix: ROS_REPO: [main, testing] - ROS_DISTRO: [humble, iron, rolling] + ROS_DISTRO: [humble, jazzy, rolling] runs-on: ubuntu-latest env: AFTER_INIT: ./scripts/ci_after_init.bash ${ROS_DISTRO} ${ROS_REPO} diff --git a/.github/workflows/test_develop.yml b/.github/workflows/test_develop.yml index ca68cb3c4..60e55bf5e 100644 --- a/.github/workflows/test_develop.yml +++ b/.github/workflows/test_develop.yml @@ -14,7 +14,7 @@ jobs: fail-fast: false matrix: ROS_REPO: [main, testing] - ROS_DISTRO: [humble, iron, rolling] + ROS_DISTRO: [humble, jazzy, rolling] runs-on: ubuntu-latest env: AFTER_INIT: ./scripts/ci_after_init.bash ${ROS_DISTRO} ${ROS_REPO} diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 000000000..ea4de44c4 --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,42 @@ +ARG ROS_DISTRO=jazzy + +FROM osrf/ros:${ROS_DISTRO}-desktop + +ARG DEBIAN_FRONTEND=noninteractive +ARG UID=1000 +ARG DOCKERUSER= +ARG DOCKERUSERCOMMENT= +ARG ROS_DISTRO=jazzy +ARG ROS_TESTING=0 +ARG WEBOTS_VERSION=2023b + +RUN userdel -r ubuntu && \ + useradd -d /${DOCKERUSER} -m \ + -u ${UID} -U \ + -s /usr/bin/bash \ + -c "${DOCKERUSERCOMMENT}" ${DOCKERUSER} && \ + echo "${DOCKERUSER} ALL=(ALL) NOPASSWD: ALL" >> /etc/sudoers && \ + usermod -a -G video ${DOCKERUSER} && \ + usermod -a -G dialout ${DOCKERUSER} + +RUN apt update && apt install -y wget && \ + wget https://github.com/cyberbotics/webots/releases/download/R${WEBOTS_VERSION}/webots_${WEBOTS_VERSION}_amd64.deb -O /tmp/webots.deb && \ + apt install -y /tmp/webots.deb && \ + rm /tmp/webots.deb + +RUN [ "$ROS_TESTING" -eq "1" ] && echo "deb [ signed-by=/usr/share/keyrings/ros2-latest-archive-keyring.gpg ] http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros2-latest.list || true + +RUN apt-get update && apt-get install -y \ + ros-${ROS_DISTRO}-vision-msgs \ + ros-${ROS_DISTRO}-ros2-control \ + ros-${ROS_DISTRO}-ros2-controllers \ + ros-${ROS_DISTRO}-xacro \ + gdb + +COPY ./docker/bashrc /tmp/bashrc +COPY ./docker/webots.conf /${DOCKERUSER}/.config/Cyberbotics/Webots-R${WEBOTS_VERSION}.conf +RUN cat /tmp/bashrc >> /${DOCKERUSER}/.bashrc + +ENV USERNAME=default +USER ${DOCKERUSER} +WORKDIR /${DOCKERUSER}/ros2_ws diff --git a/docker/Makefile b/docker/Makefile new file mode 100644 index 000000000..3114b56c5 --- /dev/null +++ b/docker/Makefile @@ -0,0 +1,44 @@ +UID:=$(shell id -u) +ROOT_DIR:=$(shell dirname $(realpath $(firstword $(MAKEFILE_LIST))))/../ +NVIDIA_GPU:=$(shell docker info | grep Runtimes | grep nvidia 1> /dev/null && nvidia-smi 1> /dev/null && echo '--runtime nvidia --gpus all' || echo '') +DOCKERUSER:=cyberbotics +BASENAME:=webots_ros2 +DOCKERUSERCOMMENT:="Cyberbotics" +ROS_DISTRO?=jazzy +ROS_TESTING?=0 +WEBOTS_VERSION?=2023b + +build: + echo "ROS_TESTING=${ROS_TESTING}" + docker build ${ROOT_DIR} \ + -f ${ROOT_DIR}/docker/Dockerfile \ + -t ${BASENAME}-image ${DOCKER_ARGS} \ + --build-arg UID=${UID} \ + --build-arg DOCKERUSER=${DOCKERUSER} \ + --build-arg DOCKERUSERCOMMENT=${DOCKERUSERCOMMENT} \ + --build-arg ROS_TESTING=${ROS_TESTING} \ + --build-arg WEBOTS_VERSION=${WEBOTS_VERSION} \ + --build-arg ROS_DISTRO=${ROS_DISTRO} + +run: + @docker container rm -f ${BASENAME}-container + docker run \ + --net=host \ + --ipc=host \ + --cap-add SYS_ADMIN \ + --name ${BASENAME}-container \ + --privileged \ + --restart unless-stopped \ + -e NVIDIA_DRIVER_CAPABILITIES=all ${NVIDIA_GPU} \ + -e DISPLAY=${DISPLAY} \ + -v ~/.Xauthority:/${DOCKERUSER}/.Xauthority:ro \ + -v /tmp/.X11-unix/:/tmp/.X11-unix:rw \ + -v /dev/dri:/dev/dri:ro \ + -v ${ROOT_DIR}:/${DOCKERUSER}/ros2_ws/src/${BASENAME} \ + -v /dev:/dev:rw \ + --entrypoint bash \ + -d -it ${BASENAME}-image + + +exec: + docker exec -it ${BASENAME}-container bash diff --git a/docker/README.md b/docker/README.md new file mode 100644 index 000000000..5a7bd67df --- /dev/null +++ b/docker/README.md @@ -0,0 +1,22 @@ +# Docker + +Development container for the project. + +## Dependencies + +```bash +sudo apt install git make curl +curl -sSL https://get.docker.com | sh && sudo usermod -aG docker $USER +``` + +## Usage + +You need to build & run the container only the first time: +```bash +ROS_DISTRO=jazzy ROS_TESTING=1 WEBOTS_VERSION=2023b make build run exec +``` + +After that, you can just attach to the container with: +```bash +make exec +``` diff --git a/docker/bashrc b/docker/bashrc new file mode 100644 index 000000000..0d5d7c1df --- /dev/null +++ b/docker/bashrc @@ -0,0 +1,8 @@ +source /opt/ros/${ROS_DISTRO}/setup.bash +source ~/ros2_ws/install/local_setup.bash +alias cb='colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Debug' +alias teleop='ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true' + +# For debugging +export WEBOTS_HOME=/usr/local/webots +export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/cyberbotics/ros2_ws/src/webots_ros2/webots_ros2_driver/webots/lib/controller diff --git a/docker/webots.conf b/docker/webots.conf new file mode 100644 index 000000000..b3eba5e5f --- /dev/null +++ b/docker/webots.conf @@ -0,0 +1,17 @@ +[%General] +checkWebotsUpdateOnStartup=false +pythonCommand=python3 +rendering=true +startupMode=Real-time +telemetry=false +theme=webots_night.qss + +[OpenGL] +GTAO=0 +disableAntiAliasing=true +disableShadows=true +textureFiltering=1 +textureQuality=0 + +[Internal] +firstLaunch=false diff --git a/scripts/ci_after_init.bash b/scripts/ci_after_init.bash index 7c5f00795..b7a578eaf 100755 --- a/scripts/ci_after_init.bash +++ b/scripts/ci_after_init.bash @@ -3,20 +3,24 @@ ROS_DISTRO=$1 ROS_REPO=$2 -# Take the latest nightly build -YESTERDAY_WEEK_DAY_NUMBER=`date --date="1 day ago" +"%u"` -LAST_NIGHTLY_DAY_OLD=1 -# There is no nightly build the weekend -if [ ${YESTERDAY_WEEK_DAY_NUMBER} -gt 5 ]; then - LAST_NIGHTLY_DAY_OLD="$((${YESTERDAY_WEEK_DAY_NUMBER}-4))" +WEBOTS_VERSION="R${WEBOTS_RELEASE_VERSION}" + +if [ "${TEST_WITH_WEBOTS_NIGTHLY}" == "1" ]; then + # Take the latest nightly build + YESTERDAY_WEEK_DAY_NUMBER=`date --date="1 day ago" +"%u"` + LAST_NIGHTLY_DAY_OLD=1 + # There is no nightly build the weekend + if [ ${YESTERDAY_WEEK_DAY_NUMBER} -gt 5 ]; then + LAST_NIGHTLY_DAY_OLD="$((${YESTERDAY_WEEK_DAY_NUMBER}-4))" + fi + NIGHTLY_DATE=`date --date="${LAST_NIGHTLY_DAY_OLD} day ago" +"%-d_%-m_%Y"` + WEBOTS_VERSION="nightly_${NIGHTLY_DATE}" fi -NIGHTLY_DATE=`date --date="${LAST_NIGHTLY_DAY_OLD} day ago" +"%-d_%-m_%Y"` -WEBOTS_NIGHTLY_VERSION="nightly_${NIGHTLY_DATE}" -apt update -apt install -y wget dialog apt-utils psmisc lsb-release git -wget https://github.com/cyberbotics/webots/releases/download/${WEBOTS_NIGHTLY_VERSION}/webots_${WEBOTS_RELEASE_VERSION}_amd64.deb -O /tmp/webots.deb -apt install -y /tmp/webots.deb xvfb +apt update > /dev/null +apt install -y wget dialog apt-utils psmisc lsb-release git > /dev/null +wget https://github.com/cyberbotics/webots/releases/download/${WEBOTS_VERSION}/webots_${WEBOTS_RELEASE_VERSION}_amd64.deb -O /tmp/webots.deb > /dev/null +apt install -y /tmp/webots.deb xvfb > /dev/null # OpenSSL patch for ubuntu 22 if [[ $(lsb_release -rs) == "22.04" && ${WEBOTS_RELEASE_VERSION} == "2022a" ]]; then @@ -44,13 +48,10 @@ fi # Setup Qt plugins for RViz (can be used once RViz does not randomly crash anymore in GitHub CI). #export QT_PLUGIN_PATH=/usr/lib/x86_64-linux-gnu/qt5/plugins -# TODO: Remove once the https://packages.ubuntu.com/noble/python3-flake8 package version is updated. -# Manually upgrade python3-flake8 to 7.0.0 for noble + +# Fixes: +# MESA: error: ZINK: failed to choose pdev +# 2024-07-27T19:23:20.1063344Z [webots-4] glx: failed to create drisw screen if [[ "${ROS_DISTRO}" == "rolling" ]]; then - apt install -y python3-flake8 - wget http://ftp.ubuntu.com/ubuntu/ubuntu/pool/universe/p/pyflakes/python3-pyflakes_3.2.0-1_all.deb -P /tmp - dpkg -i /tmp/python3-pyflakes_3.2.0-1_all.deb - wget http://ftp.ubuntu.com/ubuntu/ubuntu/pool/universe/p/python-flake8/python3-flake8_7.0.0-1_all.deb -P /tmp - dpkg -i /tmp/python3-flake8_7.0.0-1_all.deb - apt --fix-broken install -y + apt-get install -y libqt5quickcontrols2-5 qtquickcontrols2-5-dev fi diff --git a/scripts/ci_before_init_embed.bash b/scripts/ci_before_init_embed.bash index dbcc2563f..d3c511dec 100755 --- a/scripts/ci_before_init_embed.bash +++ b/scripts/ci_before_init_embed.bash @@ -2,7 +2,8 @@ ROS_DISTRO=$1 -export WEBOTS_RELEASE_VERSION=2023b-rev1 +export TEST_WITH_WEBOTS_NIGTHLY=0 +export WEBOTS_RELEASE_VERSION=2023b export WEBOTS_OFFSCREEN=1 export CI=1 export DEBIAN_FRONTEND=noninteractive diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst index 61145ba9d..6f9da25a3 100644 --- a/webots_ros2/CHANGELOG.rst +++ b/webots_ros2/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package webots_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.1.3 (2024-XX-XX) +------------------ +* Added support for ROS 2 Jazzy. +* Make webots_ros2_driver scripts executable. +* Fixed passing the `robot_description` parameter to ros2_control. +* Fixed the produced URDF to also contain joint limits necessary for ros2_control. +* Added support for the new `ros2_control` API affecting resource_manager and controller_manager. + 2023.1.2 (2024-04-08) ------------------ * Fixed errors showing up when launching moveit for ur5e. diff --git a/webots_ros2_control/CHANGELOG.rst b/webots_ros2_control/CHANGELOG.rst index 42b93121e..c591533a2 100644 --- a/webots_ros2_control/CHANGELOG.rst +++ b/webots_ros2_control/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.1.3 (2024-XX-XX) +------------------ +* Added support for the new `ros2_control` API affecting resource_manager and controller_manager. + 2023.1.0 (2023-06-29) ------------------ * Fixed component activation. diff --git a/webots_ros2_control/CMakeLists.txt b/webots_ros2_control/CMakeLists.txt index ec4986a34..6765df2af 100644 --- a/webots_ros2_control/CMakeLists.txt +++ b/webots_ros2_control/CMakeLists.txt @@ -4,8 +4,8 @@ project(webots_ros2_control) # Check which ROS distribution is used, ros2control depends of that if($ENV{ROS_DISTRO} MATCHES "humble") add_compile_definitions(HUMBLE) -elseif($ENV{ROS_DISTRO} MATCHES "iron") - add_compile_definitions(IRON) +elseif($ENV{ROS_DISTRO} MATCHES "jazzy") + add_compile_definitions(JAZZY) elseif($ENV{ROS_DISTRO} MATCHES "rolling") add_compile_definitions(ROLLING) endif() diff --git a/webots_ros2_control/src/Ros2Control.cpp b/webots_ros2_control/src/Ros2Control.cpp index 94694a5fe..d73d3bdc1 100644 --- a/webots_ros2_control/src/Ros2Control.cpp +++ b/webots_ros2_control/src/Ros2Control.cpp @@ -36,6 +36,53 @@ const double CONTROLLER_MANAGER_ALLOWED_SAMPLE_ERROR_MS = 1.0; namespace webots_ros2_control { +#if HARDWARE_INTERFACE_VERSION_MAJOR >= 4 && HARDWARE_INTERFACE_VERSION_MINOR >= 12 + class WebotsResourceManager : public hardware_interface::ResourceManager { + public: + WebotsResourceManager(webots_ros2_driver::WebotsNode *node) : + hardware_interface::ResourceManager(node->get_node_clock_interface(), node->get_node_logging_interface()), + mHardwareLoader("webots_ros2_control", "webots_ros2_control::Ros2ControlSystemInterface"), + mLogger(node->get_logger().get_child("WebotsResourceManager")) { + mNode = node; + } + + WebotsResourceManager(const WebotsResourceManager &) = delete; + + bool load_and_initialize_components(const std::string &urdf, unsigned int update_rate) override { + components_are_loaded_and_initialized_ = true; + + std::vector controlHardware; + try { + controlHardware = hardware_interface::parse_control_resources_from_urdf(urdf); + } catch (const std::runtime_error &ex) { + throw std::runtime_error("URDF cannot be parsed by a `ros2_control` component parser: " + std::string(ex.what())); + } + for (unsigned int i = 0; i < controlHardware.size(); i++) { + const std::string pluginName = controlHardware[i].hardware_plugin_name; + + std::unique_ptr webotsSystem; + try { + webotsSystem = std::unique_ptr( + mHardwareLoader.createUnmanagedInstance(pluginName)); + } catch (pluginlib::PluginlibException &ex) { + RCLCPP_ERROR(mLogger, "The plugin failed to load for some reason. Error: %s\n", ex.what()); + continue; + } + + webotsSystem->init(mNode, controlHardware[i]); + import_component(std::move(webotsSystem), controlHardware[i]); + } + + return components_are_loaded_and_initialized_; + } + + private: + webots_ros2_driver::WebotsNode *mNode; + pluginlib::ClassLoader mHardwareLoader; + rclcpp::Logger mLogger; + }; +#endif + Ros2Control::Ros2Control() { mNode = NULL; } @@ -62,10 +109,14 @@ namespace webots_ros2_control { mHardwareLoader.reset(new pluginlib::ClassLoader( "webots_ros2_control", "webots_ros2_control::Ros2ControlSystemInterface")); } catch (pluginlib::LibraryLoadException &ex) { - throw std::runtime_error("Hardware loader cannot be created: " + atoi(ex.what())); + throw std::runtime_error("Hardware loader cannot be created: " + std::string(ex.what())); } // Control Hardware +#if HARDWARE_INTERFACE_VERSION_MAJOR >= 4 && HARDWARE_INTERFACE_VERSION_MINOR >= 12 + std::unique_ptr resourceManager = + std::make_unique(node); +#else std::string urdfString; std::vector controlHardware; std::unique_ptr resourceManager = @@ -74,7 +125,7 @@ namespace webots_ros2_control { urdfString = mNode->urdf(); controlHardware = hardware_interface::parse_control_resources_from_urdf(urdfString); } catch (const std::runtime_error &ex) { - throw std::runtime_error("URDF cannot be parsed by a `ros2_control` component parser: " + atoi(ex.what())); + throw std::runtime_error("URDF cannot be parsed by a `ros2_control` component parser: " + std::string(ex.what())); } for (unsigned int i = 0; i < controlHardware.size(); i++) { // Necessary hotfix for renamed variables present in "hardware_interface" package for versions above 3.5 (#590) @@ -94,8 +145,10 @@ namespace webots_ros2_control { using lifecycle_msgs::msg::State; rclcpp_lifecycle::State active_state(State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); resourceManager->set_component_state(controlHardware[i].name, active_state); + resourceManager->load_urdf(urdfString, false, false); } +#endif // Controller Manager mExecutor = std::make_shared(); diff --git a/webots_ros2_driver/CHANGELOG.rst b/webots_ros2_driver/CHANGELOG.rst index 473ba67b6..4e5e563de 100644 --- a/webots_ros2_driver/CHANGELOG.rst +++ b/webots_ros2_driver/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package webots_ros2_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.1.3 (2024-XX-XX) +------------------ +* Make webots_ros2_driver scripts executable. +* Fixed the produced URDF to also contain joint limits necessary for ros2_control. + 2023.1.1 (2023-07-11) ------------------ * Fixed RangeFinder activation to also check for point cloud subscriptions. diff --git a/webots_ros2_driver/CMakeLists.txt b/webots_ros2_driver/CMakeLists.txt index 801385bfc..8856bcb90 100644 --- a/webots_ros2_driver/CMakeLists.txt +++ b/webots_ros2_driver/CMakeLists.txt @@ -8,8 +8,8 @@ set(CMAKE_BUILD_WITH_INSTALL_RPATH ON) # Check which ROS distribution is used, vision_msgs depends of that if($ENV{ROS_DISTRO} MATCHES "humble") add_compile_definitions(HUMBLE) -elseif($ENV{ROS_DISTRO} MATCHES "iron") - add_compile_definitions(IRON) +elseif($ENV{ROS_DISTRO} MATCHES "jazzy") + add_compile_definitions(JAZZY) elseif($ENV{ROS_DISTRO} MATCHES "rolling") add_compile_definitions(ROLLING) endif() @@ -32,8 +32,8 @@ find_package(yaml-cpp REQUIRED) if($ENV{ROS_DISTRO} MATCHES "humble") find_package(Python 3.10 EXACT REQUIRED COMPONENTS Development) -elseif($ENV{ROS_DISTRO} MATCHES "iron") - find_package(Python 3.10 EXACT REQUIRED COMPONENTS Development) +elseif($ENV{ROS_DISTRO} MATCHES "jazzy") + find_package(Python 3.12 EXACT REQUIRED COMPONENTS Development) elseif($ENV{ROS_DISTRO} MATCHES "rolling") find_package(Python 3.12 EXACT REQUIRED COMPONENTS Development) endif() diff --git a/webots_ros2_driver/src/WebotsNode.cpp b/webots_ros2_driver/src/WebotsNode.cpp index 5126fa74f..9a66ad4a2 100644 --- a/webots_ros2_driver/src/WebotsNode.cpp +++ b/webots_ros2_driver/src/WebotsNode.cpp @@ -183,7 +183,26 @@ namespace webots_ros2_driver { std::string prefix = ""; std::string webotsUrdf = wb_robot_get_urdf(prefix.c_str()); replaceUrdfNames(webotsUrdf); - setAnotherNodeParameter("robot_state_publisher", "robot_description", webotsUrdf); + + // Add and other tags to the Webots generated URDF + const std::vector tags = {"ros2_control", "webots"}; + std::shared_ptr webotsUrdfModelDocument = std::make_shared(); + webotsUrdfModelDocument->Parse(webotsUrdf.c_str()); + tinyxml2::XMLElement *webotsRobotXMLElement = webotsUrdfModelDocument->FirstChildElement("robot"); + tinyxml2::XMLElement *robotXMLElement = mRobotDescriptionDocument->FirstChildElement("robot"); + if (!robotXMLElement) + throw std::runtime_error("Invalid URDF, it doesn't contain a tag"); + + for (tinyxml2::XMLElement *child = robotXMLElement->FirstChildElement(); child; child = child->NextSiblingElement()) + if (std::find(tags.begin(), tags.end(), child->Name()) != tags.end()) + webotsRobotXMLElement->InsertEndChild(child->DeepClone(webotsUrdfModelDocument.get())); + + std::string expandedUrdf = ""; + tinyxml2::XMLPrinter printer; + webotsUrdfModelDocument->Accept(&printer); + expandedUrdf = printer.CStr(); + + setAnotherNodeParameter("robot_state_publisher", "robot_description", expandedUrdf); } mStep = wb_robot_get_basic_time_step(); diff --git a/webots_ros2_driver/webots/lib/controller/python/vehicle/car.py b/webots_ros2_driver/webots/lib/controller/python/vehicle/car.py index 5eb9c3974..f7dae7339 100644 --- a/webots_ros2_driver/webots/lib/controller/python/vehicle/car.py +++ b/webots_ros2_driver/webots/lib/controller/python/vehicle/car.py @@ -41,7 +41,7 @@ def __init__(self): elif sys.platform == 'win32': path = os.path.join('lib', 'controller', 'car.dll') elif sys.platform == 'darwin': - path = os.path.join('Contents', 'lib', 'controller', 'libcar.dylib') + path = os.path.join('Contents', 'MacOS', 'lib', 'controller', 'libcar.dylib') self.api = ctypes.cdll.LoadLibrary(os.path.join(os.environ['WEBOTS_HOME'], path)) self.api.wbu_car_get_front_wheel_radius.restype = ctypes.c_double self.api.wbu_car_get_indicator_period.restype = ctypes.c_double diff --git a/webots_ros2_driver/webots/lib/controller/python/vehicle/driver.py b/webots_ros2_driver/webots/lib/controller/python/vehicle/driver.py index 04677cafd..e2e7b2eb5 100644 --- a/webots_ros2_driver/webots/lib/controller/python/vehicle/driver.py +++ b/webots_ros2_driver/webots/lib/controller/python/vehicle/driver.py @@ -43,7 +43,7 @@ def loadApi() -> ctypes.cdll: car = 'car.dll' driver = 'driver.dll' elif sys.platform == 'darwin': - path = os.path.join('Contents', 'lib', 'controller') + path = os.path.join('Contents', 'MacOS', 'lib', 'controller') car = 'libcar.dylib' driver = 'libdriver.dylib' ctypes.cdll.LoadLibrary(os.path.join(os.environ['WEBOTS_HOME'], path, car)) diff --git a/webots_ros2_driver/webots/resources/version.txt b/webots_ros2_driver/webots/resources/version.txt index 51fc7befd..0b9133bdf 100644 --- a/webots_ros2_driver/webots/resources/version.txt +++ b/webots_ros2_driver/webots/resources/version.txt @@ -1 +1 @@ -R2024a \ No newline at end of file +R2023b revision 1 diff --git a/webots_ros2_driver/webots/src/controller/java/Makefile b/webots_ros2_driver/webots/src/controller/java/Makefile index 8f1057def..5c433b67b 100644 --- a/webots_ros2_driver/webots/src/controller/java/Makefile +++ b/webots_ros2_driver/webots/src/controller/java/Makefile @@ -113,7 +113,7 @@ CFLAGS1 = -isysroot $(MACOSX_SDK_PATH) -mmacosx-version-min=$(MACO PLATFORM_INCLUDE = -I"$(MACOSX_SDK_PATH)/System/Library/Frameworks/JavaVM.framework/Versions/Current/Headers" TARGET = $(WEBOTS_CONTROLLER_LIB_PATH)/java/lib$(LIBNAME).jnilib CFLAGS2 = -c -Wall -isysroot $(MACOSX_SDK_PATH) -mmacosx-version-min=$(MACOSX_MIN_SDK_VERSION) -Wno-unused-function -JNILIB_FLAGS = -dynamiclib -install_name @rpath/Contents/lib/controller/java/libJavaController.jnilib -Wl,-rpath,@loader_path/../../../.. +JNILIB_FLAGS = -dynamiclib -install_name @rpath/Contents/lib/controller/java/libJavaController.jnilib -Wl,-rpath,@loader_path/../../.. JAVA_INCLUDES = -I"$(JAVA_HOME)/include" -I"$(JAVA_HOME)/include/darwin" LIBCONTROLLER = $(WEBOTS_CONTROLLER_LIB_PATH)/libController.dylib LIBCPPCONTROLLER = $(WEBOTS_CONTROLLER_LIB_PATH)/libCppController.dylib diff --git a/webots_ros2_driver/webots/src/controller/java/controller.i b/webots_ros2_driver/webots/src/controller/java/controller.i index e62517837..fda1946e3 100644 --- a/webots_ros2_driver/webots/src/controller/java/controller.i +++ b/webots_ros2_driver/webots/src/controller/java/controller.i @@ -83,17 +83,13 @@ using namespace std; $result = SWIG_JavaArrayOutDouble(jenv, $1, 6); else if (test == "getOrientation" || test == "virtualRealityHeadsetGetOrientation") $result = SWIG_JavaArrayOutDouble(jenv, $1, 9); - else if (test == "getPose") - $result = SWIG_JavaArrayOutDouble(jenv, $1, 16); - else if (test != "colors" && test != "getLookupTable") + else if (test != "getLookupTable") $result = SWIG_JavaArrayOutDouble(jenv, $1, 3); + else if (test != "getPose") + $result = SWIG_JavaArrayOutDouble(jenv, $1, 16); } %apply double[] {double *}; -%typemap(out) double *colors { - $result = SWIG_JavaArrayOutDouble(jenv, (double *) $1, arg1->number_of_colors*3); -} - %typemap(out) const double *getLookupTable { $result = SWIG_JavaArrayOutDouble(jenv, (double *) $1, arg1->getLookupTableSize()*3); } diff --git a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py old mode 100644 new mode 100755 diff --git a/webots_ros2_driver/webots_ros2_driver/urdf_spawner.py b/webots_ros2_driver/webots_ros2_driver/urdf_spawner.py old mode 100644 new mode 100755 diff --git a/webots_ros2_driver/webots_ros2_driver/utils.py b/webots_ros2_driver/webots_ros2_driver/utils.py index 8a91ea0fb..1b8f2e439 100644 --- a/webots_ros2_driver/webots_ros2_driver/utils.py +++ b/webots_ros2_driver/webots_ros2_driver/utils.py @@ -1,5 +1,3 @@ -#!/usr/bin/env python - # Copyright 1996-2023 Cyberbotics Ltd. # # Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/webots_ros2_driver/webots_ros2_driver/wait_for_controller_connection.py b/webots_ros2_driver/webots_ros2_driver/wait_for_controller_connection.py index 1f57bb87f..20570a826 100644 --- a/webots_ros2_driver/webots_ros2_driver/wait_for_controller_connection.py +++ b/webots_ros2_driver/webots_ros2_driver/wait_for_controller_connection.py @@ -1,5 +1,3 @@ -#!/usr/bin/env python - # Copyright 1996-2023 Cyberbotics Ltd. # # Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/webots_ros2_driver/webots_ros2_driver/webots_controller.py b/webots_ros2_driver/webots_ros2_driver/webots_controller.py index 45dc698ba..c21ac76d0 100644 --- a/webots_ros2_driver/webots_ros2_driver/webots_controller.py +++ b/webots_ros2_driver/webots_ros2_driver/webots_controller.py @@ -1,5 +1,3 @@ -#!/usr/bin/env python - # Copyright 1996-2023 Cyberbotics Ltd. # # Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/webots_ros2_driver/webots_ros2_driver/webots_launcher.py b/webots_ros2_driver/webots_ros2_driver/webots_launcher.py index dfd9729a9..39529066f 100644 --- a/webots_ros2_driver/webots_ros2_driver/webots_launcher.py +++ b/webots_ros2_driver/webots_ros2_driver/webots_launcher.py @@ -1,5 +1,3 @@ -#!/usr/bin/env python - # Copyright 1996-2023 Cyberbotics Ltd. # # Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/webots_ros2_epuck/launch/robot_launch.py b/webots_ros2_epuck/launch/robot_launch.py index 2eff20ecd..be30eec4b 100644 --- a/webots_ros2_epuck/launch/robot_launch.py +++ b/webots_ros2_epuck/launch/robot_launch.py @@ -88,7 +88,7 @@ def generate_launch_description(): robot_description_path = os.path.join(package_dir, 'resource', 'epuck_webots.urdf') ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml') - use_twist_stamped = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling' + use_twist_stamped = 'ROS_DISTRO' in os.environ and (os.environ['ROS_DISTRO'] in ['rolling', 'jazzy']) if use_twist_stamped: mappings = [('/diffdrive_controller/cmd_vel', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] else: diff --git a/webots_ros2_epuck/launch/robot_tools_launch.py b/webots_ros2_epuck/launch/robot_tools_launch.py index f6eb15fb4..501783907 100644 --- a/webots_ros2_epuck/launch/robot_tools_launch.py +++ b/webots_ros2_epuck/launch/robot_tools_launch.py @@ -51,8 +51,7 @@ def generate_launch_description(): ) # Navigation - nav2_params_file = 'nav2_params_iron.yaml' if ('ROS_DISTRO' in os.environ - and os.environ['ROS_DISTRO'] == 'iron') else 'nav2_params.yaml' + nav2_params_file = 'nav2_params.yaml' if 'nav2_bringup' in get_packages_with_prefixes(): launch_description_nodes.append( IncludeLaunchDescription( diff --git a/webots_ros2_tests/test/test_system_epuck.py b/webots_ros2_tests/test/test_system_epuck.py index b5ab6ae24..e451f998b 100644 --- a/webots_ros2_tests/test/test_system_epuck.py +++ b/webots_ros2_tests/test/test_system_epuck.py @@ -89,7 +89,7 @@ def on_range_message_received(message): self.wait_for_messages(self.__node, Range, '/tof', condition=on_range_message_received) def testMovement(self): - use_twist_stamped = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling' + use_twist_stamped = 'ROS_DISTRO' in os.environ and (os.environ['ROS_DISTRO'] in ['rolling', 'jazzy']) if use_twist_stamped: publisher = self.__node.create_publisher(TwistStamped, '/cmd_vel', 1) diff --git a/webots_ros2_tests/test/test_system_epuck_with_tools.py b/webots_ros2_tests/test/test_system_epuck_with_tools.py index ba899e9d3..d6ae5c83e 100644 --- a/webots_ros2_tests/test/test_system_epuck_with_tools.py +++ b/webots_ros2_tests/test/test_system_epuck_with_tools.py @@ -35,8 +35,8 @@ def generate_test_description(): initialize_webots_test() # If ROS_DISTRO is rolling, skip the test as some required packages are missing (cf. ci_after_init.bash) - if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling': - pytest.skip('ROS_DISTRO is rolling, skipping this test') + if 'ROS_DISTRO' in os.environ and (os.environ['ROS_DISTRO'] in ['rolling', 'jazzy']): + pytest.skip('ROS_DISTRO is rolling or jazzy, skipping this test') epuck_with_tools_webots = IncludeLaunchDescription( PythonLaunchDescriptionSource( diff --git a/webots_ros2_tests/test/test_system_tiago.py b/webots_ros2_tests/test/test_system_tiago.py index 6af26a5ce..f2442718f 100644 --- a/webots_ros2_tests/test/test_system_tiago.py +++ b/webots_ros2_tests/test/test_system_tiago.py @@ -35,8 +35,8 @@ def generate_test_description(): initialize_webots_test() # If ROS_DISTRO is rolling, skip the test as some required packages are missing (cf. ci_after_init.bash) - if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling': - pytest.skip('ROS_DISTRO is rolling, skipping this test') + if 'ROS_DISTRO' in os.environ and (os.environ['ROS_DISTRO'] in ['rolling', 'jazzy']): + pytest.skip('ROS_DISTRO is rolling or jazzy, skipping this test') tiago_webots = IncludeLaunchDescription( PythonLaunchDescriptionSource( diff --git a/webots_ros2_tests/test/test_system_turtlebot.py b/webots_ros2_tests/test/test_system_turtlebot.py index 549b07c91..22d454f5a 100644 --- a/webots_ros2_tests/test/test_system_turtlebot.py +++ b/webots_ros2_tests/test/test_system_turtlebot.py @@ -61,7 +61,7 @@ def setUp(self): self.wait_for_clock(self.__node, messages_to_receive=20) def testMovement(self): - use_twist_stamped = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling' + use_twist_stamped = 'ROS_DISTRO' in os.environ and (os.environ['ROS_DISTRO'] in ['rolling', 'jazzy']) if use_twist_stamped: publisher = self.__node.create_publisher(TwistStamped, '/cmd_vel', 1) diff --git a/webots_ros2_tests/test/test_system_turtlebot_tutorial_navigation.py b/webots_ros2_tests/test/test_system_turtlebot_tutorial_navigation.py index 746f51d87..fd03901fc 100644 --- a/webots_ros2_tests/test/test_system_turtlebot_tutorial_navigation.py +++ b/webots_ros2_tests/test/test_system_turtlebot_tutorial_navigation.py @@ -39,7 +39,7 @@ def generate_test_description(): # If ROS_DISTRO is rolling, skip the test as some required packages are missing (cf. ci_after_init.bash) # If ROS_DISTRO is iron, skip the test as the Navigation packages are not yet available. if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] != 'humble': - pytest.skip('ROS_DISTRO is rolling or iron, skipping this test') + pytest.skip('ROS_DISTRO is rolling or jazzy, skipping this test') # Webots turtlebot_webots = IncludeLaunchDescription( diff --git a/webots_ros2_tests/test/test_system_turtlebot_tutorial_slam.py b/webots_ros2_tests/test/test_system_turtlebot_tutorial_slam.py index effd2d67d..85d70918d 100644 --- a/webots_ros2_tests/test/test_system_turtlebot_tutorial_slam.py +++ b/webots_ros2_tests/test/test_system_turtlebot_tutorial_slam.py @@ -36,7 +36,7 @@ def generate_test_description(): # If ROS_DISTRO is rolling, skip the test as some required packages are missing (cf. ci_after_init.bash) # If ROS_DISTRO is iron, skip the test as the Navigation packages are not yet available. if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] != 'humble': - pytest.skip('ROS_DISTRO is rolling or iron, skipping this test') + pytest.skip('ROS_DISTRO is rolling or jazzy, skipping this test') # Webots turtlebot_webots = IncludeLaunchDescription( diff --git a/webots_ros2_tiago/launch/robot_launch.py b/webots_ros2_tiago/launch/robot_launch.py index 20f84b8b5..c2f248637 100644 --- a/webots_ros2_tiago/launch/robot_launch.py +++ b/webots_ros2_tiago/launch/robot_launch.py @@ -84,7 +84,7 @@ def generate_launch_description(): robot_description_path = os.path.join(package_dir, 'resource', 'tiago_webots.urdf') ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml') - use_twist_stamped = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling' + use_twist_stamped = 'ROS_DISTRO' in os.environ and (os.environ['ROS_DISTRO'] in ['rolling', 'jazzy']) if use_twist_stamped: mappings = [('/diffdrive_controller/cmd_vel', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] else: @@ -114,8 +114,7 @@ def generate_launch_description(): # Navigation navigation_nodes = [] - nav2_params_file = 'nav2_params_iron.yaml' if ('ROS_DISTRO' in os.environ - and os.environ['ROS_DISTRO'] == 'iron') else 'nav2_params.yaml' + nav2_params_file = 'nav2_params.yaml' nav2_params = os.path.join(package_dir, 'resource', nav2_params_file) nav2_map = os.path.join(package_dir, 'resource', 'map.yaml') if 'nav2_bringup' in get_packages_with_prefixes(): diff --git a/webots_ros2_turtlebot/launch/robot_launch.py b/webots_ros2_turtlebot/launch/robot_launch.py index 8b75e44ae..c374766f2 100644 --- a/webots_ros2_turtlebot/launch/robot_launch.py +++ b/webots_ros2_turtlebot/launch/robot_launch.py @@ -82,7 +82,7 @@ def generate_launch_description(): robot_description_path = os.path.join(package_dir, 'resource', 'turtlebot_webots.urdf') ros2_control_params = os.path.join(package_dir, 'resource', 'ros2control.yml') - use_twist_stamped = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'rolling' + use_twist_stamped = 'ROS_DISTRO' in os.environ and (os.environ['ROS_DISTRO'] in ['rolling', 'jazzy']) if use_twist_stamped: mappings = [('/diffdrive_controller/cmd_vel', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')] else: diff --git a/webots_ros2_universal_robot/CHANGELOG.rst b/webots_ros2_universal_robot/CHANGELOG.rst index 0f1c1c91f..b9ec0fd51 100644 --- a/webots_ros2_universal_robot/CHANGELOG.rst +++ b/webots_ros2_universal_robot/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_universal_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.1.3 (2024-XX-XX) +------------------ +* Fixed passing the `robot_description` parameter to ros2_control. + 2023.1.2 (2023-07-28) ------------------ * Fixed errors showing up when launching moveit for ur5e. diff --git a/webots_ros2_universal_robot/launch/multirobot_launch.py b/webots_ros2_universal_robot/launch/multirobot_launch.py index 6f33fccc8..fa1d78edc 100644 --- a/webots_ros2_universal_robot/launch/multirobot_launch.py +++ b/webots_ros2_universal_robot/launch/multirobot_launch.py @@ -99,6 +99,26 @@ def get_ros2_nodes(*args): output='screen' ) + ur5e_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[{ + 'robot_description': '' + }], + namespace='ur5e' + ) + + abb_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[{ + 'robot_description': '' + }], + namespace='abb' + ) + return [ # Request to spawn the URDF robot spawn_URDF_ur5e, @@ -109,7 +129,12 @@ def get_ros2_nodes(*args): event_handler=launch.event_handlers.OnProcessIO( target_action=spawn_URDF_ur5e, on_stdout=lambda event: get_webots_driver_node( - event, [ur5e_controller, abb_controller] + ur5e_spawners + abb_spawners + event, [ + ur5e_controller, + abb_controller, + abb_robot_state_publisher, + ur5e_robot_state_publisher + ] + ur5e_spawners + abb_spawners ), ) ), @@ -137,6 +162,7 @@ def generate_launch_description(): {'robot_description': ur5e_xacro_path}, {'xacro_mappings': ['name:=UR5eWithGripper']}, {'use_sim_time': True}, + {'set_robot_state_publisher': True}, ur5e_control_params ], respawn=True @@ -151,6 +177,7 @@ def generate_launch_description(): parameters=[ {'robot_description': abb_description_path}, {'use_sim_time': True}, + {'set_robot_state_publisher': True}, abb_control_params ], respawn=True