Skip to content

Numeric error produces unexpected behavior on RPY angles computation #796

@AlessioLovato

Description

@AlessioLovato

Generated by Generative AI

No response

Operating System:

Ubuntu 22.04.5 LTS

ROS version or commit hash:

humble

RMW implementation (if applicable):

rmw_fastrtps_cpp

RMW Configuration (if applicable):

No response

Client library (if applicable):

No response

'ros2 doctor --report' output

ros2 doctor --report
   PACKAGE VERSIONS
tf2_echo_bug                              : latest=, local=0.0.0
camera_test                               : latest=, local=0.0.0
mask_interfaces                           : latest=, local=0.0.0
ament_cmake_export_definitions            : latest=1.3.11, local=1.3.11
ament_cmake                               : latest=1.3.11, local=1.3.11
rcl_action                                : latest=5.3.9, local=5.3.9
ament_cmake_gmock                         : latest=1.3.11, local=1.3.11
rosidl_typesupport_introspection_cpp      : latest=3.1.6, local=3.1.6
std_srvs                                  : latest=4.8.0, local=4.8.0
fastrtps_cmake_module                     : latest=2.2.2, local=2.2.2
ros_workspace                             : latest=1.0.2, local=1.0.2
tracetools                                : latest=4.1.1, local=4.1.1
ros2node                                  : latest=0.18.12, local=0.18.12
ros2service                               : latest=0.18.12, local=0.18.12
rcutils                                   : latest=5.1.6, local=5.1.6
rclcpp_components                         : latest=16.0.12, local=16.0.12
rosidl_generator_cpp                      : latest=3.1.6, local=3.1.6
composition_interfaces                    : latest=1.2.1, local=1.2.1
ament_cmake_cppcheck                      : latest=0.12.12, local=0.12.12
lifecycle_msgs                            : latest=1.2.1, local=1.2.1
launch_testing                            : latest=1.0.9, local=1.0.8
ament_lint_cmake                          : latest=0.12.12, local=0.12.12
rosidl_typesupport_interface              : latest=3.1.6, local=3.1.6
ament_flake8                              : latest=0.12.12, local=0.12.12
ros2component                             : latest=0.18.12, local=0.18.12
ament_cmake_pytest                        : latest=1.3.11, local=1.3.11
ament_cmake_xmllint                       : latest=0.12.12, local=0.12.12
common_interfaces                         : latest=4.8.0, local=4.8.0
shape_msgs                                : latest=4.8.0, local=4.8.0
ament_cmake_export_include_directories    : latest=1.3.11, local=1.3.11
rcpputils                                 : latest=2.4.5, local=2.4.5
rosidl_typesupport_fastrtps_cpp           : latest=2.2.2, local=2.2.2
sros2                                     : latest=0.10.6, local=0.10.6
ament_cmake_include_directories           : latest=1.3.11, local=1.3.11
unique_identifier_msgs                    : latest=2.2.1, local=2.2.1
libstatistics_collector                   : latest=1.3.4, local=1.3.4
class_loader                              : latest=2.2.0, local=2.2.0
rcl_logging_spdlog                        : latest=2.3.1, local=2.3.1
libyaml_vendor                            : latest=1.2.2, local=1.2.2
ament_cmake_export_targets                : latest=1.3.11, local=1.3.11
tinyxml2_vendor                           : latest=0.7.6, local=0.7.6
ros2lifecycle                             : latest=0.18.12, local=0.18.12
sros2_cmake                               : latest=0.10.6, local=0.10.6
rpyutils                                  : latest=0.2.1, local=0.2.1
launch_ros                                : latest=0.19.9, local=0.19.9
ament_cmake_export_dependencies           : latest=1.3.11, local=1.3.11
trajectory_msgs                           : latest=4.8.0, local=4.8.0
ros_core                                  : latest=0.10.0, local=0.10.0
rmw_fastrtps_cpp                          : latest=6.2.7, local=6.2.7
ament_cmake_pep257                        : latest=0.12.12, local=0.12.12
rosidl_cmake                              : latest=3.1.6, local=3.1.6
rcl_interfaces                            : latest=1.2.1, local=1.2.1
launch_xml                                : latest=1.0.9, local=1.0.8
rosidl_typesupport_fastrtps_c             : latest=2.2.2, local=2.2.2
ament_cmake_export_libraries              : latest=1.3.11, local=1.3.11
rosidl_parser                             : latest=3.1.6, local=3.1.6
rosidl_adapter                            : latest=3.1.6, local=3.1.6
rcl_lifecycle                             : latest=5.3.9, local=5.3.9
rosidl_runtime_py                         : latest=0.9.3, local=0.9.3
uncrustify_vendor                         : latest=2.0.2, local=2.0.2
rosidl_generator_py                       : latest=0.14.4, local=0.14.4
rosidl_typesupport_c                      : latest=2.0.2, local=2.0.2
ament_cmake_export_interfaces             : latest=1.3.11, local=1.3.11
ros2cli                                   : latest=0.18.12, local=0.18.12
ament_cmake_lint_cmake                    : latest=0.12.12, local=0.12.12
python_cmake_module                       : latest=0.10.0, local=0.10.0
ament_lint_auto                           : latest=0.12.12, local=0.12.12
ros2param                                 : latest=0.18.12, local=0.18.12
ament_cmake_cpplint                       : latest=0.12.12, local=0.12.12
rcl_yaml_param_parser                     : latest=5.3.9, local=5.3.9
ros2topic                                 : latest=0.18.12, local=0.18.12
ament_cmake_copyright                     : latest=0.12.12, local=0.12.12
rosidl_generator_c                        : latest=3.1.6, local=3.1.6
ament_cmake_libraries                     : latest=1.3.11, local=1.3.11
action_msgs                               : latest=1.2.1, local=1.2.1
ament_pep257                              : latest=0.12.12, local=0.12.12
ros2doctor                                : latest=0.18.12, local=0.18.12
ament_cppcheck                            : latest=0.12.12, local=0.12.12
rclpy                                     : latest=3.3.16, local=3.3.16
actionlib_msgs                            : latest=4.8.0, local=4.8.0
launch                                    : latest=1.0.9, local=1.0.8
rmw_implementation_cmake                  : latest=6.1.2, local=6.1.2
rclcpp                                    : latest=16.0.12, local=16.0.12
rosidl_typesupport_cpp                    : latest=2.0.2, local=2.0.2
ament_index_cpp                           : latest=1.4.0, local=1.4.0
ros2pkg                                   : latest=0.18.12, local=0.18.12
ament_package                             : latest=0.14.1, local=0.14.0
diagnostic_msgs                           : latest=4.8.0, local=4.8.0
ament_cmake_flake8                        : latest=0.12.12, local=0.12.12
rosidl_runtime_c                          : latest=3.1.6, local=3.1.6
ament_cmake_auto                          : latest=1.3.11, local=1.3.11
ament_cmake_gtest                         : latest=1.3.11, local=1.3.11
sensor_msgs                               : latest=4.8.0, local=4.8.0
ros2interface                             : latest=0.18.12, local=0.18.12
ament_cmake_core                          : latest=1.3.11, local=1.3.11
ament_uncrustify                          : latest=0.12.12, local=0.12.12
ament_cmake_target_dependencies           : latest=1.3.11, local=1.3.11
rosidl_typesupport_introspection_c        : latest=3.1.6, local=3.1.6
domain_coordinator                        : latest=0.10.0, local=0.10.0
rosidl_default_runtime                    : latest=1.2.0, local=1.2.0
ros2action                                : latest=0.18.12, local=0.18.12
ament_cmake_uncrustify                    : latest=0.12.12, local=0.12.12
stereo_msgs                               : latest=4.8.0, local=4.8.0
ament_lint_common                         : latest=0.12.12, local=0.12.12
ament_xmllint                             : latest=0.12.12, local=0.12.12
ros2multicast                             : latest=0.18.12, local=0.18.12
launch_testing_ament_cmake                : latest=1.0.9, local=1.0.8
rosidl_default_generators                 : latest=1.2.0, local=1.2.0
ros2run                                   : latest=0.18.12, local=0.18.12
rmw_dds_common                            : latest=1.6.0, local=1.6.0
ros2cli_common_extensions                 : latest=0.1.1, local=0.1.1
ament_cmake_ros                           : latest=0.10.0, local=0.10.0
osrf_pycommon                             : latest=2.1.6, local=2.1.6
rmw_implementation                        : latest=2.8.4, local=2.8.4
geometry_msgs                             : latest=4.8.0, local=4.8.0
ament_cmake_python                        : latest=1.3.11, local=1.3.11
ament_copyright                           : latest=0.12.12, local=0.12.12
ros2launch                                : latest=0.19.9, local=0.19.9
ament_cpplint                             : latest=0.12.12, local=0.12.12
visualization_msgs                        : latest=4.8.0, local=4.8.0
ament_cmake_test                          : latest=1.3.11, local=1.3.11
rmw                                       : latest=6.1.2, local=6.1.2
rmw_fastrtps_shared_cpp                   : latest=6.2.7, local=6.2.7
rosidl_cli                                : latest=3.1.6, local=3.1.6
console_bridge_vendor                     : latest=1.4.1, local=1.4.1
rcl                                       : latest=5.3.9, local=5.3.9
statistics_msgs                           : latest=1.2.1, local=1.2.1
ament_cmake_export_link_flags             : latest=1.3.11, local=1.3.11
rosidl_runtime_cpp                        : latest=3.1.6, local=3.1.6
rclcpp_lifecycle                          : latest=16.0.12, local=16.0.12
nav_msgs                                  : latest=4.8.0, local=4.8.0
ros_environment                           : latest=3.2.2, local=3.2.2
launch_testing_ros                        : latest=0.19.9, local=0.19.9
ament_cmake_gen_version_h                 : latest=1.3.11, local=1.3.11
ament_cmake_version                       : latest=1.3.11, local=1.3.11
builtin_interfaces                        : latest=1.2.1, local=1.2.1
rosgraph_msgs                             : latest=1.2.1, local=1.2.1
pluginlib                                 : latest=5.1.0, local=5.1.0
ament_index_python                        : latest=1.4.0, local=1.4.0
ament_lint                                : latest=0.12.12, local=0.12.12
launch_yaml                               : latest=1.0.9, local=1.0.8
std_msgs                                  : latest=4.8.0, local=4.8.0
spdlog_vendor                             : latest=1.3.1, local=1.3.1
rcl_logging_interface                     : latest=2.3.1, local=2.3.1
rclcpp_action                             : latest=16.0.12, local=16.0.12
realsense2_description                    : latest=4.55.1, local=4.55.1
realsense2_camera_msgs                    : latest=4.55.1, local=4.55.1
realsense2_camera                         : latest=4.55.1, local=4.55.1
ffmpeg_encoder_decoder                    : latest=2.0.1, local=2.0.0
point_cloud_transport                     : latest=1.0.18, local=1.0.18
point_cloud_transport_plugins             : latest=1.0.11, local=1.0.11
ffmpeg_image_transport                    : latest=2.0.3, local=2.0.2
zstd_point_cloud_transport                : latest=1.0.11, local=1.0.11
robot_localization                        : latest=3.5.3, local=3.5.3
nmea_msgs                                 : latest=2.0.0, local=2.0.0
ffmpeg_image_transport_msgs               : latest=1.1.2, local=1.1.2
cob_srvs                                  : latest=2.7.10, local=2.7.10
point_cloud_interfaces                    : latest=1.0.11, local=1.0.11
diagnostic_updater                        : latest=4.0.6, local=4.0.3
geographic_msgs                           : latest=1.0.6, local=1.0.6
draco_point_cloud_transport               : latest=1.0.11, local=1.0.11
zlib_point_cloud_transport                : latest=1.0.11, local=1.0.11
image_transport_plugins                   : latest=2.5.3, local=2.5.3
pcl_ros                                   : latest=2.4.5, local=2.4.5
ros_ign_image                             : latest=0.244.19, local=0.244.16
stereo_image_proc                         : latest=3.0.8, local=3.0.7
compressed_depth_image_transport          : latest=2.5.3, local=2.5.3
ros_gz_interfaces                         : latest=0.244.19, local=0.244.16
image_pipeline                            : latest=3.0.8, local=3.0.7
ros_gz_bridge                             : latest=0.244.19, local=0.244.16
ros_gz_image                              : latest=0.244.19, local=0.244.16
camera_info_manager                       : latest=3.1.11, local=3.1.11
filters                                   : latest=2.2.1, local=2.1.0
image_rotate                              : latest=3.0.8, local=3.0.7
theora_image_transport                    : latest=2.5.3, local=2.5.3
perception_pcl                            : latest=2.4.5, local=2.4.5
image_view                                : latest=3.0.8, local=3.0.7
xacro                                     : latest=2.0.13, local=2.0.13
image_proc                                : latest=3.0.8, local=3.0.7
vision_msgs                               : latest=4.1.1, local=4.1.1
perception                                : latest=0.10.0, local=0.10.0
ros_ign_bridge                            : latest=0.244.19, local=0.244.16
sdformat_urdf                             : latest=1.0.1, local=1.0.1
camera_calibration_parsers                : latest=3.1.11, local=3.1.11
compressed_image_transport                : latest=2.5.3, local=2.5.3
image_common                              : latest=3.1.11, local=3.1.11
image_publisher                           : latest=3.0.8, local=3.0.7
laser_filters                             : latest=2.0.7, local=2.0.7
camera_calibration                        : latest=3.0.8, local=3.0.7
ros_ign_gazebo_demos                      : latest=0.244.19, local=0.244.16
actuator_msgs                             : latest=0.0.1, local=0.0.1
vision_opencv                             : latest=3.2.1, local=3.2.1
ros_gz_sim_demos                          : latest=0.244.19, local=0.244.16
tracetools_image_pipeline                 : latest=3.0.8, local=3.0.7
simulation                                : latest=0.10.0, local=0.10.0
gps_msgs                                  : latest=2.1.0, local=2.0.4
desktop_full                              : latest=0.10.0, local=0.10.0
ros_ign_interfaces                        : latest=0.244.19, local=0.244.16
ros_ign_gazebo                            : latest=0.244.19, local=0.244.16
ros_gz_sim                                : latest=0.244.19, local=0.244.16
depth_image_proc                          : latest=3.0.8, local=3.0.7
rqt_py_common                             : latest=1.1.7, local=1.1.7
interactive_markers                       : latest=2.3.2, local=2.3.2
intra_process_demo                        : latest=0.20.5, local=0.20.5
tlsf                                      : latest=0.7.0, local=0.7.0
image_geometry                            : latest=3.2.1, local=3.2.1
rqt_bag                                   : latest=1.1.5, local=1.1.5
dummy_map_server                          : latest=0.20.5, local=0.20.5
qt_gui                                    : latest=2.2.4, local=2.2.4
rqt_bag_plugins                           : latest=1.1.5, local=1.1.5
rviz_rendering                            : latest=11.2.17, local=11.2.16
example_interfaces                        : latest=0.9.3, local=0.9.3
rqt_plot                                  : latest=1.1.5, local=1.1.4
examples_rclcpp_minimal_timer             : latest=0.15.3, local=0.15.3
angles                                    : latest=1.15.0, local=1.15.0
demo_nodes_py                             : latest=0.20.5, local=0.20.5
examples_rclcpp_minimal_action_client     : latest=0.15.3, local=0.15.3
pcl_conversions                           : latest=2.4.5, local=2.4.5
composition                               : latest=0.20.5, local=0.20.5
laser_geometry                            : latest=2.4.0, local=2.4.0
examples_rclcpp_minimal_action_server     : latest=0.15.3, local=0.15.3
resource_retriever                        : latest=3.1.3, local=3.1.3
rviz_common                               : latest=11.2.17, local=11.2.16
qt_dotgraph                               : latest=2.2.4, local=2.2.4
rqt_console                               : latest=2.0.3, local=2.0.3
action_tutorials_interfaces               : latest=0.20.5, local=0.20.5
rviz_ogre_vendor                          : latest=11.2.17, local=11.2.16
rqt_image_view                            : latest=1.2.0, local=1.2.0
dummy_sensors                             : latest=0.20.5, local=0.20.5
qt_gui_py_common                          : latest=2.2.4, local=2.2.4
rviz_assimp_vendor                        : latest=11.2.17, local=11.2.16
rqt_msg                                   : latest=1.2.0, local=1.2.0
rqt_gui_cpp                               : latest=1.1.7, local=1.1.7
sensor_msgs_py                            : latest=4.8.0, local=4.8.0
rqt_srv                                   : latest=1.0.3, local=1.0.3
libcurl_vendor                            : latest=3.1.3, local=3.1.3
examples_rclpy_executors                  : latest=0.15.3, local=0.15.3
tlsf_cpp                                  : latest=0.13.0, local=0.13.0
examples_rclcpp_minimal_client            : latest=0.15.3, local=0.15.3
examples_rclcpp_minimal_composition       : latest=0.15.3, local=0.15.3
demo_nodes_cpp                            : latest=0.20.5, local=0.20.5
qt_gui_cpp                                : latest=2.2.4, local=2.2.4
pendulum_msgs                             : latest=0.20.5, local=0.20.5
quality_of_service_demo_py                : latest=0.20.5, local=0.20.5
rqt_service_caller                        : latest=1.0.5, local=1.0.5
examples_rclpy_minimal_client             : latest=0.15.3, local=0.15.3
rttest                                    : latest=0.13.0, local=0.13.0
pendulum_control                          : latest=0.20.5, local=0.20.5
tango_icons_vendor                        : latest=0.1.1, local=0.1.1
cv_bridge                                 : latest=3.2.1, local=3.2.1
examples_rclpy_minimal_service            : latest=0.15.3, local=0.15.3
action_tutorials_py                       : latest=0.20.5, local=0.20.5
examples_rclpy_minimal_action_server      : latest=0.15.3, local=0.15.3
teleop_twist_keyboard                     : latest=2.4.0, local=2.4.0
rqt_common_plugins                        : latest=1.2.0, local=1.2.0
examples_rclcpp_minimal_subscriber        : latest=0.15.3, local=0.15.3
dummy_robot_bringup                       : latest=0.20.5, local=0.20.5
rqt_reconfigure                           : latest=1.1.2, local=1.1.2
rqt_py_console                            : latest=1.0.2, local=1.0.2
joy                                       : latest=3.3.0, local=3.3.0
image_tools                               : latest=0.20.5, local=0.20.5
rqt_gui                                   : latest=1.1.7, local=1.1.7
turtlesim                                 : latest=1.4.2, local=1.4.2
rqt_gui_py                                : latest=1.1.7, local=1.1.7
rqt_topic                                 : latest=1.5.0, local=1.5.0
demo_nodes_cpp_native                     : latest=0.20.5, local=0.20.5
pcl_msgs                                  : latest=1.0.0, local=1.0.0
sdl2_vendor                               : latest=3.3.0, local=3.3.0
map_msgs                                  : latest=2.1.0, local=2.1.0
examples_rclpy_minimal_subscriber         : latest=0.15.3, local=0.15.3
examples_rclpy_minimal_action_client      : latest=0.15.3, local=0.15.3
python_qt_binding                         : latest=1.1.2, local=1.1.2
topic_monitor                             : latest=0.20.5, local=0.20.5
action_tutorials_cpp                      : latest=0.20.5, local=0.20.5
rqt_publisher                             : latest=1.5.0, local=1.5.0
lifecycle                                 : latest=0.20.5, local=0.20.5
teleop_twist_joy                          : latest=2.4.7, local=2.4.7
rviz2                                     : latest=11.2.17, local=11.2.16
examples_rclcpp_minimal_publisher         : latest=0.15.3, local=0.15.3
rqt_graph                                 : latest=1.3.1, local=1.3.1
rqt_shell                                 : latest=1.0.2, local=1.0.2
examples_rclcpp_minimal_service           : latest=0.15.3, local=0.15.3
quality_of_service_demo_cpp               : latest=0.20.5, local=0.20.5
examples_rclcpp_multithreaded_executor    : latest=0.15.3, local=0.15.3
examples_rclpy_minimal_publisher          : latest=0.15.3, local=0.15.3
rqt_action                                : latest=2.0.1, local=2.0.1
depthimage_to_laserscan                   : latest=2.5.1, local=2.5.1
rviz_default_plugins                      : latest=11.2.17, local=11.2.16
image_transport                           : latest=3.1.11, local=3.1.11
desktop                                   : latest=0.10.0, local=0.10.0
logging_demo                              : latest=0.20.5, local=0.20.5
message_filters                           : latest=4.3.7, local=4.3.6
zstd_vendor                               : latest=0.15.14, local=0.15.14
geometry2                                 : latest=0.25.13, local=0.25.12
rosbag2_storage                           : latest=0.15.14, local=0.15.14
rosbag2_interfaces                        : latest=0.15.14, local=0.15.14
python_orocos_kdl_vendor                  : latest=0.2.5, local=0.2.5
tf2_eigen                                 : latest=0.25.13, local=0.25.12
kdl_parser                                : latest=2.6.4, local=2.6.4
tf2_ros_py                                : latest=0.25.13, local=0.25.12
yaml_cpp_vendor                           : latest=8.0.2, local=8.0.2
keyboard_handler                          : latest=0.0.5, local=0.0.5
sqlite3_vendor                            : latest=0.15.14, local=0.15.14
tf2_sensor_msgs                           : latest=0.25.13, local=0.25.12
rosbag2_cpp                               : latest=0.15.14, local=0.15.14
tf2_eigen_kdl                             : latest=0.25.13, local=0.25.12
rosbag2                                   : latest=0.15.14, local=0.15.14
urdf                                      : latest=2.6.1, local=2.6.1
tf2_py                                    : latest=0.25.13, local=0.25.12
rosbag2_transport                         : latest=0.15.14, local=0.15.14
urdf_parser_plugin                        : latest=2.6.1, local=2.6.1
tf2_ros                                   : latest=0.25.13, local=0.25.12
tf2_geometry_msgs                         : latest=0.25.13, local=0.25.12
rosbag2_storage_default_plugins           : latest=0.15.14, local=0.15.14
tf2_bullet                                : latest=0.25.13, local=0.25.12
tf2                                       : latest=0.25.13, local=0.25.12
shared_queues_vendor                      : latest=0.15.14, local=0.15.14
robot_state_publisher                     : latest=3.0.3, local=3.0.3
eigen3_cmake_module                       : latest=0.1.1, local=0.1.1
tf2_msgs                                  : latest=0.25.13, local=0.25.12
rosbag2_compression_zstd                  : latest=0.15.14, local=0.15.14
tinyxml_vendor                            : latest=0.8.3, local=0.8.3
ros2bag                                   : latest=0.15.14, local=0.15.14
tf2_kdl                                   : latest=0.25.13, local=0.25.12
tf2_tools                                 : latest=0.25.13, local=0.25.12
rosbag2_py                                : latest=0.15.14, local=0.15.14
pybind11_vendor                           : latest=2.4.2, local=2.4.2
orocos_kdl_vendor                         : latest=0.2.5, local=0.2.5
rosbag2_compression                       : latest=0.15.14, local=0.15.14
ros_base                                  : latest=0.10.0, local=0.10.0

   PLATFORM INFORMATION
