CMakeLists.txt

@@ -20,6 +20,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## System dependencies are found with CMake's conventions
 find_package(wolfcore REQUIRED)
 find_package(wolflaser REQUIRED)
+find_package(PCL 1.10 COMPONENTS common registration io)
 
 # produces the XXXConfig.cmake file among other things
 catkin_package(
@@ -44,22 +45,30 @@ add_library(subscriber_${PROJECT_NAME}
 )
 
 get_target_property(wolflaser_INCLUDE_DIR wolflaser INTERFACE_INCLUDE_DIRECTORIES)
-message("wolflaser_INCLUDE_DIRS: ${wolflaser_INCLUDE_DIRS}")
 find_file(FALKO laser/processor/processor_loop_closure_falko.h PATHS ${wolflaser_INCLUDE_DIR})
 find_file(ICP laser/processor/processor_odom_icp.h PATHS ${wolflaser_INCLUDE_DIR})
+find_file(PCL laser/sensor/sensor_laser_3d.h PATHS ${wolflaser_INCLUDE_DIR})
 
 if (FALKO)
 	message("Found 'laser/processor/processor_loop_closure_falko.h'. Compiling publisher_falko.")
 	target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_falko.cpp)
 else()
-  message("Didn't find 'laser/processor/processor_loop_closure_falko.h'. Not compiling publisher_falko.")
+  message(WARNING "Didn't find 'laser/processor/processor_loop_closure_falko.h'. Not compiling publisher_falko.")
 endif()
 
 if (ICP)
 	message("Found 'laser/processor/processor_odom_icp.h'. Compiling publisher_odom_icp.")
 	target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_odom_icp.cpp)
 else()
