Numeric error produces unexpected behavior on RPY angles computation #796
Description
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
- Download the repo at https://github.com/AlessioLovato/tf2_echo_bug
- Build the package
- 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