wolf in ros

src/capture_laser_2D.cpp

@@ -63,7 +63,14 @@ void CaptureLaser2D::processCapture()
 
 unsigned int CaptureLaser2D::extractCorners(std::list<laserscanutils::Corner> & _corner_list) const
 {
-    return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, _corner_list);
+  std::list<laserscanutils::Line> line_list;
+  laserscanutils::extractLines(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, line_list);
+  return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), line_list, _corner_list);
+}
+
+unsigned int CaptureLaser2D::extractLines(std::list<laserscanutils::Line> & _line_list) const
+{
+    return laserscanutils::extractLines(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, _line_list);
 }
 
 void CaptureLaser2D::createFeatures(std::list<laserscanutils::Corner> & _corner_list)

src/capture_laser_2D.h

@@ -84,6 +84,13 @@ class CaptureLaser2D : public CaptureBase
 //         virtual unsigned int extractCorners_old(std::list<Eigen::Vector4s> & _corner_list) const;
 //         void fitLine(unsigned int _idx_from, unsigned int _idx_to, const Eigen::MatrixXs& _points, Line& line_) const;
 
+        /** \brief Extract lines from the capture
+         *
+         * Extract lines from the capture
+         *
+         **/
+        unsigned int extractLines(std::list<laserscanutils::Line> & _line_list) const;
+
         /** \brief get corners
          *
          * Get corners

src/sensor_laser_2D.cpp

@@ -68,6 +68,11 @@ void SensorLaser2D::setDefaultCornerAlgParams()
 
 }
 
+void SensorLaser2D::setCornerAlgParams(const laserscanutils::ExtractCornerParams & _corner_alg_params)
+{
+  corners_alg_params_ = _corner_alg_params;
+}
+
 const laserscanutils::ScanParams & SensorLaser2D::getScanParams() const
 {
     return scan_params_;
@@ -121,6 +126,8 @@ WolfScalar SensorLaser2D::getScanTime() const
 void SensorLaser2D::printSensorParameters() const
 {
     std::cout << "LASER 2D SENSOR" << std::endl;
+    scan_params_.print();
+    corners_alg_params_.print();
 //     std::cout << "   angle min: " << getAngleMin() << std::endl;
 //     std::cout << "   angle min: " << getAngleMax() << std::endl;
 //     std::cout << "   angle increment: " << getAngleIncrement() << std::endl;

src/sensor_laser_2D.h

@@ -66,6 +66,14 @@ class SensorLaser2D : public SensorBase
         
         void setDefaultCornerAlgParams();
 
+        /** \brief Set corner detection algorithm parameters
+         *
+         * Set corner detection algorithm parameters
+         * \param _params struct with corner detection algorithm parameters. See laser_scan_utils library API for reference.
+         *
+         **/
+        void setCornerAlgParams(const laserscanutils::ExtractCornerParams & _corner_alg_params);
+
         /** \brief Get scanner intrinsic parameters
          * 
          * Get scanner intrinsic parameters

src/wolf_manager.h

@@ -148,17 +148,20 @@ class WolfManager
         {
           // ADD CAPTURE TO THE LAST FRAME (or substitute the same sensor previous capture)
           //std::cout << "adding not odometry capture..." << std::endl;
+          bool same_sensor_capture_found = false;
           for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++)
           {
             if ((*capture_it)->getSensorPtr() == new_capture->getSensorPtr())
             {
               //std::cout << "removing previous capture" << std::endl;
-              problem_->getTrajectoryPtr()->getFrameListPtr()->back()->removeCapture(capture_it);
+              //problem_->getTrajectoryPtr()->getFrameListPtr()->back()->removeCapture(capture_it);
+              same_sensor_capture_found = true;
               //std::cout << "removed!" << std::endl;
               break;
             }
           }
-          problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new_capture);
+          if (!same_sensor_capture_found)
+            problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new_capture);
         }
       }
     }