diff --git a/cfg/DynamicTransformPublisher.cfg b/cfg/DynamicTransformPublisher.cfg index 9503154ce5a013a6e05fb7264ccb378c6da1446c..39091bccbd3655b441ffa574e1e8f19e240ee558 100755 --- a/cfg/DynamicTransformPublisher.cfg +++ b/cfg/DynamicTransformPublisher.cfg @@ -42,6 +42,7 @@ gen.add("rate", double_t, 0, "Main loop rate (Hz)", gen.add("broadcast", bool_t, 0, "enable broadcasting", True) gen.add("parent_id", str_t, 0, "parent frame id", "parent") gen.add("frame_id", str_t, 0, "child frame id", "child") +gen.add("time_offset", double_t, 0, "Seconds offset to add to time now", 0.0, -10000.0, 10000.0) gen.add("reset_translation", bool_t, 0, "reset x y z offsets to zero", False) gen.add("x", double_t, 0, "x offset in meters", 0.0, -10000.0, 10000.0) gen.add("y", double_t, 0, "y offset in meters", 0.0, -10000.0, 10000.0) diff --git a/src/dynamic_transform_publisher_alg_node.cpp b/src/dynamic_transform_publisher_alg_node.cpp index fa9a38d8ed4db3027a410910199407ff3a12b15c..b97563db2a546e89ecd48b61f3f98b757c0abf6f 100644 --- a/src/dynamic_transform_publisher_alg_node.cpp +++ b/src/dynamic_transform_publisher_alg_node.cpp @@ -38,7 +38,7 @@ void DynamicTransformPublisherAlgNode::mainNodeThread(void) this->alg_.lock(); if(this->config_.broadcast) { - this->transform_msg.header.stamp = ros::Time::now(); + this->transform_msg.header.stamp = ros::Time::now() + ros::Duration(this->config_.time_offset); this->tf2_broadcaster.sendTransform(this->transform_msg); } @@ -60,7 +60,7 @@ void DynamicTransformPublisherAlgNode::node_config_update(Config &config, uint32 if(config.rate!=this->getRate()) this->setRate(config.rate); - this->transform_msg.header.stamp = ros::Time::now(); + this->transform_msg.header.stamp = ros::Time::now() + ros::Duration(this->config_.time_offset); this->transform_msg.header.frame_id = config.parent_id; this->transform_msg.child_frame_id = config.frame_id;