ros / geometry2 Goto Github PK

View Code? Open in Web Editor NEW

188.0 26.0 273.0 2.66 MB

A set of ROS packages for keeping track of coordinate transforms.

CMake 2.78% C++ 82.22% Python 14.98% C 0.03%

geometry2's People

Stargazers

Watchers

Forkers

kaijenhsiao tfoote bulwahn danepowell jhu-lcsr-forks chadrockey garfonso fkie-forks vikingx kwatts braincorp hidof-forks v4hn vrabaud spacejockey trainman419 garyservin forouher k-okada tspicer01 jonbinney hordurk marbosjo mrath jacquelinekay rosebudflyaway terakoji jmtatsch lgeo3 sarahangle bigjun jspricke cmansley g-gemignani firesurfer daenny ellon dbking77 gavanderhoorn eric-wieser princeofdarkness76 felixduvallet jckchow cwecht phil0stine ericyao2013 mikepurvis xiekuncn mbs-technologie drmateo rhaschke achim-k nlamprian fryguy1013 karsten1987 lsouchet dhood jihoonl spaghetti- tik0 intermodalics stwirth swagner91 yxxumich hollisjoe at-wat kojiterada sergioramosjubierre fizyr-forks vsherrod clearpathrobotics smnogar miguelprada smart-robotics snrkiwi chenrui2014 d-walsh 6riversystems ros-hunter racko tommyvsfu1 wasowski lucasw samkhal meyerj iantheengineer lianghu3124 robust-rosin hunter-packages elaske pinto0309 agammal fabolhak kitrobotics magazino charliezon bluenote10 cicelprg open-cv ms-iot

geometry2's Issues

Getting an error that says there may be race condition issues with TF2

[ERROR] You've passed in true for auto_start for the C++ action server at [/tf2_buffer_server]. You should always pass in false to avoid race conditions.

Is this a bug? Is there a reason why true is used instead of false here?

Transitive link dependency on orocos_kdl is not handled properly.

I'm not entirely sure if this is a bug against tf2_geometry_msgs, orocos_kdl or catkin. Filing here first because I'm seeing it when trying to link against tf2_geometry_msgs.

When using and linking against tf2_geometry_msgs, I get the following linker error. I think this is because either tf2_geometry_msgs doesn't specify it's dependency on orocos_kdl properly, or the library dependency isn't properly captured by catkin during the catkin_package() cmake function.

In particular, the 'libraries' line in /opt/ros/hydro/share/tf2_geometry_msgs/cmake/tf2_geometry_msgsConfig.cmake is empty:

set(libraries "")

Which suggests that the dependency isn't getting captured correctly.

