From eb67cde9e246a214c6014ab24ad1fa23c7fe1857 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Mon, 21 Feb 2022 13:00:30 +0100
Subject: [PATCH] publishing processor motion odom from origin
---
src/subscriber_imu.cpp | 13 +++++++------
1 file changed, 7 insertions(+), 6 deletions(-)
diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp
index 8216681..9234aa4 100644
--- a/src/subscriber_imu.cpp
+++ b/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;
--
GitLab