ros2 / geometry2 Goto Github PK

I'm not able to install ros-dashing-tf2-kdl from debians.

erle@kaladin:/opt/ros/dashing$ sudo apt install ros-dashing-tf2-
ros-dashing-tf2-dbgsym         ros-dashing-tf2-msgs           ros-dashing-tf2-ros-dbgsym
ros-dashing-tf2-eigen          ros-dashing-tf2-msgs-dbgsym    ros-dashing-tf2-sensor-msgs
ros-dashing-tf2-geometry-msgs  ros-dashing-tf2-ros 

But installying it from source it compiles and works perfectly.

Now that we forked this repo and started opening PRs against this repos' ros2 branch. Can we delete the ros/geometry2 ros2 branch?

There is a nasty bug in https://github.com/ros2/geometry2/blob/ros2/tf2/src/buffer_core.cpp line 143. It passes the quaternion arguments in the wrong order. It should be x, y, z, w.

I'm not sure if the underlying Quaternion has always taken them in the order it does now, or this bug was introduced recently. The various quaternion frameworks have a variety of point orderings; we must be very careful in this regard.

The affect of this bug is that any tool that pulls transforms using tf2_ros;:Buffer::lookupTransform will have the wrong rotation (unless it's all zeros). Cartographer relies on this feature.

The CMakeLists.txt require the Eigen3 library while in the package.xml file is declared as eigen

Bug report

Required Info:

• Operating System:
  • gentoo with profile amd64/17.0/desktop/plasma
• Installation type:
  • from source
• Version or commit hash:
  • db72716
• DDS implementation:
  • Fast-RTPS
• Client library (if applicable):
  • N/A

Steps to reproduce issue

following the instruction for install ROS2 as
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws
wget https://raw.githubusercontent.com/ros2/ros2/master/ros2.repos
vcs import ~/ros2_ws/src < ros2.repos
colcon build --symlink-install

Expected behavior

tf2_sensor_msgs build and install

Actual behavior

tf2_sensor_msgs fails configuration phase because Eigen3 is not declared

What do we need to do to get rid of this warning? It spews a lot of useless output.

ROS_INFO("LOOPING due to no latching at the moment\n")

Based on #92 Geometry doesn't make use of the RCLCPP_* logging mechanism, but instead uses printf

Additional information

The likely offenders are:

tf2_ros/src/buffer.cpp
41:#define ROS_ERROR printf
42:#define ROS_FATAL printf
43:#define ROS_INFO printf
44:#define ROS_WARN printf

tf2_ros/src/tf2_monitor.cpp
43:#define ROS_ERROR printf
44:#define ROS_FATAL printf
45:#define ROS_WARN printf
46:#define ROS_INFO printf

tf2_ros/src/transform_listener.cpp
38:#define ROS_ERROR printf
39:#define ROS_FATAL printf
40:#define ROS_INFO printf
41:#define ROS_WARN printf

tf2_py/src/tf2_py.cpp
77:    printf("Can't get geometry_msgs.msg.TransformedStamped\n");
84:    printf("Can't build argument list\n");
93:    printf("Can't create class\n");
574:    printf("Cannot load geometry_msgs module");

tf2/src/buffer_core.cpp
224:    printf("Warning old setTransform Failed but was not caught\n");

I've seen that get_now() only returns the system clock. Does this eventually lead to out-of-sync with rclcpp-Time (ROS-time, system-time, steady-time, etc.)?

Bug report

• Operating System:
  • Ubuntu 18.04
• Installation type:
  • source
• Version or commit hash:
  • f282d27
• DDS implementation:
  • Fast-RTPS
• Client library (if applicable):
  • N/A

Steps to reproduce issue

The following test is failed.

TEST(tf2_clear, LookUp_Static_Transfrom_Fail)
{
  tf2::BufferCore tfc;
  geometry_msgs::msg::TransformStamped st;
  st.header.frame_id = "foo";
  st.header.stamp = builtin_interfaces::msg::Time();
  st.header.stamp.sec = 1;
  st.header.stamp.nanosec = 0;
  st.child_frame_id = "bar";
  st.transform.rotation.w = 1;
  EXPECT_TRUE(tfc.setTransform(st, "authority1"));
  tfc.clear();
  EXPECT_TRUE(tfc.setTransform(st, "authority1", true));
  EXPECT_NO_THROW(
    auto trans = tfc.lookupTransform("foo", "bar", tf2::TimePoint(std::chrono::seconds(2)));
  );
}

In contrast, the following test is succeed.

TEST(tf2_clear, LookUp_Static_Transfrom_Succeed)
{
  tf2::BufferCore tfc;
  geometry_msgs::msg::TransformStamped st;
  st.header.frame_id = "foo";
  st.header.stamp = builtin_interfaces::msg::Time();
  st.header.stamp.sec = 1;
  st.header.stamp.nanosec = 0;
  st.child_frame_id = "bar";
  st.transform.rotation.w = 1;
  tfc.clear();
  EXPECT_TRUE(tfc.setTransform(st, "authority1", true));
  EXPECT_NO_THROW(
    auto trans = tfc.lookupTransform("foo", "bar", tf2::TimePoint(std::chrono::seconds(2)));
  );
}

Expected behavior

The first test should be succeed.

Actual behavior

It is failed.

Additional information

It seems there would be some error on clearing the buffer. Like the tests above, if we set non-static transform to the buffer firstly, then set the identical transform except it's static to the buffer, lookupTransform throws up an extrapolation exception:

C++ exception with description "Lookup would require extrapolation at time 2.00000, but only time 1.00000 is in the buffer, when looking up transform from frame [bar] to frame [foo]" thrown in the test body.

It seems a bug since a static transform should have no extrapolation issue like the second test above is passing.

Feature request

Can the message_filter.h of tf2_ros be ported to ROS2.0 (https://github.com/ros2/geometry2/blob/ros2/tf2_ros/include/tf2_ros/message_filter.h)? There is already a message_filter package available for ROS2.0 (https://github.com/intel/ros2_message_filters).

Feature request

Feature description

Add constructor for LifecycleNode to tf2_ros::TransformBroadcaster.

Implementation considerations

Refer to ROS answer thread:
https://answers.ros.org/question/302459/ros2-tf2_rostransformbroadcaster-and-rclcpp_lifecyclelifecyclenode/

Bug report

Required Info:

• Operating System:
  • Ubuntu 18.04 LTS
• Installation type:
  • Binaries or source
• Version or commit hash:
  • d89b9a2
• DDS implementation:
  • Fast-RTPS
• Client library (if applicable):
  • rclpy

Steps to reproduce issue

import tf2_ros

Expected behavior

Module tf2_ros is imported.

Actual behavior

Error ModuleNotFoundError: No module named 'tf2_ros' is raised.

Additional information

A pull request for this issue is here.

The package is currently commented out and allowed the regression fixed in #39

This will likely hit some limits on our current test infrastructure. But it will be good to find out what the current limits are.

In tf2::Buffer::lookupTransform, there are consecutive calls to canTransform and lookupTransform. Internally, both calls take a lock on the underlying BufferCore object, but the lock is relinquished in between those two function calls.

This can result in a failure to transform with no error message or an incorrect error message.

Feature request

Feature description

Similar to #70 and #94, the MessageFilter class needs support for lifecycle nodes.

Implementation considerations

There could be a constructor that takes the required node interfaces, perhaps in addition to rclcpp::Node and rclcpp_lifecycle::LifecycleNode.

Bug report

Required Info:

• Operating System:
  • Ubuntu 18.04
• Installation type:
  • source
• Version or commit hash:
  • ros2 branch
• DDS implementation:
  • default / FastRTPS
• Client library (if applicable):
  • rclcpp

Steps to reproduce issue

The argument parsing in static_transform_publisher can't currently handle the params passed by the launch file:

ros2 launch nav2_bringup test_bringup_launch.py

test_bringup_launch.py file is this:

from launch import LaunchDescription

import launch.actions
import launch_ros.actions


def generate_launch_description():
    params_file = launch.substitutions.LaunchConfiguration('params', default='nav2_params.yaml')
    
    return LaunchDescription([

        launch.actions.DeclareLaunchArgument(
            'params', default_value='nav2_params.yaml', description='Full path to params file to load'),
        
        launch_ros.actions.Node(
            package='tf2_ros',
            node_executable='static_transform_publisher',
            output='screen',
            arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'],
            parameters=[params_file])
    ])

Params file is this:

static_transform_publisher:
    ros__parameters:
        use_sim_time: True

Expected behavior

The parameter is set correctly and the TF is published correctly.

Actual behavior

The parameter is set correctly.

However the TF is not published correctly. When I view the TF using this command:

ros2 run tf2_ros tf2_echo base_link base_footprint

I get these errors:

Failure at 1.54169e+09
Exception thrown:"base_link" passed to lookupTransform argument target_frame does not exist. 
The current list of frames is:

Error:   TF_NAN_INPUT: Ignoring transform for child_frame_id "__params:=nav2_params.yaml" from authority "Authority undetectable" because of a nan value in the transform (0.000000 0.000000 0.000000) (-nan -nan -nan -nan)
         at line 258 in /home/mkhansen/ros2_dev/ros2_ws/src/ros2/geometry2/tf2/src/buffer_core.cpp
Error:   TF_DENORMALIZED_QUATERNION: Ignoring transform for child_frame_id "__params:=nav2_params.yaml" from authority "Authority undetectable" because of an invalid quaternion in the transform (-nan -nan -nan -nan)
         at line 271 in /home/mkhansen/ros2_dev/ros2_ws/src/ros2/geometry2/tf2/src/buffer_core.cpp

Additional information

I looked at the static_transform_broadcaster_program.cpp and can see where it is parsing the arguments. It is currently looking at 'argc' to decide how to assign the variables. It would need to first strip the __params:= and __name:= from the args list before proceeding.

Feature request

• transforms function the same way as namespaces, allowing for pushing down of frame_ids
• loading in static_tf_params from a yaml file not directly from a launch file

Feature description

With ROS 1.0 namespaces you could "push down" different nodes into unique namespaces so that you could have multiple of the same-named nodes in different namespaces (i.e. /left_arm/hand, /right_arm/hand).

This concept is addressed in the tf2/CommonQuestions page however I don't necessarily agree with the argument here.

When creating a static transform structure for robot A, and you want to create another instance of that same robot but call it robot B - with the current naming structure you are required to rename every single coordinate frame and make it unique instead of just changing the name to robot_B. Why cant we use by default the same structure of pushing down like we do with namespaces?

Also, if we could add a feature to static_tf_params.cpp to load in the xyzrpy values from a yaml it would help a lot too I think. I've had a modified version of static_tf_params.cpp for the last year and it has helped standardize my tf structure and launch files - allowing me to use the same launch file for many different robots.

Implementation considerations

• follow the same structure as ros namespaces, just for the frame_id by default use the full namespace name
• add in functionality to static_tf_params.cpp file to load in the xyzrpy values from the parameter server....letting users load in a yaml file at launch to the parameter server using rosparam load

┆Issue is synchronized with this Asana task

The behavior in ROS 1 was to subscribe to /tf and /tf_static in the global namespace.

It appears that was changed in ROS 2 and it instead subscribes in the node's current namespace. Is this expected behavior?

Note the lack of the beginning slash in the topic names:
https://github.com/ros2/geometry2/blob/ros2/tf2_ros/src/transform_listener.cpp#L84

Since ROS 2 lacks remapping currently this poses problems with trying to use namespaces and TF. The only way around it is to give the listener a new node in the global namespace. This makes the rosgraph pretty nasty and adds overhead in complex systems.

Steps to reproduce issue

• Using ROS 2 call transform (e.g. via buffer_interface.h) with a message containing an arbitrary frame (e.g. "map") and the same target_frame name.
• Do not add a transform to that particular frame.
• A lookup exception will be thrown, because "map" does not exist.

Expected behavior

It's clear how to transform a frame to itself (identity transform). Since both frame names are equal, I would have expected that answer regardless of whether transforms to other frames are published.

Actual behavior

An error is thrown.

When trying to use AMCL, I got the following errors. They are problematic because (1) it's hard to tell they are coming from a MessageFilter, (2) it gives no idea of severity (INFO seems innocuous but signalFailure sounds scary). (3) the "reason" code is not interpretable.

[amcl-2] [INFO] [amcl_rclcpp_node]: [signalFailure] Drop message: frame 'lidar_link' at time 1560375247.070 for reason(0)
[amcl-2] [INFO] [amcl_rclcpp_node]: [signalFailure] Drop message: frame 'lidar_link' at time 1560375247.142 for reason(0)
[amcl-2] [INFO] [amcl_rclcpp_node]: [signalFailure] Drop message: frame 'lidar_link' at time 1560375247.214 for reason(0)

Like ros/geometry@868fd89

Currently the Python portions of tf2_kdl are commented out: https://github.com/ros2/geometry2/pull/90/files#diff-61737c5a9ef69ef2118eafbc59732b6aR21 . This is because at the time the tf2_kdl port was done, the tf2_py package was not yet ported (see #99). Once that is done, we can finish the port of the tf2_kdl python portions.

@mlautman has additional followup tasks (mostly around the python) in #90 (comment), which should be completed when we complete the python port.

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:
  • Ubuntu 18.04
• Installation type:
  • Tarball installation in Docker
• Version or commit hash:
  • Dashing
• DDS implementation:
  • Fast-RTPS
• Client library (if applicable):
  • rclcpp

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.)

As suggested in ros2/rclcpp#536 (comment) the logic could be moved to rcutils and be used in rclcpp::Time and rclcpp::Duration.

tf2_monitor about Frame Average Delay is not accurate, the calculation missed nanosec.

Frame: base_link, published by <no authority available>, Average Delay: 0.5390258, Max Delay: 0.952545
The Delay is higher than the facts.

Root cause:
tf2_ros::timeToSec(message.transforms[i].header.stamp). did not return nanosec.

printf:
message.transforms[i].header.stamp.sec = 1521181659
message.transforms[i].header.stamp.nanosec = 874698954
tf2_ros::timeToSec(message.transforms[i].header.stamp) = 1521181659.000000

expect:
tf2_ros::timeToSec(message.transforms[i].header.stamp) = 1521181659.874698

nanosec missed.

If a TransformListener object goes out of scope before rclcpp::shutdown is called, it will hang forever in the destructor waiting for the worker thread to join

Bug report

Required Info:

• Operating System:
  • Ubuntu 16.04
• Installation type:
  • source
• Version or commit hash:
  • 0.9.1
• DDS implementation:
  -Fast-RTPS
• Client library (if applicable):

Steps to reproduce issue

This example is a bit pathological but shows the issue.

#include <memory>
#include <exception>
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  try {
    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener tfListener(tfBuffer);
    std::cerr << "Finished construction" << std::endl;
    throw std::runtime_error("blah");
  } catch (...) {
    std::cerr << "caught exception" << std::endl;
  }
  rclcpp::shutdown();

  return 0;
}

