Add runtime tests (#268)

* Add WebotsLauncher * Add test package * Add distance sensor tests * Add range finder test * Add plugin test * Fix spelling * Add IMU test * Improve publishing * Add light sensor test * Fix colcon test * Add CI * Use sudo * Update * Update with sudo * Add missing packages * Add cache * Test * Test * Test * Test * Fix * Test * Test * Test * Test * Test * Increase timeout * Revert camera test * pylint * Fix Windows arguments * Update webots_ros2_driver/webots_ros2_driver/webots_launcher.py Co-authored-by: Stefania Pedrazzi <[email protected]> * Update webots_ros2_driver/webots_ros2_driver/utils.py Co-authored-by: Stefania Pedrazzi <[email protected]> * Update webots_ros2_driver/webots_ros2_driver/utils.py Co-authored-by: Stefania Pedrazzi <[email protected]> * Update webots_ros2_driver/webots_ros2_driver/utils.py Co-authored-by: Stefania Pedrazzi <[email protected]> * Update utils.py * Update driver_test.wbt Co-authored-by: Stefania Pedrazzi <[email protected]>
cyberbotics · Aug 18, 2021 · 08a061e · 08a061e
1 parent b746298
commit 08a061e
Show file tree

Hide file tree

Showing 37 changed files with 1,012 additions and 83 deletions.
diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml
@@ -12,6 +12,9 @@ jobs:
         ROS_REPO: [main, testing]
         ROS_DISTRO: [foxy, rolling]
     runs-on: ubuntu-latest
+    env:
+      AFTER_INIT: ./scripts/ci_before_init.bash
+      BEFORE_INIT_EMBED: source scripts/ci_environment.bash
     steps:
       - uses: actions/checkout@v1
         with:

diff --git a/scripts/ci_before_init.bash b/scripts/ci_before_init.bash
@@ -0,0 +1,6 @@
+#!/usr/bin/env bash
+
+apt update
+apt install -y wget dialog apt-utils
+wget https://github.com/cyberbotics/webots/releases/download/R${WEBOTS_VERSION}/webots_${WEBOTS_VERSION}_amd64.deb -O /tmp/webots.deb
+apt install -y /tmp/webots.deb xvfb
diff --git a/scripts/ci_environment.bash b/scripts/ci_environment.bash
@@ -0,0 +1,6 @@
+#!/usr/bin/env bash
+
+export WEBOTS_VERSION=2021b
+export WEBOTS_OFFSCREEN=1
+export DEBIAN_FRONTEND=noninteractive
+export QTWEBENGINE_DISABLE_SANDBOX=1
diff --git a/webots_ros2_driver/CMakeLists.txt b/webots_ros2_driver/CMakeLists.txt
@@ -70,6 +70,9 @@ endif()
 ament_python_install_package(${PROJECT_NAME}_webots
   PACKAGE_DIR ${WEBOTS_LIB_BASE}/python38)
 
+ament_python_install_package(${PROJECT_NAME}
+  PACKAGE_DIR ${PROJECT_NAME})
+
 add_executable(driver
   src/Driver.cpp
   src/WebotsNode.cpp

diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp
@@ -57,9 +57,9 @@ namespace webots_ros2_driver
     rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr mCameraInfoPublisher;
     sensor_msgs::msg::CameraInfo mCameraInfoMessage;
 
-    rclcpp::Publisher<vision_msgs::msg::Detection2DArray>::SharedPtr mRecogntionPublisher;
+    rclcpp::Publisher<vision_msgs::msg::Detection2DArray>::SharedPtr mRecognitionPublisher;
     rclcpp::Publisher<webots_ros2_msgs::msg::WbCameraRecognitionObjects>::SharedPtr mWebotsRecognitionPublisher;
-    vision_msgs::msg::Detection2DArray mRecogntionMessage;
+    vision_msgs::msg::Detection2DArray mRecognitionMessage;
     webots_ros2_msgs::msg::WbCameraRecognitionObjects mWebotsRecognitionMessage;
 
     bool mIsEnabled;

diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Lidar.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Lidar.hpp
@@ -45,7 +45,8 @@ namespace webots_ros2_driver
     rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr mPointCloudPublisher;
     sensor_msgs::msg::PointCloud2 mPointCloudMessage;
 
-    bool mIsEnabled;
+    bool mIsSensorEnabled;
+    bool mIsPointCloudEnabled;
   };
 
 } // end namespace webots_ros2_driver

