From e4a03f06ed100010c7be76def017f17ed693702b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 11 May 2022 11:10:11 +0200
Subject: [PATCH] adapted to const new API

---
 include/publisher_imu_bias.h      |  6 +++---
 src/publisher_imu_bias.cpp        |  4 ++--
 src/subscriber_imu_enableable.cpp | 12 ++++++------
 3 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/include/publisher_imu_bias.h b/include/publisher_imu_bias.h
index 5cf9658..d085dd2 100644
--- a/include/publisher_imu_bias.h
+++ b/include/publisher_imu_bias.h
@@ -37,12 +37,12 @@ class PublisherImuBias: public Publisher
 {
         std::string map_frame_id_;
         ros::Publisher pub_accel_, pub_gyro_;
-        SensorImuPtr sensor_imu_;
+        SensorImuConstPtr sensor_imu_;
 
     public:
         PublisherImuBias(const std::string& _unique_name,
-                      const ParamsServer& _server,
-                      const ProblemPtr _problem);
+                         const ParamsServer& _server,
+                         ProblemConstPtr _problem);
         WOLF_PUBLISHER_CREATE(PublisherImuBias);
 
         virtual ~PublisherImuBias(){};
diff --git a/src/publisher_imu_bias.cpp b/src/publisher_imu_bias.cpp
index 2feec6c..cdfb11a 100644
--- a/src/publisher_imu_bias.cpp
+++ b/src/publisher_imu_bias.cpp
@@ -29,10 +29,10 @@ namespace wolf
 
 PublisherImuBias::PublisherImuBias(const std::string& _unique_name,
                                    const ParamsServer& _server,
-                                   const ProblemPtr _problem) :
+                                   ProblemConstPtr _problem) :
         Publisher(_unique_name, _server, _problem)
 {
-    sensor_imu_ = std::static_pointer_cast<SensorImu>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_name")));
+    sensor_imu_ = std::static_pointer_cast<const SensorImu>(_problem->findSensor(_server.getParam<std::string>(prefix_ + "/sensor_name")));
 }
 
 void PublisherImuBias::initialize(ros::NodeHandle& nh, const std::string& topic)
diff --git a/src/subscriber_imu_enableable.cpp b/src/subscriber_imu_enableable.cpp
index 4daa589..4861f72 100644
--- a/src/subscriber_imu_enableable.cpp
+++ b/src/subscriber_imu_enableable.cpp
@@ -155,11 +155,11 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
                         Eigen::VectorXd zero_motion = Eigen::VectorXd::Zero(size);
                         if(sensor_ptr_->getProblem()->getDim() == 3) 
                           zero_motion.tail<4>() = Eigen::Quaterniond::Identity().coeffs();
-                        for (auto frm_pair =  sensor_ptr_->getProblem()->getTrajectory()->begin();
-                             frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end();
-                             frm_pair++)
+                        
+                        auto frame_map = sensor_ptr_->getProblem()->getTrajectory()->getFrameMap();
+                        for (auto frm_pair : frame_map)
                         {
-                            if (frm_pair->second == last_frame_)
+                            if (frm_pair.second == last_frame_)
                                 break;
                             auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr);
                             auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero,
@@ -169,14 +169,14 @@ void SubscriberImuEnableable::callback(const sensor_msgs::Imu::ConstPtr& msg)
                             if(sensor_ptr_->getProblem()->getDim() == 3) 
                               FactorBase::emplace<FactorRelativePose3d>(feature_zero,
                                                                         feature_zero,
-                                                                        frm_pair->second,
+                                                                        frm_pair.second,
                                                                         nullptr,
                                                                         false,
                                                                         TOP_MOTION);
                             else
                               FactorBase::emplace<FactorRelativePose2d>(feature_zero,
                                                                         feature_zero,
-                                                                        frm_pair->second,
+                                                                        frm_pair.second,
                                                                         nullptr,
                                                                         false,
                                                                         TOP_MOTION);
-- 
GitLab