Expected behavior

I expect the program to terminate.

Actual behavior

Program hangs trying to unwind the try block

Additional information

The destructor of the TransformListener object calls join on the worker thread. Since nothing tells the worker thread to terminate in this case, the program hangs until someone presses Ctrl-C.

I believe the TransformListener destructor should tell the worker thread to terminate before it calls join

This repo does not have a top level LICENSE file.
The MIT license makes the most sense for a project like this.

We have been investigating why we can't seem to get lookupTransform to work properly for us, and came across this function.

Instead of doing the big long thing that is going on there:

inline double durationToSec(const tf2::Duration& input){
    int64_t count = input.count();
    int32_t sec, nsec;
    nsec = static_cast<int32_t>(count % 1000000000l);
    sec = static_cast<int32_t>((count - nsec) / 1000000000l);

    double sec_double, nsec_double;
    nsec_double = 1e-9 * static_cast<double>(nsec);
    sec_double = static_cast<double>(sec);
    return sec_double + nsec_double;
  }

couldn't we just do

inline double durationToSec(const tf2::Duration& input){
    return input.count() * 1e-9;
  }

I can't see a reason for the extra stuff that is going on in there.

tf2 has two public getYaw methods, one in include/tf2/impl/utils.h as inline double tf2::impl::getYaw(const tf2::Quaternion& q) and one in include/tf2/utils.h as template <class A> double tf2::getYaw(const A& a), which internally uses the first.

