publisher_map optionally specify sensor

include/publisher_laser_map.h

@@ -59,6 +59,7 @@ class PublisherLaserMap: public Publisher
     protected:
 
         double update_dist_th_, update_angle_th_;
+        SensorBaseConstPtr sensor_;
 
         // OCCUPANCY MAP parameters and auxiliar variables
         double Lfree_, Lobst_, Lobst_th_, Lfree_th_;

src/publisher_laser_map.cpp

@@ -38,6 +38,7 @@ PublisherLaserMap::PublisherLaserMap(const std::string& _unique_name,
                                      const ParamsServer& _server,
                                      ProblemConstPtr _problem) :
     Publisher(_unique_name, _server, _problem),
+    sensor_(nullptr),
     n_cells_(Eigen::Vector2i::Constant(100)),
     resized_(false),
     logodds_array_(Eigen::ArrayXXd::Zero(n_cells_(0),n_cells_(1))),
@@ -48,6 +49,12 @@ PublisherLaserMap::PublisherLaserMap(const std::string& _unique_name,
     // common params
     update_dist_th_             = _server.getParam<double>      (prefix_ + "/update_dist_th");
     update_angle_th_            = _server.getParam<double>      (prefix_ + "/update_angle_th");
+    auto sensor_name            = getParamWithDefault<std::string> (_server, prefix_ + "/sensor", "");
+    if (not sensor_name.empty())
+    {
+        sensor_ = _problem->findSensor(sensor_name);
+        WOLF_WARN_COND(not sensor_, "PublisherLaserMap: Sensor name not found: ", sensor_name);
+    }
 
     // occmap params
     occ_msg_.header.frame_id    = _server.getParam<std::string> (prefix_ + "/map_frame_id");
@@ -150,7 +157,9 @@ void PublisherLaserMap::updateTrajectory()
         for (auto cap : cap_list)
         {
             auto cap_laser = std::dynamic_pointer_cast<const CaptureLaser2d>(cap);
-            if (cap_laser and not cap_laser->isRemoving())
+            if (cap_laser and 
+                not cap_laser->isRemoving() and
+                (not sensor_ or cap_laser->getSensor() == sensor_))
                 storeScan(frame_pair.second, cap_laser);
         }