publishing processor motion odom from origin

src/subscriber_imu.cpp

@@ -265,11 +265,12 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
         // initialize processor_motion pointer
         if (not processor_motion_ptr_)
             for (auto proc : sensor_ptr_->getProcessorList())
-                if (std::dynamic_pointer_cast<ProcessorMotion>(proc) != nullptr)
-                {
-                    processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(proc);
+            {
+                processor_motion_ptr_ = std::dynamic_pointer_cast<ProcessorMotion>(proc);
+                if (processor_motion_ptr_ != nullptr)
                     break;
-                }
+            }
+            
         // abort if no processor_motion (strange...)
         if (not processor_motion_ptr_)
         {
@@ -283,8 +284,8 @@ void SubscriberImu::callback(const sensor_msgs::Imu::ConstPtr& msg)
 
         // get current and origin states
         auto origin_state = processor_motion_ptr_->getOrigin()->getFrame()->getState("PO");
-        auto current_state = sensor_ptr_->getProblem()->getState("PO");
-        auto current_ts = sensor_ptr_->getProblem()->getTimeStamp();
+        auto current_state = processor_motion_ptr_->getState("PO");
+        auto current_ts = processor_motion_ptr_->getTimeStamp();
         if (not origin_state.includesStructure("PO") or not current_state.includesStructure("PO"))
             return;