The first method should probably not be in a public header, since the second subsumes it. I'm guessing that the headers in include/tf2/impl were not meant to be public.

Bug report

Required Info:

• Operating System:
  Ubuntu 18.04
• Installation type:
  source
• Version or commit hash:
  e29db0a
• DDS implementation:
  Fast-RTPS
• Client library (if applicable):
  rclcpp, nav2_amcl

Steps to reproduce issue

Launching nav2_bringup's nav2_simulation_launch.py and waiting some minutes.

Expected behavior

Nothing happen

Actual behavior

Exception(what(): std::future_error: Promise already satisfied) is raised at the following code.

Additional information

It seems Buffer::timerCallback is called

Bug report

Required Info:

• Operating System:
  Ubuntu 18.04
• Installation type:
  Source
• Version or commit hash:
  ros2 branch, 0.10.1 tag
• DDS implementation:
  Fast RTPS
• Client library (if applicable):
  rclcpp

Steps to reproduce issue

Not sure how exactly to reproduce as I don't fully understand yet.

Expected behavior

Actual behavior

Getting this message spewed to the screen continuously:

Do not call canTransform or lookupTransform with a timeout unless you are using another thread for populating data. Without a dedicated thread it will always timeout.  If you have a seperate thread servicing tf messages, call setUsingDedicatedThread(true) on your Buffer instance.

