diff --git a/README.md b/README.md index 42eddb5..6e689c7 100644 --- a/README.md +++ b/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` topic (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. diff --git a/include/robot_state_publisher/robot_state_publisher.hpp b/include/robot_state_publisher/robot_state_publisher.hpp index 760264a..feb7215 100644 --- a/include/robot_state_publisher/robot_state_publisher.hpp +++ b/include/robot_state_publisher/robot_state_publisher.hpp @@ -95,11 +95,9 @@ class RobotStatePublisher : public rclcpp::Node rclcpp::Publisher::SharedPtr description_pub_; std::chrono::milliseconds publish_interval_ms_; rclcpp::Subscription::SharedPtr joint_state_sub_; - rclcpp::TimerBase::SharedPtr timer_; rclcpp::Time last_callback_time_; std::map last_publish_time_; MimicMap mimic_; - bool use_tf_static_; bool ignore_timestamp_; std::string frame_prefix_; rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_; diff --git a/src/robot_state_publisher.cpp b/src/robot_state_publisher.cpp index 39fd009..3a7ed15 100644 --- a/src/robot_state_publisher.cpp +++ b/src/robot_state_publisher.cpp @@ -120,12 +120,6 @@ RobotStatePublisher::RobotStatePublisher(const rclcpp::NodeOptions & options) publish_interval_ms_ = std::chrono::milliseconds(static_cast(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 tf_transforms; // loop over all fixed segments + rclcpp::Time now = this->now(); for (const std::pair & 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(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)); - } } }