-  message("Didn't find 'laser/processor/processor_odom_icp.h'. Not compiling publisher_odom_icp.")
+  message(WARNING "Didn't find 'laser/processor/processor_odom_icp.h'. Not compiling publisher_odom_icp.")
+endif ()
+
+if (PCL)
+	message("Found 'laser/sensor/sensor_laser3d.h'. Compiling publisher_laser_map3d and subscriber_laser3d.")
+	target_sources(publisher_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/publisher_laser_map3d.cpp)
+  target_sources(subscriber_${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src/subscriber_laser3d.cpp)
+else()
+  message(WARNING "Didn't find 'laser/sensor/sensor_laser3d.h'. Not compiling publisher_laser_map3d and subscriber_laser3d.")
 endif ()
 
 ## Specify libraries to link a library or executable target against

include/publisher_laser_map3d.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <sensor_msgs/PointCloud2.h>
+
+/**************************
+ *      PCL includes      *
+ **************************/
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include <core/common/wolf.h>
+#include <core/problem/problem.h>
+#include <laser/capture/capture_laser_3d.h>
+
+#include "publisher.h"
+
+namespace wolf
+{
+
+struct PointCloudStruct
+{
+    CaptureLaser3dConstPtr capture;
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr subsampled_pointcloud;
+};
+
+class PublisherLaserMap3d: public Publisher
+{
+    protected:
+
+        double update_dist_th_, update_angle_th_;
+
+        // AGGREGATED POINTCLOUD
+        sensor_msgs::PointCloud2 pointcloud_msg_;
+        pcl::PointCloud<pcl::PointXYZRGB> aggregated_pc_;
+        pcl::VoxelGrid<pcl::PointXYZ> pcl_filter_grid_;
+        double color_cycle_period_;
+
+        // std::maps
+        std::map<FrameBaseConstPtr, std::list<PointCloudStruct>> pointclouds_;
+        std::map<FrameBaseConstPtr, VectorComposite> current_trajectory_, last_trajectory_;
+
+    public:
+        PublisherLaserMap3d(const std::string& _unique_name,
+                            const ParamsServer& _server,
+                            ProblemConstPtr _problem);
+        WOLF_PUBLISHER_CREATE(PublisherLaserMap3d);
+
+        virtual ~PublisherLaserMap3d(){};
+        void initialize(ros::NodeHandle &nh, const std::string& topic) override;
+        void publishDerived() override;
+
+    protected:
+        void updateTrajectory();
+        bool trajectoryChanged(const std::map<FrameBaseConstPtr,VectorComposite>& _last_trajectory,
+                               const std::map<FrameBaseConstPtr,VectorComposite>& _current_trajectory) const;
+        void storePointCloud(FrameBaseConstPtr frame, CaptureLaser3dConstPtr capture);
+
+        void reset();
+        bool updatePointCloud();
+        void addCapture(const PointCloudStruct& capture_data, const VectorComposite& pose);
+        void updateMsg();
+
+        void computeRGB(const TimeStamp& _ts, int& r, int& g, int& b) const;
+};
+
+}

include/subscriber_laser2d.h

@@ -24,24 +24,13 @@
 /**************************
  *      WOLF includes     *
  **************************/
-//#include <core/capture/capture_odom_2d.h>
-//#include <core/sensor/sensor_odom_2d.h>
-//#include <core/processor/processor_odom_2d.h>
-//#include <core/yaml/parser_yaml.h>
 #include <core/common/wolf.h>
 #include <core/problem/problem.h>
-//#include <core/utils/params_server.h>
 #include <laser/capture/capture_laser_2d.h>
 #include <laser/sensor/sensor_laser_2d.h>
 
 #include "subscriber.h"
 
-/**************************
- *     CERES includes     *
- **************************/
-#include <core/ceres_wrapper/solver_ceres.h>
-//#include "glog/logging.h"
-
 /**************************
  *      ROS includes      *
  **************************/

include/subscriber_laser3d.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+/**************************
+ *      WOLF includes     *
+ **************************/
+#include <laser/sensor/sensor_laser_3d.h>
+#include "subscriber.h"
+
+/**************************
+ *      ROS includes      *
+ **************************/
+#include <sensor_msgs/PointCloud2.h>
+
+namespace wolf
+{
+class SubscriberLaser3d : public Subscriber
+{
+    protected:
+        SensorLaser3dPtr sensor_laser_;
+        //SensorBasePtr sensor_laser_;
+
+    public:
+        // Constructor
+        SubscriberLaser3d(const std::string& _unique_name,
+                          const ParamsServer& _server,
+                          const SensorBasePtr _sensor_ptr);
+        WOLF_SUBSCRIBER_CREATE(SubscriberLaser3d);
+
+        void initialize(ros::NodeHandle& nh, const std::string& topic) override;
+
+        void callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
+};
+
+}

src/publisher_laser_map.cpp

@@ -138,27 +138,29 @@ void PublisherLaserMap::updateTrajectory()
     auto frame_map = problem_->getTrajectory()->getFrameMap();
     for (auto frame_pair : frame_map)
     {
-        if (frame_pair.second->isRemoving())
+        if (frame_pair.second->isRemoving() or 
+            not frame_pair.second->hasStateBlock('P') or 
+            not frame_pair.second->hasStateBlock('O'))
             continue;
-
-        // insert frame to maps
-        current_trajectory_[frame_pair.second] = frame_pair.second->getState("PO").vector("PO");
-        scans_[frame_pair.second] = std::list<ScanData>();
+        
+        // FIXME: NEAR FUTURE IMPLEMENTATION
+        // if (frame_pair.second->isRemoving() or not frame_pair.second->has("PO"))
+        //     continue;
 
         // search all CaptureLaser2d
-        auto cap_list = frame_pair.second->getCaptureList();
-        for (auto cap : cap_list)
-        {
-            auto cap_laser = std::dynamic_pointer_cast<const CaptureLaser2d>(cap);
-            if (cap_laser and not cap_laser->isRemoving())
-                storeScan(frame_pair.second, cap_laser);
-        }
+        auto cap_laser2d_list = frame_pair.second->getCapturesOfType<CaptureLaser2d>();
 
-        // remove frames without scans
-        if (scans_.at(frame_pair.second).empty())
+        if (not cap_laser2d_list.empty())
         {
-            scans_.erase(frame_pair.second);
-            current_trajectory_.erase(frame_pair.second);
+            // insert frame to map
+            current_trajectory_[frame_pair.second] = frame_pair.second->getState("PO").vector("PO");
+            
+            // insert scans to map
+            scans_[frame_pair.second] = std::list<ScanData>();
+            for (auto cap : cap_laser2d_list)
+            {
+                storeScan(frame_pair.second, cap);
+            }
         }
     }
 }

