message: odom: publish TF odom correction when lost if guess frame is provided
diff: @@ -815,7 +815,9 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
  {
  return;
  }
  - else if(publishNullWhenLost_)
  + else // pose is null / lost
  + {
  + if(publishNullWhenLost_)
  {
  //NODELET_WARN( "Odometry lost!");
  @@ -841,6 +843,19 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
  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::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);
  + }
  + }
  +
  if(pose.isNull() && resetCurrentCount_ > 0)
  {
  NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
  