Linking CXX executable /home/hendrix/catkin_ws/devel/lib/fiducials_ros/fiducials_localization
cd /home/hendrix/catkin_ws/build/fiducials_ros && /usr/bin/cmake -E cmake_link_script CMakeFiles/fiducials_localization.dir/link.txt --verbose=1
/usr/bin/c++       CMakeFiles/fiducials_localization.dir/src/fiducials_localization.cpp.o  -o /home/hendrix/catkin_ws/devel/lib/fiducials_ros/fiducials_localization -rdynamic /home/hendrix/catkin_ws/devel/lib/libfiducials.so /home/hendrix/catkin_ws/devel/lib/libfiducials_rviz.so /opt/ros/hydro/lib/libtf2_ros.so /opt/ros/hydro/lib/libactionlib.so /opt/ros/hydro/lib/libtf2.so /opt/ros/hydro/lib/libimage_transport.so /opt/ros/hydro/lib/libmessage_filters.so -ltinyxml /opt/ros/hydro/lib/libclass_loader.so -lPocoFoundation -ldl /opt/ros/hydro/lib/libroscpp.so -lboost_signals-mt -lboost_filesystem-mt /opt/ros/hydro/lib/libxmlrpcpp.so /opt/ros/hydro/lib/libroslib.so /opt/ros/hydro/lib/libcv_bridge.so /opt/ros/hydro/lib/libopencv_calib3d.so /opt/ros/hydro/lib/libopencv_contrib.so /opt/ros/hydro/lib/libopencv_core.so /opt/ros/hydro/lib/libopencv_features2d.so /opt/ros/hydro/lib/libopencv_flann.so /opt/ros/hydro/lib/libopencv_gpu.so /opt/ros/hydro/lib/libopencv_highgui.so /opt/ros/hydro/lib/libopencv_imgproc.so /opt/ros/hydro/lib/libopencv_legacy.so /opt/ros/hydro/lib/libopencv_ml.so /opt/ros/hydro/lib/libopencv_nonfree.so /opt/ros/hydro/lib/libopencv_objdetect.so /opt/ros/hydro/lib/libopencv_photo.so /opt/ros/hydro/lib/libopencv_stitching.so /opt/ros/hydro/lib/libopencv_superres.so /opt/ros/hydro/lib/libopencv_video.so /opt/ros/hydro/lib/libopencv_videostab.so /opt/ros/hydro/lib/librosconsole.so /opt/ros/hydro/lib/librosconsole_log4cxx.so /opt/ros/hydro/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex-mt /opt/ros/hydro/lib/libroscpp_serialization.so /opt/ros/hydro/lib/librostime.so -lboost_date_time-mt -lboost_system-mt -lboost_thread-mt -lpthread /opt/ros/hydro/lib/libcpp_common.so /opt/ros/hydro/lib/libconsole_bridge.so /home/hendrix/catkin_ws/devel/lib/libfiducials_cv.so /home/hendrix/catkin_ws/devel/lib/libfiducials_base.so /opt/ros/hydro/lib/libtf.so /opt/ros/hydro/lib/libtf2_ros.so /opt/ros/hydro/lib/libactionlib.so /opt/ros/hydro/lib/libtf2.so /opt/ros/hydro/lib/libmessage_filters.so /opt/ros/hydro/lib/libroscpp.so -lboost_signals-mt -lboost_filesystem-mt /opt/ros/hydro/lib/libxmlrpcpp.so /opt/ros/hydro/lib/libopencv_calib3d.so /opt/ros/hydro/lib/libopencv_contrib.so /opt/ros/hydro/lib/libopencv_core.so /opt/ros/hydro/lib/libopencv_features2d.so /opt/ros/hydro/lib/libopencv_flann.so /opt/ros/hydro/lib/libopencv_gpu.so /opt/ros/hydro/lib/libopencv_highgui.so /opt/ros/hydro/lib/libopencv_imgproc.so /opt/ros/hydro/lib/libopencv_legacy.so /opt/ros/hydro/lib/libopencv_ml.so /opt/ros/hydro/lib/libopencv_nonfree.so /opt/ros/hydro/lib/libopencv_objdetect.so /opt/ros/hydro/lib/libopencv_photo.so /opt/ros/hydro/lib/libopencv_stitching.so /opt/ros/hydro/lib/libopencv_superres.so /opt/ros/hydro/lib/libopencv_video.so /opt/ros/hydro/lib/libopencv_videostab.so /opt/ros/hydro/lib/librosconsole.so /opt/ros/hydro/lib/librosconsole_log4cxx.so /opt/ros/hydro/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex-mt /opt/ros/hydro/lib/libroscpp_serialization.so /opt/ros/hydro/lib/librostime.so -lboost_date_time-mt -lboost_system-mt -lboost_thread-mt -lpthread /opt/ros/hydro/lib/libcpp_common.so /opt/ros/hydro/lib/libconsole_bridge.so -Wl,-rpath,/home/hendrix/catkin_ws/devel/lib:/opt/ros/hydro/lib: 
CMakeFiles/fiducials_localization.dir/src/fiducials_localization.cpp.o: In function `tf2::gmTransformToKDL(geometry_msgs::TransformStamped_<std::allocator<void> > const&)':
fiducials_localization.cpp:(.text+0x3ad): undefined reference to `KDL::Rotation::Quaternion(double, double, double, double)'
CMakeFiles/fiducials_localization.dir/src/fiducials_localization.cpp.o: In function `void tf2::doTransform<geometry_msgs::PoseStamped_<std::allocator<void> > >(geometry_msgs::PoseStamped_<std::allocator<void> > const&, geometry_msgs::PoseStamped_<std::allocator<void> >&, geometry_msgs::TransformStamped_<std::allocator<void> > const&)':
fiducials_localization.cpp:(.text+0x950): undefined reference to `KDL::Rotation::Quaternion(double, double, double, double)'
fiducials_localization.cpp:(.text+0xa74): undefined reference to `KDL::Rotation::GetQuaternion(double&, double&, double&, double&) const'
CMakeFiles/fiducials_localization.dir/src/fiducials_localization.cpp.o: In function `KDL::operator*(KDL::Frame const&, KDL::Frame const&)':
fiducials_localization.cpp:(.text._ZN3KDLmlERKNS_5FrameES2_[_ZN3KDLmlERKNS_5FrameES2_]+0x86): undefined reference to `KDL::operator*(KDL::Rotation const&, KDL::Rotation const&)'
collect2: error: ld returned 1 exit status
make[2]: *** [/home/hendrix/catkin_ws/devel/lib/fiducials_ros/fiducials_localization] Error 1
make[2]: Leaving directory `/home/hendrix/catkin_ws/build'
make[1]: *** [fiducials_ros/CMakeFiles/fiducials_localization.dir/all] Error 2
make[1]: Leaving directory `/home/hendrix/catkin_ws/build'
make: *** [all] Error 2
Invoking "make" failed

Successive sended transforms from a node not being received by tf2 python

Hello,

we are porting the camera_pose_calibration (https://github.com/ros-perception/camera_pose) package to indigo and have found a bug in the tf2 python implementation. If two or more transforms are sended from the same node using successive calls to:

tf2_ros.TransformBroadcaster().sendTransform()

Even with different TransformBroadcaster objects, only the last one is received by the tf core. If a delay is introduced by successive calls, all the transformations are received.

This bug does not happen in the tf2 cpp implementation neither in tf. I attach a minimal example covering the error, and the output shown by view_frames:

#!/usr/bin/python
import tf2_ros
import rospy
from geometry_msgs.msg import TransformStamped


def send(pub, child_frame_id):
    transform = TransformStamped()
    transform.header.frame_id = 'world'
    transform.child_frame_id = child_frame_id
    transform.transform.translation.x = 0
    transform.transform.translation.y = 1
    transform.transform.translation.z = 2
    transform.transform.rotation.x = 0
    transform.transform.rotation.y = 0 
    transform.transform.rotation.z = 0 
    transform.transform.rotation.w = 1 
    transform.header.stamp = rospy.Time.now()
    pub.sendTransform(transform)


if __name__ == '__main__':
    rospy.init_node('tf2_timing_test')
    r = rospy.Rate(5)
    pub1 = tf2_ros.TransformBroadcaster()
    pub2 = tf2_ros.TransformBroadcaster()

    while not rospy.is_shutdown():
        send(pub1, 'frame1_from_pub1')
#        rospy.sleep(0.01);
        send(pub2, 'frame2_from_pub2')
#        rospy.sleep(0.01);
        send(pub1, 'frame3_from_pub1')
        r.sleep()

view frames should mention the node name of the listener it monitors

This is non-trivial in some cases. Setting to untargeted.

Originally: https://code.ros.org/trac/ros-pkg/ticket/2946

TransformListener: ability to subscribe to arbitrary TF topic

I found out that for some multirobot experiments it would be nice to have ability to construct TransformListener which will listen to arbitrary TF topic (let's say /robot1/tf instead of /tf). This change should be very simple - I would add another constructor with parameters for tf and tf_static and use provided topics in TransformListener::init(). If no one has objection, I can prepare pull request. Or is this completely silly idea? :) Originally, I wanted to derive custom class from TransformListener but the problem is that message_subscriber_tf_ and message_subscriber_tf_static_ are private so init method can't be overridden.

tf chaining broken on pr2

Originally filed by @tkruse at: https://code.ros.org/trac/wg-ros-pkg/ticket/5176

Hi, using latest apt-get electric on lucid on the PR2, I do:

$ roscore

Just to make sure no overlays are used

$ export ROS_PACKAGE_PATH=/opt/ros/electric/stacks
$ roslaunch /etc/ros/robot.launch
...
$ rosrun tf tf_monitor /base_link /r_elbow_flex_link
...
RESULTS: for /base_link to /r_elbow_flex_link
Chain is: /base_link -> /base_footprint -> /odom_combined -> /base_link -> /sensor_mount_link -> /head_plate_frame -> /head_tilt_link -> /head_pan_link -> /sensor_mount_link -> /high_def_frame -> /torso_lift_link -> /base_link -> /l_forearm_roll_link -> /l_elbow_flex_link -> /l_forearm_cam_frame -> /l_forearm_roll_link -> /l_gripper_palm_link -> /l_wrist_roll_link -> /l_gripper_palm_link -> /l_wrist_flex_link -> /l_gripper_palm_link -> /torso_lift_link -> /l_upper_arm_roll_link -> /l_shoulder_lift_link -> /laser_tilt_mount_link -> /torso_lift_link -> /double_stereo_link -> /narrow_stereo_link -> /narrow_stereo_gazebo_l_stereo_camera_frame -> /narrow_stereo_gazebo_l_stereo_camera_frame -> /narrow_stereo_gazebo_r_stereo_camera_frame -> /narrow_stereo_link -> /projector_wg6802418_frame -> /head_plate_frame -> /r_forearm_roll_link -> /r_elbow_flex_link -> /r_forearm_cam_frame -> /r_forearm_roll_link -> /r_gripper_palm_link -> /r_wrist_roll_link -> /r_gripper_palm_link -> /r_wrist_flex_link -> /r_gripper_palm_link -> /torso_lift_link -> /r_upper_arm_roll_link -> /r_shoulder_lift_link -> /double_stereo_link -> /wide_stereo_link -> /wide_stereo_gazebo_l_stereo_camera_frame -> /wide_stereo_gazebo_l_stereo_camera_frame -> /wide_stereo_gazebo_r_stereo_camera_frame -> /wide_stereo_link -> NO_PARENT -> /bl_caster_rotation_link -> /base_link -> /bl_caster_rotation_link -> /br_caster_rotation_link -> /base_link -> /br_caster_rotation_link -> /fl_caster_rotation_link -> /base_link -> /fl_caster_rotation_link -> /fr_caster_rotation_link -> /base_link -> /fr_caster_rotation_link -> /torso_lift_link -> /l_upper_arm_link -> /l_gripper_r_finger_tip_link -> /l_gripper_r_finger_link -> /l_gripper_palm_link -> /l_gripper_l_finger_link -> /l_gripper_motor_slider_link -> /l_gripper_palm_link -> /l_gripper_palm_link -> /l_shoulder_pan_link -> /torso_lift_link -> /l_forearm_link -> /r_upper_arm_link -> /r_gripper_r_finger_tip_link -> /r_gripper_r_finger_link -> /r_gripper_palm_link -> /r_gripper_l_finger_link -> /r_gripper_motor_slider_link -> /r_gripper_palm_link -> /r_gripper_palm_link -> /r_shoulder_pan_link -> /torso_lift_link -> /r_forearm_link -> /base_link
...

I also tend to get other tf problems within my custom nodes, but I guess they relate to this original problem.

I don't even know where to start looking for the cause (tf, URDF, some standard node?).

Also not sure whether to report this to pr2 support instead. But the same happens for e.g.:

$ roslaunch pr2_gazebo pr2_empty_world.launch
$ rosrun tf tf_monitor /torso_lift_link /r_gripper_palm_link

Is lookupTwist deprecated?

In tf2 BufferCore the lookupTwist functions are commented out. Are they deprecated and / or is there some alternative?

tf2_tools view_frames.py doesn't listen to /tf_static

To duplicate issue:

rosrun tf2_ros static_transform_publisher 1 2 3 0 0 0 1 parent child

rosrun tf2_tools view_frames.py

It doesn't show the parent and child in this case, just shows no frames received.

create action based transform publisher server

Originally: https://code.ros.org/trac/ros-pkg/ticket/4179

During the poolshark project, I wrote a node that is an action server, takes as a goal a PoseStamped and a frame id, and then periodically publishes the resulting transform to /tf. It's a simple piece of code that might be more generally useful. I'm not sure where it belongs, as it depends on both tf and actionlib.

The code is: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/billiards/billiards_executive/nodes/posestamped_to_tf

The action spec is: https://code.ros.org/svn/wg-ros-pkg/trunk/stacks/billiards/billiards_msgs/action/PoseStampedToTF.action

Also it allows the publishing to be recinded relatively easily.

I like this as a potential way to deal with static transforms. It is a little heavy on the dependency list.

restore tests deleted in 04cf29d1b58c660fdc999ab83563a5d4b76ab331

They should not have been deleted

add getYaw helper function

It saves several lines of code which could easily be done wrong.

https://github.com/ros-perception/slam_gmapping/pull/31/files#diff-ee79bfdef4f6452c01f74517ee41c050R371

undefined reference to `tf2::BufferCore::_allFramesAsDot() const'