diff --git a/webots_ros2_driver/src/Driver.cpp b/webots_ros2_driver/src/Driver.cpp
@@ -21,10 +21,16 @@ int main(int argc, char **argv)
   rclcpp::init(argc, argv);
 
   webots::Supervisor* robot = new webots::Supervisor();
-  auto node = std::make_shared<webots_ros2_driver::WebotsNode>(robot->getName(), robot);
+
+  std::string robotName = robot->getName();
+  for (char notAllowedChar : " -.)(")
+    std::replace(robotName.begin(), robotName.end(), notAllowedChar, '_');
+
+  std::shared_ptr<webots_ros2_driver::WebotsNode> node = std::make_shared<webots_ros2_driver::WebotsNode>(robotName, robot);
   node->init();
 
   rclcpp::spin(node);
+  delete robot;
   rclcpp::shutdown();
   return 0;
 }
diff --git a/webots_ros2_driver/src/WebotsNode.cpp b/webots_ros2_driver/src/WebotsNode.cpp
@@ -213,7 +213,10 @@ namespace webots_ros2_driver
 
   void WebotsNode::timerCallback()
   {
-    mRobot->step(mStep);
+    if (mRobot->step(mStep) == -1) {
+      mTimer->cancel();
+      return;
+    }
     for (std::shared_ptr<PluginInterface> plugin : mPlugins)
       plugin->step();
 

diff --git a/webots_ros2_driver/src/plugins/Ros2SensorPlugin.cpp b/webots_ros2_driver/src/plugins/Ros2SensorPlugin.cpp
@@ -27,7 +27,7 @@ namespace webots_ros2_driver
     mAlwaysOn = parameters.count("alwaysOn") ? (parameters["alwaysOn"] == "true") : false;
     mFrameName = parameters.count("frameName") ? parameters["frameName"] : getFixedNameString(parameters["name"]);
 
-    // Calcualte timestep
+    // Calculate timestep
     mPublishTimestepSyncedMs = getDeviceTimestepMsFromPublishTimestep(mPublishTimestep, mNode->robot()->getBasicTimeStep());
   }
 

diff --git a/webots_ros2_driver/src/plugins/dynamic/Ros2IMU.cpp b/webots_ros2_driver/src/plugins/dynamic/Ros2IMU.cpp
@@ -53,6 +53,11 @@ namespace webots_ros2_driver
 
     mPublisher = mNode->create_publisher<sensor_msgs::msg::Imu>(mTopicName, rclcpp::SensorDataQoS().reliable());
     mMessage.header.frame_id = mFrameName;
+
+    if (mAlwaysOn) {
+      enable();
+      mIsEnabled = true;
+    }
   }
 
   void Ros2IMU::enable()
@@ -80,10 +85,14 @@ namespace webots_ros2_driver
     if (!preStep())
       return;
 
-    // Enable/Disable sensor
-    const bool imageSubscriptionsExist = mPublisher->get_subscription_count() > 0;
-    const bool shouldBeEnabled = mAlwaysOn || imageSubscriptionsExist;
+    if (mIsEnabled)
+      publishData();
 
+    if (mAlwaysOn)
+      return;
+
+    // Enable/Disable sensor
+    const bool shouldBeEnabled = mPublisher->get_subscription_count() > 0;
     if (shouldBeEnabled != mIsEnabled)
     {
       if (shouldBeEnabled)
@@ -92,10 +101,6 @@ namespace webots_ros2_driver
         disable();
       mIsEnabled = shouldBeEnabled;
     }
-
-    // Publish data
-    if (mAlwaysOn || imageSubscriptionsExist)
-      publishData();
   }
 
   void Ros2IMU::publishData()