src/publisher_laser_map3d.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "../include/publisher_laser_map3d.h"
+#include <core/math/SE3.h>
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.h>
+
+using namespace pcl;
+using namespace Eigen;
+
+namespace wolf
+{
+
+PublisherLaserMap3d::PublisherLaserMap3d(const std::string& _unique_name,
+                                         const ParamsServer& _server,
+                                         ProblemConstPtr _problem) :
+    Publisher(_unique_name, _server, _problem),
+    aggregated_pc_(0,0)
+{
+    assert(_problem->getDim() == 3 && "PublisherLaserMap3d only implemented for 3D problems");
+
+    update_dist_th_             = _server.getParam<double>      (prefix_ + "/update_dist_th");
+    update_angle_th_            = _server.getParam<double>      (prefix_ + "/update_angle_th");
+    pointcloud_msg_.header.frame_id     = _server.getParam<std::string> (prefix_ + "/map_frame_id");
+    color_cycle_period_         = getParamWithDefault<double>(_server, prefix_ + "/color_cycle_period", 10);
+    double subsample_size       = getParamWithDefault<double>(_server, prefix_ + "/subsample_size", 0.2);
+    pcl_filter_grid_.setLeafSize(subsample_size, subsample_size, subsample_size);
+    
+}
+
+void PublisherLaserMap3d::initialize(ros::NodeHandle& nh, const std::string& topic)
+{
+    publisher_ = nh.advertise<sensor_msgs::PointCloud2>(topic, 1);
+}
+
+void PublisherLaserMap3d::publishDerived()
+{
+    if (publisher_.getNumSubscribers() == 0)
+        return;
+
+    // update trajectory (scans and poses)
+    updateTrajectory();
+
+    // AGGREGATED POINTCLOUD
+    if (publisher_.getNumSubscribers() != 0)
+    {
+        // Reset pointcloud map
+        if (trajectoryChanged(last_trajectory_, current_trajectory_))
+            reset();
+
+        // Recompute pointcloud map
+        if(updatePointCloud())
+            updateMsg();
+
+        // publish
+        publisher_.publish(pointcloud_msg_);
+    }
+}
+
+void PublisherLaserMap3d::updateTrajectory()
+{
+    current_trajectory_.clear();
+    pointclouds_.clear();
+
+    // copy new trajectory poses and scans
+    auto frame_map = problem_->getTrajectory()->getFrameMap();
+    for (auto frame_pair : frame_map)
+    {
+        if (frame_pair.second->isRemoving() or 
+            not frame_pair.second->hasStateBlock('P') or 
+            not frame_pair.second->hasStateBlock('O'))
+            continue;
+        
+        // FIXME: NEAR FUTURE IMPLEMENTATION
+        // if (frame_pair.second->isRemoving() or not frame_pair.second->has("PO"))
+        //     continue;
+
+        // search all CaptureLaser3d
+        auto cap_laser3d_list = frame_pair.second->getCapturesOfType<CaptureLaser3d>();
+
+        if (not cap_laser3d_list.empty())
+        {
+            // insert frame to map
+            current_trajectory_[frame_pair.second] = frame_pair.second->getState("PO");
+
+            // insert scans to map
+            pointclouds_[frame_pair.second] = std::list<PointCloudStruct>();
+            for (auto cap : cap_laser3d_list)
+            {
+                storePointCloud(frame_pair.second, cap);
+            }
+        }
+    }
+}
+
+void PublisherLaserMap3d::storePointCloud(FrameBaseConstPtr frame, CaptureLaser3dConstPtr capture)
+{
+    PointCloudStruct pc_struct;
+    pc_struct.capture = capture;
+    
+    // subsample pointcloud
+    auto sub_pc = PointCloud<PointXYZ>();
+    pcl_filter_grid_.setInputCloud(capture->getPointCloud());
+    pcl_filter_grid_.filter(sub_pc);
+
+    // color pointcloud
+    int r, g, b;
+    computeRGB(capture->getTimeStamp(), r, g, b);
+    pc_struct.subsampled_pointcloud = make_shared<PointCloud<PointXYZRGB>>(sub_pc.width, 
+                                                                                sub_pc.height, 
+                                                                                PointXYZRGB(r, g, b));
+    assert(pc_struct.subsampled_pointcloud->points.size() == sub_pc.points.size());
+    for (auto i = 0; i < sub_pc.points.size(); i++) 
+    {
+        pc_struct.subsampled_pointcloud->points[i].x = sub_pc.points[i].x;
+        pc_struct.subsampled_pointcloud->points[i].y = sub_pc.points[i].y;
+        pc_struct.subsampled_pointcloud->points[i].z = sub_pc.points[i].z;
+    }
+
+    // store data
+    pointclouds_.at(frame).push_back(pc_struct);
+}
+
+bool PublisherLaserMap3d::trajectoryChanged(const std::map<FrameBaseConstPtr,VectorComposite>& _old_trajectory,
+                                            const std::map<FrameBaseConstPtr,VectorComposite>& _new_trajectory) const
+{
+    // check if trajectory changed enough
+    for (auto old_pair : _old_trajectory)
+    {
+        assert(old_pair.second.count('P') and old_pair.second.count('O'));
+        //assert(old_pair.second.has("PO")); //FIXME: New implementation comming soon
+
+        // any old frame was removed -> recompute
+        if (_new_trajectory.count(old_pair.first) == 0)
+        {
+            WOLF_INFO("PublisherLaserMap3d::trajectoryChanged: Any old frame was removed. Recomputing from scratch");
+            return true;
+        }
+
+        assert(_new_trajectory.at(old_pair.first).count('P') and _new_trajectory.at(old_pair.first).count('O'));
+        //assert(_new_trajectory.at(old_pair.first).has("PO"));//FIXME: New implementation comming soon
+        
+        // any old frame moved significantly (angle or position) -> recompute occupancy map
+        const Vector3d& old_frame_p = old_pair.second.at('P');
+        const Vector3d& new_frame_p = _new_trajectory.at(old_pair.first).at('P');
+        //std::cout << "old_frame_p " << old_frame_p.transpose() << std::endl;
+        //std::cout << "new_frame_p " << new_frame_p.transpose() << std::endl;
+
+        Quaterniond old_frame_q = Quaterniond(Vector4d(old_pair.second.at('O')));
+        Quaterniond new_frame_q = Quaterniond(Vector4d(_new_trajectory.at(old_pair.first).at('O')));
+        //std::cout << "old_frame_q " << old_frame_q.coeffs().transpose() << std::endl;
+        //std::cout << "new_frame_q " << new_frame_q.coeffs().transpose() << std::endl;
+
+        if ((old_frame_p-new_frame_p).norm() > update_dist_th_ or
+            std::abs(old_frame_q.angularDistance(new_frame_q)) > update_angle_th_)
+        {
+            WOLF_INFO("PublisherLaserMap3d::trajectoryChanged: Trajectory estimation changed enough. Recomputing from scratch");
+            return true;
+        }
+    }
+
+    return false;
+}
+
+// POINTCLOUD MAP =========================================================
+void PublisherLaserMap3d::reset()
+{
+    // Reset aggregated pointcloud
+    aggregated_pc_.clear();
+
+    // reset last trajectory poses
+    last_trajectory_.clear();
+}
+
+bool PublisherLaserMap3d::updatePointCloud()
+{
+    bool updated = false;
+
+    // Update the map with captures of frames in trajectory_poses_ that are not in last_trajectory_
+    for (auto frame_pose : current_trajectory_)
+    {
+        if (last_trajectory_.count(frame_pose.first) == 1)
+            continue;
+
+        WOLF_DEBUG("Frame ", frame_pose.first->id(), " not in last_trajectory_, adding ", pointclouds_.at(frame_pose.first).size(), " scans to pointcloud");
+
+        // add trajectory pose
+        last_trajectory_[frame_pose.first] = frame_pose.second;
+
+        // add all scans of the frame the map
+        for (auto cap_laser : pointclouds_.at(frame_pose.first))
+            addCapture(cap_laser, frame_pose.second);
+
+        updated = true;
+    }
+
+    WOLF_DEBUG("PublisherLaserMap3d::updatePcMap updated = ", (updated ? "true" : "false"));
+    return updated;
+}
+
+void PublisherLaserMap3d::addCapture(const PointCloudStruct& capture_data, const VectorComposite& pose)
+{
+    //FIXME: new implementation comming soon
+    //assert(pose.has("PO"));
+    assert(pose.count('P') and pose.count('O'));
+
+    // frame
+    Vector3d     frm_p(pose.at('P'));
+    Quaterniond  frm_q(Vector4d(pose.at('O')));
+    
+    // extrinsics
+    Vector3d     sen_p(capture_data.capture->getSensor()->getP()->getState());
+    Quaterniond  sen_q(Vector4d(capture_data.capture->getSensor()->getO()->getState()));
+
+    // transformation matrix
+    Matrix4d T = Matrix4d::Identity();
+    T.topLeftCorner<3,3>() = q2R(frm_q * sen_q);
+    T.topRightCorner<3,1>() = frm_p + frm_q * sen_p;
+    WOLF_DEBUG("addCapture T:\n", T);
+
+    // Pointcloud in global coordinates
+    PointCloud<PointXYZRGB> capture_pc(*capture_data.subsampled_pointcloud);
+    transformPointCloud(*capture_data.subsampled_pointcloud, capture_pc, T);
+
+    // Aggregate scan
+    aggregated_pc_ += capture_pc;
+}
+
+void PublisherLaserMap3d::updateMsg()
+{
+    std::string frame_id = pointcloud_msg_.header.frame_id;
+
+    toROSMsg(aggregated_pc_, pointcloud_msg_);
+
+    pointcloud_msg_.header.stamp = ros::Time::now();
+    pointcloud_msg_.header.frame_id = frame_id;
+}
+
+void PublisherLaserMap3d::computeRGB(const TimeStamp& _ts, int& r, int& g, int& b) const
+{ 
+    double ratio = fmod(_ts.get() / color_cycle_period_, 1);
+
+    // ratio in [0, 1]
+    assert (ratio >= 0 and ratio <= 1);
+
+    // ratio in [0, 3]
+    ratio *= 3;
+
+    // R - G
+    if (ratio < 1)
+    {
+        r = 255 * (1 - ratio);
+        g = 255 * ratio;
+        b = 0;
+    }
+    // G - B
+    else if (ratio < 2)
+    {
+        ratio -=1;
+        r = 0;
+        g = 255 * (1 - ratio);
+        b = 255 * ratio;
+    }
+    // B - R
+    else
+    {
+        ratio -=2;
+        r = 255 * ratio;
+        g = 0;
+        b = 255 * (1 - ratio);
+    }
+}
+
+
+WOLF_REGISTER_PUBLISHER(PublisherLaserMap3d)
+}
\ No newline at end of file