after #74 I get this error
/opt/ros/indigo/lib/libtf.so: undefined reference to `tf2::BufferCore::_allFramesAsDot() const'

to reproduce: create a workspace with the ros/velodyne repo and the ros/geometry_experimental repo, both of them checked out to the latest version.

I bisected on ros/geometry_experimental and I confirmed #74 is the culprit

port backwards time loop deadlock to tf2

From: ros/geometry#31

tf2 missing include_directories for console_bridge

tf2 calls find_package on console_bridge, but does not call include_directories on console_bridge_INCLUDE_DIRS:

https://github.com/ros/geometry_experimental/blob/hydro-devel/tf2/CMakeLists.txt#L4

This causes a build failure in a truly isolated catkin workspace build.

create a tool to project camera coordinates

This would be a nice tool to have built on top of tf could project camera pixels with depth into 3d points in space easily. Originally: https://code.ros.org/trac/ros-pkg/ticket/1656

We now have lots of cameras in the system, and need to project in and out of pixel coordinates. Each imager/camera can be thought of as being in its own frame, which is non-orthogonal and in pixel coordinates. Points from the world can be transformed into this frame using the projection matrix (currently in CamInfo?.msg) with a perspective transform.

Ideally going from 3D points to camera coordinates should be as simple as a TF call. This means that the user will have to publish a new type of message on a topic like "tf_cam_message" or similar:

Header header
string parent_id
PerspectiveTransform transform
For the left-stereo, this would probably be populated as follows:

msg.header.frame_id="stereo_left_rect_frame"
msg.parent_id="stereo_optical_link"
There are definitely some more details that need to be hammered out about disparity, and going up-and-down through the transforms, but this is definitely a start

tf2 tutorials outdated

This relates to pretty much every C++ tutorial linked here:
http://www.ros.org/wiki/tf2/Tutorials
did not check the python tutorials.

Mostly the packages / include subfolder names are wrong, dating back to 2010 before tf2 was cleaned up i guess. E.g.:

http://www.ros.org/wiki/tf2/Tutorials/Migration/TransformListener

Depend on tf_ros package. 
#include "tf2_cpp/transform_listener.h"
tf2::Buffer buffer;
tf2::TransformListener tfl(buffer);

should read:
depend on tf2_ros... not tf_ros
include subfolder should be tf2_ros, not tf2_cpp
Missing include of tf2_ros/buffer.h

tf2_ros: Python documentation missing

It looks like there was an attempt to move the Python documentation from the tf2 package to tf2_ros, but that activity was not completed.

The fix is probably not difficult. I notice that index.rst is missing and also the rosdoc element in package.xml:

  <export>
    <rosdoc config="rosdoc.yaml" />
  </export>

The Sphinx conf.py looks wrong, too. If I can find some time, I'll submit a pull request.

Replace deprecated boost signals with boost signals2

This is to match ros/ros_comm#267 and ros/geometry#23

use new rospy publisher syntax to avoid hanging

Take advantage of: ros/ros_comm#308

double free or corruption (fasttop)

I migrated some of my code to tf2 and it crashes from time to time. I am running the latest version from source. Here is the backtrace:

#0  0x00007ffff4852cc9 in __GI_raise (sig=sig@entry=6)
    at ../nptl/sysdeps/unix/sysv/linux/raise.c:56
#1  0x00007ffff48560d8 in __GI_abort () at abort.c:89
#2  0x00007ffff488f394 in __libc_message (do_abort=do_abort@entry=1, 
    fmt=fmt@entry=0x7ffff499db28 "*** Error in `%s': %s: 0x%s ***\n")
    at ../sysdeps/posix/libc_fatal.c:175
#3  0x00007ffff489b66e in malloc_printerr (ptr=<optimized out>, 
    str=0x7ffff499dcf0 "double free or corruption (fasttop)", action=1)
    at malloc.c:4996
#4  _int_free (av=<optimized out>, p=<optimized out>, have_lock=0)
    at malloc.c:3840