diff --git a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp
@@ -61,14 +61,14 @@ namespace webots_ros2_driver
     // Recognition publisher
     if (mCamera->hasRecognition())
     {
-      mRecogntionPublisher = mNode->create_publisher<vision_msgs::msg::Detection2DArray>(
+      mRecognitionPublisher = mNode->create_publisher<vision_msgs::msg::Detection2DArray>(
           mTopicName + "/recognitions",
           rclcpp::SensorDataQoS().reliable());
       mWebotsRecognitionPublisher = mNode->create_publisher<
           webots_ros2_msgs::msg::WbCameraRecognitionObjects>(
           mTopicName + "/recognitions/webots",
           rclcpp::SensorDataQoS().reliable());
-      mRecogntionMessage.header.frame_id = mFrameName;
+      mRecognitionMessage.header.frame_id = mFrameName;
       mWebotsRecognitionMessage.header.frame_id = mFrameName;
     }
   }
@@ -81,7 +81,7 @@ namespace webots_ros2_driver
     // Enable/Disable sensor
     const bool imageSubscriptionsExist = mImagePublisher->get_subscription_count() > 0;
     const bool recognitionSubscriptionsExist =
-        (mRecogntionPublisher != nullptr && mRecogntionPublisher->get_subscription_count() > 0) ||
+        (mRecognitionPublisher != nullptr && mRecognitionPublisher->get_subscription_count() > 0) ||
         (mWebotsRecognitionPublisher != nullptr && mWebotsRecognitionPublisher->get_subscription_count() > 0);
     const bool shouldBeEnabled = mAlwaysOn || imageSubscriptionsExist || recognitionSubscriptionsExist;
 
@@ -118,7 +118,7 @@ namespace webots_ros2_driver
       return;
 
     auto objects = mCamera->getRecognitionObjects();
-    mRecogntionMessage.header.stamp = mNode->get_clock()->now();
+    mRecognitionMessage.header.stamp = mNode->get_clock()->now();
     mWebotsRecognitionMessage.header.stamp = mNode->get_clock()->now();
 
     for (size_t i = 0; i < mCamera->getRecognitionNumberOfObjects(); i++)
@@ -141,7 +141,7 @@ namespace webots_ros2_driver
       detection.bbox.center.y = objects[i].position_on_image[1];
       detection.bbox.size_x = objects[i].size_on_image[0];
       detection.bbox.size_y = objects[i].size_on_image[1];
-      mRecogntionMessage.detections.push_back(detection);
+      mRecognitionMessage.detections.push_back(detection);
 
       // Object Info -> WbCameraRecognitionObject
       webots_ros2_msgs::msg::WbCameraRecognitionObject recognitionWebotsObject;
@@ -164,6 +164,6 @@ namespace webots_ros2_driver
       mWebotsRecognitionMessage.objects.push_back(recognitionWebotsObject);
     }
     mWebotsRecognitionPublisher->publish(mWebotsRecognitionMessage);
-    mRecogntionPublisher->publish(mRecogntionMessage);
+    mRecognitionPublisher->publish(mRecognitionMessage);
   }
 }
diff --git a/webots_ros2_driver/src/plugins/static/Ros2DistanceSensor.cpp b/webots_ros2_driver/src/plugins/static/Ros2DistanceSensor.cpp
@@ -26,25 +26,42 @@ namespace webots_ros2_driver
 
     assert(mDistanceSensor != NULL);
 
+    mLookupTable.assign(mDistanceSensor->getLookupTable(), mDistanceSensor->getLookupTable() + mDistanceSensor->getLookupTableSize() * 3);
+
+    const int size = mLookupTable.size();
+    const double maxValue = std::max(mLookupTable[0], mLookupTable[size - 3]);
+    const double minValue = std::min(mLookupTable[0], mLookupTable[size - 3]);
+    const double lowerStd = (mLookupTable[0] < mLookupTable[size - 3]) ? mLookupTable[2] * mLookupTable[0] : mLookupTable[size - 1] * mLookupTable[size - 3];
+    const double upperStd = (mLookupTable[0] > mLookupTable[size - 3]) ? mLookupTable[2] * mLookupTable[0] : mLookupTable[size - 1] * mLookupTable[size - 3];
+    const double minRange = minValue + lowerStd;
+    const double maxRange = maxValue - upperStd;
+
     mPublisher = mNode->create_publisher<sensor_msgs::msg::Range>(mTopicName, rclcpp::SensorDataQoS().reliable());
     mMessage.header.frame_id = mFrameName;
     mMessage.field_of_view = mDistanceSensor->getAperture();