Warning: Do not taunt happy fun ball! :)

Additional information

Looking at the code, I see that first off this is using ROS_ERROR to print, which is #defined as 'printf', with a TODO to fix it.

//TODO(tfoote replace these terrible macros)
#define ROS_ERROR printf
#define ROS_FATAL printf
#define ROS_INFO printf
#define ROS_WARN printf

As for the message itself, what does it really mean? I'm not sure what is causing it exactly except that I get it here:

  node_handle_ = std::shared_ptr<::rclcpp::Node>(this, [](::rclcpp::Node *) {});

  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  tf_broadcaster_.reset(new tf2_ros::TransformBroadcaster(node_handle_));
  tf_buffer_.reset(new tf2_ros::Buffer(get_clock(), tf2::durationFromSec(kTfBufferCacheTimeInSeconds)));
 tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_));

When I change the last line to be this to re-use the existing node instead of creating a new one:

  tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_, node_handle_, false))

Then I get this error.

I have to make this change, or I get an exception thrown by the logger due to duplicate node names.
ros2/rcl#375

Please let me know how to properly declare a TransformListener so that I don't get this error.

Hello,

I tryed to clone/checkout this package on different machines and received the following error:

ros/geometry2:
---------------------------------------------------------------
mkdir -p /tmp/ros/ros2_ws/src/ros
>> cloning sources
change path to: /tmp/ros/ros2_ws/src/ros
git clone https://github.com/ros/geometry2.git
Cloning into 'geometry2'...
remote: Counting objects: 8784, done.
remote: Compressing objects: 100% (3/3), done.
remote: Total 8784 (delta 0), reused 0 (delta 0), pack-reused 8781
Receiving objects: 100% (8784/8784), 2.04 MiB | 610.00 KiB/s, done.
Resolving deltas: 100% (5363/5363), done.
Checking connectivity... done.
>> checkout desired version
change path to: /tmp/ros/ros2_ws/src/ros/geometry2
git checkout 3a820ee7238e406282c901991186adc6558cdb5e
fatal: reference is not a tree: 3a820ee7238e406282c901991186adc6558cdb5e
error while checkout with git

Currently the static transform publisher requires a shared pointer to a complete node in order to create the publisher. This means, that as-is, this TF publisher can't be easily used in a composable node constructor, essentially because a shared pointer to this can't be created when the object itself is not fully created.

The feature request here is to use the NodeTopicsInterface instead and call create publisher on this instead of the full node instance.
See here:
https://github.com/ros2/rclcpp/blob/master/rclcpp/include/rclcpp/create_publisher.hpp#L28-L47

This will also ease up the tickets regarding the lifecycle nodes #94 and #95 as they also provide the node topic interface.

