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