| 216 | } |
| 217 | |
| 218 | bool LocalizationNode::PublishTf() { |
| 219 | ros::Time transform_expiration = (last_laser_msg_timestamp_ + transform_tolerance_); |
| 220 | if (amcl_ptr_->CheckTfUpdate()) { |
| 221 | // Subtracting base to odom from map to base and send map to odom instead |
| 222 | tf::Stamped<tf::Pose> odom_to_map; |
| 223 | try { |
| 224 | tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyp_pose_.pose_mean[2]), |
| 225 | tf::Vector3(hyp_pose_.pose_mean[0], |
| 226 | hyp_pose_.pose_mean[1], |
| 227 | 0.0)); |
| 228 | tf::Stamped<tf::Pose> tmp_tf_stamped(tmp_tf.inverse(), |
| 229 | last_laser_msg_timestamp_, |
| 230 | base_frame_); |
| 231 | this->tf_listener_ptr_->transformPose(odom_frame_, |
| 232 | tmp_tf_stamped, |
| 233 | odom_to_map); |
| 234 | } catch (tf::TransformException &e) { |
| 235 | LOG_ERROR << "Failed to subtract base to odom transform" << e.what(); |
| 236 | return false; |
| 237 | } |
| 238 | |
| 239 | latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()), |
| 240 | tf::Point(odom_to_map.getOrigin())); |
| 241 | latest_tf_valid_ = true; |
| 242 | |
| 243 | tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), |
| 244 | transform_expiration, |
| 245 | global_frame_, |
| 246 | odom_frame_); |
| 247 | this->tf_broadcaster_ptr_->sendTransform(tmp_tf_stamped); |
| 248 | sent_first_transform_ = true; |
| 249 | return true; |
| 250 | } else if (latest_tf_valid_) { |
| 251 | // Nothing changed, so we'll just republish the last transform |
| 252 | tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(), |
| 253 | transform_expiration, |
| 254 | global_frame_, |
| 255 | odom_frame_); |
| 256 | this->tf_broadcaster_ptr_->sendTransform(tmp_tf_stamped); |
| 257 | return true; |
| 258 | } |
| 259 | else{ |
| 260 | return false; |
| 261 | } |
| 262 | } |
| 263 | |
| 264 | bool LocalizationNode::GetPoseFromTf(const std::string &target_frame, |
| 265 | const std::string &source_frame, |
nothing calls this directly
no test coverage detected