diff --git a/joint_state_publisher/README.md b/joint_state_publisher/README.md index 7d3b6af..a5a88fb 100644 --- a/joint_state_publisher/README.md +++ b/joint_state_publisher/README.md @@ -18,6 +18,7 @@ Subscribed Topics Parameters ---------- * `rate` (int) - The rate at which to publish updates to the `/joint_states` topic. Defaults to 10. +* `offset_timestamp`(double) - The offset to add to the timestamp of the published message, in seconds. Defaults to 0.0. * `publish_default_positions` (bool) - Whether to publish a default position for each movable joint to the `/joint_states` topic. Defaults to True. * `publish_default_velocities` (bool) - Whether to publish a default velocity for each movable joint to the `/joint_states` topic. Defaults to False. * `publish_default_efforts` (bool) - Whether to publish a default effort for each movable joint to the `/joint_states` topic. Defaults to False. diff --git a/joint_state_publisher/joint_state_publisher/joint_state_publisher.py b/joint_state_publisher/joint_state_publisher/joint_state_publisher.py index fc2ae96..0cefb1c 100644 --- a/joint_state_publisher/joint_state_publisher/joint_state_publisher.py +++ b/joint_state_publisher/joint_state_publisher/joint_state_publisher.py @@ -370,6 +370,8 @@ def __init__(self, description_file): ParameterDescriptor(type=ParameterType.PARAMETER_BOOL)) self.declare_ros_parameter('delta', 0.0, ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE)) + self.declare_ros_parameter('offset_timestamp', 0.0, + ParameterDescriptor(type=ParameterType.PARAMETER_DOUBLE)) # In theory we would also declare 'dependent_joints' and 'zeros' here. # Since rclpy doesn't support maps natively, though, we just end up # letting 'automatically_declare_parameters_from_overrides' declare @@ -390,6 +392,7 @@ def __init__(self, description_file): self.pub_def_positions = self.get_param('publish_default_positions') self.pub_def_vels = self.get_param('publish_default_velocities') self.pub_def_efforts = self.get_param('publish_default_efforts') + self.offset_timestamp = self.get_param('offset_timestamp') self.robot_description_update_cb = None @@ -468,8 +471,7 @@ def set_robot_description_update_cb(self, user_cb): def timer_callback(self): # Publish Joint States msg = sensor_msgs.msg.JointState() - msg.header.stamp = self.get_clock().now().to_msg() - + msg.header.stamp = (self.get_clock().now() + rclpy.duration.Duration(seconds=self.offset_timestamp)).to_msg() if self.delta > 0: self.update(self.delta)