Similar work is started in here: #100

Bug report

Required Info:

• Operating System:
  • macOS 10.14.5
• Installation type:
  • From source
• Version or commit hash:
• DDS implementation:
  • Fast-RTPS

Steps to reproduce issue

Followed build from source
On running
colcon build --symlink-install

code that can be copy-pasted is preferred -->

--- stderr: tf2
In file included from /Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/src/gtest-all.cc:41:
In file included from /Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest.cc:135:
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-internal-inl.h:63:1: error: C++ requires a type specifier for all declarations
GTEST_DISABLE_MSC_WARNINGS_PUSH_(4251 \
^
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-internal-inl.h:64:76: error: expected ';' after top level declarator
/* class A needs to have dll-interface to be used by clients of class B */)
                                                                           ^
                                                                           ;
In file included from /Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/src/gtest-all.cc:42:
In file included from /Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:47:
In file included from /Applications/Xcode.app/Contents/Developer/Platforms/MacOSX.platform/Developer/SDKs/MacOSX10.14.sdk/usr/include/fcntl.h:23:
/Applications/Xcode.app/Contents/Developer/Platforms/MacOSX.platform/Developer/SDKs/MacOSX10.14.sdk/usr/include/sys/fcntl.h:351:18: error: field has incomplete type 'struct flock'
        struct flock    fl;             /* flock passed for file locking */
                        ^
/Applications/Xcode.app/Contents/Developer/Platforms/MacOSX.platform/Developer/SDKs/MacOSX10.14.sdk/usr/include/sys/fcntl.h:351:9: note: forward declaration of 'flock'
        struct flock    fl;             /* flock passed for file locking */
               ^
In file included from /Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/src/gtest-all.cc:42:
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:95:46: error: use of undeclared identifier 'GTEST_DEFAULT_DEATH_TEST_STYLE'
static const char kDefaultDeathTestStyle[] = GTEST_DEFAULT_DEATH_TEST_STYLE;
                                             ^
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:291:24: error: member access into incomplete type 'internal::UnitTestImpl'
      GetUnitTestImpl()->internal_run_death_test_flag();
                       ^
/usr/local/include/gtest/internal/gtest-internal.h:98:7: note: forward declaration of 'testing::internal::UnitTestImpl'
class UnitTestImpl;                    // Opaque implementation of UnitTest
      ^
In file included from /Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/src/gtest-all.cc:42:
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:371:43: error: member access into incomplete type 'internal::UnitTestImpl'
  TestInfo* const info = GetUnitTestImpl()->current_test_info();
                                          ^
/usr/local/include/gtest/internal/gtest-internal.h:98:7: note: forward declaration of 'testing::internal::UnitTestImpl'
class UnitTestImpl;                    // Opaque implementation of UnitTest
      ^
In file included from /Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/src/gtest-all.cc:42:
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:381:24: error: no template named 'Matcher'
                       Matcher<const std::string&> matcher, const char* file,
                       ^
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:383:27: error: member access into incomplete type 'internal::UnitTestImpl'
  return GetUnitTestImpl()->death_test_factory()->Create(
                          ^
/usr/local/include/gtest/internal/gtest-internal.h:98:7: note: forward declaration of 'testing::internal::UnitTestImpl'
class UnitTestImpl;                    // Opaque implementation of UnitTest
      ^
In file included from /Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/src/gtest-all.cc:42:
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:400:42: error: no template named 'Matcher'
  DeathTestImpl(const char* a_statement, Matcher<const std::string&> matcher)
                                         ^
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:441:3: error: no template named 'Matcher'
  Matcher<const std::string&> matcher_;
  ^
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:1065:43: error: no template named 'Matcher'
  ForkingDeathTest(const char* statement, Matcher<const std::string&> matcher);
                                          ^
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:1080:36: error: no template named 'Matcher'
                                   Matcher<const std::string&> matcher)
                                   ^
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:1087:8: error: cannot initialize object parameter of type 'const testing::internal::DeathTestImpl' with an expression of type 'testing::internal::ForkingDeathTest'
  if (!spawned())
       ^~~~~~~
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:1090:3: error: cannot initialize object parameter of type 'testing::internal::DeathTestImpl' with an expression of type 'testing::internal::ForkingDeathTest'
  ReadAndInterpretStatusByte();
  ^~~~~~~~~~~~~~~~~~~~~~~~~~
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:1094:3: error: cannot initialize object parameter of type 'testing::internal::DeathTestImpl' with an expression of type 'testing::internal::ForkingDeathTest'
  set_status(status_value);
  ^~~~~~~~~~
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:1102:44: error: no template named 'Matcher'
  NoExecDeathTest(const char* a_statement, Matcher<const std::string&> matcher)
                                           ^
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:1131:3: error: cannot initialize object parameter of type 'testing::internal::ForkingDeathTest' with an expression of type 'testing::internal::NoExecDeathTest'
  set_child_pid(child_pid);
  ^~~~~~~~~~~~~
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:1134:5: error: cannot initialize object parameter of type 'testing::internal::DeathTestImpl' with an expression of type 'testing::internal::NoExecDeathTest'
    set_write_fd(pipe_fd[1]);
    ^~~~~~~~~~~~
