Merge master into develop (#430)

* 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 <[email protected]> * CallbackReturn should include namespae in its signature (#435) Signed-off-by: Kenji Brameld <[email protected]> Co-authored-by: ad-daniel <[email protected]> * use new bbox msg signature (#433) * Handle new bbox msg signature from humble onwards Signed-off-by: Kenji Brameld <[email protected]> Co-authored-by: Olivier Michel <[email protected]> * Fix pep257 D407 Missing dashed underline after section (#437) * fix pep257 D407 Missing dashed underline after section Signed-off-by: Kenji Brameld <[email protected]> Co-authored-by: Olivier Michel <[email protected]> * 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 <[email protected]> Co-authored-by: Olivier Michel <[email protected]> * Change call to spawner.py executable to spawner (#438) * rename spawner.py executable to spawner Signed-off-by: Kenji Brameld <[email protected]> * use spawner.py if on foxy Signed-off-by: Kenji Brameld <[email protected]> * 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 ec39e10. * 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 <[email protected]> Co-authored-by: ad-daniel <[email protected]> Co-authored-by: ad-daniel <[email protected]> * 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 <[email protected]> Co-authored-by: Darko Lukić <[email protected]> Co-authored-by: Kenji Brameld <[email protected]> Co-authored-by: Olivier Michel <[email protected]> Co-authored-by: Benjamin Hug <[email protected]>
cyberbotics · Sep 9, 2022 · f60aca9 · f60aca9
1 parent 47c7e8e
commit f60aca9
Show file tree

Hide file tree

Showing 73 changed files with 677 additions and 321 deletions.
diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml
@@ -1,23 +1,25 @@
-name: CI
+name: ROS2 tests
 
 on:
   pull_request:
     types: [opened, synchronize, reopened, labeled, unlabeled]
+  schedule:
+    - cron:  '0 23 * * *'
 
 jobs:
   industrial_ci:
     strategy:
       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
@@ -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
@@ -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
@@ -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
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>webots_ros2</name>
-  <version>1.2.2</version>
+  <version>1.2.3</version>
   <description>Interface between Webots and ROS2</description>
 
   <maintainer email="[email protected]">Cyberbotics</maintainer>

diff --git 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
@@ -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
@@ -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
@@ -47,8 +47,8 @@ namespace webots_ros2_control
       webots_ros2_driver::WebotsNode *mNode;
       std::shared_ptr<pluginlib::ClassLoader<Ros2ControlSystemInterface>> mHardwareLoader;
       std::shared_ptr<controller_manager::ControllerManager> 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
@@ -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<hardware_interface::StateInterface> export_state_interfaces() override;
     std::vector<hardware_interface::CommandInterface> 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
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>webots_ros2_control</name>
-  <version>1.2.2</version>
+  <version>1.2.3</version>
   <description>ros2_control plugin for Webots</description>
   <maintainer email="[email protected]">Cyberbotics</maintainer>
   <url type="website">http://wiki.ros.org/webots_ros2</url>

diff --git 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<std::string, std::string> &)
@@ -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