src/subscriber_laser3d.cpp

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "subscriber_laser3d.h"
+#include <laser/capture/capture_laser_3d.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.h>
+
+namespace wolf
+{
+// Constructor
+SubscriberLaser3d::SubscriberLaser3d(const std::string& _unique_name,
+                                     const ParamsServer& _server,
+                                     const SensorBasePtr _sensor_ptr)
+    : Subscriber(_unique_name, _server, _sensor_ptr)
+{
+    sensor_laser_ = std::dynamic_pointer_cast<SensorLaser3d>(_sensor_ptr);
+    
+    if (not sensor_laser_)
+    {
+        throw std::runtime_error("SubscriberLaser3d: sensor provided is not of type SensorLaser3d");
+    }
+}
+
+void SubscriberLaser3d::initialize(ros::NodeHandle& nh, const std::string& topic)
+{
+    sub_ = nh.subscribe(topic, 10, &SubscriberLaser3d::callback, this);
+}
+
+void SubscriberLaser3d::callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
+{
+    updateLastHeader(msg->header);
+
+    if(msg->height == 1)
+    {
+        ROS_WARN("SubscriberLaser3d::callback: unordered pointcloud not supported.");
+        return;
+    }
+
+    // Create pcl::pointcloud from msg
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+    pcl::fromROSMsg(*msg, *cloud);
+
+    // Create and process capture
+    CaptureLaser3dPtr new_capture = std::make_shared<CaptureLaser3d>(TimeStamp(msg->header.stamp.sec,
+                                                                               msg->header.stamp.nsec),
+                                                                     sensor_ptr_,
+                                                                     cloud);
+    new_capture->process();
+}
+
+WOLF_REGISTER_SUBSCRIBER(SubscriberLaser3d)
+}