Currently, very careful usage is required with tf2_ros::TransformListener
to work around breaking parameters or breaking tf receipt.
When tf2_ros::TransformListener
is created with the short constructor that does not take a rclcpp::Node
, the TransformListener
creates its own node. If a node using the TransformListener
gets renamed, this separate tf node gets renamed too, and you end up with two nodes of the same name (in ros2 node list
). Then parameters do not work (ros2 param list
shows only /use_sim_time
parameter and nothing else).
Even when tf2_ros::TransformListener
is created with the constructor that takes a rclcpp::Node
, tf receipt does not work in practice. To eliminate runtime errors, specific combination of spin_thread and spin() is required. If spin_thread=true
, executor.add_node() should not be called, otherwise this error is printed:
terminate called after throwing an instance of 'std::runtime_error'
what(): Node has already been added to an executor.
Even when all run-time errors are eliminated, canTransform()
still returns false on existing frames.
@cottsay has hit similar problems.
@clalancette suggests changing TransformListener constructor API to be more usable.
Bug report
Required Info:
- Operating System:
- Installation type:
- Tarball installation in Docker
- Version or commit hash:
- DDS implementation:
- Client library (if applicable):
Steps to reproduce issue (constructor without rclcpp::Node)
Create test_tl_pkg/src/test_transform_listener.cpp:
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
using namespace std::chrono_literals;
int main(int argc, char **argv)
{
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("test_transform_listener");
node->declare_parameter("foo");
auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node);
while (!parameters_client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for the service. Exiting.");
return 0;
RCLCPP_INFO(node->get_logger(), "service not available, waiting again...");
std::shared_ptr<tf2_ros::Buffer> tf2_buffer =
std::make_shared<tf2_ros::Buffer>(node->get_clock());
tf2_ros::TransformListener tf2_listener(*tf2_buffer);
rclcpp::Rate r(50.0);
while (rclcpp::ok())
rclcpp::spin_some(node);
r.sleep();
rclcpp::shutdown();
return 0;
}
Create launch file test_tl_pkg/launch/test_transform_listener.py:
#!/usr/bin/env python3
import sys
import os
import ament_index_python.packages
import launch
import launch_ros.actions
def generate_launch_description():
# YAML file to default parameters
params_yaml = os.path.join(
ament_index_python.packages.get_package_share_directory('path_follower'),
'cfg', 'test_tl_params.yaml')
if len(sys.argv) > 3:
i = 3
while i < len(sys.argv):
if sys.argv[i] == '--params-yaml':
i += 1
params_yaml = sys.argv[i]
i += 1
args = None
if len(params_yaml) > 0:
args = ['__params:={}'.format(params_yaml)]
print('Default parameters YAML file set to {}'.format(params_yaml))
test_tl_node = launch_ros.actions.Node(package='path_follower',
node_executable='test_transform_listener',
node_name='test_transform_listener',
output='screen',
arguments=args,
return launch.LaunchDescription([test_tl_node,
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=test_tl_node,
on_exit=[launch.actions.EmitEvent(
event=launch.events.Shutdown())],
)),
])
Expected behavior
Parameter foo
should appear in ros2 param list
.
Actual behavior
Parameter does not appear. Two nodes have the same name.
$ ros2 launch test_tl_pkg test_transform_listener.py
[test_transform_listener-1] [WARN] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
$ ros2 node list
/launch_ros
/test_transform_listener
/test_transform_listener
$ ros2 param list
/launch_ros:
/test_transform_listener:
use_sim_time
Without renaming node in launch file, nodes appear with original names, and parameter foo
appears:
$ ros2 run test_tl_pkg test_transform_listener
$ ros2 node list
/test_transform_listener
/transform_listener_impl
$ ros2 param list
/test_transform_listener:
foo
use_sim_time
/transform_listener_impl:
use_sim_time
Steps to reproduce issue (constructor with rclcpp::Node)
Add node to TransformListener
constructor call, and assuming spin_thread, add_node(), and spin() are set up correctly as noted above with no runtime errors:
std::shared_ptr<tf2_ros::Buffer> tf2_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
tf2_ros::TransformListener tf2_listener(*tf2_buffer, node, spin_thread);
Regardless of whether spin_thread
is true
or false
.
Expected behavior
tf2_buffer->canTransform()
returns true for tf frames being broadcasted.
Actual behavior
tf2_buffer->canTransform()
returns false.
Additional information
To sum up, currently, in order for tf to work:
- Use the TransformListener constructor that does not take a Node object
- Do not rename the node (This leads to another workaround if the user wishes to pass in default parameters in launch file. It must be done via
__params:=<YAML_path>
, because passing in parameters to launch_ros.actions.Node()
in the launch file requires the node to be renamed.)