#5  0x00007ffff6ed0146 in __gnu_cxx::new_allocator<std::pair<ros::Time, unsigned int> >::deallocate(std::pair<ros::Time, unsigned int>*, unsigned long) ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2.so
#6  0x00007ffff6eccd40 in std::_Vector_base<std::pair<ros::Time, unsigned int>, std::allocator<std::pair<ros::Time, unsigned int> > >::_M_deallocate(std::pair<ros::Time, unsigned int>*, unsigned long) ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2.so
#7  0x00007ffff6ecaa72 in std::vector<std::pair<ros::Time, unsigned int>, std::allocator<std::pair<ros::Time, unsigned int> > >::_M_insert_aux(__gnu_cxx::__normal_iterator<std::pair<ros::Time, unsigned int>*, std::vector<std::pair<ros::Time, unsigned int>, std::allocator<std::pair<ros::Time, unsigned int> > > >, std::pair<ros::Time, unsigned int> const&) ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2.so
#8  0x00007ffff6ec7d12 in std::vector<std::pair<ros::Time, unsigned int>, std::allocator<std::pair<ros::Time, unsigned int> > >::push_back(std::pair<ros::Time, unsigned int> const&) ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2.so
#9  0x00007ffff6ebd21b in tf2::BufferCore::getLatestCommonTime(unsigned int, unsigned int, ros::Time&, std::string*) const ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2.so
#10 0x00007ffff6ebf046 in tf2::BufferCore::testTransformableRequests() ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2.so
#11 0x00007ffff6ebb907 in tf2::BufferCore::setTransform(geometry_msgs::TransformStamped_<std::allocator<void> > const&, std::string const&, bool) ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2.so
#12 0x00007ffff7201251 in tf2_ros::TransformListener::subscription_callback_impl(ros::MessageEvent<tf2_msgs::TFMessage_<std::allocator<void> > const> const&, bool) () from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#13 0x00007ffff7201042 in tf2_ros::TransformListener::subscription_callback(ros::MessageEvent<tf2_msgs::TFMessage_<std::allocator<void> > const> const&) ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#14 0x00007ffff72098ae in boost::_mfi::mf1<void, tf2_ros::TransformListener, ros::MessageEvent<tf2_msgs::TFMessage_<std::allocator<void> > const> const&>::operator()(tf2_ros::TransformListener*, ros::MessageEvent<tf2_msgs::TFMessage_<std::allocator<void> > const> const&) const ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#15 0x00007ffff7208d65 in void boost::_bi::list2<boost::_bi::value<tf2_ros::TransformListener*>, boost::arg<1> >::operator()<boost::_mfi::mf1<void, tf2_ros::TransformListener, ros::MessageEvent<tf2_msgs::TFMessage_<std::allocator<void> > const> const&>, boost::_bi::list1<boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const> const&> >(boost::_bi::type<void>, boost::_mfi::mf1<void, tf2_ros::TransformListener, ros::MessageEvent<tf2_msgs::TFMessage_<std::allocator<void> > const> const&>&, boost::_bi::list1<boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const> const&>&, int) ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#16 0x00007ffff7208214 in void boost::_bi::bind_t<void, boost::_mfi::mf1<void, tf2_ros::TransformListener, ros::MessageEvent<tf2_msgs::TFMessage_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<tf2_ros::TransformListener*>, boost::arg<1> > >::operator()<boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const> >(boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const> const&) ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#17 0x00007ffff7207595 in boost::detail::function::void_function_obj_invoker1<boost::_bi::bind_t<void, boost::_mfi::mf1<void, tf2_ros::TransformListener, ros::MessageEvent<tf2_msgs::TFMessage_<std::allocator<void> > const> const&>, boost::_bi::list2<boost::_bi::value<tf2_ros::TransformListener*>, boost::arg<1> > >, void, boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const> const&>::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const> const&) ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#18 0x00007ffff7209afd in boost::function1<void, boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const> const&>::operator()(boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const> const&) const ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#19 0x00007ffff7208f48 in boost::detail::function::void_function_obj_invoker1<boost::function<void (boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const> const&)>, void, boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const> >::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const>) ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#20 0x00007ffff720b95a in boost::function1<void, boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const> >::operator()(boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const>) const ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#21 0x00007ffff720b156 in ros::SubscriptionCallbackHelperT<boost::shared_ptr<tf2_msgs::TFMessage_<std::allocator<void> > const> const&, void>::call(ros::SubscriptionCallbackHelperCallParams&) ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#22 0x00007ffff6194695 in ros::SubscriptionQueue::call() ()
   from /opt/ros/indigo/lib/libroscpp.so
#23 0x00007ffff61524f7 in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) () from /opt/ros/indigo/lib/libroscpp.so
#24 0x00007ffff6153303 in ros::CallbackQueue::callAvailable(ros::WallDuration)
    () from /opt/ros/indigo/lib/libroscpp.so
#25 0x00007ffff720289b in tf2_ros::TransformListener::dedicatedListenerThread()
    () from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#26 0x00007ffff720bec9 in boost::_mfi::mf0<void, tf2_ros::TransformListener>::operator()(tf2_ros::TransformListener*) const ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#27 0x00007ffff720bc46 in void boost::_bi::list1<boost::_bi::value<tf2_ros::TransformListener*> >::operator()<boost::_mfi::mf0<void, tf2_ros::TransformListener>, boost::_bi::list0>(boost::_bi::type<void>, boost::_mfi::mf0<void, tf2_ros::TransformListener>&, boost::_bi::list0&, int) ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#28 0x00007ffff720b4c7 in boost::_bi::bind_t<void, boost::_mfi::mf0<void, tf2_ros::TransformListener>, boost::_bi::list1<boost::_bi::value<tf2_ros::TransformListener*> > >::operator()() ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#29 0x00007ffff720adcc in boost::detail::thread_data<boost::_bi::bind_t<void, boost::_mfi::mf0<void, tf2_ros::TransformListener>, boost::_bi::list1<boost::_bi::value<tf2_ros::TransformListener*> > > >::run() ()
   from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#30 0x00007ffff562aa4a in ?? ()
   from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.54.0
#31 0x00007ffff5409182 in start_thread (arg=0x7fffd9c0a700)
    at pthread_create.c:312
#32 0x00007ffff491647d in clone ()
    at ../sysdeps/unix/sysv/linux/x86_64/clone.S:111

intermittent test failure of time_reset_test

This test is failing intermittently during my development.

[tf2_ros.rosunit-transform_listener_time_reset_test/time_backwards][FAILURE]----
/home/tfoote/work/indigo_geometry/src/geometry_experimental/tf2_ros/test/time_reset_test.cpp:79
Value of: buffer.canTransform("foo", "bar", ros::Time(101, 0))
  Actual: true
Expected: false
--------------------------------------------------------------------------------


SUMMARY
 * RESULT: FAIL
 * TESTS: 1
 * ERRORS: 0
 * FAILURES: 1

Static Transform Broadcaster keeps incomplete list of transforms

https://github.com/ros/geometry_experimental/blob/hydro-devel/tf2_ros/src/static_transform_broadcaster.cpp#L60

Shouldn't this compare both header.frame_id AND child_frame_id? As I see it now, it would be possible to have two static transforms, one from parent -> child and one from parent -> step_child. However, only one of these would make it through this check and be published.

Any inner implementation details I'm missing? @tfoote

rosdistro update to 0.4.11 for hydro

The 0.4.11 release in the hydro-devel branch didn't make it to the official package repo.
As this release fixes a bug I fixed in march, it would be great to see this in rosdistro and the binary packages.

tf2 in groovy defines unnecessary CATKIN_DEPENDS (e.g. on rospy)

I checked the .pc files generated for a fresh groovy source install.

I get:

