void ExcavaROBOdometry::publishOdometry()
{
if (fabs((last_publish_time_ - current_time_).toSec()) < expected_publish_time_)
return;
if (odometry_publisher_->trylock()) {
getOdometryMessage(odometry_publisher_->msg_);
odometry_publisher_->unlockAndPublish();
last_publish_time_ = current_time_;
}
}
void RosieOdometry::publish()
{
if(fabs((last_publish_time_ - current_time_).toSec()) < expected_publish_time_)
return;
if(odometry_publisher_->trylock())
{
getOdometryMessage(odometry_publisher_->msg_);
odometry_publisher_->unlockAndPublish();
}
if(transform_publisher_->trylock())
{
double x(0.), y(0.0), yaw(0.0), vx(0.0), vy(0.0), vyaw(0.0);
this->getOdometry(x, y, yaw, vx, vy, vyaw);
geometry_msgs::TransformStamped &out = transform_publisher_->msg_.transforms[0];
out.header.stamp = current_time_;
out.header.frame_id = odom_frame_;
out.child_frame_id = base_link_frame_;
out.transform.translation.x = x;
out.transform.translation.y = y;
out.transform.translation.z = 0;
tf::Quaternion quat_trans = tf::Quaternion(yaw, 0.0, 0.0);
out.transform.rotation.x = quat_trans.x();
out.transform.rotation.y = quat_trans.y();
out.transform.rotation.z = quat_trans.z();
out.transform.rotation.w = quat_trans.w();
transform_publisher_->unlockAndPublish();
}
last_publish_time_ = current_time_;
}
