Merge remote-tracking branch 'origin/devel' into 287-tris

CMakeLists.txt

@@ -358,6 +358,7 @@ SET(SRCS_LANDMARK
   src/landmark/landmark_base.cpp
   )
 SET(SRCS_PROCESSOR
+  src/processor/is_motion.cpp
   src/processor/motion_buffer.cpp
   src/processor/processor_base.cpp
   src/processor/processor_diff_drive.cpp

include/core/problem/problem.h

@@ -50,6 +50,7 @@ class Problem : public std::enable_shared_from_this<Problem>
     friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
     friend ProcessorBase;
     friend ProcessorMotion;
+    friend IsMotion;
 
 
     protected:

include/core/processor/is_motion.h

@@ -22,7 +22,6 @@ class IsMotion
 {
     public:
 
-
         virtual ~IsMotion();
 
         // Queries to the processor:
@@ -30,16 +29,17 @@ class IsMotion
         virtual void    getTimeStamp      ( TimeStamp& _ts) const = 0;
         virtual bool    getState ( VectorComposite& _state) const = 0;
         virtual bool    getState ( const TimeStamp& _ts,
-                                            VectorComposite& _state) const = 0;
-
+                                   VectorComposite& _state) const = 0;
+        // implemented here
         TimeStamp       getTimeStamp() const;
-        // Get composite states
         VectorComposite getState() const;
         VectorComposite getState(const TimeStamp& _ts) const;
 
         const StateStructure& getStateStructure() const {return state_structure_;};
         void            setStateStructure(const StateStructure& _state_structure){state_structure_ = _state_structure;};
 
+        void addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr);
+   
     protected:
         StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
 
@@ -56,13 +56,6 @@ inline IsMotion::~IsMotion()
 {
 }
 
-//inline Eigen::VectorXd IsMotion::getCurrentState() const
-//{
-//    Eigen::VectorXd x;
-//    getCurrentState(x);
-//    return x;
-//}
-
 inline TimeStamp IsMotion::getTimeStamp() const
 {
     TimeStamp ts;

src/processor/is_motion.cpp

+#include "core/processor/is_motion.h"
+#include "core/problem/problem.h"
+
+using namespace wolf;
+
+void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
+{
+    _prb_ptr->addProcessorIsMotion(_motion_ptr);
+}

src/processor/processor_tracker.cpp

@@ -102,7 +102,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
         	// No-break case only for debug. Next case will be executed too.
             PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
             WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
-        } // @suppress("No break at end of case")
+        }
+        // Fall through
         case SECOND_TIME_WITHOUT_PACK :
         {
             WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );

test/gtest_pack_KF_buffer.cpp

@@ -137,7 +137,9 @@ TEST_F(BufferPackKeyFrameTest, selectPack)
             {
                 PackKeyFramePtr packQ = pack_kf_buffer.selectPack(16, q[iq]);
                 if (packQ!=nullptr)
+                {
                     ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq));
+                }
             }
             pack_kf_buffer.clear();
         }

wolf_scripts/sed_rename.sh

+#!/bin/bash
+#$1 flag telling whether we are in test mode or not
+
+modify=$1
+apply=false
+
+if [[ "$modify" = true || "$modify" = t ]]; then
+    read -p "Are you sure? " -n 1 -r
+    echo    # (optional) move to a new line
+    if [[ $REPLY =~ ^[Yy]$ ]]
+    then
+        apply=true
+        echo "Modifying..."
+    fi
+fi
+
+string="StringToBeReplaced"
+replace="ReplacementString"
+
+for file in $(find include/ src/ test/ demos/ -type f); do
+    if [ "$apply" = true ]; then
+        # echo "APPLYING"
+        sed -Ei 's%'$string'%'$replace'%g' $file
+    else
+        # echo "NOT APPLYING"
+        sed -En 's%'$string'%'$replace'%gp' $file
+    fi
+done