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

Merge branch 'master' of github.com:introlab/rtabmap_ros into ros2

Browse Source
This commit is contained in:
matlabbe
2025-03-01 16:18:10 -08:00
parent e561bd190f 12e61a952e
commit 5e22c31b7f
4 changed files with 41 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_slam/src/CoreWrapper.cpp
+32 -5
View File
@@ -2693,14 +2693,41 @@ void CoreWrapper::interOdomInfoCallback(const nav_msgs::msg::Odometry::ConstShar
void CoreWrapper::initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{
Transform intialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose);
if(intialPose.isNull())
Transform mapToPose = Transform::getIdentity();
if(msg->header.frame_id.empty())
{
RCLCPP_ERROR(this->get_logger(), "Pose received is null!");
return;
RCLCPP_WARN(this->get_logger(), "Received initialpose doesn't have frame_id set, assuming it is in %s frame.", mapFrameId_.c_str());
}
else if(msg->header.frame_id != mapFrameId_)
{
mapToPose = rtabmap_conversions::getTransform(mapFrameId_, msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_);
if(mapToPose.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Failed to transform initialpose from frame %s to map frame %s", msg->header.frame_id.c_str(), mapFrameId_.c_str());
return;
}
}
rtabmap_.setInitialPose(intialPose);
Transform initialPose = rtabmap_conversions::transformFromPoseMsg(msg->pose.pose);
if(initialPose.isNull())
{
RCLCPP_ERROR(this->get_logger(), "initialpose received is null!");
return;
}
if(mapToPose.isIdentity())
{
RCLCPP_INFO(this->get_logger(), "initialpose received: %s", initialPose.prettyPrint().c_str());
rtabmap_.setInitialPose(initialPose);
}
else
{
RCLCPP_INFO(this->get_logger(), "initialpose received: %s in %s frame, transformed to %s in %s frame.",
initialPose.prettyPrint().c_str(),
msg->header.frame_id.c_str(),
(mapToPose * initialPose).prettyPrint().c_str(),
mapFrameId_.c_str());
rtabmap_.setInitialPose(mapToPose*initialPose);
}
}
void CoreWrapper::goalCommonCallback(