/Users/danielluu/ros2_ws/install/gtest_vendor/src/gtest_vendor/./src/gtest-death-test.cc:1141:22: error: member access into incomplete type 'internal::UnitTestImpl'
    GetUnitTestImpl()->listeners()->SuppressEventForwarding();
                     ^
/usr/local/include/gtest/internal/gtest-internal.h:98:7: note: forward declaration of 'testing::internal::UnitTestImpl'
class UnitTestImpl;                    // Opaque implementation of UnitTest
      ^
fatal error: too many errors emitted, stopping now [-ferror-limit=]
20 errors generated.
make[2]: *** [gtest/CMakeFiles/gtest.dir/src/gtest-all.cc.o] Error 1
make[1]: *** [gtest/CMakeFiles/gtest.dir/all] Error 2
make: *** [all] Error 2
---
Failed   <<< tf2	[ Exited with code 2 ]
Aborted  <<< rcl
Aborted  <<< shape_msgs
Aborted  <<< nav_msgs
Aborted  <<< diagnostic_msgs
Aborted  <<< visualization_msgs
Aborted  <<< tf2_msgs
Aborted  <<< sensor_msgs

Expected behavior

All packages processed

Actual behavior

Erroring on tf2 would like to see if anyone has run into this

Bug report

Required Info:

• Operating System: Ubuntu 18.04
• Installation type: source
• Version or commit hash: ros2
• DDS implementation: n/a
• Client library (if applicable):

Steps to reproduce issue

build from source: colcon build

Expected behavior

Compiling

Actual behavior

Not compiling

Additional information