system           : Linux
platform info    : Linux-6.11.0-24-generic-x86_64-with-glibc2.35
release          : 6.11.0-24-generic
processor        : x86_64

   QOS COMPATIBILITY LIST
topic [type]            : /parameter_events [rcl_interfaces/msg/ParameterEvent]
publisher node          : _ros2cli_daemon_5_1c35e2bcfe1142438de6140411a4393d
subscriber node         : robot_state_publisher
compatibility status    : OK
topic [type]            : /parameter_events [rcl_interfaces/msg/ParameterEvent]
publisher node          : _ros2cli_daemon_5_1c35e2bcfe1142438de6140411a4393d
subscriber node         : robot_state_publisher
compatibility status    : OK
topic [type]            : /parameter_events [rcl_interfaces/msg/ParameterEvent]
publisher node          : _ros2cli_daemon_5_1c35e2bcfe1142438de6140411a4393d
subscriber node         : tf2_echo_normal
compatibility status    : OK
topic [type]            : /parameter_events [rcl_interfaces/msg/ParameterEvent]
publisher node          : _ros2cli_daemon_5_1c35e2bcfe1142438de6140411a4393d
subscriber node         : transform_listener_impl_5db955c77ea0
compatibility status    : OK
topic [type]            : /parameter_events [rcl_interfaces/msg/ParameterEvent]
publisher node          : robot_state_publisher
subscriber node         : robot_state_publisher
compatibility status    : OK
topic [type]            : /parameter_events [rcl_interfaces/msg/ParameterEvent]
publisher node          : robot_state_publisher
subscriber node         : robot_state_publisher
compatibility status    : OK
topic [type]            : /parameter_events [rcl_interfaces/msg/ParameterEvent]
publisher node          : robot_state_publisher
subscriber node         : tf2_echo_normal
compatibility status    : OK
topic [type]            : /parameter_events [rcl_interfaces/msg/ParameterEvent]
publisher node          : robot_state_publisher
subscriber node         : transform_listener_impl_5db955c77ea0
compatibility status    : OK
topic [type]            : /parameter_events [rcl_interfaces/msg/ParameterEvent]
publisher node          : tf2_echo_normal
subscriber node         : robot_state_publisher
compatibility status    : OK
topic [type]            : /parameter_events [rcl_interfaces/msg/ParameterEvent]
publisher node          : tf2_echo_normal
subscriber node         : robot_state_publisher
compatibility status    : OK
topic [type]            : /parameter_events [rcl_interfaces/msg/ParameterEvent]
publisher node          : tf2_echo_normal
subscriber node         : tf2_echo_normal
compatibility status    : OK
topic [type]            : /parameter_events [rcl_interfaces/msg/ParameterEvent]
publisher node          : tf2_echo_normal
subscriber node         : transform_listener_impl_5db955c77ea0
compatibility status    : OK
topic [type]            : /tf [tf2_msgs/msg/TFMessage]
publisher node          : robot_state_publisher
subscriber node         : transform_listener_impl_5db955c77ea0
compatibility status    : OK
topic [type]            : /tf_static [tf2_msgs/msg/TFMessage]
publisher node          : robot_state_publisher
subscriber node         : transform_listener_impl_5db955c77ea0
compatibility status    : OK

   RMW MIDDLEWARE
