Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Always publish fixed frames to /tf_static #158

Merged
merged 2 commits into from
Jul 12, 2021
Merged

Always publish fixed frames to /tf_static #158

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6048c06
Always publish fixed frames to /tf_static
jacobperron Feb 8, 2021
2636175
Minor fix to the README wording.
clalancette Jul 6, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 8 additions & 4 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.

Expand All @@ -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.
2 changes: 0 additions & 2 deletions include/robot_state_publisher/robot_state_publisher.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down
39 changes: 5 additions & 34 deletions src/robot_state_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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", "");

Expand All @@ -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.
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand All @@ -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));
}
}
}

Expand Down