From e2b70a6707f0fc5002622cc0d581b7c9204b2a3a Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sat, 9 Dec 2023 19:03:08 -0800
Subject: [PATCH] Fixed odom initial_pose wrongly logged (#1079)

---
 rtabmap_odom/src/OdometryROS.cpp | 43 ++++++++++++++++----------------
 1 file changed, 21 insertions(+), 22 deletions(-)

diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp
index 5232750ff..0e80d6e50 100644
--- a/rtabmap_odom/src/OdometryROS.cpp
+++ b/rtabmap_odom/src/OdometryROS.cpp
@@ -141,6 +141,27 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 
 	waitIMUToinit_ = this->declare_parameter("wait_imu_to_init", waitIMUToinit_);
 
+	configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
+	if(configPath_.size() && configPath_.at(0) != '/')
+	{
+		configPath_ = UDirectory::currentDir(true) + configPath_;
+	}
+
+	if(initialPoseStr.size())
+	{
+		std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));
+		if(values.size() == 6)
+		{
+			initialPose_ = Transform(
+					uStr2Float(values[0]), uStr2Float(values[1]), uStr2Float(values[2]),
+					uStr2Float(values[3]), uStr2Float(values[4]), uStr2Float(values[5]));
+		}
+		else
+		{
+			RCLCPP_ERROR(this->get_logger(), "Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). "
+					  "Identity will be used...", initialPoseStr.c_str());
+		}
+	}
 
 	int eventLevel = ULogger::kFatal;
 	eventLevel = this->declare_parameter("log_to_rosout_level", eventLevel);
@@ -175,28 +196,6 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
 	RCLCPP_INFO(this->get_logger(), "Odometry: wait_imu_to_init       = %s", waitIMUToinit_?"true":"false");
 	RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_compression_format = %s", compressionImgFormat_.c_str());
 	RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_parallel_compression = %s", compressionParallelized_?"true":"false");
-
-	configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
-	if(configPath_.size() && configPath_.at(0) != '/')
-	{
-		configPath_ = UDirectory::currentDir(true) + configPath_;
-	}
-
-	if(initialPoseStr.size())
-	{
-		std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));
-		if(values.size() == 6)
-		{
-			initialPose_ = Transform(
-					uStr2Float(values[0]), uStr2Float(values[1]), uStr2Float(values[2]),
-					uStr2Float(values[3]), uStr2Float(values[4]), uStr2Float(values[5]));
-		}
-		else
-		{
-			RCLCPP_ERROR(this->get_logger(), "Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). "
-					  "Identity will be used...", initialPoseStr.c_str());
-		}
-	}
 }
 
 OdometryROS::~OdometryROS()