$ find devel_isolated/ -name *.pc | xargs grep Req | grep rospy
devel_isolated/diagnostic_aggregator/lib/pkgconfig/diagnostic_aggregator.pc:Requires: diagnostic_msgs pluginlib roscpp rospy xmlrpcpp
devel_isolated/tf2_ros/lib/pkgconfig/tf2_ros.pc:Requires: actionlib actionlib_msgs geometry_msgs message_filters roscpp rosgraph rospy tf2 tf2_msgs
devel_isolated/tf2/lib/pkgconfig/tf2.pc:Requires: geometry_msgs tf2_msgs rospy roscpp
devel_isolated/interactive_markers/lib/pkgconfig/interactive_markers.pc:Requires: roscpp rosconsole rospy tf visualization_msgs

I am not 110% sure, but I believe there is no need ever for a library to know of rospy to build against tf2, diagnostic_aggregator or interactive_markers. So it seems to me those have flawed CMakeLists.txt

Possibly more exported dependencies than rospy are superfluous there (e.g. actionlib stuff).

Also see:
http://answers.ros.org/question/58498/what-is-the-purpose-of-catkin_depends/

I am too lazy to create individual tickets for each of the packages above, especially since I might be wrong.

tf2 causes SIGABRT errors in rivz if the tf buffer is cleared

Whenever the tf cache is cleared in rviz (e.g. by clicking the Reset button in the bottom-left corner or if a negative time step is detected using an external clock source), rviz aborts with the following error:

