i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed odom initial_pose wrongly logged (#1079)

Browse Source
This commit is contained in:
matlabbe
2023-12-09 19:03:08 -08:00
parent 41fd09da2e
commit e2b70a6707
1 changed files with 21 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/src/OdometryROS.cpp
+21 -22
View File
@@ -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()