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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2022-10-01 18:09:19 -07:00
parent 4fb73b4279 5024574e4a
commit 99b9891ab8
57 changed files with 1988 additions and 1096 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+61 -32
View File
@@ -499,7 +499,11 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{
if(odometry_->getPose().isIdentity())
// Use only XYZ to handle the case odometry was previously initialized with IMU,
// we assume that the ground truth contains also a real initial orientation
float x,y,z;
odometry_->getPose().getTranslation(x, y, z);
if(x==0.0f && y==0.0f && z==0.0f)
{
// sync with the first value of the ground truth
if(groundTruth.isNull())
@@ -772,30 +776,46 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
{
return;
}
else if(publishNullWhenLost_)
else // pose is null / lost
{
//RCLCPP_WARN(this->get_logger(), "Odometry lost!");
if(publishNullWhenLost_)
{
//RCLCPP_WARN(this->get_logger(), "Odometry lost!");
//send null pose to notify that odometry is lost
nav_msgs::msg::Odometry odom;
odom.header.stamp = header.stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
odom.pose.covariance.at(0) = BAD_COVARIANCE; // xx
odom.pose.covariance.at(7) = BAD_COVARIANCE; // yy
odom.pose.covariance.at(14) = BAD_COVARIANCE; // zz
odom.pose.covariance.at(21) = BAD_COVARIANCE; // rr
odom.pose.covariance.at(28) = BAD_COVARIANCE; // pp
odom.pose.covariance.at(35) = BAD_COVARIANCE; // yawyaw
odom.twist.covariance.at(0) = BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = BAD_COVARIANCE; // yawyaw
odom.pose.pose.orientation.w=0; // invalid (null transform)
//publish the message
odomPub_->publish(odom);
}
// Publish the Tf correction using guess pose directly so that TF tree is not broken when vo is lost
if(publishTf_ && !guess_.isNull())
{
geometry_msgs::msg::TransformStamped correctionMsg;
correctionMsg.child_frame_id = guessFrameId_;
correctionMsg.header.frame_id = odomFrameId_;
correctionMsg.header.stamp = header.stamp;
Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse();
rtabmap_ros::transformToGeometryMsg(correction, correctionMsg.transform);
tfBroadcaster_->sendTransform(correctionMsg);
}
//send null pose to notify that odometry is lost
nav_msgs::msg::Odometry odom;
odom.header.stamp = header.stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
odom.pose.covariance.at(0) = BAD_COVARIANCE; // xx
odom.pose.covariance.at(7) = BAD_COVARIANCE; // yy
odom.pose.covariance.at(14) = BAD_COVARIANCE; // zz
odom.pose.covariance.at(21) = BAD_COVARIANCE; // rr
odom.pose.covariance.at(28) = BAD_COVARIANCE; // pp
odom.pose.covariance.at(35) = BAD_COVARIANCE; // yawyaw
odom.twist.covariance.at(0) = BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = BAD_COVARIANCE; // yawyaw
odom.pose.pose.orientation.w=0; // invalid (null transform)
//publish the message
odomPub_->publish(odom);
}
if(pose.isNull() && resetCurrentCount_ > 0)
@@ -805,20 +825,29 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
--resetCurrentCount_;
if(resetCurrentCount_ == 0)
{
// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
Transform tfPose = getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
if(tfPose.isNull())
if(!guess_.isNull())
{
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
odometry_->reset(odometry_->getPose());
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!",
guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str());
odometry_->reset(odometry_->getPose() * guess_);
guess_.setNull();
}
else
{
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
odomFrameId_.c_str(), frameId_.c_str());
odometry_->reset(tfPose);
// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
Transform tfPose = getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
if(tfPose.isNull())
{
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
odometry_->reset(odometry_->getPose());
}
else
{
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
odomFrameId_.c_str(), frameId_.c_str());
odometry_->reset(tfPose);
}
}
}
}