/home/s.macenski/Documents/navigation2_ws/src/geometry2/tf2_ros/include/tf2_ros/message_filter.h:131:40: error: ‘BufferCoreInterface’ is not a member of ‘tf2’
     static_assert(std::is_base_of<tf2::BufferCoreInterface, BufferT>::value,

I've tried to add explicitly the header for buffer core interface (though it should be found) and the error changes to

/home/s.macenski/Documents/navigation2_ws/src/geometry2/tf2_ros/include/tf2_ros/message_filter.h: In instantiation of ‘tf2_ros::MessageFilter<M, BufferT>::MessageFilter(BufferT&, const string&, uint32_t, const SharedPtr&) [with M = geometry_msgs::msg::PointStamped_<std::allocator<void> >; BufferT = tf2_ros::Buffer; std::__cxx11::string = std::__cxx11::basic_string<char>; uint32_t = unsigned int; rclcpp::Node::SharedPtr = std::shared_ptr<rclcpp::Node>]’:
/home/s.macenski/Documents/navigation2_ws/src/geometry2/tf2_ros/test/message_filter_test.cpp:65:90:   required from here
/home/s.macenski/Documents/navigation2_ws/src/geometry2/tf2_ros/include/tf2_ros/message_filter.h:132:5: error: static assertion failed: Buffer type must implement tf2::BufferCoreInterface
     static_assert(std::is_base_of<tf2::BufferCoreInterface, BufferT>::value,
     ^~~~~~~~~~~~~

It begins to work when I purge all the current dashing debians from my computer, which is obviously not ideal. I'm not sure why its linking outside the workspace when its available

sudo apt remove ros-dashing-tf*

Currently tf2_ros::TransformListener has a hard coded queue depth of 100 for the /tf topic subscription.

It also (optionally) has a dedicated thread to call rclcpp::spin(). New subscriptions seem to get messages in order with this QoS and fastrtps. When this thread cannot process subscription callbacks fast enough the transform listener's callback TransformListener::subscription_callback_impl is always being called with the oldest transform in the queue. However, there are 99 newer messages available in the queue. In the case of one tf publisher publishing at 100Hz the transforms looked up will always be 1 second out of date even though there is newer data in a lower layer. If tf2 is going to be missing messages then what it really wants the latest message in the queue.

The best solution is for rclcpp::spin to always call callbacks fast enough, but maybe there is something TransformListener could do to mitigate this case.

Say there was a maintenance task on a timer that changed the queue size by creating a new subscription, and then shutting the old one down when the new one started receiving messages. The new QoS history size could be calculated by from statistics about the transform sources. I think the sum of ( period of the transform broadcaster with the longest period between messages * the frequency of each transform broadcaster) is the minimum queue size needed to ensure no data is lost.

This would mitigate ros2/ros1_bridge#133, which appears to be caused by subscription callbacks not being processed fast enough in RViz2.

After discussion with @dhood and @dirk-thomas, starting with this comment:

https://github.com/ros2/geometry2/pull/7/files#r111020855

We need to fix up tf2_geometry_msgs to not return tf2_ros::fromMsg reference, since that returns a temporary and thus can access invalid memory.

We can:

• Make the problematic getTimestamp() calls return objects, instead of references, or
• Make the problematic getTimestamp() calls return builtin_msgs::interface::Time references, which would then be tied to the lifetime of the passed in message. Callers would then have to translate those to tf2::TimePoints as needed.

Opening issue to avoid duplicating work. Has anyone ( @clalancette) looked at porting the transform() methods in tf2_ros/buffer_interface.h yet? They're still commented out

lookupTransform outputs a transform whose timestamp does not match the time that has been asked for.

We see the transform's timestamp looping from being 0.5s behind the requested timestamp to being up to 4s ahead of it. These numbers are of course with our specific setup. We have the tf being broadcast at 50 hz and it being queried at 4 hz.

The broadcasting and listening code we are using can be found in the following files:

tf_listener.txt

tf_broadcaster.txt

Here is the sample output of the listener:

tf_listener_output.txt

We have seen similar behavior in querying tfs on an actual robot as well.

My ros is ardent, and I use 'ament build' command to build the ros2/geometry2 package , there is no error, but there only a tf2_msgs model int the path of install/lib/python3.5/site-packages, no tf2_ros, no tf2_py, there is sth wrong with me? Any help will be appriciated! And python3 cannot import tf2 or tf2_ros. Tf2 python model is now available in ros2?

The ROS2 port has had the heterogeneous convert function disabled for quite a while now due to the inability to distinguish ROS messages at compile time (see here). Twice now I've spend a good chunk of time debugging why the call to convert wasn't compiling before finally opening the source and discovering the functionality was removed.

How can this be resolved? If a trait-based conversion system isn't visible on the horizon, should it be declared conversions will have to use fromMsg/toMsg explicitly?

Bug report

Required Info:

• Operating System:
  • OSX (High Sierra)
• Installation type:
  • Binary
• Version or commit hash:
  • Crystal (pre-release)
• DDS implementation:
  • Fast-RTPS, Connext (5.3.1), and Opensplice (6.9)

Steps to reproduce issue

Follow the tf2 tutorial.
In one terminal:

ros2 run tf2_ros static_transform_publisher 1 2 3 0.5 0.1 -1.0 foo bar

In a second terminal:

ros2 run tf2_ros tf2_echo foo bar

Expected behavior

Second terminal (tf2_echo) starts outputting:

At time 0.0
- Translation: [1.000, 2.000, 3.000]
- Rotation: in Quaternion [-0.475, -0.076, 0.240, 0.843]

Actual behavior

tf2_echo outputs the following error message one or more times before we see the expected output:

Failure at 1.54475e+09
Exception thrown:"foo" passed to lookupTransform argument target_frame does not exist. 
The current list of frames is:

Additional information

The time between the exception error message and the expected output is especially long if the transform publisher is using Fast-RTPS and tf2_echo is using OpenSplice.

Not sure what to do here.

I started looking at this because the usage string of tf2_monitor is missing a trailing newline.
Right now it's using ROS_INFO aliased with printf:

We could do one of the following:

• Change to call printf explicitely with a trailing new line like it's done in tf2_echo:

  geometry2/tf2_ros/src/tf2_echo.cpp

  Line 69 in d8958f6

  printf("Usage: tf_echo source_frame target_frame [echo_rate]\n\n");

• Switch to ros2 logging but it will make the usage string hard to read because of the current lack of configuration:

[INFO] []: TF_Monitor: usage: tf2_monitor framea frameb (main() at /home/mikael/work/ros2/test_sprint_ws/src/ros2/geometry2/tf2_ros/src/tf2_monitor.cpp:290)

• Do nothing for now

[Windows, FastRTPS]
When running static_transform_publisher, tf_monitor does not list any provided transforms:

> static_transform_publisher 1 2 3 0.1 0.1 0.1 foo bar

LOOPING due to no latching at the moment
LOOPING due to no latching at the moment
...

> tf2_monitor

RESULTS: for all Frames

Frames:

All Broadcasters:

RESULTS: for all Frames

Frames:

All Broadcasters:

...

TransformListener has a constructor that takes a Node pointer, but if you use it you get this error:

terminate called after throwing an instance of 'std::runtime_error'
  what():  Node has already been added to an executor.

Does the listener require a dedicated Node (like the one created in the constructor that doesn't take a node)?

-- run_test.py: invoking following command in '/home/rosbuild/ci_scripts/ws/src/ros2/geometry2/tf2_geometry_msgs':
 - /home/rosbuild/ci_scripts/ws/build/tf2_geometry_msgs/test_tf2_geometry_msgs --gtest_output=xml:/home/rosbuild/ci_scripts/ws/build/tf2_geometry_msgs/test_results/tf2_geometry_msgs/test_tf2_geometry_msgs.gtest.xml
[==========] Running 3 tests from 1 test case.
[----------] Global test environment set-up.
[----------] 3 tests from TfGeometry
[ RUN      ] TfGeometry.Frame
/home/rosbuild/ci_scripts/ws/src/ros2/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp:57: Failure
The difference between v_simple.pose.orientation.x and 0.0 is nan, which exceeds EPS, where
v_simple.pose.orientation.x evaluates to -nan,
0.0 evaluates to 0, and
EPS evaluates to 0.001.
/home/rosbuild/ci_scripts/ws/src/ros2/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp:58: Failure
The difference between v_simple.pose.orientation.y and 0.0 is nan, which exceeds EPS, where
v_simple.pose.orientation.y evaluates to -nan,
0.0 evaluates to 0, and
EPS evaluates to 0.001.
/home/rosbuild/ci_scripts/ws/src/ros2/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp:59: Failure
The difference between v_simple.pose.orientation.z and 0.0 is nan, which exceeds EPS, where
v_simple.pose.orientation.z evaluates to -nan,
0.0 evaluates to 0, and
EPS evaluates to 0.001.
/home/rosbuild/ci_scripts/ws/src/ros2/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp:60: Failure
The difference between v_simple.pose.orientation.w and 1.0 is nan, which exceeds EPS, where
v_simple.pose.orientation.w evaluates to -nan,
1.0 evaluates to 1, and
EPS evaluates to 0.001.
/home/rosbuild/ci_scripts/ws/src/ros2/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp:69: Failure
The difference between v_advanced.pose.orientation.x and 0.0 is nan, which exceeds EPS, where
v_advanced.pose.orientation.x evaluates to -nan,
0.0 evaluates to 0, and
EPS evaluates to 0.001.
/home/rosbuild/ci_scripts/ws/src/ros2/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp:70: Failure
The difference between v_advanced.pose.orientation.y and 0.0 is nan, which exceeds EPS, where
v_advanced.pose.orientation.y evaluates to -nan,
0.0 evaluates to 0, and
EPS evaluates to 0.001.
/home/rosbuild/ci_scripts/ws/src/ros2/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp:71: Failure
The difference between v_advanced.pose.orientation.z and 0.0 is nan, which exceeds EPS, where
v_advanced.pose.orientation.z evaluates to -nan,
0.0 evaluates to 0, and
EPS evaluates to 0.001.
/home/rosbuild/ci_scripts/ws/src/ros2/geometry2/tf2_geometry_msgs/test/test_tf2_geometry_msgs.cpp:72: Failure
The difference between v_advanced.pose.orientation.w and 1.0 is nan, which exceeds EPS, where
v_advanced.pose.orientation.w evaluates to -nan,
1.0 evaluates to 1, and
EPS evaluates to 0.001.
[  FAILED  ] TfGeometry.Frame (2 ms)
[ RUN      ] TfGeometry.Vector
[       OK ] TfGeometry.Vector (0 ms)
[ RUN      ] TfGeometry.Point
[       OK ] TfGeometry.Point (0 ms)
[----------] 3 tests from TfGeometry (2 ms total)

[----------] Global test environment tear-down
[==========] 3 tests from 1 test case ran. (2 ms total)
[  PASSED  ] 2 tests.
[  FAILED  ] 1 test, listed below:
[  FAILED  ] TfGeometry.Frame

 1 FAILED TEST
-- run_test.py: return code 1
-- run_test.py: verify result file '/home/rosbuild/ci_scripts/ws/build/tf2_geometry_msgs/test_results/tf2_geometry_msgs/test_tf2_geometry_msgs.gtest.xml'

-- http://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/76/testReport/(root)/projectroot/test_tf2_geometry_msgs/

I notice a lot of cases (e.g. spinning up nodes, transform_listeners without their own thread, network latency due to WiFi multicast) where tf throughput will slow to a crawl. Would it make sense to change the /tf QoS from reliable to best effort so it is not subject to this backpressure?

Feature request

Feature description

Similar to #70, the TransformListener class needs support for lifecycle nodes.

Implementation considerations

There could be a constructor that takes the required node interfaces, perhaps in addition to rclcpp::Node and rclcpp_lifecycle::LifecycleNode.

  File "ros_pp.py", line 12, in <module>
    from tf2_ros.src.tf2_ros.transform_listener import TransformListener
  File "../../../src/geometry2/tf2_ros/src/tf2_ros/__init__.py", line 39, in <module>
    from buffer_interface import *
ModuleNotFoundError: No module named 'buffer_interface'

I have built the geometry library for my project using colcon for ROS2 Dashing and Python 3 and sourced it using source install/setup.bash but am getting this error on importing the TransformListener from tf2_ros.

My directory structure is as follows:

-workspace
         - build
         - install
         - src
                -  my_module
                          - src
                              - file.py
                -  geometry2

It would be great if anyone could help me know where exactly the error is coming from.

Bug report

Required Info:

• Operating System:
  • Ubuntu 18.04
• Installation type:
  • source
• Version or commit hash:
  • fccfd98 (latest on master 6 Aug 2019)
• DDS implementation:
  • fastRTPS
• Client library (if applicable):
  • rclcpp

Steps to reproduce issue

I see this problem when running the navigation2 stack in Gazebo. I can try to provide a simple way to reproduce if needed.

Expected behavior

Message filter will wait a reasonable time for the incoming transform

Actual behavior

Almost every incoming laser scan message results in the following error message

[signalFailure] Drop message: frame 'base_scan' at time 13.201 for reason(0)

Additional information

nav2_amcl is using message_filters to wait for the transform associated with lidar scan data. With the introduction of PR #121, we now get nearly a 100% failure rate on message_filters. I tracked it down to "Timed out waiting for transform" exceptions from here

I noticed message_filter sets a timeout of 0ns on waitForTransform. here

0ns timeout is not enough time to receive the incoming transform data, so nearly every waitForTransform call times out. If I use a reasonable value for the timeout (200ms in this case) message filter starts working again.

It seems like this timeout should be a parameter to the message filter constructor. Thoughts?