rviz: /usr/include/boost/smart_ptr/shared_ptr.hpp:418: T* boost::shared_ptr<T>::operator->() const [with T = tf2::TimeCacheInterface]: Assertion `px != 0' failed.

Program received signal SIGABRT, Aborted.

Here is a stack trace (no debug build):

#0  0x00007ffff6475425 in __GI_raise (sig=<optimized out>)
    at ../nptl/sysdeps/unix/sysv/linux/raise.c:64
#1  0x00007ffff6478b8b in __GI_abort () at abort.c:91
#2  0x00007ffff646e0ee in __assert_fail_base (fmt=<optimized out>, 
    assertion=0x7fffeefd0e1a "px != 0", 
    file=0x7fffeefd10a8 "/usr/include/boost/smart_ptr/shared_ptr.hpp", 
    line=<optimized out>, function=<optimized out>) at assert.c:94
#3  0x00007ffff646e192 in __GI___assert_fail (
    assertion=0x7fffeefd0e1a "px != 0", 
    file=0x7fffeefd10a8 "/usr/include/boost/smart_ptr/shared_ptr.hpp", 
    line=418, 
    function=0x7fffeefd20e0 "T* boost::shared_ptr<T>::operator->() const [with T = tf2::TimeCacheInterface]") at assert.c:103
#4  0x00007fffeefbfd80 in ?? () from /opt/ros/hydro/lib/libtf2.so
#5  0x00007fffeefc4edd in tf2::BufferCore::clear() ()
   from /opt/ros/hydro/lib/libtf2.so
#6  0x00007ffff7b58071 in rviz::VisualizationManager::resetTime() ()
   from /opt/ros/hydro/lib/librviz.so
#7  0x00007ffff2b71281 in QMetaObject::activate(QObject*, QMetaObject const*, int, void**) () from /usr/lib/x86_64-linux-gnu/libQtCore.so.4
#8  0x00007ffff756fc72 in QAbstractButton::clicked(bool) ()
   from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#9  0x00007ffff72ada4e in ?? () from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#10 0x00007ffff72aed8b in ?? () from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#11 0x00007ffff72aeffc in QAbstractButton::mouseReleaseEvent(QMouseEvent*) ()
   from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#12 0x00007ffff736c5da in QToolButton::mouseReleaseEvent(QMouseEvent*) ()
   from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#13 0x00007ffff6f30144 in QWidget::event(QEvent*) ()
   from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#14 0x00007ffff6edf894 in QApplicationPrivate::notify_helper(QObject*, QEvent*)
    () from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#15 0x00007ffff6ee50bf in QApplication::notify(QObject*, QEvent*) ()
   from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#16 0x00007ffff2b5ce9c in QCoreApplication::notifyInternal(QObject*, QEvent*)
    () from /usr/lib/x86_64-linux-gnu/libQtCore.so.4
#17 0x00007ffff6ee0862 in QApplicationPrivate::sendMouseEvent(QWidget*, QMouseEvent*, QWidget*, QWidget*, QWidget**, QPointer<QWidget>&, bool) ()
   from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#18 0x00007ffff6f5fbf5 in ?? () from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#19 0x00007ffff6f5ebae in QApplication::x11ProcessEvent(_XEvent*) ()
   from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#20 0x00007ffff6f880d2 in ?? () from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#21 0x00007ffff18e1d53 in g_main_context_dispatch ()
   from /lib/x86_64-linux-gnu/libglib-2.0.so.0
#22 0x00007ffff18e20a0 in ?? () from /lib/x86_64-linux-gnu/libglib-2.0.so.0
#23 0x00007ffff18e2164 in g_main_context_iteration ()
   from /lib/x86_64-linux-gnu/libglib-2.0.so.0
#24 0x00007ffff2b8c3bf in QEventDispatcherGlib::processEvents(QFlags<QEventLoop::ProcessEventsFlag>) () from /usr/lib/x86_64-linux-gnu/libQtCore.so.4
#25 0x00007ffff6f87d5e in ?? () from /usr/lib/x86_64-linux-gnu/libQtGui.so.4
#26 0x00007ffff2b5bc82 in QEventLoop::processEvents(QFlags<QEventLoop::ProcessEventsFlag>) () from /usr/lib/x86_64-linux-gnu/libQtCore.so.4
#27 0x00007ffff2b5bed7 in QEventLoop::exec(QFlags<QEventLoop::ProcessEventsFlag>) () from /usr/lib/x86_64-linux-gnu/libQtCore.so.4
#28 0x00007ffff2b60f67 in QCoreApplication::exec() ()
   from /usr/lib/x86_64-linux-gnu/libQtCore.so.4
#29 0x0000000000400b0c in main ()

I am using the following binary packages from the official ROS hydro package repository:

Desired=Unknown/Install/Remove/Purge/Hold
| Status=Not/Inst/Conf-files/Unpacked/halF-conf/Half-inst/trig-aWait/Trig-pend
|/ Err?=(none)/Reinst-required (Status,Err: uppercase=bad)
||/ Name                                         Version                                      Description
+++-============================================-============================================-========================================================================================================
ii  ros-hydro-geometry                           1.10.5-0precise-20130721-0540-+0000          Geometry Library
ii  ros-hydro-rviz                               1.10.1-0precise-20130722-0059-+0000          3D visualization tool for ROS.
ii  ros-hydro-tf2                                0.4.5-0precise-20130721-0218-+0000           tf2 is the second generation of the transform libraray, which lets the user keep track of multiple coord

Potential feature suggestion for tf extrapolation

Hi admins. I was wondering if you are interested in adding a feature of transform extrapolation to lookupTransform. This can be naively implemented by allowing TimeCache::findClosest to return the last two transforms if extrapolation needed. Then the targeted transform can be obtained using the returned last two transforms. I can do a pr on this if this is useful. Thanks.

canTransform does not reset error string when it just times out

I am using this little function to wait for a tf to become available:

/**
 * Blocks for the tf from->to to become available.
 */
inline bool waitForTf(const tf2_ros::Buffer &tf2_buffer, const std::string &from, const std::string& to)
{
  std::string err;
  while (ros::ok() && !tf2_buffer.canTransform(from, to, ros::Time(0), ros::Duration(1.0), &err)) {
    ROS_INFO_STREAM("Waiting for transform " << from << "->" << to << " to be available. (tf says: " << err << ")");
  }
}

When the required transform does not become available, after some time I get errors like this:

[ INFO] [1438933887.448176468]: Waiting for transform map->base_link to be available. (tf says: . canTransform returned after 1.00534 timeout was 1.. canTransform returned after 1.0094 timeout was 1.. canTransform returned after 1.00617 timeout was 1.. canTransform returned after 1.00913 timeout was 1.. canTransform returned after 1.00304 timeout was 1.. canTransform returned after 1.00011 timeout was 1.. canTransform returned after 1.00064 timeout was 1.. canTransform returned after 1.00281 timeout was 1.. canTransform returned after 1.00711 timeout was 1.. canTransform returned after 1.00703 timeout was 1.. canTransform returned after 1.00794 timeout was 1.. canTransform returned after 1.00724 timeout was 1.. canTransform returned after 1.00759 timeout was 1.. canTransform returned after 1.00654 timeout was 1..)

That is caused by err not being reset to an empty string in canTransform. The message is only appended. If an actual error is detected in canTransform, the error message is assigned to err_str (e.g. here).
I would propose to make this consistent by setting err_str to an empty string in the beginning of Buffer::canTransform.

create a tool to provide callback on events

Originally here: https://code.ros.org/trac/ros-pkg/ticket/4422

A way to generate events when relative/absolute constraints between frames are violated/reached/broken/....?

This would be the next generation of MessageFilter?

Likely register a callback on a precondition.

We need to be careful of the threading model and not cross threads.

Other suggestions from Herman on the email thread,

Suggestion: make this "level-triggered events" and not "edge-triggered"

< http://en.wikipedia.org/wiki/Edge_triggered_interrupt#Types_of_Interrupts>

because (i) "a specific time" will never be reached exactly so the value should be given with a certain "tolerance", and (ii) there should be some "memory" about how long the event of the transform becoming available will remain "visible", since not all users of the event will have been listening to the event at the time of sending it. (E.g., listeners could be created as a reaction to such events.)

And (ii) events when certain transforms have changed.

Same remarks as above: the "change" has to have a "tolerance" and a "memory".

Based on these two events in the core tf library, you could implement any type of constraint checking outside of the core library.

I think you miss one fundamental other "event": a constraint can have "softness", and subscribers to the event can be interested in (i) knowing the current value of how much the constraint is violated, and (ii) getting events (as above) when specified levels of violation have been reached. (The latter thing might result in the same solution as the "tolerance" suggestion in the previous paragraphs...)

Tf2 Tests Calls BufferCore API in Reverse

https://github.com/ros/geometry_experimental/blob/hydro-devel/test_tf2/test/buffer_core_test.cpp#L660

This calls the function mBC.lookupTransform(SOURCE, TARGET....);

Whereas the API is:
https://github.com/ros/geometry_experimental/blob/hydro-devel/tf2/src/buffer_core.cpp#L517

lookupTransform(TARGET, SOURCE,....)

This probbably isn't a big deal as long as the tests are consistent within themselves, but it is a little disorienting.

TF_OLD_DATA has partial error string and can be bad for performance

It tries to reference a blank page.

https://github.com/ros/geometry_experimental/blob/0838d70c5f89d622c542f64bc93432c53060ff05/tf2/src/buffer_core.cpp#L260

Originally: https://code.ros.org/trac/ros-pkg/ticket/4239

tf_frame ignores tf2 static latch transforms

tf_frame ignores tf2 static latch transforms. tf2 static latch transforms do not show in tf_frame graph.

int main(int argc, char **argv)
{
  ros::init(argc, argv, "robot_odometry");
  tf2_ros::StaticTransformBroadcaster static_broadcaster;

  geometry_msgs::TransformStamped msg2;

  msg2.header.stamp = ros::Time::now();
  msg2.transform.translation.z = 0.1;
  msg2.transform.rotation.w =  1;
  msg2.transform.translation.z = 0.1;
  msg2.transform.translation.y = 0.13; 
  msg2.transform.translation.x = 0.13; 
  msg2.header.frame_id = "base_link";   
  msg2.child_frame_id  = "right";;
  static_broadcaster.sendTransform(msg2);


  ros::spin();
  return 0;
}



<?xml version="1.0"?>
<robot name="rrbot">
   <link name='base_link'>
      <visual name='visual'>
        <geometry>
          <box  size=".4 .2 .1"/>
        </geometry>
      </visual>
  </link>


  <link name="right">
    <visual name="visual">
      <origin xyz="0.13 0.13 0.1" />
      <geometry>
        <cylinder  length="0.5" radius="0.5"/>
      </geometry>
    </visual>
  </link>

  <joint type="fixed" name="joint2">
    <origin xyz="0.13 0.13 0.1" />
    <parent link="base_link"/>
    <child link="right"/>
  </joint>

</robot>

Also to note : no matter how many transforms I send weather independently or via vector only the last transform is published. The model was updated to new links in this case.

  std::vector<geometry_msgs::TransformStamped> v1;
  v1.push_back(msg1);
  v1.push_back(msg2);
      .
      .
      .
  static_broadcaster.sendTransform(v1);

handful of TF tools

I've been maintaining a handful of TF tools over in https://github.com/jhu-lcsr/lcsr_tf_tools

Of these, two of them could be useful in general to the larger community:

interactive_transform_publisher Publishes a TF frame based in an interactive marker
twist_frame_integrator Publishes a TF frame based on the integration of a geometry_msgs/Twist message

Any interest in merging these two into geometry_experimental?

python3 compatability issue

From @focs at ros-gbp/geometry2-release#1

Hi,
I'm trying to compile ros on my gentoo system. I got stuck while trying to compile the tf2 package:

./src/catkin/bin/catkin_make_isolated --pkg tf2 --force-cmake

The error is:

[100%] Building CXX object CMakeFiles/tf2_py.dir/src/tf2_py.cpp.o
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp:62:1: error: invalid conversion from 'const char_' to 'Py_ssize_t {aka long int}' [-fpermissive]
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp: In function 'PyObject_ transform_converter(const TransformStamped_)':
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp:109:103: error: 'PyString_FromString' was not declared in this scope
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp: In function 'PyObject_ allFramesAsYAML(PyObject_, PyObject_)':
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp:186:59: error: 'PyString_FromString' was not declared in this scope
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp: In function 'PyObject* allFramesAsString(PyObject_, PyObject_)':
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp:192:61: error: 'PyString_FromString' was not declared in this scope
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp: In function 'PyObject* setTransform(PyObject_, PyObject_)':
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp:388:105: error: 'PyString_AsString' was not declared in this scope
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp: At global scope:
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp:457:1: warning: converting to non-pointer type 'int' from NULL [-Wconversion-null]
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp: In function 'void init_tf2()':
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp:479:67: error: 'PyString_FromString' was not declared in this scope
/home/fox/ros_catkin_ws/src/tf2/src/tf2_py.cpp:496:43: error: 'Py_InitModule' was not declared in this scope
make[2]: *** [CMakeFiles/tf2_py.dir/src/tf2_py.cpp.o] Error 1
make[1]: *** [CMakeFiles/tf2_py.dir/all] Error 2

I think it is partially related to the python2-3 problem. I tried to force python 2 by:

./src/catkin/bin/catkin_make_isolated --pkg tf2 --force-cmake --cmake-args "-DPYTHON_LIBRARIES=/usr/lib64/libpython2.7.so -DPYTHON_INCLUDE_PATH=/usr/include/python2.7/"

So it should take the python2 library and header but there is no change on the output. Maybe this is not the correct way to spesifie the library.

Let me know if I can provide more info.

Thanks

tf2: _tf2.so install rule is missing devel prefix

Build : http://jenkins.willowgarage.com:8080/job/ros-groovy-tf2_binarydeb_quantal_i386/1/

@jkammerl Note it's missing the devel/

Error:

  file INSTALL cannot find "/lib/python2.7/dist-packages/tf2/_tf2.so".

Built location

/usr/lib/ccache/c++  -fPIC    -shared -Wl,-soname,_tf2.so -o devel/lib/python2.7/dist-packages/tf2/_tf2.so CMakeFiles/tf2_py.dir/src/tf2_py.cpp.o -

tf.Exception thrown while using waitForTransform when a frame doesn't exist

As reported by other users as well in http://answers.ros.org/question/207039/tfexception-thrown-while-using-waitfortransform a bug seems to have appeared when using TF.

waitForTransform throws a tf.Exception if one of the frames doesn't exist yet (which is actually the point of waitForTransform).

It must be a bug in TF2. When I checked out geometry 1.11.4 the bug was still there, but when I also checked out geometry-experimental 0.5.7 the bug disappeared.

Any idea about the reason?

Handle re-setting of simulation-time more gracefully

Currently, buffer_core issues a warning when "old data" is published when a simulation clock source like gazebo gets re-set. In the context of simulation time, if the /clock topic also goes backwards in time, then this should be considered neither an error or a warning.

What would be the right way to make buffer_core re-set when tf_ros detects a simulation time reset?

tf2: Standardize a convention to replace the tf_prefix API

The migration guide leaves this as a "longer term solution" but I think there are a lot of people who use and rely on tf_prefix for multi-robot (even single-master) systems, like modular robots. I think having a way to scope a node, or a component, or a namespace to a specific TF namespace is too common a problem to leave to each individual application.

Deadlock in tf2's buffer_core

When using rviz in Hydro, I ran into a dead lock issue, while debugging I found the problem in this part of the backtrace:

...
0   __lll_lock_wait /lib/x86_64-linux-gnu/libpthread.so.0   0   0x7f98996fa89c  
1   _L_lock_858 /lib/x86_64-linux-gnu/libpthread.so.0   0   0x7f98996f6065  
2   pthread_mutex_lock  /lib/x86_64-linux-gnu/libpthread.so.0   0   0x7f98996f5eba  
3   boost::mutex::lock  mutex.hpp   52  0x7f989ca0e167  
4   boost::unique_lock<boost::mutex>::lock  locks.hpp   412 0x7f989ca0faac  
5   boost::unique_lock<boost::mutex>::unique_lock   locks.hpp   290 0x7f989ca0f9e5  
6   tf2::BufferCore::allFramesAsString  buffer_core.cpp 809 0x7f9893903ede  
7   tf2::BufferCore::walkToTopParent<tf2::CanTransformAccum>    buffer_core.cpp 355 0x7f989390dc3a  
8   tf2::BufferCore::canTransformNoLock buffer_core.cpp 694 0x7f9893903691  
9   tf2::BufferCore::canTransform   buffer_core.cpp 726 0x7f989390385b  
10  tf::Transformer::canTransform   tf.cpp  362 0x7f98989e6a7c  
...

tf2::BufferCore::allFramesAsString tries to lock the frame_mutex_ before calling tf2::BufferCore::allFramesAsStringNoLock, but it has been previously locked by tf2::BufferCore::canTransform before calling tf2::BufferCore::canTransformNoLock which in turn eventually calls tf2::BufferCore::allFramesAsString in some error handling here:

https://github.com/ros/geometry_experimental/blob/hydro-devel/tf2/src/buffer_core.cpp#L357

tf2 introduction

I looked over the tf2 documentation, in particular the migration guide: http://www.ros.org/wiki/tf2/Migration and there are several things that seem weird to me.

If tf was really depreated in favour of tf2, then the tf wiki page and tutorials should say so.

geometry_experimental is not a good name for the thing to be used (was not a future proof name).

Several cripts like tf_echo seem to still only exist in package tf. While I generally like that, maybe the migration guide could mention that this is where scripts shall live. And then this should also be the place for other scripts (I'd love to have all tf scripts in one package, so that tab completion can help me remember the name of a script).

There is still both tf/view_frames and tf2_tools/view_frames, seemingly doing the same thing, only the tf one seems to be installed, but hydro also install tf2_tools, consisting of nothing else then view_frames. In a groovy devel space, one can rosrun tfw/view_frames.py /though not in a groovy install).

The same goes for tf/static_transform_publisher and tf2_ros/static_transform_publisher, why have 2 of them?

[EDIT: Removed text, several static latching publishers are no problem, sorry]

The migration wiki pages speaks of an "Action Based Query", but does not point at a resource where this can be found. (Also I have no clue why anyone would want to send a tf query as an action rather than a service).

http://www.ros.org/wiki/tf2 mentions: "tf2_ros also provides rospy bindings for tf2" I assume this has to be changed for Hydro, saying thos bindings in hydro are in tf2_py?

deadlock

I migrated some of my code to tf2 and I'm getting some deadlocks. I am running the latest version from source, which included @vrabaud 's fix #79

Here is the backtrace

#0  __lll_lock_wait ()
    at ../nptl/sysdeps/unix/sysv/linux/x86_64/lowlevellock.S:135
#1  0x00007ffff540b657 in _L_lock_909 ()
   from /lib/x86_64-linux-gnu/libpthread.so.0
#2  0x00007ffff540b480 in __GI___pthread_mutex_lock (mutex=0x7ffffffefba8)
    at ../nptl/pthread_mutex_lock.c:79
#3  0x0000000000431368 in pthread_mutex_lock (m=0x7ffffffefba8)
    at /usr/include/boost/thread/pthread/mutex.hpp:61
#4  boost::mutex::lock (this=0x7ffffffefba8)
    at /usr/include/boost/thread/pthread/mutex.hpp:113
#5  0x00000000004313db in boost::unique_lock<boost::mutex>::lock (
    this=0x7ffffffeed90) at /usr/include/boost/thread/lock_types.hpp:346
#6  0x00007ffff7211e11 in boost::unique_lock<boost::mutex>::unique_lock(boost::mutex&) () from /home/brice/code/workspaces/merged/devel/lib/libtf2_ros.so
#7  0x00007ffff6ebea9f in tf2::BufferCore::cancelTransformableRequest(unsigned long) () from /home/brice/code/workspaces/merged/devel/lib/libtf2.so
#8  0x0000000000441e0c in tf2_ros::MessageFilter<pcl::PointCloud<stdr_velodyne::PointType> >::add (this=0x7fffffff00c0, evt=...)
    at /home/brice/code/workspaces/merged/src/driving/external/geometry_experimental/tf2_ros/include/tf2_ros/message_filter.h:381
#9  0x0000000000437eed in operator() (a0=..., this=0x6d2d28)
    at /usr/include/boost/function/function_template.hpp:767
#10 message_filters::CallbackHelper1T<ros::MessageEvent<pcl::PointCloud<stdr_velodyne::PointType> const> const&, pcl::PointCloud<stdr_velodyne::PointType> >::call (this=0x6d2d20, event=..., nonconst_force_copy=<optimized out>)
    at /opt/ros/indigo/include/message_filters/signal1.h:76
#11 0x0000000000437a1f in message_filters::Signal1<pcl::PointCloud<stdr_velodyne::PointType> >::call (this=<optimized out>, event=...)
    at /opt/ros/indigo/include/message_filters/signal1.h:119
#12 0x0000000000435e86 in operator() (a0=..., this=0x6d2b68)
    at /usr/include/boost/function/function_template.hpp:767
#13 ros::SubscriptionCallbackHelperT<ros::MessageEvent<pcl::PointCloud<stdr_velodyne::PointType> const> const&, void>::call (this=0x6d2b60, params=...)
    at /opt/ros/indigo/include/ros/subscription_callback_helper.h:144
#14 0x00007ffff6194695 in ros::SubscriptionQueue::call() ()
   from /opt/ros/indigo/lib/libroscpp.so
#15 0x00007ffff61524f7 in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) () from /opt/ros/indigo/lib/libroscpp.so
#16 0x00007ffff6153303 in ros::CallbackQueue::callAvailable(ros::WallDuration)
    () from /opt/ros/indigo/lib/libroscpp.so
#17 0x00007ffff61971b5 in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*)
    () from /opt/ros/indigo/lib/libroscpp.so
#18 0x00007ffff617f7bb in ros::spin() () from /opt/ros/indigo/lib/libroscpp.so
#19 0x000000000042394c in main (argc=1, argv=0x7fffffffd818)
    at /home/brice/code/workspaces/merged/src/driving/packages/velodyne_localizer/src/velodyne_localizer.cpp:745

Missing convertions

Once understood the convert method is quite helpfull. But IMO there are some basic convertion methods missing.

For example:
tf2_geometry_msgs.h missing Vector3, Pose, Point (only stamped versions available)
tf2_eigen.h missing Quaterniond, Matrix3d

Furthermore the documentation on creating convertion methods is incomplete or hard to find.

tf2_buffer_server doesn't set up action server properly.

Whenever the tf2 buffer server starts, it prints the following warning:

[ WARN] [1373931844.009500433]: You've passed in true for auto_start for the C++ action server at [/tf2_buffer_server]. You should always pass in false to avoid race conditions.

Invalid argument "/any_frame" passed to canTransform argument source_frame in tf2 frame_ids cannot start with a '/'

The warning message on line 139 of src/buffer_core.cpp prints thousands of times per second in my program (rviz) in hydro because all the sensor data has frame_id values that start with "/", which is incorrect as of today.

I can remove it from things where the source of the frame_ids is rviz, like the gui and config files, but all the sensor data is processed by tf::MessageFilter templates. Do I really need to change all my sensor driver code to remove the / before this will quiet down?

tf2 provide way to preserve tf snapshot past end of buffer

Originally filed: https://code.ros.org/trac/ros-pkg/ticket/4483

This will allow things to keep data in memory and allow random access back in time noteably.

though this could also be done by using a server fallback

Provide a way to clear tf2 buffers when the clock is reset

When running a simulation or playing back a bag file with --clock, I often try to leave most of my nodes running, and only restart the thing that is publishing to /clock

Unfortunately, the tf2 internal buffer does not reset when /clock jumps backwards, and the tf2 subscribers don't have a manual mechanism to tell them that the clock has been reset, and they should clear their buffers.

See http://answers.ros.org/question/164026/how-can-i-reset-the-simulated-clock/ for more information.

rviz tf display tool does not handle static transforms correctly

Seems to me static transforms should always display in Rviz, but instead they fade out as if they were non-static. Resetting the tf display makes the transform show again until the "Frame Timeout" is reached. Thoughts?

message_filter behavior with "/" frames

Just wondering whether it's a design decision or a bug.

With tf2, your frames cannot start with "/". Good.

Now, I have an old ROS bag (from navigation) where frames do start with "/". There is a tf2_ros::message_filter on those but it assumes the frames do not start with "/". This results in miscellaneous errors.

What is the correct behavior ?

this bag is wrong and cannot work with tf2 so the current behavior is right
tf2_ros::message_filter should clean the frames before using tf2. I actually fixed it and that works but I am not sure you want to allow such a loose behavior.

Let me know if that is more appropriate and I'll make a PR

tf2_ros: transform in python doesn't work

I think the transform-Method of buffer_interface.py doesn't work or can someone show me how to use it?

I need something like transformPose from tf.listener to transform a PoseStamped but with tf2...

As a first solution i have written a helper-method that uses the transform from tfBuffer.lookup_transform to transform the PoseStamped but shouldn't there be a method in tf2_ros for that?

tf2_ros::Buffer::canTransform() does not abort on node shutdown

I just had a problem with tf in ROS hydro similar to the one described in #4882 on code.ros.org 2 years ago.

tf::waitForTransform() stays in an endless loop and nodes do not shutdown properly if simulated time has been paused. The function seems to use tf2_ros::Buffer::canTransform() (buffer.cpp:87ff) with a timeout internally without checking ros::ok(), therefore posting it here.

extend tf2_message_filter to avoid race conditions on callback

One possibility is to bundle the transform with the callback.

Another is to persist the transform. Possibly related #34

A third option is to transform the data for the user before the callback.

Related tickets: Threading issue in tf MessageFilter - No guarantee that transform is actually possible https://code.ros.org/trac/ros-pkg/ticket/5488

tf::MessageFilter? passes on a message when it can be transformed into a given list of tf frames, according to the message's header.

The problem is the following: the transform is not passed on together with the message, which means that there is no guarantee that it is actually available when the callback is executed. If the tf Transformer runs in it's own thread, it might happen that it discards the necessary information in between.

This is not a bug in the implementation, but a design flaw.

Ideas for possible solutions:

Changing the tf::MessageFilter? API to output a message plus the transform(s). This would make it impossible to connect it to other Message Filters, however.
Storing the transforms in the tf:MessageFilter itself, so it can be requested later. This is not the perfect solution API-wise. It also opens the question when tf::MessageFilter? should discard the kept transforms so they don't build up in memory. However, it does not break compatibility with the MessageFilter? pattern.

tf::message_filters are not intuitive to use https://code.ros.org/trac/ros-pkg/ticket/3007

I'm not sure what the solution is to this problem, but I'm putting it here for the record since it's occurred in multiple usability tests.

tf::message_filters are difficult to use because you're only guaranteed to have a transform at the beginning of the callback, not when you actually want to use the transform. I know that you can deal with this through judicious use of try-catch loops and setTolerance(), but that seems hacky. Multiple users have had trouble with this concept. Perhaps one partial solution would be to return the requested transform along with the message?

ros / geometry2 Goto Github PK

geometry2's People

Stargazers

Watchers

Forkers

geometry2's Issues

Recommend Projects

Recommend Topics

Recommend Org

Jobs