-    mMessage.min_range = mDistanceSensor->getMinValue();
-    mMessage.max_range = mDistanceSensor->getMaxValue();
+    mMessage.min_range = minRange;
+    mMessage.max_range = maxRange;
     mMessage.radiation_type = sensor_msgs::msg::Range::INFRARED;
 
-    mLookupTable.assign(mDistanceSensor->getLookupTable(), mDistanceSensor->getLookupTable() + mDistanceSensor->getLookupTableSize());
+    if (mAlwaysOn) {
+      mDistanceSensor->enable(mPublishTimestepSyncedMs);
+      mIsEnabled = true;
+    }
   }
 
   void Ros2DistanceSensor::step()
   {
     if (!preStep())
       return;
 
-    // Enable/Disable sensor
-    const bool imageSubscriptionsExist = mPublisher->get_subscription_count() > 0;
-    const bool shouldBeEnabled = mAlwaysOn || imageSubscriptionsExist;
+    if (mIsEnabled)
+      publishRange();
 
+    if (mAlwaysOn)
+      return;
+
+    // Enable/Disable sensor
+    const bool shouldBeEnabled = mPublisher->get_subscription_count() > 0;
     if (shouldBeEnabled != mIsEnabled)
     {
       if (shouldBeEnabled)
@@ -53,16 +70,15 @@ namespace webots_ros2_driver
         mDistanceSensor->disable();
       mIsEnabled = shouldBeEnabled;
     }
-
-    // Publish data
-    if (mAlwaysOn || imageSubscriptionsExist)
-      publishRange();
   }
 
   void Ros2DistanceSensor::publishRange()
   {
+    const double value = mDistanceSensor->getValue();
+    if (std::isnan(value))
+      return;
     mMessage.header.stamp = mNode->get_clock()->now();
-    mMessage.range = interpolateLookupTable(mDistanceSensor->getValue(), mLookupTable);
+    mMessage.range = interpolateLookupTable(value, mLookupTable);
     mPublisher->publish(mMessage);
   }
 }
diff --git a/webots_ros2_driver/src/plugins/static/Ros2GPS.cpp b/webots_ros2_driver/src/plugins/static/Ros2GPS.cpp
@@ -38,18 +38,39 @@ namespace webots_ros2_driver
       mPointMessage.header.frame_id = mFrameName;
     }
     mVelocityPublisher = mNode->create_publisher<std_msgs::msg::Float32>(mTopicName + "/velocity", rclcpp::SensorDataQoS().reliable());
+
+    if (mAlwaysOn) {
+      mGPS->enable(mPublishTimestepSyncedMs);
+      mIsEnabled = true;
+    }
   }
 
   void Ros2GPS::step()
   {
     if (!preStep())
       return;
 
+    if (mIsEnabled) {
+      if (mPointPublisher != nullptr)
+        pubishPoint();
+      else if (mGPSPublisher != nullptr)
+        publishGPS();
+      else 
+        assert(false);
+
+      std_msgs::msg::Float32 mSpeedMessage;
+      mSpeedMessage.data = mGPS->getSpeed();
+      mVelocityPublisher->publish(mSpeedMessage);
+    }
+
+    if (mAlwaysOn)
+      return;
+
     // Enable/Disable sensor
     const bool pointSubscriptionsExist = mPointPublisher != nullptr && mPointPublisher->get_subscription_count() > 0;
     const bool gpsSubscriptionsExist = mGPSPublisher != nullptr && mGPSPublisher->get_subscription_count() > 0;
     const bool velocitySubscriptionsExist = mVelocityPublisher->get_subscription_count() > 0;
-    const bool shouldBeEnabled = mAlwaysOn || pointSubscriptionsExist || gpsSubscriptionsExist || velocitySubscriptionsExist;
+    const bool shouldBeEnabled = pointSubscriptionsExist || gpsSubscriptionsExist || velocitySubscriptionsExist;
     if (shouldBeEnabled != mIsEnabled)
     {
       if (shouldBeEnabled)
@@ -58,18 +79,6 @@ namespace webots_ros2_driver
         mGPS->disable();
       mIsEnabled = shouldBeEnabled;
     }
-
-    // Publish data
-    if (mPointPublisher && (mAlwaysOn || pointSubscriptionsExist))
-      pubishPoint();
-    if (mGPSPublisher && (mAlwaysOn || gpsSubscriptionsExist))
-      publishGPS();
-    if (mAlwaysOn || velocitySubscriptionsExist)
-    {
-      std_msgs::msg::Float32 mSpeedMessage;
-      mSpeedMessage.data = mGPS->getSpeed();
-      mVelocityPublisher->publish(mSpeedMessage);
-    }
   }
 
   void Ros2GPS::pubishPoint()

