From 407a7bb4b3a6a58dc6d6d617adf9b5b5d5355f3b Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 27 Feb 2019 22:30:56 +0300 Subject: [PATCH] vpe_publisher: fixes --- clever/src/vpe_publisher.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/clever/src/vpe_publisher.cpp b/clever/src/vpe_publisher.cpp index bf90a81b..109b3f8e 100644 --- a/clever/src/vpe_publisher.cpp +++ b/clever/src/vpe_publisher.cpp @@ -52,7 +52,7 @@ void publishZero(const ros::TimerEvent& e) ROS_INFO_THROTTLE(10, "vpe_publisher: publish zero"); static geometry_msgs::PoseStamped zero; - zero.header.frame_id = vpe.header.frame_id; + zero.header.frame_id = local_frame_id; zero.header.stamp = e.current_real; zero.pose.orientation.w = 1; vpe_pub.publish(zero); @@ -84,14 +84,14 @@ void callback(const T& msg) // offset if (!offset_frame_id.empty()) { - if (ros::Time::now() - vpe.header.stamp > offset_timeout) { + if (msg->header.stamp - vpe.header.stamp > offset_timeout) { // calculate the offset - ROS_INFO("vpe_publisher: reset offset"); offset = tf_buffer.lookupTransform(local_frame_id, frame_id, - msg->header.stamp, ros::Duration(0.02)); + msg->header.stamp, ros::Duration(0.02)); // offset.header.frame_id = vpe.header.frame_id; offset.child_frame_id = offset_frame_id; br.sendTransform(offset); + ROS_INFO("vpe_publisher: offset reset"); } // apply the offset tf2::doTransform(vpe, vpe, offset); @@ -102,7 +102,7 @@ void callback(const T& msg) vpe_pub.publish(vpe); } catch (const tf2::TransformException& e) { - ROS_WARN_THROTTLE(10, "vpe_publisher: %s", e.what()); + ROS_WARN_THROTTLE(5, "vpe_publisher: %s", e.what()); } } @@ -131,8 +131,6 @@ int main(int argc, char **argv) { vpe_pub = nh_priv.advertise("vpe", 1); //vpe_cov_pub = nh_priv_.advertise("pose_cov_pub", 1); - vpe.header.stamp = ros::Time(0); - if (nh_priv.param("publish_zero", false)) { // publish zero to initialize the local position zero_timer = nh.createTimer(ros::Duration(0.1), &publishZero);