middleware name    : rmw_fastrtps_cpp

   ROS 2 INFORMATION
distribution name      : humble
distribution type      : ros2
distribution status    : active
release platforms      : {'rhel': ['8'], 'ubuntu': ['jammy']}

   TOPIC LIST
topic               : /joint_states
publisher count     : 0
subscriber count    : 1
topic               : /robot_description
publisher count     : 1
subscriber count    : 0
topic               : /tf
publisher count     : 1
subscriber count    : 1
topic               : /tf_static
publisher count     : 1
subscriber count    : 1

Steps to reproduce issue

  1. Download the repo at https://github.com/AlessioLovato/tf2_echo_bug
  2. Build the package
  3. Follow the usage instruction in the README.md

Expected behavior

The tf2_echo of the 'inverse' case should return a RPY(deg) =[0, -90, 90] (using Euler ZYX) since the quaternion is [0.500, -0.500, 0.500, 0.500]

Actual behavior

The RPY value is wrong, but the quaternion is right:

[tf2_echo-2] - Rotation: in Quaternion [0.500, -0.500, 0.500, 0.500]
[tf2_echo-2] - Rotation: in RPY (radian) [0.785, -1.571, 0.000]
[tf2_echo-2] - Rotation: in RPY (degree) [45.000, -90.000, 0.000]

Additional information

This problem is caused by the numeric error that origins from the LookupTransform function. Changing the std::cout precision to 20 is possible to see that the actual value -0.500 in the quaternion.y is truncated from -0.4999...98988. This error then creates a rotation matrix with values comparable to the epsilon of the machine (at least in my case) instead of zeros.

In case of a singularity, this propagates in the computation of RPY values from the rotation matrix. In particular, when the atan2 function is invoked with the two equals-but-non-zeros arguments, the function returns a roll of 45 degrees, due to the ratio of the two arguments being 1.

Please refer to https://github.com/AlessioLovato/tf2_echo_bug to have better understanding and to reproduce the bug.

Co-Autor of the issue: @ngazzanelli

Metadata

Metadata

Assignees

No one assigned

    Labels

    bugSomething isn't working

    Type

    No type
    No fields configured for issues without a type.

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions