Always publish fixed frames to /tf_static

Remove the deprecated 'use_tf_static' parameter. Now fixed transforms
will always be published to the topic /tf_static.

This change also avoid repeatedly publishing the same transforms.
This restores the behavior from ROS 1; now a single "latched" message
is published over /tf_static.

Signed-off-by: Jacob Perron <[email protected]>

Author: jacobperron

README.md

@@ -2,8 +2,13 @@ Robot State Publisher
 =====================
 
 This package contains the Robot State Publisher, a node and a class to publish the state of a robot to tf2.
-Once the state gets published, it is available to all components in the system that also use tf2.
-The package takes the joint angles of the robot as input and publishes the 3D poses of the robot links, using a kinematic tree model of the robot.
+At startup time, Robot State Publisher is supplied with a kinematic tree model (URDF) of the robot.
+It then subscribes to the `joint_states` message (of type `sensor_msgs/msg/JointState`) to get individual joint states.
+These joint states are used to update the kinematic tree model, and the resulting 3D poses are then published to tf2.
+
+Robot State Publisher deals with two different "classes" of joint types: fixed and movable.
+Fixed joints (with the type "fixed") are published to the transient_local `/tf_static` topic once on startup (transient_local topics keep a history of what they published, so a later subscription can always get the latest state of the world).
+Movable joints are published to the regular `/tf` topic any time the appropriate joint is updated in the `joint_states` message.
 
 Examples showing how to pass the `robot_description` parameter using a launch file are available in the 'launch' subdirectory.
 
@@ -20,7 +25,6 @@ Subscribed Topics
 Parameters
 ----------
 * `robot_description` (string) - The original description of the robot in URDF form.  This *must* be set at robot_state_publisher startup time, or the node will fail to start.  Updates to this parameter will be reflected in the `robot_description` topic.
-* `publish_frequency` (double) - The frequency at which fixed transforms will be republished to the network.  Defaults to 20.0 Hz.
-* `use_tf_static` (bool) - Whether to publish fixed joints on the static broadcaster (`/tf_static` topic) or on the dynamic one (`/tf` topic).  Defaults to true, so it publishes on the `/tf_static` topic.
+* `publish_frequency` (double) - The maximum frequency at which non-static transforms (e.g. joint states) will be published to `/tf`.  Defaults to 20.0 Hz.
 * `ignore_timestamp` (bool) - Whether to accept all joint states no matter what the timestamp (true), or to only publish joint state updates if they are newer than the last publish_frequency (false).  Defaults to false.
 * `frame_prefix` (string) - An arbitrary prefix to add to the published tf2 frames.  Defaults to the empty string.

include/robot_state_publisher/robot_state_publisher.hpp

@@ -95,11 +95,9 @@ class RobotStatePublisher : public rclcpp::Node
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr description_pub_;
   std::chrono::milliseconds publish_interval_ms_;
   rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
-  rclcpp::TimerBase::SharedPtr timer_;
   rclcpp::Time last_callback_time_;
   std::map<std::string, builtin_interfaces::msg::Time> last_publish_time_;
   MimicMap mimic_;
-  bool use_tf_static_;
   bool ignore_timestamp_;
   std::string frame_prefix_;
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_;

src/robot_state_publisher.cpp

@@ -120,12 +120,6 @@ RobotStatePublisher::RobotStatePublisher(const rclcpp::NodeOptions & options)
   publish_interval_ms_ =
     std::chrono::milliseconds(static_cast<uint64_t>(1000.0 / publish_freq));
 
-  // set whether to use the /tf_static latched static transform broadcaster
-  use_tf_static_ = this->declare_parameter("use_tf_static", true);
-  if (!use_tf_static_) {
-    RCLCPP_WARN(get_logger(), "use_tf_static is deprecated and will be removed in the future");
-  }
-
   // set frame_prefix
   frame_prefix_ = this->declare_parameter("frame_prefix", "");
 
@@ -148,9 +142,7 @@ RobotStatePublisher::RobotStatePublisher(const rclcpp::NodeOptions & options)
       &RobotStatePublisher::callbackJointState, this,
       std::placeholders::_1));
 
-  // trigger to publish fixed joints
-  timer_ = this->create_wall_timer(
-    publish_interval_ms_, std::bind(&RobotStatePublisher::publishFixedTransforms, this));
+  publishFixedTransforms();
 
   // Now that we have successfully declared the parameters and done all
   // necessary setup, install the callback for updating parameters.
@@ -260,23 +252,16 @@ void RobotStatePublisher::publishFixedTransforms()
   std::vector<geometry_msgs::msg::TransformStamped> tf_transforms;
 
   // loop over all fixed segments
+  rclcpp::Time now = this->now();
   for (const std::pair<const std::string, SegmentPair> & seg : segments_fixed_) {
     geometry_msgs::msg::TransformStamped tf_transform = kdlToTransform(seg.second.segment.pose(0));
-    rclcpp::Time now = this->now();
-    if (!use_tf_static_) {
-      now = now + rclcpp::Duration(std::chrono::milliseconds(500));
-    }
     tf_transform.header.stamp = now;
 
     tf_transform.header.frame_id = frame_prefix_ + seg.second.root;
     tf_transform.child_frame_id = frame_prefix_ + seg.second.tip;
     tf_transforms.push_back(tf_transform);
   }
-  if (use_tf_static_) {
-    static_tf_broadcaster_->sendTransform(tf_transforms);
-  } else {
-    tf_broadcaster_->sendTransform(tf_transforms);
-  }
+  static_tf_broadcaster_->sendTransform(tf_transforms);
 }
 
 void RobotStatePublisher::callbackJointState(const sensor_msgs::msg::JointState::SharedPtr state)
@@ -370,13 +355,6 @@ rcl_interfaces::msg::SetParametersResult RobotStatePublisher::parameterUpdate(
         result.reason = err.what();
         break;
       }
-    } else if (parameter.get_name() == "use_tf_static") {
-      if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_BOOL) {
-        result.successful = false;
-        result.reason = "use_tf_static must be a boolean";
-        break;
-      }
-      use_tf_static_ = parameter.as_bool();
     } else if (parameter.get_name() == "frame_prefix") {
       if (parameter.get_type() != rclcpp::ParameterType::PARAMETER_STRING) {
         result.successful = false;
@@ -401,18 +379,11 @@ rcl_interfaces::msg::SetParametersResult RobotStatePublisher::parameterUpdate(
       double publish_freq = parameter.as_double();
       if (publish_freq < 0.0 || publish_freq > 1000.0) {
         result.successful = false;
-        result.reason = "publish_frequency must be between 0 and 1000";
+        result.reason = "publish_frequency must be between 0.0 and 1000.0";
         break;
       }
-      std::chrono::milliseconds new_publish_interval =
+      publish_interval_ms_ =
         std::chrono::milliseconds(static_cast<uint64_t>(1000.0 / publish_freq));
-
-      if (new_publish_interval != publish_interval_ms_) {
-        publish_interval_ms_ = new_publish_interval;
-        timer_->cancel();
-        timer_ = this->create_wall_timer(
-          publish_interval_ms_, std::bind(&RobotStatePublisher::publishFixedTransforms, this));
-      }
     }
   }