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_; }