From f60aca9a78d34576f1d60d380e1d8ebdc4b1b522 Mon Sep 17 00:00:00 2001 From: ad-daniel <44834743+ad-daniel@users.noreply.github.com> Date: Fri, 9 Sep 2022 12:37:23 +0200 Subject: [PATCH] Merge `master` into `develop` (#430) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix position + velocity control in ros2_control (#415) * Fix position + velocity control in ros2_control * Fix camera FoV * fix * Remove timer * Add acceleration * change camera info qos * fix rate * fix * Fix laser publisher (#425) * Allow custom ros2_control hardware_interface (#426) * Fix motor without sensor (#428) * Fix * Fix * Fix merge * Fix * include string (#434) Signed-off-by: Kenji Brameld * CallbackReturn should include namespae in its signature (#435) Signed-off-by: Kenji Brameld Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> * use new bbox msg signature (#433) * Handle new bbox msg signature from humble onwards Signed-off-by: Kenji Brameld Co-authored-by: Olivier Michel * Fix pep257 D407 Missing dashed underline after section (#437) * fix pep257 D407 Missing dashed underline after section Signed-off-by: Kenji Brameld Co-authored-by: Olivier Michel * bump python version to 3.10 for rolling (#436) * bump python version to 3.10 for rolling * use different python versions depending on distros * update submodule to most recent R2022a Signed-off-by: Kenji Brameld Co-authored-by: Olivier Michel * Change call to spawner.py executable to spawner (#438) * rename spawner.py executable to spawner Signed-off-by: Kenji Brameld * use spawner.py if on foxy Signed-off-by: Kenji Brameld * Sync (#445) * prepare version 1.2.3 (#402) * prepare version 1.2.3 * update libcontroller * install manually libpython3-dev * test python 3.9 rolling * removed install lib-python-dev * typo * update libcontroller * Upgrade webots_ros2_importer * fix std string * flake 8 fix importer * lidar new value * changelog importer * Fixes * Fix for foxy, galactic * Added GALACTIC definition * Update CHANGELOG.rst * Update CHANGELOG.rst * Update CHANGELOG.rst * Updated changelog * Added humble * skip broken tests * Fixed tests * fix * Update ci_after_init.bash * Update ci_after_init.bash * Fix build * Fix compilation * simplified * Possible fix * Fix * Synch urdf2webots to latest * Fixed a compilation warning with python 3.10 * Apply openssl patch * Fix path * Add debug message * debug * Fix * Fix * Debug * Hardcode * Increase timeout * Rolling fix * Revert "Rolling fix" This reverts commit ec39e10621a6b6a3e414b80ec75548d3954c2535. * Test * Revert * Fixed wrong condition * Fix * Rework * Attempt to fix testing * Missing case * Rework * Fix * Fix * Update changelogs * Upgrade urdf2webots to 1.0.18 * Review fix Co-authored-by: Olivier Michel Co-authored-by: ad-daniel Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> * Fix build from repo (#452) * Fix build from repo * Fix * Fix changelog * Add nightly tests (#453) * Add nightly tests * Fix humble after sync * Make develop tests only nightly * Sync * Fix test * Test urdf2webots branch * Adapt to urdf2webots changes * revert urdf * Sync urdf2webots * Test * Fix style * update libcontroller * Add declaration to worlds (#456) * update urls (#457) * Fix master nightly (#459) * Fix master nightly * Add R * Revert nightly URL change * Sync libcontroller Signed-off-by: Kenji Brameld Co-authored-by: Darko Lukić Co-authored-by: Kenji Brameld Co-authored-by: Olivier Michel Co-authored-by: Benjamin Hug <61198898+BenjaminHug8@users.noreply.github.com> --- .github/workflows/test.yml | 8 +- .github/workflows/test_develop.yml | 34 ++++ scripts/ci_after_init.bash | 17 +- webots_ros2/CHANGELOG.rst | 4 + webots_ros2/package.xml | 2 +- webots_ros2/setup.py | 2 +- webots_ros2_control/CHANGELOG.rst | 4 + webots_ros2_control/CMakeLists.txt | 13 +- .../webots_ros2_control/Ros2Control.hpp | 4 +- .../webots_ros2_control/Ros2ControlSystem.hpp | 13 +- webots_ros2_control/package.xml | 2 +- webots_ros2_control/src/Ros2Control.cpp | 32 +++- webots_ros2_control/src/Ros2ControlSystem.cpp | 127 +++++++++------ webots_ros2_core/package.xml | 2 +- webots_ros2_core/setup.py | 2 +- .../webots_ros2_core/devices/camera_device.py | 33 ++-- webots_ros2_driver/CHANGELOG.rst | 6 + webots_ros2_driver/CMakeLists.txt | 26 +++- .../webots_ros2_driver/PluginInterface.hpp | 1 + .../webots_ros2_driver/PythonPlugin.hpp | 1 + .../include/webots_ros2_driver/WebotsNode.hpp | 2 +- webots_ros2_driver/package.xml | 4 +- webots_ros2_driver/src/Driver.cpp | 7 +- webots_ros2_driver/src/WebotsNode.cpp | 15 +- .../src/plugins/static/Ros2Camera.cpp | 26 +++- .../src/plugins/static/Ros2Lidar.cpp | 6 +- .../src/plugins/static/Ros2RangeFinder.cpp | 10 +- webots_ros2_driver/webots | 2 +- webots_ros2_epuck/CHANGELOG.rst | 4 + webots_ros2_epuck/launch/robot_launch.py | 14 +- webots_ros2_epuck/package.xml | 2 +- webots_ros2_epuck/setup.py | 2 +- webots_ros2_epuck/worlds/epuck_world.wbt | 12 +- .../worlds/rats_life_benchmark.wbt | 12 +- webots_ros2_importer/CHANGELOG.rst | 4 + webots_ros2_importer/package.xml | 2 +- webots_ros2_importer/setup.py | 2 +- .../webots_ros2_importer/urdf2proto.py | 105 +++++-------- .../webots_ros2_importer/urdf2webots | 2 +- .../webots_ros2_importer/xacro2proto.py | 9 +- webots_ros2_mavic/package.xml | 2 +- webots_ros2_mavic/setup.py | 2 +- webots_ros2_mavic/worlds/mavic_world.wbt | 20 ++- webots_ros2_msgs/package.xml | 2 +- webots_ros2_tesla/CHANGELOG.rst | 4 + webots_ros2_tesla/package.xml | 2 +- webots_ros2_tesla/setup.py | 2 +- .../webots_ros2_tesla/lane_follower.py | 4 +- webots_ros2_tesla/worlds/tesla_world.wbt | 146 ++++++++++++------ webots_ros2_tests/package.xml | 2 +- webots_ros2_tests/setup.cfg | 4 +- webots_ros2_tests/setup.py | 2 +- webots_ros2_tests/test/test_system_tiago.py | 7 +- ...st_system_turtlebot_tutorial_navigation.py | 7 +- .../test_system_turtlebot_tutorial_slam.py | 7 +- .../test/test_system_universal_robot.py | 2 +- webots_ros2_tests/webots_ros2_tests/utils.py | 4 +- webots_ros2_tests/worlds/driver_test.wbt | 14 +- webots_ros2_tiago/launch/robot_launch.py | 14 +- webots_ros2_tiago/package.xml | 2 +- webots_ros2_tiago/setup.py | 2 +- webots_ros2_tiago/worlds/default.wbt | 28 +++- webots_ros2_turtlebot/CHANGELOG.rst | 4 + webots_ros2_turtlebot/launch/robot_launch.py | 14 +- webots_ros2_turtlebot/package.xml | 2 +- webots_ros2_turtlebot/setup.py | 2 +- .../worlds/turtlebot3_burger_example.wbt | 25 ++- .../launch/multirobot_launch.py | 9 +- .../launch/robot_launch/robot_nodes_launch.py | 6 +- webots_ros2_universal_robot/package.xml | 2 +- webots_ros2_universal_robot/setup.py | 2 +- .../worlds/robotic_arms.wbt | 69 +++++++-- .../worlds/universal_robot.wbt | 9 +- 73 files changed, 677 insertions(+), 321 deletions(-) create mode 100644 .github/workflows/test_develop.yml diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index e389bb4ea..523dec2ce 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -1,8 +1,10 @@ -name: CI +name: ROS2 tests on: pull_request: types: [opened, synchronize, reopened, labeled, unlabeled] + schedule: + - cron: '0 23 * * *' jobs: industrial_ci: @@ -10,14 +12,14 @@ jobs: fail-fast: false matrix: ROS_REPO: [main, testing] - ROS_DISTRO: [foxy, galactic, rolling] + ROS_DISTRO: [foxy, galactic, humble, rolling] runs-on: ubuntu-latest env: AFTER_INIT: ./scripts/ci_after_init.bash ${ROS_DISTRO} ${ROS_REPO} BEFORE_INIT_EMBED: source scripts/ci_before_init_embed.bash DOCKER_RUN_OPTS: -v /artifacts:/tmp/artifacts steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v3 with: submodules: recursive - uses: 'ros-industrial/industrial_ci@master' diff --git a/.github/workflows/test_develop.yml b/.github/workflows/test_develop.yml new file mode 100644 index 000000000..5461017af --- /dev/null +++ b/.github/workflows/test_develop.yml @@ -0,0 +1,34 @@ +name: ROS2 tests (develop) + +on: + schedule: + - cron: '0 23 * * *' + +jobs: + industrial_ci: + strategy: + fail-fast: false + matrix: + ROS_REPO: [main, testing] + ROS_DISTRO: [foxy, galactic, humble, rolling] + runs-on: ubuntu-latest + env: + AFTER_INIT: ./scripts/ci_after_init.bash ${ROS_DISTRO} ${ROS_REPO} + BEFORE_INIT_EMBED: source scripts/ci_before_init_embed.bash + DOCKER_RUN_OPTS: -v /artifacts:/tmp/artifacts + steps: + - uses: actions/checkout@v3 + with: + submodules: recursive + ref: develop + - uses: 'ros-industrial/industrial_ci@master' + env: + ROS_REPO: ${{matrix.ROS_REPO}} + ROS_DISTRO: ${{matrix.ROS_DISTRO}} + - name: Upload Artifact + if: ${{ failure() }} + uses: actions/upload-artifact@v2 + with: + name: generic_artifacts_develop_${{matrix.ROS_REPO}}_${{matrix.ROS_DISTRO}} + path: /artifacts + retention-days: 5 diff --git a/scripts/ci_after_init.bash b/scripts/ci_after_init.bash index 121be0ea6..523f9d808 100755 --- a/scripts/ci_after_init.bash +++ b/scripts/ci_after_init.bash @@ -14,13 +14,22 @@ 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 +apt install -y wget dialog apt-utils psmisc lsb-release 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 -# The following packages are only available in the ROS 2 Foxy distribution. Therefore, we cannot include them in the package.xml, but we have to install them manually here. -if [ "${ROS_DISTRO}" = "foxy" ]; then - apt install -y ros-foxy-turtlebot3-cartographer ros-foxy-turtlebot3-navigation2 +# OpenSSL patch for ubuntu 22 +if [[ $(lsb_release -rs) == "22.04" && ${WEBOTS_RELEASE_VERSION} == "2022a" ]]; then + echo applying openssl patch + wget https://cyberbotics.com/files/repository/dependencies/linux64/release/libssl_1.1.tar.xz -O /tmp/libssl_1.1.tar.xz + tar xvf /tmp/libssl_1.1.tar.xz -C /tmp + mv /tmp/openssl-1.1/* /usr/local/webots/lib/webots/ +fi + +# The following packages are only available in the ROS 2 Foxy/Galactic distributions. Therefore, we cannot include them in the package.xml, but we have to install them manually here. + +if [[ "${ROS_DISTRO}" == "foxy" || "${ROS_DISTRO}" == "galactic" ]]; then + apt install -y ros-${ROS_DISTRO}-turtlebot3-cartographer ros-${ROS_DISTRO}-turtlebot3-navigation2 fi # Setup Qt plugins for RViz (can be used once RViz does not randomly crash anymore in GitHub CI). diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst index c088daf50..02a6cf9b6 100644 --- a/webots_ros2/CHANGELOG.rst +++ b/webots_ros2/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.2.3 (2022-06-01) +------------------ +* Fixed support for Humble and Rolling. + 1.1.2 (2021-11-03) ------------------ * Adapted the 'webots_ros2_driver' package to be also a python alternative to the 'webots_ros2_core' package. diff --git a/webots_ros2/package.xml b/webots_ros2/package.xml index 0c4002f56..dab324628 100644 --- a/webots_ros2/package.xml +++ b/webots_ros2/package.xml @@ -2,7 +2,7 @@ webots_ros2 - 1.2.2 + 1.2.3 Interface between Webots and ROS2 Cyberbotics diff --git a/webots_ros2/setup.py b/webots_ros2/setup.py index e0b41ee61..6b5fe4330 100644 --- a/webots_ros2/setup.py +++ b/webots_ros2/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version='1.2.2', + version='1.2.3', packages=[package_name], data_files=[ ('share/' + package_name, ['package.xml']), diff --git a/webots_ros2_control/CHANGELOG.rst b/webots_ros2_control/CHANGELOG.rst index 96a17efc5..606300736 100644 --- a/webots_ros2_control/CHANGELOG.rst +++ b/webots_ros2_control/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.2.3 (2022-05-30) +------------------ +* Fixed support for Humble and Rolling. + 1.2.0 (2021-12-21) ------------------ * Fix the controller_manager update rate. diff --git a/webots_ros2_control/CMakeLists.txt b/webots_ros2_control/CMakeLists.txt index f87fae491..80b632310 100644 --- a/webots_ros2_control/CMakeLists.txt +++ b/webots_ros2_control/CMakeLists.txt @@ -4,9 +4,12 @@ project(webots_ros2_control) # Check which ROS distribution is used, ros2control depends of that if($ENV{ROS_DISTRO} MATCHES "foxy") add_compile_definitions(FOXY) -endif() -if($ENV{ROS_DISTRO} MATCHES "galactic") +elseif($ENV{ROS_DISTRO} MATCHES "galactic") add_compile_definitions(GALACTIC) +elseif($ENV{ROS_DISTRO} MATCHES "humble") + add_compile_definitions(HUMBLE) +elseif($ENV{ROS_DISTRO} MATCHES "rolling") + add_compile_definitions(ROLLING) endif() # Default to C99 @@ -92,15 +95,19 @@ install(TARGETS LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) +install( + DIRECTORY include/ + DESTINATION include +) ament_export_include_directories( include ) - ament_export_dependencies( hardware_interface pluginlib rclcpp rclcpp_lifecycle + webots_ros2_driver ) ament_export_libraries( ${PROJECT_NAME} diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2Control.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2Control.hpp index eae25164d..a6bbce8ba 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2Control.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2Control.hpp @@ -47,8 +47,8 @@ namespace webots_ros2_control webots_ros2_driver::WebotsNode *mNode; std::shared_ptr> mHardwareLoader; std::shared_ptr mControllerManager; - double mControlPeriodMs; - double mLastControlUpdateMs; + int mControlPeriodMs; + int mLastControlUpdateMs; std::thread mThreadExecutor; rclcpp::executors::MultiThreadedExecutor::SharedPtr mExecutor; diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp index 285325eb1..68872601c 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp @@ -44,7 +44,7 @@ namespace webots_ros2_control double velocityCommand; double velocity; double effortCommand; - double effort; + double acceleration; bool controlPosition; bool controlVelocity; bool controlEffort; @@ -63,15 +63,20 @@ namespace webots_ros2_control hardware_interface::return_type start() override; hardware_interface::return_type stop() override; #else - CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) override; #endif std::vector export_state_interfaces() override; std::vector export_command_interfaces() override; +#if FOXY || GALACTIC hardware_interface::return_type read() override; hardware_interface::return_type write() override; +#else // HUMBLE, ROLLING + hardware_interface::return_type read(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) override; + hardware_interface::return_type write(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) override; +#endif private: webots_ros2_driver::WebotsNode *mNode; diff --git a/webots_ros2_control/package.xml b/webots_ros2_control/package.xml index fb634e7b4..6a7b32c90 100644 --- a/webots_ros2_control/package.xml +++ b/webots_ros2_control/package.xml @@ -2,7 +2,7 @@ webots_ros2_control - 1.2.2 + 1.2.3 ros2_control plugin for Webots Cyberbotics http://wiki.ros.org/webots_ros2 diff --git a/webots_ros2_control/src/Ros2Control.cpp b/webots_ros2_control/src/Ros2Control.cpp index 37ba64913..9b98b21b2 100644 --- a/webots_ros2_control/src/Ros2Control.cpp +++ b/webots_ros2_control/src/Ros2Control.cpp @@ -35,19 +35,30 @@ namespace webots_ros2_control void Ros2Control::step() { - const double nowMs = mNode->get_clock()->now().seconds() * 1000.0; - if (nowMs >= mLastControlUpdateMs + mControlPeriodMs) + const int nowMs = mNode->robot()->getTime() * 1000.0; + const int periodMs = nowMs - mLastControlUpdateMs; + const rclcpp::Duration dt = rclcpp::Duration::from_seconds(mControlPeriodMs / 1000.0); + if (periodMs >= mControlPeriodMs) { +#if FOXY || GALACTIC mControllerManager->read(); +#else + mControllerManager->read(mNode->get_clock()->now(), dt); +#endif + #if FOXY mControllerManager->update(); #else - rclcpp::Duration dt = rclcpp::Duration::from_nanoseconds(RCL_MS_TO_NS(mNode->robot()->getBasicTimeStep())); mControllerManager->update(mNode->get_clock()->now(), dt); -#endif mLastControlUpdateMs = nowMs; - } +#endif + +#if FOXY || GALACTIC mControllerManager->write(); +#else // HUMBLE, ROLLING + mControllerManager->write(mNode->get_clock()->now(), dt); +#endif + } } void Ros2Control::init(webots_ros2_driver::WebotsNode *node, std::unordered_map &) @@ -91,6 +102,10 @@ namespace webots_ros2_control #else resourceManager->import_component(std::move(webotsSystem), controlHardware[i]); #endif + +#if HUMBLE || ROLLING + resourceManager->activate_all_components(); +#endif } // Controller Manager @@ -100,7 +115,12 @@ namespace webots_ros2_control // Update rate const int updateRate = mControllerManager->get_parameter("update_rate").as_int(); mControlPeriodMs = (1.0 / updateRate) * 1000.0; - if (abs(mControlPeriodMs - mNode->robot()->getBasicTimeStep()) > CONTROLLER_MANAGER_ALLOWED_SAMPLE_ERROR_MS) + + int controlPeriodProductMs = mNode->robot()->getBasicTimeStep(); + while (controlPeriodProductMs < mControlPeriodMs) + controlPeriodProductMs += mNode->robot()->getBasicTimeStep(); + + if (abs(controlPeriodProductMs - mControlPeriodMs) > CONTROLLER_MANAGER_ALLOWED_SAMPLE_ERROR_MS) RCLCPP_WARN_STREAM(node->get_logger(), "Desired controller update period (" << mControlPeriodMs << "ms / " << updateRate << "Hz) is different from the Webots timestep (" << mNode->robot()->getBasicTimeStep() << "ms). Please adjust the `update_rate` parameter in the `controller_manager` or the `basicTimeStep` parameter in the Webots `WorldInfo` node."); // Spin diff --git a/webots_ros2_control/src/Ros2ControlSystem.cpp b/webots_ros2_control/src/Ros2ControlSystem.cpp index e8c84c6de..5b0400534 100644 --- a/webots_ros2_control/src/Ros2ControlSystem.cpp +++ b/webots_ros2_control/src/Ros2ControlSystem.cpp @@ -43,62 +43,69 @@ namespace webots_ros2_control if (!joint.sensor && !joint.motor) throw std::runtime_error("Cannot find a Motor or PositionSensor with name " + joint.name); + // Initialize the state joint.controlPosition = false; joint.controlVelocity = false; joint.controlEffort = false; joint.positionCommand = NAN; joint.velocityCommand = NAN; joint.effortCommand = NAN; + joint.position = NAN; + joint.velocity = NAN; + joint.acceleration = NAN; + + // Configure the command interface for (hardware_interface::InterfaceInfo commandInterface : component.command_interfaces) { if (commandInterface.name == "position") joint.controlPosition = true; else if (commandInterface.name == "velocity") - { joint.controlVelocity = true; - if (joint.motor) - { - joint.motor->setPosition(INFINITY); - joint.motor->setVelocity(0.0); - } - } else if (commandInterface.name == "effort") joint.controlEffort = true; else throw std::runtime_error("Invalid hardware info name `" + commandInterface.name + "`"); } + if (joint.motor && joint.controlVelocity && !joint.controlPosition) + { + joint.motor->setPosition(INFINITY); + joint.motor->setVelocity(0.0); + } mJoints.push_back(joint); } } - #if FOXY - hardware_interface::return_type Ros2ControlSystem::configure(const hardware_interface::HardwareInfo &info) +#if FOXY + hardware_interface::return_type Ros2ControlSystem::configure(const hardware_interface::HardwareInfo &info) + { + if (configure_default(info) != hardware_interface::return_type::OK) { - if (configure_default(info) != hardware_interface::return_type::OK) - { - return hardware_interface::return_type::ERROR; - } - status_ = hardware_interface::status::CONFIGURED; - return hardware_interface::return_type::OK; + return hardware_interface::return_type::ERROR; } - #else - CallbackReturn Ros2ControlSystem::on_init(const hardware_interface::HardwareInfo &info) + status_ = hardware_interface::status::CONFIGURED; + return hardware_interface::return_type::OK; + } +#else + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2ControlSystem::on_init(const hardware_interface::HardwareInfo &info) + { + if (hardware_interface::SystemInterface::on_init(info) != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) { - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) - { - return CallbackReturn::ERROR; - } - return CallbackReturn::SUCCESS; + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - #endif + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } +#endif std::vector Ros2ControlSystem::export_state_interfaces() { std::vector interfaces; for (Joint &joint : mJoints) - if (joint.sensor) + if (joint.sensor) { interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &(joint.position))); + interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_VELOCITY, &(joint.velocity))); + interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_ACCELERATION, &(joint.acceleration))); + } return interfaces; } @@ -119,42 +126,62 @@ namespace webots_ros2_control return interfaces; } - #if FOXY - hardware_interface::return_type Ros2ControlSystem::start() - { - status_ = hardware_interface::status::STARTED; - return hardware_interface::return_type::OK; - } +#if FOXY + hardware_interface::return_type Ros2ControlSystem::start() + { + status_ = hardware_interface::status::STARTED; + return hardware_interface::return_type::OK; + } - hardware_interface::return_type Ros2ControlSystem::stop() - { - status_ = hardware_interface::status::STOPPED; - return hardware_interface::return_type::OK; - } - #else - CallbackReturn Ros2ControlSystem::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) - { - return CallbackReturn::SUCCESS; - } + hardware_interface::return_type Ros2ControlSystem::stop() + { + status_ = hardware_interface::status::STOPPED; + return hardware_interface::return_type::OK; + } +#else + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2ControlSystem::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) + { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } - CallbackReturn Ros2ControlSystem::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) - { - return CallbackReturn::SUCCESS; - } - #endif + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2ControlSystem::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) + { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } +#endif +#if FOXY || GALACTIC hardware_interface::return_type Ros2ControlSystem::read() +#else // HUMBLE, ROLLING + hardware_interface::return_type Ros2ControlSystem::read(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) +#endif { + static double lastReadTime = 0; + + const double deltaTime = mNode->robot()->getTime() - lastReadTime; + lastReadTime = mNode->robot()->getTime(); + for (Joint &joint : mJoints) { - if (joint.sensor) - joint.position = joint.sensor->getValue(); + if (joint.sensor) { + const double position = joint.sensor->getValue(); + const double velocity = std::isnan(joint.position) ? NAN : (position - joint.position) / deltaTime; + + if (!std::isnan(joint.velocity)) + joint.acceleration = (joint.velocity - velocity) / deltaTime; + joint.velocity = velocity; + joint.position = position; + } } return hardware_interface::return_type::OK; } +#if FOXY || GALACTIC hardware_interface::return_type Ros2ControlSystem::write() +#else // HUMBLE, ROLLING + hardware_interface::return_type Ros2ControlSystem::write(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) +#endif { for (Joint &joint : mJoints) { @@ -163,7 +190,11 @@ namespace webots_ros2_control if (joint.controlPosition && !std::isnan(joint.positionCommand)) joint.motor->setPosition(joint.positionCommand); if (joint.controlVelocity && !std::isnan(joint.velocityCommand)) - joint.motor->setVelocity(joint.velocityCommand); + { + // In the position control mode the velocity cannot be negative. + const double velocityCommand = joint.controlPosition ? abs(joint.velocityCommand) : joint.velocityCommand; + joint.motor->setVelocity(velocityCommand); + } if (joint.controlEffort && !std::isnan(joint.effortCommand)) joint.motor->setTorque(joint.effortCommand); } diff --git a/webots_ros2_core/package.xml b/webots_ros2_core/package.xml index 528f559b0..c3fef0666 100644 --- a/webots_ros2_core/package.xml +++ b/webots_ros2_core/package.xml @@ -2,7 +2,7 @@ webots_ros2_core - 1.2.2 + 1.2.3 Core interface between Webots and ROS2 Cyberbotics diff --git a/webots_ros2_core/setup.py b/webots_ros2_core/setup.py index f6f6d42a2..b3e019ebe 100644 --- a/webots_ros2_core/setup.py +++ b/webots_ros2_core/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version='1.2.2', + version='1.2.3', packages=[package_name, package_name + '.devices', package_name + '.math', package_name + '.webots'], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_core/webots_ros2_core/devices/camera_device.py b/webots_ros2_core/webots_ros2_core/devices/camera_device.py index c84aaf6f1..96b756dce 100644 --- a/webots_ros2_core/webots_ros2_core/devices/camera_device.py +++ b/webots_ros2_core/webots_ros2_core/devices/camera_device.py @@ -14,6 +14,7 @@ """Camera device.""" +import os from sensor_msgs.msg import Image, CameraInfo from vision_msgs.msg import Detection2D, Detection2DArray, ObjectHypothesisWithPose from geometry_msgs.msg import Point, Quaternion @@ -35,13 +36,17 @@ class CameraDevice(SensorDevice): - Publishes raw image of type `sensor_msgs/Image` - Publishes intrinsic camera parameters of type `sensor_msgs/CameraInfo` (latched topic) - Args: - node (WebotsNode): The ROS2 node. - device_key (str): Unique identifier of the device used for configuration. - wb_device (Camera): Webots node of type Camera. - Kwargs: - params (dict): Inherited from `SensorDevice` + Parameters + ---------- + node: WebotsNode + The ROS2 node. + device_key: str + Unique identifier of the device used for configuration. + wb_device: Camera + Webots node of type Camera. + params: dict + Inherited from `SensorDevice` """ @@ -188,8 +193,12 @@ def step(self): hyp.pose.pose.position = position hyp.pose.pose.orientation = orientation reco_obj.results.append(hyp) - reco_obj.bbox.center.x = obj_center[0] - reco_obj.bbox.center.y = obj_center[1] + if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] in ('foxy', 'galactic'): + reco_obj.bbox.center.x = obj_center[0] + reco_obj.bbox.center.y = obj_center[1] + else: + reco_obj.bbox.center.position.x = obj_center[0] + reco_obj.bbox.center.position.y = obj_center[1] reco_obj.bbox.size_x = obj_size[0] reco_obj.bbox.size_y = obj_size[1] reco_msg.detections.append(reco_obj) @@ -200,8 +209,12 @@ def step(self): reco_webots_obj.model = obj_model reco_webots_obj.pose.pose.position = position reco_webots_obj.pose.pose.orientation = orientation - reco_webots_obj.bbox.center.x = obj_center[0] - reco_webots_obj.bbox.center.y = obj_center[1] + if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] in ('foxy', 'galactic'): + reco_webots_obj.bbox.center.x = obj_center[0] + reco_webots_obj.bbox.center.y = obj_center[1] + else: + reco_webots_obj.bbox.center.position.x = obj_center[0] + reco_webots_obj.bbox.center.position.y = obj_center[1] reco_webots_obj.bbox.size_x = obj_size[0] reco_webots_obj.bbox.size_y = obj_size[1] for i in range(0, obj.get_number_of_colors()): diff --git a/webots_ros2_driver/CHANGELOG.rst b/webots_ros2_driver/CHANGELOG.rst index 8393bf8f6..e9bc0bcd9 100644 --- a/webots_ros2_driver/CHANGELOG.rst +++ b/webots_ros2_driver/CHANGELOG.rst @@ -6,6 +6,12 @@ Changelog for package webots_ros2_driver ------------------ * Added an URDF importer feature to spawn robots from URDF files. +1.2.3 (2022-05-30) +------------------ +* Add option to set 'robot_description' parameter for 'robot_state_publisher' node. +* Fix recognition camera. +* Add a 'PointCloud2' publisher for the 'RangeFinder' device. + 1.2.2 (2022-01-19) ------------------ * Fix the Supervisor API access from plugins. diff --git a/webots_ros2_driver/CMakeLists.txt b/webots_ros2_driver/CMakeLists.txt index 6cec32349..6cff856a3 100644 --- a/webots_ros2_driver/CMakeLists.txt +++ b/webots_ros2_driver/CMakeLists.txt @@ -12,6 +12,17 @@ endif() set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) +# Check which ROS distribution is used, vision_msgs depends of that +if($ENV{ROS_DISTRO} MATCHES "foxy") + add_compile_definitions(FOXY) +elseif($ENV{ROS_DISTRO} MATCHES "galactic") + add_compile_definitions(GALACTIC) +elseif($ENV{ROS_DISTRO} MATCHES "humble") + add_compile_definitions(HUMBLE) +elseif($ENV{ROS_DISTRO} MATCHES "rolling") + add_compile_definitions(ROLLING) +endif() + # ROS2 Packages find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) @@ -25,7 +36,11 @@ find_package(vision_msgs REQUIRED) find_package(webots_ros2_msgs REQUIRED) find_package(tinyxml2_vendor REQUIRED) find_package(TinyXML2 REQUIRED) -find_package(PythonLibs 3.8 EXACT REQUIRED) +if($ENV{ROS_DISTRO} MATCHES "foxy" OR $ENV{ROS_DISTRO} MATCHES "galactic") + find_package(PythonLibs 3.8 EXACT REQUIRED) +else() + find_package(PythonLibs 3.10 EXACT REQUIRED) +endif() if (UNIX AND NOT APPLE) set(WEBOTS_LIB_BASE webots/lib/linux-gnu) @@ -85,8 +100,13 @@ else() ) endif() -ament_python_install_package(${PROJECT_NAME}_webots - PACKAGE_DIR ${WEBOTS_LIB_BASE}/python38) +if($ENV{ROS_DISTRO} MATCHES "foxy" OR $ENV{ROS_DISTRO} MATCHES "galactic") + ament_python_install_package(${PROJECT_NAME}_webots + PACKAGE_DIR ${WEBOTS_LIB_BASE}/python38) +else() + ament_python_install_package(${PROJECT_NAME}_webots + PACKAGE_DIR ${WEBOTS_LIB_BASE}/python310) +endif() ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR ${PROJECT_NAME}) diff --git a/webots_ros2_driver/include/webots_ros2_driver/PluginInterface.hpp b/webots_ros2_driver/include/webots_ros2_driver/PluginInterface.hpp index 5cc030fa6..55c3f92fc 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/PluginInterface.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/PluginInterface.hpp @@ -16,6 +16,7 @@ #define PLUGIN_INTERFACE #include +#include namespace webots_ros2_driver { diff --git a/webots_ros2_driver/include/webots_ros2_driver/PythonPlugin.hpp b/webots_ros2_driver/include/webots_ros2_driver/PythonPlugin.hpp index 59468d969..9b24a92ee 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/PythonPlugin.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/PythonPlugin.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include diff --git a/webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp b/webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp index f5703bc89..d246fef88 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp @@ -40,12 +40,12 @@ namespace webots_ros2_driver public: WebotsNode(std::string name, webots::Supervisor *robot); void init(); + int step(); webots::Supervisor *robot() { return mRobot; } std::string urdf() const { return mRobotDescription; }; static void handleSignals(); private: - void timerCallback(); std::unordered_map getDeviceRosProperties(const std::string &name) const; std::unordered_map getPluginProperties(tinyxml2::XMLElement *pluginElement) const; void setAnotherNodeParameter(std::string anotherNodeName, std::string parameterName, std::string parameterValue); diff --git a/webots_ros2_driver/package.xml b/webots_ros2_driver/package.xml index 67ae75a7b..23236d831 100644 --- a/webots_ros2_driver/package.xml +++ b/webots_ros2_driver/package.xml @@ -1,7 +1,7 @@ webots_ros2_driver - 1.2.2 + 1.2.3 Implementation of the Webots - ROS 2 interface Cyberbotics Apache License 2.0 @@ -9,6 +9,8 @@ https://github.com/cyberbotics/webots_ros2/issues https://github.com/cyberbotics/webots_ros2 + ros_environment + ament_cmake python_cmake_module ament_cmake_python diff --git a/webots_ros2_driver/src/Driver.cpp b/webots_ros2_driver/src/Driver.cpp index b9982757b..0cc9d771d 100644 --- a/webots_ros2_driver/src/Driver.cpp +++ b/webots_ros2_driver/src/Driver.cpp @@ -44,8 +44,11 @@ int main(int argc, char **argv) std::shared_ptr node = std::make_shared(robotName, robot); node->init(); - - rclcpp::spin(node); + while (true) { + if (node->step() == -1) + break; + rclcpp::spin_some(node); + } delete robot; rclcpp::shutdown(); return 0; diff --git a/webots_ros2_driver/src/WebotsNode.cpp b/webots_ros2_driver/src/WebotsNode.cpp index 834d5a01d..b1a610815 100644 --- a/webots_ros2_driver/src/WebotsNode.cpp +++ b/webots_ros2_driver/src/WebotsNode.cpp @@ -125,7 +125,6 @@ namespace webots_ros2_driver setAnotherNodeParameter("robot_state_publisher", "robot_description", mRobot->getUrdf()); mStep = mRobot->getBasicTimeStep(); - mTimer = this->create_wall_timer(std::chrono::milliseconds(1), std::bind(&WebotsNode::timerCallback, this)); // Load static plugins // Static plugins are automatically configured based on the robot model. @@ -222,7 +221,7 @@ namespace webots_ros2_driver return plugin; } - void WebotsNode::timerCallback() + int WebotsNode::step() { if (gShutdownSignalReceived && !mWaitingForUrdfRobotToBeRemoved) { @@ -230,14 +229,14 @@ namespace webots_ros2_driver mRemoveUrdfRobotPublisher->publish(mRemoveUrdfRobotMessage); mWaitingForUrdfRobotToBeRemoved = true; } - if (mRobot->step(mStep) == -1) - { - mTimer->cancel(); - exit(0); - return; - } + + const int result = mRobot->step(mStep); + if (result == -1) + return result; for (std::shared_ptr plugin : mPlugins) plugin->step(); + + return result; } void WebotsNode::setAnotherNodeParameter(std::string anotherNodeName, std::string parameterName, std::string parameterValue) diff --git a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp index a34ea922d..2719ec402 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp @@ -36,17 +36,18 @@ namespace webots_ros2_driver mImageMessage.encoding = sensor_msgs::image_encodings::BGRA8; // CameraInfo publisher - rclcpp::QoS cameraInfoQos(1); - cameraInfoQos.reliable(); - cameraInfoQos.transient_local(); - cameraInfoQos.keep_last(1); - mCameraInfoPublisher = mNode->create_publisher(mTopicName + "/camera_info", cameraInfoQos); + mCameraInfoPublisher = mNode->create_publisher(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable()); mCameraInfoMessage.header.stamp = mNode->get_clock()->now(); mCameraInfoMessage.header.frame_id = mFrameName; mCameraInfoMessage.height = mCamera->getHeight(); mCameraInfoMessage.width = mCamera->getWidth(); mCameraInfoMessage.distortion_model = "plumb_bob"; - const double focalLength = (mCamera->getFocalLength() == 0) ? 570.34 : mCamera->getFocalLength(); + + // Convert FoV to focal length. + // Reference: https://en.wikipedia.org/wiki/Focal_length#In_photography + const double diagonal = sqrt(pow(mCamera->getWidth(), 2) + pow(mCamera->getHeight(), 2)); + const double focalLength = 0.5 * diagonal * (cos(0.5 * mCamera->getFov()) / sin(0.5 * mCamera->getFov())); + mCameraInfoMessage.d = {0.0, 0.0, 0.0, 0.0, 0.0}; mCameraInfoMessage.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; mCameraInfoMessage.k = { @@ -57,7 +58,6 @@ namespace webots_ros2_driver focalLength, 0.0, (double)mCamera->getWidth() / 2, 0.0, 0.0, focalLength, (double)mCamera->getHeight() / 2, 0.0, 0.0, 0.0, 1.0, 0.0}; - mCameraInfoPublisher->publish(mCameraInfoMessage); // Recognition publisher if (mCamera->hasRecognition()) @@ -109,6 +109,8 @@ namespace webots_ros2_driver publishImage(); if (recognitionSubscriptionsExist) publishRecognition(); + if (mCameraInfoPublisher->get_subscription_count() > 0) + mCameraInfoPublisher->publish(mCameraInfoMessage); } void Ros2Camera::publishImage() @@ -149,8 +151,13 @@ namespace webots_ros2_driver hypothesis.pose.pose.position = position; hypothesis.pose.pose.orientation = orientation; detection.results.push_back(hypothesis); + #if FOXY || GALACTIC detection.bbox.center.x = objects[i].position_on_image[0]; detection.bbox.center.y = objects[i].position_on_image[1]; + #else + detection.bbox.center.position.x = objects[i].position_on_image[0]; + detection.bbox.center.position.y = objects[i].position_on_image[1]; + #endif detection.bbox.size_x = objects[i].size_on_image[0]; detection.bbox.size_y = objects[i].size_on_image[1]; mRecognitionMessage.detections.push_back(detection); @@ -161,8 +168,13 @@ namespace webots_ros2_driver recognitionWebotsObject.model = std::string(objects[i].model); recognitionWebotsObject.pose.pose.position = position; recognitionWebotsObject.pose.pose.orientation = orientation; + #if FOXY || GALACTIC recognitionWebotsObject.bbox.center.x = objects[i].position_on_image[0]; recognitionWebotsObject.bbox.center.y = objects[i].position_on_image[1]; + #else + recognitionWebotsObject.bbox.center.position.x = objects[i].position_on_image[0]; + recognitionWebotsObject.bbox.center.position.y = objects[i].position_on_image[1]; + #endif recognitionWebotsObject.bbox.size_x = objects[i].size_on_image[0]; recognitionWebotsObject.bbox.size_y = objects[i].size_on_image[1]; for (size_t j = 0; j < objects[i].number_of_colors; j++) diff --git a/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp b/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp index 38697448f..363802c87 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp @@ -34,8 +34,8 @@ namespace webots_ros2_driver mLaserPublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); const int resolution = mLidar->getHorizontalResolution(); mLaserMessage.header.frame_id = mFrameName; - mLaserMessage.angle_increment = -mLidar->getFov() / resolution; - mLaserMessage.angle_min = mLidar->getFov() / 2.0 - mLaserMessage.angle_increment; + mLaserMessage.angle_increment = -mLidar->getFov() / (resolution - 1); + mLaserMessage.angle_min = mLidar->getFov() / 2.0; mLaserMessage.angle_max = -mLidar->getFov() / 2.0; mLaserMessage.time_increment = (double)mLidar->getSamplingPeriod() / (1000.0 * resolution); mLaserMessage.scan_time = (double)mLidar->getSamplingPeriod() / 1000.0; @@ -86,7 +86,7 @@ namespace webots_ros2_driver if (mAlwaysOn) return; - + const bool shouldPointCloudBeEnabled = mPointCloudPublisher->get_subscription_count() > 0; const bool shouldSensorBeEnabled = shouldPointCloudBeEnabled || (mLaserPublisher != nullptr && mLaserPublisher->get_subscription_count() > 0); diff --git a/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp b/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp index 2e00b2531..dc5755f0d 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp @@ -41,11 +41,7 @@ namespace webots_ros2_driver mImageMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; // CameraInfo publisher - rclcpp::QoS cameraInfoQos(1); - cameraInfoQos.reliable(); - cameraInfoQos.transient_local(); - cameraInfoQos.keep_last(1); - mCameraInfoPublisher = mNode->create_publisher(mTopicName + "/camera_info", cameraInfoQos); + mCameraInfoPublisher = mNode->create_publisher(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable()); mCameraInfoMessage.header.stamp = mNode->get_clock()->now(); mCameraInfoMessage.header.frame_id = mFrameName; mCameraInfoMessage.height = height; @@ -63,7 +59,6 @@ namespace webots_ros2_driver focalLengthX, 0.0, (double)width / 2, 0.0, 0.0, focalLengthY, (double)height / 2, 0.0, 0.0, 0.0, 1.0, 0.0}; - mCameraInfoPublisher->publish(mCameraInfoMessage); // Point cloud publisher mPointCloudPublisher = mNode->create_publisher(mTopicName + "/point_cloud", rclcpp::SensorDataQoS().reliable()); @@ -104,6 +99,9 @@ namespace webots_ros2_driver publishPointCloud(); } + if (mCameraInfoPublisher->get_subscription_count() > 0) + mCameraInfoPublisher->publish(mCameraInfoMessage); + if (mAlwaysOn) return; diff --git a/webots_ros2_driver/webots b/webots_ros2_driver/webots index 25457232b..2c9b0e069 160000 --- a/webots_ros2_driver/webots +++ b/webots_ros2_driver/webots @@ -1 +1 @@ -Subproject commit 25457232bf8abf24d570e18b35fc2bc1dd981f3c +Subproject commit 2c9b0e069207530ef5c01743172d4eb51697efe6 diff --git a/webots_ros2_epuck/CHANGELOG.rst b/webots_ros2_epuck/CHANGELOG.rst index 9df386da3..d46a4beba 100644 --- a/webots_ros2_epuck/CHANGELOG.rst +++ b/webots_ros2_epuck/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_epuck ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.2.3 (2022-05-30) +------------------ +* Fixed support for Humble and Rolling. + 1.2.0 (2021-12-21) ------------------ * Adapt the worlds to the new R2022a FLU convention. diff --git a/webots_ros2_epuck/launch/robot_launch.py b/webots_ros2_epuck/launch/robot_launch.py index 446b4702c..e38da134f 100644 --- a/webots_ros2_epuck/launch/robot_launch.py +++ b/webots_ros2_epuck/launch/robot_launch.py @@ -44,9 +44,11 @@ def generate_launch_description(): controller_manager_timeout = ['--controller-manager-timeout', '50'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' + use_deprecated_spawner_py = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'foxy' + diffdrive_controller_spawner = Node( package='controller_manager', - executable='spawner.py', + executable='spawner' if not use_deprecated_spawner_py else 'spawner.py', output='screen', prefix=controller_manager_prefix, arguments=['diffdrive_controller'] + controller_manager_timeout, @@ -57,7 +59,7 @@ def generate_launch_description(): joint_state_broadcaster_spawner = Node( package='controller_manager', - executable='spawner.py', + executable='spawner' if not use_deprecated_spawner_py else 'spawner.py', output='screen', prefix=controller_manager_prefix, arguments=['joint_state_broadcaster'] + controller_manager_timeout, @@ -66,6 +68,10 @@ def generate_launch_description(): ], ) + mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel')] + if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] in ['humble', 'rolling']: + mappings.append(('/diffdrive_controller/odom', '/odom')) + epuck_driver = Node( package='webots_ros2_driver', executable='driver', @@ -77,9 +83,7 @@ def generate_launch_description(): 'set_robot_state_publisher': True}, ros2_control_params ], - remappings=[ - ('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel'), - ] + remappings=mappings ) epuck_process = Node( diff --git a/webots_ros2_epuck/package.xml b/webots_ros2_epuck/package.xml index f7e5a2aa7..2b141e7a3 100644 --- a/webots_ros2_epuck/package.xml +++ b/webots_ros2_epuck/package.xml @@ -2,7 +2,7 @@ webots_ros2_epuck - 1.2.2 + 1.2.3 E-puck2 driver for Webots simulated robot Cyberbotics diff --git a/webots_ros2_epuck/setup.py b/webots_ros2_epuck/setup.py index 019afbad4..06c8cd3dd 100644 --- a/webots_ros2_epuck/setup.py +++ b/webots_ros2_epuck/setup.py @@ -43,7 +43,7 @@ setup( name=package_name, - version='1.2.2', + version='1.2.3', packages=[package_name], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_epuck/worlds/epuck_world.wbt b/webots_ros2_epuck/worlds/epuck_world.wbt index ac8c1641a..a1d9a04c6 100644 --- a/webots_ros2_epuck/worlds/epuck_world.wbt +++ b/webots_ros2_epuck/worlds/epuck_world.wbt @@ -1,4 +1,12 @@ -#VRML_SIM R2022a utf8 +#VRML_SIM R2022b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/RectangleArena.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/containers/protos/WoodenBox.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/robots/gctronic/e-puck/protos/E-puck.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/robots/gctronic/e-puck/protos/Pi-puck.proto" + WorldInfo { info [ "Simple e-puck simulation that can be controlled with ROS2." @@ -44,8 +52,8 @@ WoodenBox { E-puck { translation 0.0530459 0.00806403 0.001 rotation 0 0 1 3.1415 - controller "" name "epuck" + controller "" version "2" camera_width 640 camera_height 480 diff --git a/webots_ros2_epuck/worlds/rats_life_benchmark.wbt b/webots_ros2_epuck/worlds/rats_life_benchmark.wbt index a54d45171..fd1d2644f 100644 --- a/webots_ros2_epuck/worlds/rats_life_benchmark.wbt +++ b/webots_ros2_epuck/worlds/rats_life_benchmark.wbt @@ -1,4 +1,14 @@ -#VRML_SIM R2022a utf8 +#VRML_SIM R2022b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "../protos/LegoTallWall.proto" +EXTERNPROTO "../protos/LegoTallInterval.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/Floor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/Parquetry.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/robots/gctronic/e-puck/protos/E-puck.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/robots/gctronic/e-puck/protos/Pi-puck.proto" + WorldInfo { info [ "Rat's life pattern" diff --git a/webots_ros2_importer/CHANGELOG.rst b/webots_ros2_importer/CHANGELOG.rst index f2672723f..f90eb43b4 100644 --- a/webots_ros2_importer/CHANGELOG.rst +++ b/webots_ros2_importer/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_importer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.2.3 (2022-05-30) +------------------ +* Upgraded to urdf2webots 1.0.18 + 1.1.0 (2021-07-19) ------------------ * Upgraded to urdf2webots 1.0.10 diff --git a/webots_ros2_importer/package.xml b/webots_ros2_importer/package.xml index 3a03e193d..c11263103 100644 --- a/webots_ros2_importer/package.xml +++ b/webots_ros2_importer/package.xml @@ -2,7 +2,7 @@ webots_ros2_importer - 1.2.2 + 1.2.3 This package allows to convert URDF and XACRO files into Webots PROTO files. Cyberbotics diff --git a/webots_ros2_importer/setup.py b/webots_ros2_importer/setup.py index cc9059c83..2b4395a78 100644 --- a/webots_ros2_importer/setup.py +++ b/webots_ros2_importer/setup.py @@ -10,7 +10,7 @@ setup( name=package_name, - version='1.2.2', + version='1.2.3', packages=[package_name, package_name + '.urdf2webots.urdf2webots'], data_files=data_files, install_requires=[ diff --git a/webots_ros2_importer/webots_ros2_importer/urdf2proto.py b/webots_ros2_importer/webots_ros2_importer/urdf2proto.py index 2b1e003bf..181cb952e 100644 --- a/webots_ros2_importer/webots_ros2_importer/urdf2proto.py +++ b/webots_ros2_importer/webots_ros2_importer/urdf2proto.py @@ -16,86 +16,59 @@ import argparse import os -import re import sys -import tempfile -from ament_index_python.packages import get_package_share_directory sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), 'urdf2webots')) # deepcode ignore C0413: We need to import the library first -from urdf2webots.importer import convert2urdf # noqa: E402 +from urdf2webots.importer import convertUrdfFile # noqa: E402 -def main(args=None, input=None): +def main(args=None, urdfInput=None): parser = argparse.ArgumentParser(usage='usage: %prog --input=my_robot.urdf [options]') - parser.add_argument('--input', dest='inFile', default='', - help='Specifies the urdf file to convert.') - parser.add_argument('--output', dest='outFile', default='', - help='Specifies the name of the resulting PROTO file.') - parser.add_argument('--normal', dest='normal', action='store_true', default=False, - help='If set, the normals are exported if defined.') - parser.add_argument('--box-collision', dest='boxCollision', - action='store_true', default=False, - help='If set, the bounding objects are approximated using boxes.') - parser.add_argument('--disable-mesh-optimization', dest='disableMeshOptimization', - action='store_true', default=False, - help='If set, the duplicated vertices are not removed from the meshes.') - parser.add_argument('--multi-file', dest='enableMultiFile', action='store_true', default=False, - help='If set, the mesh files are exported as separated PROTO files') - parser.add_argument('--static-base', dest='staticBase', action='store_true', default=False, - help='If set, the base link will have the option to be static (disable physics)') - parser.add_argument('--tool-slot', dest='toolSlot', default=None, - help='Specify the link that you want to add a tool slot to (exact link name from urdf)') - parser.add_argument('--rotation', dest='initRotation', default='0 1 0 0', - help='Set the rotation field of your PROTO file.)') - parser.add_argument('--init-pos', dest='initPos', default=None, - help='Set the initial positions of your robot joints. ' - 'Example: --init-pos="[1.2, 0.5, -1.5]" would set ' - 'the first 3 joints of your robot to the specified values, ' - 'and leave the rest with their default value.') + parser.add_option('--input', dest='input', default='', help='Specifies the URDF file.') + parser.add_option('--output', dest='output', default='', help='Specifies the path and, if ending in ".proto", name ' + 'of the resulting PROTO file. The filename minus the .proto extension will be the robot name ' + '(for PROTO conversion only).') + parser.add_option('--normal', dest='normal', action='store_true', default=False, + help='If set, the normals are exported if present in the URDF definition.') + parser.add_option('--box-collision', dest='boxCollision', action='store_true', default=False, + help='If set, the bounding objects are approximated using boxes.') + parser.add_option('--tool-slot', dest='toolSlot', default=None, + help='Specify the link that you want to add a tool slot too (exact link name from URDF, for PROTO ' + 'conversion only).') + parser.add_option('--translation', dest='initTranslation', default='0 0 0', + help='Set the translation field of your PROTO file or Webots VRML robot string.') + parser.add_option('--rotation', dest='initRotation', default='0 0 1 0', + help='Set the rotation field of your PROTO file or Webots Robot node string.') + parser.add_option('--init-pos', dest='initPos', default=None, + help='Set the initial positions of your robot joints. Example: --init-pos="[1.2, 0.5, -1.5]" would ' + 'set the first 3 joints of your robot to the specified values, and leave the rest with their ' + 'default value.') + parser.add_option('--link-to-def', dest='linkToDef', action='store_true', default=False, + help='Creates a DEF with the link name for each solid to be able to access it using ' + 'getFromProtoDef(defName) (for PROTO conversion only).') + parser.add_option('--joint-to-def', dest='jointToDef', action='store_true', default=False, + help='Creates a DEF with the joint name for each joint to be able to access it using ' + 'getFromProtoDef(defName) (for PROTO conversion only).') # use 'parse_known_args' because ROS2 adds a lot of internal arguments arguments, _ = parser.parse_known_args() - file = os.path.abspath(input) if input is not None else os.path.abspath(arguments.inFile) + file = os.path.abspath(urdfInput) if urdfInput is not None else os.path.abspath(arguments.inputConversion) if not file: sys.exit('Input file not specified (should be specified with the "--input" argument).') if not os.path.isfile(file): - sys.exit('"%s" file does not exist.' % arguments.inFile) + sys.exit('"%s" file does not exist.' % arguments.inputConversion) elif not file.endswith('.urdf'): sys.exit('"%s" is not an urdf file.' % file) - content = '' - with open(file, 'r') as f: - content = f.read() - # look for package-relative file path and replace them - urdf_file = file - generated_file = False - packages = re.findall(r'filename="package:\/\/([^\/]*)', content) - for package in set(packages): # do the replacement - package_path = '' - try: - package_path = get_package_share_directory(package) - except LookupError: - sys.exit('This urdf depends on the "%s" package, but this package could not be found' % - package) - content = content.replace('package://%s' % package, '%s' % package_path) - if packages: # some package-relative file paths have been found - # generate a temporary file with the replacement content - urdf_file = tempfile.mkstemp(suffix='.urdf')[1] - generated_file = True - with open(urdf_file, 'w') as f: - f.write(content) - convert2urdf(inFile=urdf_file, - outFile=arguments.outFile, - normal=arguments.normal, - boxCollision=arguments.boxCollision, - disableMeshOptimization=arguments.disableMeshOptimization, - enableMultiFile=arguments.enableMultiFile, - staticBase=arguments.staticBase, - toolSlot=arguments.toolSlot, - initRotation=arguments.initRotation, - initPos=arguments.initPos) - # remove temporary file - if generated_file: - os.remove(urdf_file) + convertUrdfFile(input=arguments.inputConversion, + output=arguments.output, + normal=arguments.normal, + boxCollision=arguments.boxCollision, + toolSlot=arguments.toolSlot, + initTranslation=arguments.initTranslation, + initRotation=arguments.initRotation, + initPos=arguments.initPos, + linkToDef=arguments.linkToDef, + jointToDef=arguments.jointToDef) if __name__ == '__main__': diff --git a/webots_ros2_importer/webots_ros2_importer/urdf2webots b/webots_ros2_importer/webots_ros2_importer/urdf2webots index 98528f678..5c1eb161a 160000 --- a/webots_ros2_importer/webots_ros2_importer/urdf2webots +++ b/webots_ros2_importer/webots_ros2_importer/urdf2webots @@ -1 +1 @@ -Subproject commit 98528f6783dc831481851e53969a53de8da86623 +Subproject commit 5c1eb161a404ca40624c4a278ae60d2075f8edbb diff --git a/webots_ros2_importer/webots_ros2_importer/xacro2proto.py b/webots_ros2_importer/webots_ros2_importer/xacro2proto.py index c1c3683c4..422113476 100644 --- a/webots_ros2_importer/webots_ros2_importer/xacro2proto.py +++ b/webots_ros2_importer/webots_ros2_importer/xacro2proto.py @@ -53,14 +53,17 @@ def main(args=None): # remove arguments specific to the urdf2proto converter args_for_urdf2proto = [ + '--output', '--normal', '--box-collision', '--disable-mesh-optimization', - '--output', '--multi-file', - '--static-base', '--tool-slot', - '--rotation' + '--translation', + '--rotation', + '--init-pos', + '--link-to-def', + '--joint-to-def' ] for arg in saved_argv: for arg_for_urdf2proto in args_for_urdf2proto: diff --git a/webots_ros2_mavic/package.xml b/webots_ros2_mavic/package.xml index 49f0d9319..688c5d8ec 100644 --- a/webots_ros2_mavic/package.xml +++ b/webots_ros2_mavic/package.xml @@ -2,7 +2,7 @@ webots_ros2_mavic - 1.2.2 + 1.2.3 Mavic 2 Pro robot ROS2 interface for Webots. Cyberbotics diff --git a/webots_ros2_mavic/setup.py b/webots_ros2_mavic/setup.py index 2861234c1..f2904405f 100644 --- a/webots_ros2_mavic/setup.py +++ b/webots_ros2_mavic/setup.py @@ -18,7 +18,7 @@ setup( name=package_name, - version='1.2.2', + version='1.2.3', packages=[package_name], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_mavic/worlds/mavic_world.wbt b/webots_ros2_mavic/worlds/mavic_world.wbt index 5385aa924..edc326dfa 100644 --- a/webots_ros2_mavic/worlds/mavic_world.wbt +++ b/webots_ros2_mavic/worlds/mavic_world.wbt @@ -1,4 +1,20 @@ -#VRML_SIM R2022a utf8 +#VRML_SIM R2022b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/Floor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/SandyGround.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/Road.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/Pavement.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/Windmill.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/SmallManor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/manhole/protos/SquareManhole.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/containers/protos/CardboardBox.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/vehicles/protos/tesla/TeslaModel3Simple.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/trees/protos/Pine.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/trees/protos/Forest.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/robots/dji/mavic/protos/Mavic2Pro.proto" + WorldInfo { info [ "This demonstration shows a DJI Mavic 2 PRO flying over a rural area by exposing the ROS2 Twist topic." @@ -137,8 +153,8 @@ Forest { Mavic2Pro { translation 0 0 0.1 rotation 0 0 1 3.141590777218456 - controller "" name "Mavic_2_PRO" + controller "" cameraSlot [ Camera { width 400 diff --git a/webots_ros2_msgs/package.xml b/webots_ros2_msgs/package.xml index 320091724..8c8256d55 100644 --- a/webots_ros2_msgs/package.xml +++ b/webots_ros2_msgs/package.xml @@ -2,7 +2,7 @@ webots_ros2_msgs - 1.2.2 + 1.2.3 Services and Messages of the webots_ros2 packages. Cyberbotics diff --git a/webots_ros2_tesla/CHANGELOG.rst b/webots_ros2_tesla/CHANGELOG.rst index 40153266f..8858586a3 100644 --- a/webots_ros2_tesla/CHANGELOG.rst +++ b/webots_ros2_tesla/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_tesla ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.2.3 (2022-06-01) +------------------ +* Fixed support for Humble and Rolling. + 1.2.0 (2021-12-21) ------------------ * Adapt the worlds to the new R2022a FLU convention. diff --git a/webots_ros2_tesla/package.xml b/webots_ros2_tesla/package.xml index 82b0b9387..0746e057d 100644 --- a/webots_ros2_tesla/package.xml +++ b/webots_ros2_tesla/package.xml @@ -2,7 +2,7 @@ webots_ros2_tesla - 1.2.2 + 1.2.3 Tesla ROS2 interface for Webots. Cyberbotics diff --git a/webots_ros2_tesla/setup.py b/webots_ros2_tesla/setup.py index 4a8757623..262011cbf 100644 --- a/webots_ros2_tesla/setup.py +++ b/webots_ros2_tesla/setup.py @@ -18,7 +18,7 @@ setup( name=package_name, - version='1.2.2', + version='1.2.3', packages=[package_name], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_tesla/webots_ros2_tesla/lane_follower.py b/webots_ros2_tesla/webots_ros2_tesla/lane_follower.py index 1a37a24ec..e1b144fbc 100644 --- a/webots_ros2_tesla/webots_ros2_tesla/lane_follower.py +++ b/webots_ros2_tesla/webots_ros2_tesla/lane_follower.py @@ -35,8 +35,8 @@ def __init__(self): self.__ackermann_publisher = self.create_publisher(AckermannDrive, 'cmd_ackermann', 1) qos_camera_data = qos_profile_sensor_data - # In case ROS_DISTRO is Rolling or Galactic the QoSReliabilityPolicy is strict. - if ('ROS_DISTRO' in os.environ and (os.environ['ROS_DISTRO'] == 'rolling' or os.environ['ROS_DISTRO'] == 'galactic')): + # In case ROS_DISTRO is not foxy the QoSReliabilityPolicy is strict. + if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] != 'foxy': qos_camera_data.reliability = QoSReliabilityPolicy.RELIABLE self.create_subscription(Image, 'vehicle/camera', self.__on_camera_image, qos_camera_data) diff --git a/webots_ros2_tesla/worlds/tesla_world.wbt b/webots_ros2_tesla/worlds/tesla_world.wbt index 75d9f2210..a8bc9a7c5 100644 --- a/webots_ros2_tesla/worlds/tesla_world.wbt +++ b/webots_ros2_tesla/worlds/tesla_world.wbt @@ -1,4 +1,48 @@ -#VRML_SIM R2022a utf8 +#VRML_SIM R2022b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/CurvedRoadSegment.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/RoadLine.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/StraightRoadSegment.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/RoadIntersection.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/GenericTrafficLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/CrossRoadsTrafficLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/BuildingUnderConstruction.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/CommercialBuilding.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/UBuilding.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/HollowBuilding.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/Hotel.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/TheThreeTowers.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/CyberboticsTower.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/BigGlassTower.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/Auditorium.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/Museum.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/ResidentialBuilding.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/buildings/protos/FastFoodRestaurant.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/trees/protos/Oak.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/trees/protos/Pine.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/trees/protos/Cypress.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/trees/protos/Sassafras.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/trees/protos/BigSassafras.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/PedestrianCrossing.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/CautionSign.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/CautionPanel.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/OrderSign.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/OrderPanel.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/StopSign.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/StopPanel.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/YieldSign.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/SpeedLimitSign.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/SpeedLimitPanel.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/TrafficCone.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/advertising_board/protos/AdvertisingBoard.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/HighwayPole.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/HighwaySign.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/obstacles/protos/OilBarrel.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/Crossroad.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/vehicles/protos/tesla/TeslaModel3.proto" + WorldInfo { info [ "Simulation of Tesla Model 3 that exposes the AckermannDrive ROS 2 interface." @@ -51,7 +95,7 @@ DEF GROUND Solid { baseColor 0.8 0.8 0.8 baseColorMap ImageTexture { url [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/vehicles/worlds/textures/ground.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/vehicles/worlds/textures/ground.jpg" ] } roughness 0.5 @@ -92,7 +136,7 @@ CurvedRoadSegment { curvatureRadius 40.5 } StraightRoadSegment { - translation -4.5 -105.0 0.02 + translation -4.5 -105 0.02 name "road(1)" id "1" startJunction "25" @@ -139,7 +183,7 @@ CurvedRoadSegment { curvatureRadius 40.5 } StraightRoadSegment { - translation 105.0 -64.5 0.02 + translation 105 -64.5 0.02 rotation 0 0 1 1.5708 name "road(3)" id "3" @@ -186,7 +230,7 @@ CurvedRoadSegment { curvatureRadius 40.5 } RoadIntersection { - translation 45.0 45.0 0.02 + translation 45 45 0.02 rotation 0 0 1 0.7853996938995746 id "16" connectedRoadIDs [ @@ -201,10 +245,10 @@ RoadIntersection { ] startRoadsNumberOfLanes 4 startRoadsStartLine [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/road/protos/textures/road_line_dashed.png" - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/road/protos/textures/road_line_dashed.png" - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/road/protos/textures/road_line_triangle.png" - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/road/protos/textures/road_line_triangle.png" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/textures/road_line_dashed.png" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/textures/road_line_dashed.png" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/textures/road_line_triangle.png" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/textures/road_line_triangle.png" ] startRoadsLine [ RoadLine { @@ -217,7 +261,7 @@ RoadIntersection { startRoadBorder FALSE } StraightRoadSegment { - translation -4.5 45.0 0.02 + translation -4.5 45 0.02 name "road(5)" id "5" startJunction "28" @@ -264,7 +308,7 @@ CurvedRoadSegment { curvatureRadius 40.5 } StraightRoadSegment { - translation -45.0 -25.5 0.02 + translation -45 -25.5 0.02 rotation 0 0 1 1.5708 name "road(7)" id "7" @@ -288,7 +332,7 @@ StraightRoadSegment { length 30 } RoadIntersection { - translation -45.0 -45.0 0.02 + translation -45 -45 0.02 rotation 0 0 1 0.7853996938995746 name "road intersection(1)" id "17" @@ -304,10 +348,10 @@ RoadIntersection { ] startRoadsNumberOfLanes 4 startRoadsStartLine [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/road/protos/textures/road_line_dashed.png" - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/road/protos/textures/road_line_dashed.png" - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/road/protos/textures/road_line_triangle.png" - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/road/protos/textures/road_line_triangle.png" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/textures/road_line_dashed.png" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/textures/road_line_dashed.png" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/textures/road_line_triangle.png" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/road/protos/textures/road_line_triangle.png" ] startRoadsLine [ RoadLine { @@ -320,7 +364,7 @@ RoadIntersection { startRoadBorder FALSE } StraightRoadSegment { - translation -25.5 -45.0 0.02 + translation -25.5 -45 0.02 name "road(8)" id "8" startJunction "17" @@ -367,7 +411,7 @@ CurvedRoadSegment { curvatureRadius 40.5 } StraightRoadSegment { - translation 45.0 -4.5 0.02 + translation 45 -4.5 0.02 rotation 0 0 1 1.5708 name "road(10)" id "10" @@ -414,7 +458,7 @@ CurvedRoadSegment { curvatureRadius 40.5 } StraightRoadSegment { - translation 4.5 105.0 0.02 + translation 4.5 105 0.02 rotation 0 0 1 3.1415 name "road(12)" id "12" @@ -462,7 +506,7 @@ CurvedRoadSegment { curvatureRadius 40.5 } StraightRoadSegment { - translation -105.0 64.5 0.02 + translation -105 64.5 0.02 rotation 0 0 1 -1.5708 name "road(14)" id "14" @@ -518,7 +562,7 @@ GenericTrafficLight { state "red" } CrossRoadsTrafficLight { - translation 45.0 45.0 0 + translation 45 45 0 } DEF STONES Solid { translation 136.158 5.03891 -4.23581 @@ -527,7 +571,7 @@ DEF STONES Solid { DEF STONES_GROUP Group { children [ Transform { - translation -2.0 -0.6 0 + translation -2 -0.6 0 children [ Shape { appearance DEF OBJECTS_APPEARANCE PBRAppearance { @@ -555,7 +599,7 @@ DEF STONES Solid { ] } Transform { - translation -2.0 -0.15 -4.0 + translation -2 -0.15 -4 children [ Shape { appearance USE OBJECTS_APPEARANCE @@ -567,7 +611,7 @@ DEF STONES Solid { ] } Transform { - translation 1 0 -6.0 + translation 1 0 -6 children [ Shape { appearance USE OBJECTS_APPEARANCE @@ -579,7 +623,7 @@ DEF STONES Solid { ] } Transform { - translation 0 0.15 -9.0 + translation 0 0.15 -9 children [ Shape { appearance USE OBJECTS_APPEARANCE @@ -591,7 +635,7 @@ DEF STONES Solid { ] } Transform { - translation 5.0 0.2 -5.5 + translation 5 0.2 -5.5 children [ Shape { appearance USE OBJECTS_APPEARANCE @@ -615,7 +659,7 @@ DEF STONES Solid { ] } Transform { - translation -5.0 0 -10.0 + translation -5 0 -10 children [ Shape { appearance USE OBJECTS_APPEARANCE @@ -627,7 +671,7 @@ DEF STONES Solid { ] } Transform { - translation -6.0 0.3 -1 + translation -6 0.3 -1 children [ Shape { appearance USE OBJECTS_APPEARANCE @@ -639,7 +683,7 @@ DEF STONES Solid { ] } Transform { - translation 4.0 0 -13.0 + translation 4 0 -13 children [ Shape { appearance USE OBJECTS_APPEARANCE @@ -800,7 +844,7 @@ BigSassafras { name "tree(19)" } PedestrianCrossing { - translation 26.0 45.0 -0.06 + translation 26 45 -0.06 rotation 0 0 1 -1.5707953071795862 } PedestrianCrossing { @@ -814,7 +858,7 @@ CautionSign { CautionPanel { translation 0 -0.17 0 signImage [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/signs/us/turn_left.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/signs/us/turn_left.jpg" ] } ] @@ -832,7 +876,7 @@ CautionSign { CautionPanel { translation 0 -0.17 0 signImage [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/signs/us/bump.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/signs/us/bump.jpg" ] } ] @@ -845,7 +889,7 @@ CautionSign { CautionPanel { translation 0 -0.17 0 signImage [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/signs/us/cross_roads.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/signs/us/cross_roads.jpg" ] } ] @@ -858,7 +902,7 @@ CautionSign { CautionPanel { translation 0 -0.17 0 signImage [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/signs/us/turn_right.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/signs/us/turn_right.jpg" ] } ] @@ -880,7 +924,7 @@ OrderSign { OrderPanel { translation 0 -0.175 -0.026 signImage [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/signs/no_right_turn.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/signs/no_right_turn.jpg" ] } ] @@ -893,7 +937,7 @@ OrderSign { OrderPanel { translation 0 -0.175 -0.026 signImage [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/signs/no_pedestrian_crossing.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/signs/no_pedestrian_crossing.jpg" ] } ] @@ -921,7 +965,7 @@ SpeedLimitSign { SpeedLimitPanel { translation 0 0 -0.023 signImage [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/signs/us/speed_limit_55.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/signs/us/speed_limit_55.jpg" ] } ] @@ -934,7 +978,7 @@ SpeedLimitSign { SpeedLimitPanel { translation 0 0 -0.023 signImage [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/signs/us/speed_limit_55.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/signs/us/speed_limit_55.jpg" ] } ] @@ -947,7 +991,7 @@ SpeedLimitSign { SpeedLimitPanel { translation 0 0 -0.023 signImage [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/signs/us/speed_limit_65.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/signs/us/speed_limit_65.jpg" ] } ] @@ -960,7 +1004,7 @@ SpeedLimitSign { SpeedLimitPanel { translation 0 0 -0.023 signImage [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/signs/us/speed_limit_65.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/signs/us/speed_limit_65.jpg" ] } ] @@ -973,7 +1017,7 @@ SpeedLimitSign { SpeedLimitPanel { translation 0 0 -0.023 signImage [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/signs/us/one_way_sign_left.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/signs/us/one_way_sign_left.jpg" ] } ] @@ -1154,7 +1198,7 @@ HighwayPole { height 4 length 5.5 texture [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/highway_sign_bayonne.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/highway_sign_bayonne.jpg" ] } ] @@ -1164,7 +1208,7 @@ HighwayPole { height 2.5 length 3 texture [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/objects/traffic/protos/textures/highway_sign_sebastian.jpg" + "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/textures/highway_sign_sebastian.jpg" ] } ] @@ -1208,7 +1252,7 @@ OilBarrel { } } OilBarrel { - translation 105.104 -45.0 0.6 + translation 105.104 -45 0.6 name "oil barrel(3)" height 1.2 radius 0.4 @@ -1271,7 +1315,7 @@ OilBarrel { } } Crossroad { - translation -105.0 -4.5001488 0 + translation -105 -4.5001488 0 id "18" shape [] connectedRoadIDs [ @@ -1280,7 +1324,7 @@ Crossroad { ] } Crossroad { - translation -105.0 64.499851 0 + translation -105 64.499851 0 name "crossroad(1)" id "19" shape [] @@ -1300,7 +1344,7 @@ Crossroad { ] } Crossroad { - translation 4.4999256 105.0 0 + translation 4.4999256 105 0 name "crossroad(3)" id "21" shape [] @@ -1320,7 +1364,7 @@ Crossroad { ] } Crossroad { - translation 105.0 -64.5 0 + translation 105 -64.5 0 name "crossroad(5)" id "23" shape [] @@ -1340,7 +1384,7 @@ Crossroad { ] } Crossroad { - translation -4.4999794 -105.0 0 + translation -4.4999794 -105 0 name "crossroad(7)" id "25" shape [] @@ -1360,7 +1404,7 @@ Crossroad { ] } Crossroad { - translation 45.0 -4.5 0 + translation 45 -4.5 0 name "crossroad(9)" id "27" shape [] @@ -1370,7 +1414,7 @@ Crossroad { ] } Crossroad { - translation -4.5000744 45.0 0 + translation -4.5000744 45 0 name "crossroad(10)" id "28" shape [] diff --git a/webots_ros2_tests/package.xml b/webots_ros2_tests/package.xml index b319697b8..6cff17fa9 100644 --- a/webots_ros2_tests/package.xml +++ b/webots_ros2_tests/package.xml @@ -2,7 +2,7 @@ webots_ros2_tests - 1.2.2 + 1.2.3 System tests for `webots_ros2` packages. Cyberbotics diff --git a/webots_ros2_tests/setup.cfg b/webots_ros2_tests/setup.cfg index b43aa71cf..c032e1170 100644 --- a/webots_ros2_tests/setup.cfg +++ b/webots_ros2_tests/setup.cfg @@ -1,4 +1,4 @@ [develop] -script-dir=$base/lib/webots_ros2_tests +script_dir=$base/lib/webots_ros2_tests [install] -install-scripts=$base/lib/webots_ros2_tests +install_scripts=$base/lib/webots_ros2_tests diff --git a/webots_ros2_tests/setup.py b/webots_ros2_tests/setup.py index 0346aba10..a1ae05db9 100644 --- a/webots_ros2_tests/setup.py +++ b/webots_ros2_tests/setup.py @@ -11,7 +11,7 @@ setup( name=package_name, - version='1.2.2', + version='1.2.3', packages=[package_name], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_tests/test/test_system_tiago.py b/webots_ros2_tests/test/test_system_tiago.py index 593b081b1..a6a02b7c5 100644 --- a/webots_ros2_tests/test/test_system_tiago.py +++ b/webots_ros2_tests/test/test_system_tiago.py @@ -35,10 +35,9 @@ @pytest.mark.rostest def generate_test_description(): initialize_webots_test() - # If ROS_DISTRO is rolling or galactic, 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' or os.environ['ROS_DISTRO'] == 'galactic'): - pytest.skip('ROS_DISTRO is rolling or galactic, skipping this test') + # If ROS_DISTRO is rolling or humble, 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'] != 'foxy' and os.environ['ROS_DISTRO'] != 'galactic': + pytest.skip('ROS_DISTRO is rolling or humble, skipping this test') tiago_webots = IncludeLaunchDescription( PythonLaunchDescriptionSource( 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 0ce9b8010..5734e8013 100644 --- a/webots_ros2_tests/test/test_system_turtlebot_tutorial_navigation.py +++ b/webots_ros2_tests/test/test_system_turtlebot_tutorial_navigation.py @@ -42,10 +42,9 @@ def generate_test_description(): if 'CI' in os.environ and os.environ['CI'] == '1': pytest.skip('RViz might crash in CI, skipping this test') - # If ROS_DISTRO is rolling or galactic, 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' or os.environ['ROS_DISTRO'] == 'galactic'): - pytest.skip('ROS_DISTRO is rolling or galactic, skipping this test') + # If ROS_DISTRO is rolling or humble, 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'] != 'foxy' and os.environ['ROS_DISTRO'] != 'galactic': + pytest.skip('ROS_DISTRO is rolling or humble, 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 e3cee2b5a..fee2987e6 100644 --- a/webots_ros2_tests/test/test_system_turtlebot_tutorial_slam.py +++ b/webots_ros2_tests/test/test_system_turtlebot_tutorial_slam.py @@ -36,10 +36,9 @@ def generate_test_description(): if 'CI' in os.environ and os.environ['CI'] == '1': pytest.skip('RViz might crash in CI, skipping this test') - # If ROS_DISTRO is rolling or galactic, 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' or os.environ['ROS_DISTRO'] == 'galactic'): - pytest.skip('ROS_DISTRO is rolling or galactic, skipping this test') + # If ROS_DISTRO is rolling or humble, 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'] != 'foxy' and os.environ['ROS_DISTRO'] != 'galactic': + pytest.skip('ROS_DISTRO is rolling or humble, skipping this test') # Webots turtlebot_webots = IncludeLaunchDescription( diff --git a/webots_ros2_tests/test/test_system_universal_robot.py b/webots_ros2_tests/test/test_system_universal_robot.py index 35b42cb69..e07d555ad 100644 --- a/webots_ros2_tests/test/test_system_universal_robot.py +++ b/webots_ros2_tests/test/test_system_universal_robot.py @@ -47,7 +47,7 @@ def generate_test_description(): # Therefore, make sure to store all bag files under the `/tmp/artifacts`. rosbag = ExecuteProcess( cmd=[ - 'ros2', 'bag', 'record', '-a', '--include-hidden-topicsm', + 'ros2', 'bag', 'record', '-a', '--include-hidden-topics', '-o', os.path.join( tempfile.gettempdir(), 'artifacts', diff --git a/webots_ros2_tests/webots_ros2_tests/utils.py b/webots_ros2_tests/webots_ros2_tests/utils.py index 52074639d..2a4c1d91a 100644 --- a/webots_ros2_tests/webots_ros2_tests/utils.py +++ b/webots_ros2_tests/webots_ros2_tests/utils.py @@ -24,8 +24,8 @@ from rosgraph_msgs.msg import Clock -DEFAULT_TIMEOUT = 60.0 -DEFAULT_CLOCK_TIMEOUT = 5 * DEFAULT_TIMEOUT +DEFAULT_TIMEOUT = 120.0 +DEFAULT_CLOCK_TIMEOUT = 15 * DEFAULT_TIMEOUT class TestWebots(unittest.TestCase): diff --git a/webots_ros2_tests/worlds/driver_test.wbt b/webots_ros2_tests/worlds/driver_test.wbt index 4a517c80e..4f1a135ef 100644 --- a/webots_ros2_tests/worlds/driver_test.wbt +++ b/webots_ros2_tests/worlds/driver_test.wbt @@ -1,4 +1,14 @@ -#VRML_SIM R2022a utf8 +#VRML_SIM R2022b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/Floor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/containers/protos/CardboardBox.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/robots/adept/pioneer3/protos/Pioneer3at.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/devices/robotis/protos/RobotisLds01.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/devices/microsoft/protos/Kinect.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/Plaster.proto" + WorldInfo { } Viewpoint { @@ -16,8 +26,8 @@ CardboardBox { rotation 0.7040631892342535 -0.09269012491274485 -0.7040621892339847 -2.9567353071795863 } Pioneer3at { - controller "" name "Pioneer_3_AT" + controller "" supervisor TRUE extensionSlot [ RobotisLds01 { diff --git a/webots_ros2_tiago/launch/robot_launch.py b/webots_ros2_tiago/launch/robot_launch.py index 8cd92555b..31a9a5e6b 100644 --- a/webots_ros2_tiago/launch/robot_launch.py +++ b/webots_ros2_tiago/launch/robot_launch.py @@ -53,9 +53,11 @@ def generate_launch_description(): controller_manager_timeout = ['--controller-manager-timeout', '50'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' + use_deprecated_spawner_py = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'foxy' + diffdrive_controller_spawner = Node( package='controller_manager', - executable='spawner.py', + executable='spawner' if not use_deprecated_spawner_py else 'spawner.py', output='screen', prefix=controller_manager_prefix, arguments=['diffdrive_controller'] + controller_manager_timeout, @@ -63,12 +65,16 @@ def generate_launch_description(): joint_state_broadcaster_spawner = Node( package='controller_manager', - executable='spawner.py', + executable='spawner' if not use_deprecated_spawner_py else 'spawner.py', output='screen', prefix=controller_manager_prefix, arguments=['joint_state_broadcaster'] + controller_manager_timeout, ) + mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel')] + if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] in ['humble', 'rolling']: + mappings.append(('/diffdrive_controller/odom', '/odom')) + tiago_driver = Node( package='webots_ros2_driver', executable='driver', @@ -80,9 +86,7 @@ def generate_launch_description(): 'set_robot_state_publisher': True}, ros2_control_params ], - remappings=[ - ('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel') - ] + remappings=mappings ) robot_state_publisher = Node( diff --git a/webots_ros2_tiago/package.xml b/webots_ros2_tiago/package.xml index 62ff88a42..b005c5432 100644 --- a/webots_ros2_tiago/package.xml +++ b/webots_ros2_tiago/package.xml @@ -2,7 +2,7 @@ webots_ros2_tiago - 1.2.2 + 1.2.3 TIAGo robots ROS2 interface for Webots. Cyberbotics diff --git a/webots_ros2_tiago/setup.py b/webots_ros2_tiago/setup.py index bba6d59f8..7ba841c23 100644 --- a/webots_ros2_tiago/setup.py +++ b/webots_ros2_tiago/setup.py @@ -17,7 +17,7 @@ setup( name=package_name, - version='1.2.2', + version='1.2.3', packages=[], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_tiago/worlds/default.wbt b/webots_ros2_tiago/worlds/default.wbt index 16e588379..9b94a130c 100644 --- a/webots_ros2_tiago/worlds/default.wbt +++ b/webots_ros2_tiago/worlds/default.wbt @@ -1,4 +1,28 @@ -#VRML_SIM R2022a utf8 +#VRML_SIM R2022b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/Floor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/Parquetry.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/lights/protos/CeilingLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/apartment_structure/protos/Wall.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/Roughcast.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/apartment_structure/protos/Window.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/MattePaint.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/apartment_structure/protos/Door.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/living_room_furniture/protos/Sofa.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/plants/protos/PottedTree.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/cabinet/protos/Cabinet.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/cabinet/protos/CabinetHandle.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/GlossyPaint.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/solids/protos/SolidBox.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/kitchen/components/protos/Sink.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/tables/protos/Table.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/VarnishedPine.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/computers/protos/Monitor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/computers/protos/Keyboard.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/chairs/protos/OfficeChair.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/robots/pal_robotics/tiago_iron/protos/TiagoIron.proto" + WorldInfo { info [ "An office break room, surrounded by office desks." @@ -654,8 +678,8 @@ OfficeChair { TiagoIron { translation 0.962153 -4.80117 0.235361 rotation -0.006363227570975 -0.002646197037686 -0.999976253206104 0.78447116344273 - controller "" name "Tiago_Iron" + controller "" supervisor TRUE cameraSlot [ DEF camera Camera { diff --git a/webots_ros2_turtlebot/CHANGELOG.rst b/webots_ros2_turtlebot/CHANGELOG.rst index 7fa35ef49..694bc10de 100644 --- a/webots_ros2_turtlebot/CHANGELOG.rst +++ b/webots_ros2_turtlebot/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_turtlebot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.2.3 (2022-05-30) +------------------ +* Fix warnings with the example plugin. + 1.2.1 (2022-01-10) ------------------ * Add ros2control parameters for the lidar device. diff --git a/webots_ros2_turtlebot/launch/robot_launch.py b/webots_ros2_turtlebot/launch/robot_launch.py index 793c8e736..63a484959 100644 --- a/webots_ros2_turtlebot/launch/robot_launch.py +++ b/webots_ros2_turtlebot/launch/robot_launch.py @@ -45,9 +45,11 @@ def generate_launch_description(): controller_manager_timeout = ['--controller-manager-timeout', '50'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' + use_deprecated_spawner_py = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'foxy' + diffdrive_controller_spawner = Node( package='controller_manager', - executable='spawner.py', + executable='spawner' if not use_deprecated_spawner_py else 'spawner.py', output='screen', prefix=controller_manager_prefix, arguments=['diffdrive_controller'] + controller_manager_timeout, @@ -55,12 +57,16 @@ def generate_launch_description(): joint_state_broadcaster_spawner = Node( package='controller_manager', - executable='spawner.py', + executable='spawner' if not use_deprecated_spawner_py else 'spawner.py', output='screen', prefix=controller_manager_prefix, arguments=['joint_state_broadcaster'] + controller_manager_timeout, ) + mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel')] + if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] in ['humble', 'rolling']: + mappings.append(('/diffdrive_controller/odom', '/odom')) + turtlebot_driver = Node( package='webots_ros2_driver', executable='driver', @@ -72,9 +78,7 @@ def generate_launch_description(): 'set_robot_state_publisher': True}, ros2_control_params ], - remappings=[ - ('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel') - ] + remappings=mappings ) robot_state_publisher = Node( diff --git a/webots_ros2_turtlebot/package.xml b/webots_ros2_turtlebot/package.xml index 40d2f6352..8ba25ad27 100644 --- a/webots_ros2_turtlebot/package.xml +++ b/webots_ros2_turtlebot/package.xml @@ -2,7 +2,7 @@ webots_ros2_turtlebot - 1.2.2 + 1.2.3 TurtleBot3 Burger robot ROS2 interface for Webots. Cyberbotics diff --git a/webots_ros2_turtlebot/setup.py b/webots_ros2_turtlebot/setup.py index dce81a465..feb568e7c 100644 --- a/webots_ros2_turtlebot/setup.py +++ b/webots_ros2_turtlebot/setup.py @@ -22,7 +22,7 @@ setup( name=package_name, - version='1.2.2', + version='1.2.3', packages=[package_name], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_turtlebot/worlds/turtlebot3_burger_example.wbt b/webots_ros2_turtlebot/worlds/turtlebot3_burger_example.wbt index e5698e177..8874a88da 100644 --- a/webots_ros2_turtlebot/worlds/turtlebot3_burger_example.wbt +++ b/webots_ros2_turtlebot/worlds/turtlebot3_burger_example.wbt @@ -1,4 +1,27 @@ -#VRML_SIM R2022a utf8 +#VRML_SIM R2022b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/robots/robotis/turtlebot/protos/TurtleBot3Burger.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/devices/robotis/protos/RobotisLds01.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/plants/protos/PottedTree.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/Floor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/CementTiles.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/apartment_structure/protos/Window.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/apartment_structure/protos/Wall.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/Roughcast.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/apartment_structure/protos/Radiator.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/apartment_structure/protos/Door.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/cabinet/protos/Cabinet.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/containers/protos/CardboardBox.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/kitchen/fridge/protos/Fridge.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/lights/protos/FloorLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/living_room_furniture/protos/Sofa.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/living_room_furniture/protos/Armchair.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/paintings/protos/LandscapePainting.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/tables/protos/Table.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/traffic/protos/DirectionPanel.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/plants/protos/BunchOfSunFlowers.proto" + WorldInfo { info [ "Simple TurtleBot3 Burger simulation that can be controlled by a ROS." diff --git a/webots_ros2_universal_robot/launch/multirobot_launch.py b/webots_ros2_universal_robot/launch/multirobot_launch.py index 70d5ad80e..8c2f7b99b 100644 --- a/webots_ros2_universal_robot/launch/multirobot_launch.py +++ b/webots_ros2_universal_robot/launch/multirobot_launch.py @@ -89,30 +89,31 @@ def get_ros2_nodes(*args): # Other ROS 2 nodes controller_manager_timeout = ['--controller-manager-timeout', '75'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' + use_deprecated_spawner_py = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'foxy' ur5e_trajectory_controller_spawner = Node( package='controller_manager', - executable='spawner.py', + executable='spawner' if not use_deprecated_spawner_py else 'spawner.py', output='screen', prefix=controller_manager_prefix, arguments=['ur_joint_trajectory_controller', '-c', 'ur5e/controller_manager'] + controller_manager_timeout, ) ur5e_joint_state_broadcaster_spawner = Node( package='controller_manager', - executable='spawner.py', + executable='spawner' if not use_deprecated_spawner_py else 'spawner.py', output='screen', prefix=controller_manager_prefix, arguments=['ur_joint_state_broadcaster', '-c', 'ur5e/controller_manager'] + controller_manager_timeout, ) abb_trajectory_controller_spawner = Node( package='controller_manager', - executable='spawner.py', + executable='spawner' if not use_deprecated_spawner_py else 'spawner.py', output='screen', prefix=controller_manager_prefix, arguments=['abb_joint_trajectory_controller', '-c', 'abb/controller_manager'] + controller_manager_timeout, ) abb_joint_state_broadcaster_spawner = Node( package='controller_manager', - executable='spawner.py', + executable='spawner' if not use_deprecated_spawner_py else 'spawner.py', output='screen', prefix=controller_manager_prefix, arguments=['abb_joint_state_broadcaster', '-c', 'abb/controller_manager'] + controller_manager_timeout, diff --git a/webots_ros2_universal_robot/launch/robot_launch/robot_nodes_launch.py b/webots_ros2_universal_robot/launch/robot_launch/robot_nodes_launch.py index e0fecae79..ca1c14c0f 100644 --- a/webots_ros2_universal_robot/launch/robot_launch/robot_nodes_launch.py +++ b/webots_ros2_universal_robot/launch/robot_launch/robot_nodes_launch.py @@ -65,9 +65,11 @@ def generate_launch_description(): controller_manager_timeout = ['--controller-manager-timeout', '100'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' + use_deprecated_spawner_py = 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] == 'foxy' + trajectory_controller_spawner = Node( package='controller_manager', - executable='spawner.py', + executable='spawner' if not use_deprecated_spawner_py else 'spawner.py', output='screen', prefix=controller_manager_prefix, arguments=['ur_joint_trajectory_controller'] + controller_manager_timeout, @@ -75,7 +77,7 @@ def generate_launch_description(): joint_state_broadcaster_spawner = Node( package='controller_manager', - executable='spawner.py', + executable='spawner' if not use_deprecated_spawner_py else 'spawner.py', output='screen', prefix=controller_manager_prefix, arguments=['ur_joint_state_broadcaster'] + controller_manager_timeout, diff --git a/webots_ros2_universal_robot/package.xml b/webots_ros2_universal_robot/package.xml index c31a232fb..167917f90 100644 --- a/webots_ros2_universal_robot/package.xml +++ b/webots_ros2_universal_robot/package.xml @@ -2,7 +2,7 @@ webots_ros2_universal_robot - 1.2.2 + 1.2.3 Universal Robot ROS2 interface for Webots. Cyberbotics diff --git a/webots_ros2_universal_robot/setup.py b/webots_ros2_universal_robot/setup.py index c2cba21f9..2fee1a6aa 100644 --- a/webots_ros2_universal_robot/setup.py +++ b/webots_ros2_universal_robot/setup.py @@ -30,7 +30,7 @@ setup( name=package_name, - version='1.2.2', + version='1.2.3', packages=['webots_ros2_universal_robot'], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_universal_robot/worlds/robotic_arms.wbt b/webots_ros2_universal_robot/worlds/robotic_arms.wbt index 3415485cf..0f1fe52aa 100644 --- a/webots_ros2_universal_robot/worlds/robotic_arms.wbt +++ b/webots_ros2_universal_robot/worlds/robotic_arms.wbt @@ -1,4 +1,43 @@ -#VRML_SIM R2022a utf8 +#VRML_SIM R2022b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/Floor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/BakelitePlastic.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/apartment_structure/protos/Radiator.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/tables/protos/Desk.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/cabinet/protos/Cabinet.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/GlossyPaint.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/tools/protos/PlatformCart.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/telephone/protos/OfficeTelephone.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/computers/protos/Monitor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/computers/protos/Keyboard.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/computers/protos/DesktopComputer.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/containers/protos/CardboardBox.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/fire_extinguisher/protos/FireExtinguisher.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/street_furniture/protos/ElectricalCabinet.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/manhole/protos/SquareManhole.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/pipes/protos/PipeSection.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/HammeredCopper.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/pipes/protos/LJoint.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/robots/abb/irb/protos/Irb4600-40.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/devices/robotiq/protos/Robotiq3fGripper.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/RustyMetal.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/conveyors/protos/ConveyorBelt.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/drinks/protos/Can.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/tables/protos/Table.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/BrushedSteel.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/OldSteel.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/solids/protos/SolidBox.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/BrushedAluminium.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/FormedConcrete.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/Roughcast.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/CorrugatedPlates.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/CorrodedMetal.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/geometries/protos/Extrusion.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/pallet/protos/WoodenPalletStack.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/lights/protos/ConstructionLamp.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/factory/containers/protos/PlasticCrate.proto" + WorldInfo { title "ROS2-Webots Armed Robots Demo" basicTimeStep 16 @@ -83,7 +122,7 @@ ElectricalCabinet { } SquareManhole { translation 3.24031 1.59301 -0.04 - rotation 0.7071067811841624 0.7071067811841624 0 -3.1415853071795863 + rotation 0.7071067811865477 0.7071067811865477 0 -3.1415853071795863 } PipeSection { translation 4.79 1.93 0.610012 @@ -280,7 +319,7 @@ DEF WALL Transform { ] } DEF ROOF Transform { - translation 0.79 0.69 3.0 + translation 0.79 0.69 3 rotation 1 0 0 3.14159 children [ Shape { @@ -438,73 +477,73 @@ WoodenPalletStack { rotation 0 0 1 -1.5707953071795862 } ConstructionLamp { - translation 3.86242 2.61396 3.0 + translation 3.86242 2.61396 3 rotation 0.5773522691865168 0.5773512691860505 -0.5773472691841857 2.0944 name "construction lamp(1)" spotLightIntensity 5 } ConstructionLamp { - translation 2.06242 2.61396 3.0 + translation 2.06242 2.61396 3 rotation 0.5773522691865168 0.5773512691860505 -0.5773472691841857 2.0944 name "construction lamp(4)" spotLightIntensity 5 } ConstructionLamp { - translation 0.26242 2.61396 3.0 + translation 0.26242 2.61396 3 rotation 0.5773522691865168 0.5773512691860505 -0.5773472691841857 2.0944 name "construction lamp(5)" spotLightIntensity 5 } ConstructionLamp { - translation -1.54758 2.61396 3.0 + translation -1.54758 2.61396 3 rotation 0.5773522691865168 0.5773512691860505 -0.5773472691841857 2.0944 name "construction lamp(6)" spotLightIntensity 5 } ConstructionLamp { - translation 3.86242 1.11396 3.0 + translation 3.86242 1.11396 3 rotation 0.5773522691865168 0.5773512691860505 -0.5773472691841857 2.0944 name "construction lamp(2)" spotLightIntensity 5 } ConstructionLamp { - translation 2.06242 1.11396 3.0 + translation 2.06242 1.11396 3 rotation 0.5773522691865168 0.5773512691860505 -0.5773472691841857 2.0944 name "construction lamp(7)" spotLightIntensity 5 } ConstructionLamp { - translation 0.26242 1.11396 3.0 + translation 0.26242 1.11396 3 rotation 0.5773522691865168 0.5773512691860505 -0.5773472691841857 2.0944 name "construction lamp(8)" spotLightIntensity 5 } ConstructionLamp { - translation -1.53758 1.11396 3.0 + translation -1.53758 1.11396 3 rotation 0.5773522691865168 0.5773512691860505 -0.5773472691841857 2.0944 name "construction lamp(9)" spotLightIntensity 5 } ConstructionLamp { - translation 3.86242 -0.38604 3.0 + translation 3.86242 -0.38604 3 rotation 0.5773522691865168 0.5773512691860505 -0.5773472691841857 2.0944 name "construction lamp(3)" spotLightIntensity 5 } ConstructionLamp { - translation 2.06242 -0.38604 3.0 + translation 2.06242 -0.38604 3 rotation 0.5773522691865168 0.5773512691860505 -0.5773472691841857 2.0944 name "construction lamp(10)" spotLightIntensity 5 } ConstructionLamp { - translation 0.26242 -0.38604 3.0 + translation 0.26242 -0.38604 3 rotation 0.5773522691865168 0.5773512691860505 -0.5773472691841857 2.0944 name "construction lamp(11)" spotLightIntensity 5 } ConstructionLamp { - translation -1.53758 -0.38604 3.0 + translation -1.53758 -0.38604 3 rotation 0.5773522691865168 0.5773512691860505 -0.5773472691841857 2.0944 name "construction lamp(12)" spotLightIntensity 5 diff --git a/webots_ros2_universal_robot/worlds/universal_robot.wbt b/webots_ros2_universal_robot/worlds/universal_robot.wbt index 5199c57e0..8bbecf3f3 100644 --- a/webots_ros2_universal_robot/worlds/universal_robot.wbt +++ b/webots_ros2_universal_robot/worlds/universal_robot.wbt @@ -1,4 +1,11 @@ -#VRML_SIM R2022a utf8 +#VRML_SIM R2022b utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/floors/protos/Floor.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/ThreadMetalPlate.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/objects/solids/protos/SolidBox.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/GalvanizedMetal.proto" + WorldInfo { info [ "Unviversal Robot UR3e, UR5e and UR10e grasping cans using Robotiq 3F grippers."