diff --git a/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp b/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp
@@ -22,7 +22,8 @@ namespace webots_ros2_driver
   void Ros2Lidar::init(webots_ros2_driver::WebotsNode *node, std::unordered_map<std::string, std::string> &parameters)
   {
     Ros2SensorPlugin::init(node, parameters);
-    mIsEnabled = false;
+    mIsSensorEnabled = false;
+    mIsPointCloudEnabled = false;
     mLidar = mNode->robot()->getLidar(parameters["name"]);
 
     assert(mLidar != NULL);
@@ -73,37 +74,52 @@ namespace webots_ros2_driver
     mPointCloudMessage.fields[2].count = 1;
     mPointCloudMessage.fields[2].offset = 8;
     mPointCloudMessage.is_bigendian = false;
+
+    if (mAlwaysOn) {
+      mLidar->enable(mPublishTimestepSyncedMs);
+      mLidar->enablePointCloud();
+      mIsSensorEnabled = true;
+      mIsPointCloudEnabled = true;
+    }
   }
 
   void Ros2Lidar::step()
   {
     if (!preStep())
       return;
 
-    // Enable/Disable sensor
-    const bool shouldBeEnabled = mAlwaysOn ||
-                                 mPointCloudPublisher->get_subscription_count() > 0 ||
+    if (mIsSensorEnabled && mLaserPublisher != nullptr)
+      publishLaserScan();
+
+    if (mIsPointCloudEnabled)
+      publishPointCloud();
+
+    if (mAlwaysOn)
+      return;
+
+    const bool shouldPointCloudBeEnabled = mPointCloudPublisher->get_subscription_count() > 0;
+    const bool shouldSensorBeEnabled = shouldPointCloudBeEnabled ||
                                  (mLaserPublisher != nullptr && mLaserPublisher->get_subscription_count() > 0);
 
-    if (shouldBeEnabled != mIsEnabled)
+    // Enable/Disable sensor
+    if (shouldSensorBeEnabled != mIsSensorEnabled)
     {
-      if (shouldBeEnabled)
+      if (shouldSensorBeEnabled)
         mLidar->enable(mPublishTimestepSyncedMs);
       else
         mLidar->disable();
-      mIsEnabled = shouldBeEnabled;
+      mIsSensorEnabled = shouldSensorBeEnabled;
     }
 
-    // Publish data
-    if (mLaserPublisher != nullptr && (mLaserPublisher->get_subscription_count() > 0 || mAlwaysOn))
-      publishLaserScan();
-    if (mPointCloudPublisher->get_subscription_count() > 0 || mAlwaysOn)
+    // Enable/Disable point cloud
+    if (shouldPointCloudBeEnabled != mIsPointCloudEnabled)
     {
-      mLidar->enablePointCloud();
-      publishPointCloud();
+      if (shouldPointCloudBeEnabled)
+        mLidar->enablePointCloud();
+      else
+        mLidar->disablePointCloud();
+      mIsPointCloudEnabled = shouldPointCloudBeEnabled;
     }
-    else
-      mLidar->disablePointCloud();
   }
 
   void Ros2Lidar::publishPointCloud()