Updating ROS node operability

src/examples/test_ceres_2_lasers.cpp

@@ -178,8 +178,8 @@ int main(int argc, char** argv)
     ground_truth.head(3) = pose_odom;
     odom_trajectory.head(3) = pose_odom;
 
-    WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e3, pose_odom, Eigen::Matrix3s::Identity() * 0.01, 0.3, window_size);
-
+    WolfManager* wolf_manager = new WolfManager(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3, complex_angle);
+    
     //std::cout << "START TRAJECTORY..." << std::endl;
     // START TRAJECTORY ============================================================================================
     for (unsigned int step = 1; step < n_execution; step++)

src/sensor_base.h

@@ -18,7 +18,15 @@ class SensorBase : public NodeBase
         StateOrientation* o_ptr_; 	    // sensor orientation state unit pointer
         Eigen::VectorXs params_;//sensor intrinsic params: offsets, scale factors, sizes, ...
 
-    public:
+    public:        
+        /** \brief Constructor without parameters
+         *
+         * Constructor without parameters. 
+         * Allows allocation of containers of StateBase, such as std::vector<SensorBase>
+         *
+         **/
+        //SensorBase();
+        
         /** \brief Constructor with parameter vector
          *
          * Constructor with parameter vector

src/state_base.cpp

 
 #include "state_base.h"
 
-StateBase::StateBase(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
+StateBase::StateBase(WolfScalar* _st_ptr) :
 			NodeBase("STATE"),
-			state_ptr_(_st_remote.data() + _idx),
+			state_ptr_(_st_ptr),
 			status_(ST_ESTIMATED)
 {
 	//
 }
 
-
-StateBase::StateBase(WolfScalar* _st_ptr) :
-			NodeBase("STATE"),
-			state_ptr_(_st_ptr),
-			status_(ST_ESTIMATED)
+StateBase::StateBase(Eigen::VectorXs& _st_remote, const unsigned int _idx) :
+            NodeBase("STATE"),
+            state_ptr_(_st_remote.data() + _idx),
+            status_(ST_ESTIMATED)
 {
-	//
+    //
 }
-                
+
 StateBase::~StateBase()
 {
 	//std::cout << "deleting StateBase " << nodeId() << std::endl;

src/state_base.h

@@ -19,6 +19,14 @@ class StateBase : public NodeBase
 		StateStatus status_;
         
     public:
+        /** \brief Constructor with scalar pointer
+         * 
+         * Constructor with scalar pointer
+         * \param _st_ptr is the pointer of the first element of the state unit
+         * 
+         **/
+        StateBase(WolfScalar* _st_ptr);
+
         /** \brief Constructor with whole state storage and index where starts the state unit
          * 
          * Constructor with whole state storage and index where starts the state unit
@@ -27,14 +35,6 @@ class StateBase : public NodeBase
          * 
          **/
         StateBase(Eigen::VectorXs& _st_remote, const unsigned int _idx);
-
-        /** \brief Constructor with scalar pointer
-         * 
-         * Constructor with scalar pointer
-         * \param _st_ptr is the pointer of the first element of the state unit
-         * 
-         **/
-        StateBase(WolfScalar* _st_ptr);
         
         /** \brief Destructor
          * 

src/wolf_manager.h

@@ -42,26 +42,39 @@
 class WolfManager
 {
     protected:
-        bool use_complex_angles_;
+        //sets the problem 
         WolfProblem* problem_;
-        std::queue<CaptureBase*> new_captures_;
+
+        //pointer to a sensor providing predictions
         SensorBase* sensor_prior_;
-        unsigned int window_size_;
+        
+        //auxiliar/temporary iterators, frames and captures
         FrameBaseIter first_window_frame_;
         CaptureRelative* last_capture_relative_;
         CaptureRelative* second_last_capture_relative_;
+        std::queue<CaptureBase*> new_captures_;
+
+        //Manager parameters
+        unsigned int window_size_;
         WolfScalar new_frame_elapsed_time_;
+        bool use_complex_angles_;
 
     public:
-        WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length, const Eigen::VectorXs& _init_frame,
-                const Eigen::MatrixXs& _init_frame_cov, const WolfScalar& _new_frame_elapsed_time = 0.1, const unsigned int& _w_size = 10) :
-                use_complex_angles_(_complex_angle),
+        WolfManager(const unsigned int& _state_length, 
+                    SensorBase* _sensor_prior, 
+                    const Eigen::VectorXs& _init_frame, 
+                    const Eigen::MatrixXs& _init_frame_cov, 
+                    const unsigned int& _w_size = 10, 
+                    const WolfScalar& _new_frame_elapsed_time = 0.1, 
+                    const bool _complex_angle = false) :
+                    
                 problem_(new WolfProblem(_state_length)),
                 sensor_prior_(_sensor_prior),
+                last_capture_relative_(nullptr),
+                second_last_capture_relative_(nullptr),
                 window_size_(_w_size),
                 new_frame_elapsed_time_(_new_frame_elapsed_time),
-                last_capture_relative_(nullptr),
-                second_last_capture_relative_(nullptr)
+                use_complex_angles_(_complex_angle)
         {
             assert( ((!_complex_angle && _init_frame.size() == 3 && _init_frame_cov.cols() == 3 && _init_frame_cov.rows() == 3) ||
                      (_complex_angle && _init_frame.size() == 4 && _init_frame_cov.cols() == 4 && _init_frame_cov.rows() == 4))
@@ -71,10 +84,7 @@ class WolfManager
             // Set initial covariance with a fake ODOM 2D capture to a fix frame
             createFrame(_init_frame, TimeStamp(0));
             problem_->getTrajectoryPtr()->getFrameListPtr()->back()->fix();
-            last_capture_relative_->integrateCapture((CaptureRelative*)(new CaptureOdom2D(TimeStamp(0),
-                                                                                          nullptr,
-                                                                                          Eigen::Vector3s::Zero(),
-                                                                                          _init_frame_cov)));
+            last_capture_relative_->integrateCapture((CaptureRelative*)(new CaptureOdom2D(TimeStamp(0), nullptr, Eigen::Vector3s::Zero(), _init_frame_cov)));
             createFrame(_init_frame, TimeStamp(0));
             second_last_capture_relative_->processCapture();
         }