Merge branch 'fix/race_condition_in_elevation_mapping_sensor_processo…

…r' into 'master'

[elevation_mapping] Fix race condition in elevation mapping sensor processor

GitOrigin-RevId: d0474f0466e655dc52a26adc884bea7f2fbd7e6e

README.md

@@ -244,18 +244,22 @@ This is the main Robot-Centric Elevation Mapping node. It uses the distance sens
     Here you specify your inputs to elevation mapping, currently "pointcloud" inputs are supported. 
 
     Example configuration:
-    ```
+    ```yaml
     input_sources:
-    - name: front # A name to identify the input source
-      type: pointcloud # Supported types: pointcloud
-      topic: /lidar_front/depth/points
-      queue_size: 1
-      publish_on_update: true # Wheter to publish the elevation map after a callback from this source. 
-    - name: rear
-      type: pointcloud
-      topic: /lidar_rear/depth/points
-      queue_size: 5
-      publish_on_update: false
+        front: # A name to identify the input source
+          type: pointcloud # Supported types: pointcloud
+          topic: /lidar_front/depth/points
+          queue_size: 1
+          publish_on_update: true # Wheter to publish the elevation map after a callback from this source. 
+        rear:
+          type: pointcloud
+          topic: /lidar_rear/depth/points
+          queue_size: 5
+          publish_on_update: false
+    ```
+    No input sources can be configured with an empty array:
+    ```yaml
+    input_sources: []
     ```
 * **`robot_pose_topic`** (string, default: "/robot_state/pose")
 

elevation_mapping/include/elevation_mapping/ElevationMapping.hpp

@@ -13,7 +13,6 @@
 #include "elevation_mapping/RobotMotionMapUpdater.hpp"
 #include "elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp"
 #include "elevation_mapping/input_sources/InputSourceManager.hpp"
-#include "elevation_mapping/sensor_processors/SensorProcessorBase.hpp"
 
 // Grid Map
 #include <grid_map_msgs/GetGridMap.h>
@@ -67,8 +66,10 @@ class ElevationMapping {
    *
    * @param pointCloudMsg    The point cloud to be fused with the existing data.
    * @param publishPointCloud If true, publishes the pointcloud after updating the map.
+   * @param sensorProcessor_ The sensorProcessor to use in this callback.
    */
-  void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg, bool publishPointCloud);
+  void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg, bool publishPointCloud,
+                          const SensorProcessorBase::Ptr& sensorProcessor_);
 
   /*!
    * Callback function for the update timer. Forces an update of the map from
@@ -291,7 +292,7 @@ class ElevationMapping {
   //! Elevation map.
   ElevationMap map_;
 
-  //! Sensor processors.
+  //! Sensor processors. Deprecated use the one from input sources instead.
   SensorProcessorBase::Ptr sensorProcessor_;
 
   //! Robot motion elevation map updater.

elevation_mapping/include/elevation_mapping/input_sources/Input.hpp

@@ -12,6 +12,8 @@
 #include <ros/ros.h>
 #include <string>
 
+#include "elevation_mapping/sensor_processors/SensorProcessorBase.hpp"
+
 namespace elevation_mapping {
 class ElevationMapping;  // Forward declare to avoid cyclic import dependency.
 
@@ -22,21 +24,24 @@ class ElevationMapping;  // Forward declare to avoid cyclic import dependency.
 class Input {
  public:
   template <typename MsgT>
-  using CallbackT = void (ElevationMapping::*)(const boost::shared_ptr<const MsgT>&, bool);
+  using CallbackT = void (ElevationMapping::*)(const boost::shared_ptr<const MsgT>&, bool, const SensorProcessorBase::Ptr&);
 
   /**
    * @brief Constructor.
    * @param nh Reference to the nodeHandle of the manager. Used to subscribe
    * to inputs.
    */
-  explicit Input(ros::NodeHandle& nh);
+  explicit Input(ros::NodeHandle nh);
 
   /**
    * @brief Configure the input source.
+   * @param name Name of this input source.
    * @param parameters The configuration parameters.
+   * @param generalSensorProcessorParameters Parameters shared by all sensor processors.
    * @return True if configuring was successful.
    */
-  bool configure(const XmlRpc::XmlRpcValue& parameters);
+  bool configure(std::string name, const XmlRpc::XmlRpcValue& parameters,
+                 const SensorProcessorBase::GeneralParameters& generalSensorProcessorParameters);
 
   /**
    * @brief Registers the corresponding callback in the elevationMap.
@@ -59,22 +64,36 @@ class Input {
   std::string getType() { return type_; }
 
  private:
+  /**
+   * @brief Configures the used sensor processor from the given parameters.
+   * @param name The name of this input source
+   * @param parameters The parameters of this input source
+   * @param generalSensorProcessorParameters  General parameters needed for the sensor processor that are not specific to this sensor
+   * processor.
+   * @return True if successful.
+   */
+  bool configureSensorProcessor(std::string name, const XmlRpc::XmlRpcValue& parameters,
+                                const SensorProcessorBase::GeneralParameters& generalSensorProcessorParameters);
+
+  // ROS connection.
+  ros::Subscriber subscriber_;
+  ros::NodeHandle nodeHandle_;
+
+  //! Sensor processor
+  SensorProcessorBase::Ptr sensorProcessor_;
+
   // Parameters.
   std::string name_;
   std::string type_;
   uint32_t queueSize_;
   std::string topic_;
   bool publishOnUpdate_;
-
-  // ROS connection.
-  ros::Subscriber subscriber_;
-  ros::NodeHandle& nodeHandle_;
 };
 
 template <typename MsgT>
 void Input::registerCallback(ElevationMapping& map, CallbackT<MsgT> callback) {
-  subscriber_ =
-      nodeHandle_.subscribe<MsgT>(topic_, queueSize_, std::bind(callback, std::ref(map), std::placeholders::_1, publishOnUpdate_));
+  subscriber_ = nodeHandle_.subscribe<MsgT>(
+      topic_, queueSize_, std::bind(callback, std::ref(map), std::placeholders::_1, publishOnUpdate_, std::ref(sensorProcessor_)));
   ROS_INFO("Subscribing to %s: %s, queue_size: %i.", type_.c_str(), topic_.c_str(), queueSize_);
 }
 

elevation_mapping/include/elevation_mapping/sensor_processors/LaserSensorProcessor.hpp

@@ -20,9 +20,8 @@ class LaserSensorProcessor : public SensorProcessorBase {
   /*!
    * Constructor.
    * @param nodeHandle the ROS node handle.
-   * @param transformListener the ROS transform listener.
    */
-  LaserSensorProcessor(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener);
+  LaserSensorProcessor(ros::NodeHandle& nodeHandle, const SensorProcessorBase::GeneralParameters& generalParameters);
 
   /*!
    * Destructor.

elevation_mapping/include/elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp

@@ -20,9 +20,8 @@ class PerfectSensorProcessor : public SensorProcessorBase {
   /*!
    * Constructor.
    * @param nodeHandle the ROS node handle.
-   * @param transformListener the ROS transform listener.
    */
-  PerfectSensorProcessor(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener);
+  PerfectSensorProcessor(ros::NodeHandle& nodeHandle, const SensorProcessorBase::GeneralParameters& generalParameters);
 
   /*!
    * Destructor.

elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp

@@ -41,13 +41,22 @@ class SensorProcessorBase {
  public:
   using Ptr = std::unique_ptr<SensorProcessorBase>;
   friend class ElevationMapping;
+  friend class Input;
+
+  struct GeneralParameters {
+    std::string robotBaseFrameId_;
+    std::string mapFrameId_;
+
+    GeneralParameters(std::string robotBaseFrameId = "robot", std::string mapFrameId = "map")
+        : robotBaseFrameId_(std::move(robotBaseFrameId)), mapFrameId_(std::move(mapFrameId)) {}
+  };
 
   /*!
    * Constructor.
    * @param nodeHandle the ROS node handle.
-   * @param transformListener the ROS transform listener.
+   * @param generalConfig General parameters that the sensor processor must know in order to work. // TODO (magnus) improve documentation.
    */
-  SensorProcessorBase(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener);
+  SensorProcessorBase(ros::NodeHandle& nodeHandle, const GeneralParameters& generalConfig);
 
   /*!
    * Destructor.
@@ -133,10 +142,7 @@ class SensorProcessorBase {
   ros::NodeHandle& nodeHandle_;
 
   //! TF transform listener.
-  tf::TransformListener& transformListener_;
-
-  //! The timeout duration for the lookup of the transformation between sensor frame and target frame.
-  ros::Duration transformListenerTimeout_;
+  tf::TransformListener transformListener_;
 
   //! Rotation from Base to Sensor frame (C_SB)
   kindr::RotationMatrixD rotationBaseToSensor_;
@@ -153,11 +159,7 @@ class SensorProcessorBase {
   //! Transformation from Sensor to Map frame
   Eigen::Affine3d transformationSensorToMap_;
 
-  //! TF frame id of the map.
-  std::string mapFrameId_;
-
-  //! TF frame id of the robot base.
-  std::string robotBaseFrameId_;
+  GeneralParameters generalParameters_;
 
   //! TF frame id of the range sensor for the point clouds.
   std::string sensorFrameId_;

elevation_mapping/include/elevation_mapping/sensor_processors/StereoSensorProcessor.hpp

@@ -25,9 +25,8 @@ class StereoSensorProcessor : public SensorProcessorBase {
   /*!
    * Constructor.
    * @param nodeHandle the ROS node handle.
-   * @param transformListener the ROS transform listener.
    */
-  StereoSensorProcessor(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener);
+  StereoSensorProcessor(ros::NodeHandle& nodeHandle, const SensorProcessorBase::GeneralParameters& generalParameters);
 
   /*!
    * Destructor.

elevation_mapping/include/elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp

@@ -20,9 +20,8 @@ class StructuredLightSensorProcessor : public SensorProcessorBase {
   /*!
    * Constructor.
    * @param nodeHandle the ROS node handle.
-   * @param transformListener the ROS transform listener.
    */
-  StructuredLightSensorProcessor(ros::NodeHandle& nodeHandle, tf::TransformListener& transformListener);
+  StructuredLightSensorProcessor(ros::NodeHandle& nodeHandle, const SensorProcessorBase::GeneralParameters& generalParameters);
 
   /*!
    * Destructor.

elevation_mapping/src/ElevationMapping.cpp

@@ -99,7 +99,8 @@ void ElevationMapping::setupSubscribers() {  // Handle deprecated point_cloud_to
   }
   if (!configuredInputSources && hasDeprecatedPointcloudTopic) {
     pointCloudSubscriber_ = nodeHandle_.subscribe<sensor_msgs::PointCloud2>(
-        pointCloudTopic_, 1, std::bind(&ElevationMapping::pointCloudCallback, this, std::placeholders::_1, true));
+        pointCloudTopic_, 1,
+        std::bind(&ElevationMapping::pointCloudCallback, this, std::placeholders::_1, true, std::ref(sensorProcessor_)));
   }
   if (configuredInputSources) {
     inputSources_.registerCallbacks(*this, make_pair("pointcloud", &ElevationMapping::pointCloudCallback));
@@ -230,17 +231,20 @@ bool ElevationMapping::readParameters() {
   nodeHandle_.param("init_submap_height_offset", initSubmapHeightOffset_, 0.0);
   nodeHandle_.param("target_frame_init_submap", targetFrameInitSubmap_, std::string("/footprint"));
 
-  // SensorProcessor parameters.
+  // SensorProcessor parameters. Deprecated, use the sensorProcessor from within input sources instead!
   std::string sensorType;
   nodeHandle_.param("sensor_processor/type", sensorType, std::string("structured_light"));
+
+  SensorProcessorBase::GeneralParameters generalSensorProcessorConfig{nodeHandle_.param("robot_base_frame_id", std::string("/robot")),
+                                                                      mapFrameId_};
   if (sensorType == "structured_light") {
-    sensorProcessor_.reset(new StructuredLightSensorProcessor(nodeHandle_, transformListener_));
+    sensorProcessor_.reset(new StructuredLightSensorProcessor(nodeHandle_, generalSensorProcessorConfig));
   } else if (sensorType == "stereo") {
-    sensorProcessor_.reset(new StereoSensorProcessor(nodeHandle_, transformListener_));
+    sensorProcessor_.reset(new StereoSensorProcessor(nodeHandle_, generalSensorProcessorConfig));
   } else if (sensorType == "laser") {
-    sensorProcessor_.reset(new LaserSensorProcessor(nodeHandle_, transformListener_));
+    sensorProcessor_.reset(new LaserSensorProcessor(nodeHandle_, generalSensorProcessorConfig));
   } else if (sensorType == "perfect") {
-    sensorProcessor_.reset(new PerfectSensorProcessor(nodeHandle_, transformListener_));
+    sensorProcessor_.reset(new PerfectSensorProcessor(nodeHandle_, generalSensorProcessorConfig));
   } else {
     ROS_ERROR("The sensor type %s is not available.", sensorType.c_str());
   }
@@ -283,7 +287,8 @@ void ElevationMapping::visibilityCleanupThread() {
   }
 }
 
-void ElevationMapping::pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg, bool publishPointCloud) {
+void ElevationMapping::pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg, bool publishPointCloud,
+                                          const SensorProcessorBase::Ptr& sensorProcessor_) {
   ROS_DEBUG("Processing data from: %s", pointCloudMsg->header.frame_id.c_str());
   if (!updatesEnabled_) {
     ROS_WARN_THROTTLE(10, "Updating of elevation map is disabled. (Warning message is throttled, 10s.)");

elevation_mapping/src/input_sources/Input.cpp

@@ -8,11 +8,17 @@
 
 #include "elevation_mapping/input_sources/Input.hpp"
 
+#include "elevation_mapping/sensor_processors/LaserSensorProcessor.hpp"
+#include "elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp"
+#include "elevation_mapping/sensor_processors/StereoSensorProcessor.hpp"
+#include "elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp"
+
 namespace elevation_mapping {
 
-Input::Input(ros::NodeHandle& nh) : nodeHandle_(nh), queueSize_(0), publishOnUpdate_(true) {}
+Input::Input(ros::NodeHandle nh) : nodeHandle_(nh), queueSize_(0), publishOnUpdate_(true) {}
 
-bool Input::configure(const XmlRpc::XmlRpcValue& parameters) {
+bool Input::configure(std::string name, const XmlRpc::XmlRpcValue& parameters,
+                      const SensorProcessorBase::GeneralParameters& generalSensorProcessorParameters) {
   // Configuration Guards.
   if (parameters.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
     ROS_ERROR(
@@ -24,25 +30,25 @@ bool Input::configure(const XmlRpc::XmlRpcValue& parameters) {
 
   // Check that parameters exist and has an appropriate type.
   using nameAndType = std::pair<std::string, XmlRpc::XmlRpcValue::Type>;
-  for (const nameAndType& member : std::vector<nameAndType>{{"name", XmlRpc::XmlRpcValue::TypeString},
-                                                            {"type", XmlRpc::XmlRpcValue::TypeString},
+  for (const nameAndType& member : std::vector<nameAndType>{{"type", XmlRpc::XmlRpcValue::TypeString},
                                                             {"topic", XmlRpc::XmlRpcValue::TypeString},
                                                             {"queue_size", XmlRpc::XmlRpcValue::TypeInt},
-                                                            {"publish_on_update", XmlRpc::XmlRpcValue::TypeBoolean}}) {
+                                                            {"publish_on_update", XmlRpc::XmlRpcValue::TypeBoolean},
+                                                            {"sensor_processor", XmlRpc::XmlRpcValue::TypeStruct}}) {
     if (!parameters.hasMember(member.first)) {
-      ROS_ERROR("Could not configure input source because no %s was given.", member.first.c_str());
+      ROS_ERROR("Could not configure input source %s because no %s was given.", name.c_str(), member.first.c_str());
       return false;
     }
     if (parameters[member.first].getType() != member.second) {
       ROS_ERROR(
-          "Could not configure input source because member %s has the "
+          "Could not configure input source %s because member %s has the "
           "wrong type.",
-          member.first.c_str());
+          name.c_str(), member.first.c_str());
       return false;
     }
   }
 
-  name_ = static_cast<std::string>(parameters["name"]);
+  name_ = name;
   type_ = static_cast<std::string>(parameters["type"]);
   topic_ = static_cast<std::string>(parameters["topic"]);
   const int& queueSize = static_cast<int>(parameters["queue_size"]);
@@ -53,13 +59,54 @@ bool Input::configure(const XmlRpc::XmlRpcValue& parameters) {
     return false;
   }
   publishOnUpdate_ = static_cast<bool>(parameters["publish_on_update"]);
-  ROS_DEBUG("Configured %s:%s @ %s (publishing_on_update: %s)\n", type_.c_str(), name_.c_str(), nodeHandle_.resolveName(topic_).c_str(),
-            publishOnUpdate_ ? "true" : "false");
+
+  // SensorProcessor
+  if (!configureSensorProcessor(name, parameters, generalSensorProcessorParameters)) {
+    return false;
+  }
+
+  ROS_DEBUG("Configured %s:%s @ %s (publishing_on_update: %s), using %s to process data.\n", type_.c_str(), name_.c_str(),
+            nodeHandle_.resolveName(topic_).c_str(), publishOnUpdate_ ? "true" : "false",
+            static_cast<std::string>(parameters["sensor_processor"]["type"]).c_str());
   return true;
 }
 
 std::string Input::getSubscribedTopic() const {
   return nodeHandle_.resolveName(topic_);
 }
 
+bool Input::configureSensorProcessor(std::string name, const XmlRpc::XmlRpcValue& parameters,
+                                     const SensorProcessorBase::GeneralParameters& generalSensorProcessorParameters) {
+  if (!parameters["sensor_processor"].hasMember("type")) {
+    ROS_ERROR("Could not configure sensor processor of input source %s because no type was given.", name.c_str());
+    return false;
+  }
+  if (parameters["sensor_processor"]["type"].getType() != XmlRpc::XmlRpcValue::TypeString) {
+    ROS_ERROR(
+        "Could not configure sensor processor of input source %s because the member 'type' has the "
+        "wrong type.",
+        name.c_str());
+    return false;
+  }
+  std::string sensorType = static_cast<std::string>(parameters["sensor_processor"]["type"]);
+  if (sensorType == "structured_light") {
+    sensorProcessor_.reset(new StructuredLightSensorProcessor(nodeHandle_, generalSensorProcessorParameters));
+  } else if (sensorType == "stereo") {
+    sensorProcessor_.reset(new StereoSensorProcessor(nodeHandle_, generalSensorProcessorParameters));
+  } else if (sensorType == "laser") {
+    sensorProcessor_.reset(new LaserSensorProcessor(nodeHandle_, generalSensorProcessorParameters));
+  } else if (sensorType == "perfect") {
+    sensorProcessor_.reset(new PerfectSensorProcessor(nodeHandle_, generalSensorProcessorParameters));
+  } else {
+    ROS_ERROR("The sensor type %s is not available.", sensorType.c_str());
+    return false;
+  }
+
+  if (!sensorProcessor_->readParameters()) {
+    return false;
+  }
+
+  return true;
+}
+
 }  // namespace elevation_mapping