Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ogreQuaternionAngularDistance Does Not Handle Non-Normalized Quaternions Properly #1380

Open
zhihaoshang opened this issue Apr 5, 2025 · 1 comment
Open

ogreQuaternionAngularDistance Does Not Handle Non-Normalized Quaternions Properly #1380

zhihaoshang opened this issue Apr 5, 2025 · 1 comment
Labels
bug Something isn't working

Comments

@zhihaoshang
Copy link

Operating System:

Linux shangzh-VMware-Virtual-Platform 6.11.0-19-generic #19~24.04.1-Ubuntu SMP PREEMPT_DYNAMIC Mon Feb 17 11:51:52 UTC 2 x86_64 x86_64 x86_64 GNU/Linux

ROS version or commit hash:

ros2 jazzy

RMW implementation (if applicable):

No response

RMW Configuration (if applicable):

No response

Client library (if applicable):

rviz

'ros2 doctor --report' output

ros2 doc --report 
   NETWORK CONFIGURATION
inet         : 127.0.0.1
inet4        : ['127.0.0.1']
inet6        : ['::1']
netmask      : 255.0.0.0
device       : lo
flags        : 73<UP,LOOPBACK,RUNNING>
mtu          : 65536
inet         : 192.168.148.137
inet4        : ['192.168.148.137']
ether        : 00:0c:29:be:c8:19
inet6        : ['fe80::20c:29ff:febe:c819%ens33']
netmask      : 255.255.255.0
device       : ens33
flags        : 4163<UP,BROADCAST,RUNNING,MULTICAST>
mtu          : 1500
broadcast    : 192.168.148.255

   PACKAGE VERSIONS
rqt_bag_plugins                           : latest=1.5.4, local=1.5.4
rosbag2                                   : latest=0.26.6, local=0.26.6
rosbag2_storage_default_plugins           : latest=0.26.6, local=0.26.6
rosbag2_storage_mcap                      : latest=0.26.6, local=0.26.6
rosbag2_compression_zstd                  : latest=0.26.6, local=0.26.6
mcap_vendor                               : latest=0.26.6, local=0.26.6
zstd_vendor                               : latest=0.26.6, local=0.26.6
rviz_visual_testing_framework             : latest=14.1.8, local=14.1.7
rviz2                                     : latest=14.1.8, local=14.1.7
rviz_default_plugins                      : latest=14.1.8, local=14.1.7
rviz_common                               : latest=14.1.8, local=14.1.7
rqt_bag                                   : latest=1.5.4, local=1.5.4
rosbag2_examples_py                       : latest=0.26.6, local=0.26.6
launch_testing_examples                   : latest=0.19.5, local=0.19.4
ros2bag                                   : latest=0.26.6, local=0.26.6
rosbag2_py                                : latest=0.26.6, local=0.26.6
rosbag2_transport                         : latest=0.26.6, local=0.26.6
rosbag2_storage_sqlite3                   : latest=0.26.6, local=0.26.6
rosbag2_examples_cpp                      : latest=0.26.6, local=0.26.6
rosbag2_compression                       : latest=0.26.6, local=0.26.6
rosbag2_cpp                               : latest=0.26.6, local=0.26.6
rosbag2_storage                           : latest=0.26.6, local=0.26.6
image_common                              : latest=5.1.5, local=5.1.5
camera_info_manager                       : latest=5.1.5, local=5.1.5
camera_calibration_parsers                : latest=5.1.5, local=5.1.5
yaml_cpp_vendor                           : latest=9.0.1, local=9.0.1
interactive_markers                       : latest=2.5.4, local=2.5.4
common_interfaces                         : latest=5.3.6, local=5.3.5
visualization_msgs                        : latest=5.3.6, local=5.3.5
dummy_robot_bringup                       : latest=0.33.5, local=0.33.5
robot_state_publisher                     : latest=3.3.3, local=3.3.3
kdl_parser                                : latest=2.11.0, local=2.11.0
urdf                                      : latest=2.10.0, local=2.10.0
urdf_parser_plugin                        : latest=2.10.0, local=2.10.0
turtlesim                                 : latest=1.8.3, local=1.8.3
trajectory_msgs                           : latest=5.3.6, local=5.3.5
tracetools_test                           : latest=8.2.3, local=8.2.3
tracetools_launch                         : latest=8.2.3, local=8.2.3
topic_statistics_demo                     : latest=0.33.5, local=0.33.5
topic_monitor                             : latest=0.33.5, local=0.33.5
pendulum_control                          : latest=0.33.5, local=0.33.5
tlsf_cpp                                  : latest=0.17.1, local=0.17.0
geometry2                                 : latest=0.36.9, local=0.36.8
tf2_tools                                 : latest=0.36.9, local=0.36.8
tf2_sensor_msgs                           : latest=0.36.9, local=0.36.8
test_tf2                                  : latest=N/A, local=0.36.8
tf2_kdl                                   : latest=0.36.9, local=0.36.8
tf2_geometry_msgs                         : latest=0.36.9, local=0.36.8
examples_tf2_py                           : latest=0.36.9, local=0.36.8
tf2_ros_py                                : latest=0.36.9, local=0.36.8
tf2_eigen                                 : latest=0.36.9, local=0.36.8
tf2_bullet                                : latest=0.36.9, local=0.36.8
tf2_ros                                   : latest=0.36.9, local=0.36.8
tf2_py                                    : latest=0.36.9, local=0.36.8
tf2_msgs                                  : latest=0.36.9, local=0.36.8
tf2_eigen_kdl                             : latest=0.36.9, local=0.36.8
laser_geometry                            : latest=2.7.0, local=2.7.0
tf2                                       : latest=0.36.9, local=0.36.8
test_tracetools                           : latest=N/A, local=8.2.3
rosbag2_test_common                       : latest=0.26.6, local=0.26.6
test_msgs                                 : latest=2.0.2, local=2.0.2
test_communication                        : latest=N/A, local=0.20.2
stereo_msgs                               : latest=5.3.6, local=5.3.5
std_srvs                                  : latest=5.3.6, local=5.3.5
shape_msgs                                : latest=5.3.6, local=5.3.5
examples_rclpy_pointcloud_publisher       : latest=0.19.5, local=0.19.4
sensor_msgs_py                            : latest=5.3.6, local=5.3.5
quality_of_service_demo_py                : latest=0.33.5, local=0.33.5
quality_of_service_demo_cpp               : latest=0.33.5, local=0.33.5
point_cloud_transport_py                  : latest=4.0.3, local=4.0.3
point_cloud_transport                     : latest=4.0.3, local=4.0.3
map_msgs                                  : latest=2.4.1, local=2.4.1
intra_process_demo                        : latest=0.33.5, local=0.33.5
image_transport                           : latest=5.1.5, local=5.1.5
image_tools                               : latest=0.33.5, local=0.33.5
dummy_sensors                             : latest=0.33.5, local=0.33.5
camera_info_manager_py                    : latest=5.1.5, local=5.1.5
sensor_msgs                               : latest=5.3.6, local=5.3.5
rqt_plot                                  : latest=1.4.2, local=1.4.2
ros2cli_common_extensions                 : latest=0.3.0, local=0.3.0
ros2doctor                                : latest=0.32.3, local=0.32.2
dummy_map_server                          : latest=0.33.5, local=0.33.5
nav_msgs                                  : latest=5.3.6, local=5.3.5
message_filters                           : latest=4.11.5, local=4.11.4
logging_demo                              : latest=0.33.5, local=0.33.5
lifecycle_py                              : latest=0.33.5, local=0.33.5
lifecycle                                 : latest=0.33.5, local=0.33.5
diagnostic_msgs                           : latest=5.3.6, local=5.3.5
geometry_msgs                             : latest=5.3.6, local=5.3.5
examples_rclpy_minimal_subscriber         : latest=0.19.5, local=0.19.4
examples_rclpy_minimal_service            : latest=0.19.5, local=0.19.4
examples_rclpy_minimal_publisher          : latest=0.19.5, local=0.19.4
examples_rclpy_minimal_client             : latest=0.19.5, local=0.19.4
examples_rclpy_executors                  : latest=0.19.5, local=0.19.4
examples_rclcpp_wait_set                  : latest=0.19.5, local=0.19.4
examples_rclcpp_multithreaded_executor    : latest=0.19.5, local=0.19.4
examples_rclcpp_minimal_subscriber        : latest=0.19.5, local=0.19.4
examples_rclcpp_minimal_publisher         : latest=0.19.5, local=0.19.4
examples_rclcpp_minimal_composition       : latest=0.19.5, local=0.19.4
examples_rclcpp_cbg_executor              : latest=0.19.5, local=0.19.4
demo_nodes_py                             : latest=0.33.5, local=0.33.5
demo_nodes_cpp_native                     : latest=0.33.5, local=0.33.5
demo_nodes_cpp                            : latest=0.33.5, local=0.33.5
composition                               : latest=0.33.5, local=0.33.5
actionlib_msgs                            : latest=5.3.6, local=5.3.5
std_msgs                                  : latest=5.3.6, local=5.3.5
rqt                                       : latest=1.6.0, local=1.6.0
rqt_gui_cpp                               : latest=1.6.0, local=1.6.0
ros2lifecycle_test_fixtures               : latest=0.32.3, local=0.32.2
ros2component                             : latest=0.32.3, local=0.32.2
rclcpp_lifecycle                          : latest=28.1.8, local=28.1.6
action_tutorials_cpp                      : latest=0.33.5, local=0.33.5
rclcpp_components                         : latest=28.1.8, local=28.1.6
examples_rclcpp_minimal_action_server     : latest=0.19.5, local=0.19.4
examples_rclcpp_minimal_action_client     : latest=0.19.5, local=0.19.4
rclcpp_action                             : latest=28.1.8, local=28.1.6
examples_rclcpp_minimal_timer             : latest=0.19.5, local=0.19.4
examples_rclcpp_minimal_service           : latest=0.19.5, local=0.19.4
examples_rclcpp_minimal_client            : latest=0.19.5, local=0.19.4
examples_rclcpp_async_client              : latest=0.19.5, local=0.19.4
rclcpp                                    : latest=28.1.8, local=28.1.6
libstatistics_collector                   : latest=1.7.4, local=1.7.4
statistics_msgs                           : latest=2.0.2, local=2.0.2
sros2_cmake                               : latest=0.13.3, local=0.13.2
sros2                                     : latest=0.13.3, local=0.13.2
rqt_topic                                 : latest=1.7.3, local=1.7.3
rqt_srv                                   : latest=1.2.2, local=1.2.2
rqt_shell                                 : latest=1.2.2, local=1.2.2
rqt_service_caller                        : latest=1.2.1, local=1.2.1
rqt_reconfigure                           : latest=1.6.2, local=1.6.2
rqt_py_console                            : latest=1.2.2, local=1.2.2
rqt_publisher                             : latest=1.7.2, local=1.7.2
rqt_action                                : latest=2.2.0, local=2.2.0
rqt_msg                                   : latest=1.5.1, local=1.5.1
rqt_console                               : latest=2.2.1, local=2.2.1
rqt_py_common                             : latest=1.6.0, local=1.6.0
rqt_graph                                 : latest=1.5.4, local=1.5.4
rqt_gui_py                                : latest=1.6.0, local=1.6.0
rqt_gui                                   : latest=1.6.0, local=1.6.0
ros_testing                               : latest=0.6.0, local=0.6.0
ros2trace                                 : latest=8.2.3, local=8.2.3
ros2topic                                 : latest=0.32.3, local=0.32.2
ros2test                                  : latest=0.6.0, local=0.6.0
ros2param                                 : latest=0.32.3, local=0.32.2
ros2lifecycle                             : latest=0.32.3, local=0.32.2
ros2service                               : latest=0.32.3, local=0.32.2
ros2run                                   : latest=0.32.3, local=0.32.2
ros2launch                                : latest=0.26.7, local=0.26.6
ros2pkg                                   : latest=0.32.3, local=0.32.2
ros2node                                  : latest=0.32.3, local=0.32.2
ros2multicast                             : latest=0.32.3, local=0.32.2
ros2interface                             : latest=0.32.3, local=0.32.2
ros2action                                : latest=0.32.3, local=0.32.2
ros2cli                                   : latest=0.32.3, local=0.32.2
launch_testing_ros                        : latest=0.26.7, local=0.26.6
launch_ros                                : latest=0.26.7, local=0.26.6
examples_rclpy_minimal_action_server      : latest=0.19.5, local=0.19.4
examples_rclpy_minimal_action_client      : latest=0.19.5, local=0.19.4
examples_rclpy_guard_conditions           : latest=0.19.5, local=0.19.4
action_tutorials_py                       : latest=0.33.5, local=0.33.5
rclpy                                     : latest=7.1.4, local=7.1.3
rosgraph_msgs                             : latest=2.0.2, local=2.0.2
rosbag2_interfaces                        : latest=0.26.6, local=0.26.6
ros2cli_test_interfaces                   : latest=0.32.3, local=0.32.2
rcl_lifecycle                             : latest=9.2.5, local=9.2.4
rcl_action                                : latest=9.2.5, local=9.2.4
rcl                                       : latest=9.2.5, local=9.2.4
rmw_implementation                        : latest=2.15.5, local=2.15.5
rmw_fastrtps_dynamic_cpp                  : latest=8.4.2, local=8.4.2
rmw_fastrtps_cpp                          : latest=8.4.2, local=8.4.2
rmw_fastrtps_shared_cpp                   : latest=8.4.2, local=8.4.2
rmw_cyclonedds_cpp                        : latest=2.2.3, local=2.2.3
rmw_connextddsmicro                       : latest=N/A, local=0.22.1
rmw_connextdds                            : latest=0.22.1, local=0.22.1
rmw_connextdds_common                     : latest=0.22.1, local=0.22.1
rmw_dds_common                            : latest=3.1.0, local=3.1.0
composition_interfaces                    : latest=2.0.2, local=2.0.2
rcl_interfaces                            : latest=2.0.2, local=2.0.2
pendulum_msgs                             : latest=0.33.5, local=0.33.5
lifecycle_msgs                            : latest=2.0.2, local=2.0.2
example_interfaces                        : latest=0.12.0, local=0.12.0
action_tutorials_interfaces               : latest=0.33.5, local=0.33.5
rosidl_default_runtime                    : latest=1.6.0, local=1.6.0
rosidl_default_generators                 : latest=1.6.0, local=1.6.0
action_msgs                               : latest=2.0.2, local=2.0.2
unique_identifier_msgs                    : latest=2.5.0, local=2.5.0
type_description_interfaces               : latest=2.0.2, local=2.0.2
service_msgs                              : latest=2.0.2, local=2.0.2
builtin_interfaces                        : latest=2.0.2, local=2.0.2
rosidl_core_runtime                       : latest=0.2.0, local=0.2.0
rosidl_core_generators                    : latest=0.2.0, local=0.2.0
rosidl_generator_py                       : latest=0.22.0, local=0.22.0
ament_lint_common                         : latest=0.17.2, local=0.17.1
ament_cmake_uncrustify                    : latest=0.17.2, local=0.17.1
ament_uncrustify                          : latest=0.17.2, local=0.17.1
uncrustify_vendor                         : latest=3.0.1, local=3.0.0
tracetools_trace                          : latest=8.2.3, local=8.2.3
tracetools_read                           : latest=8.2.3, local=8.2.3
tracetools                                : latest=8.2.3, local=8.2.3
tlsf                                      : latest=0.9.0, local=0.9.0
qt_gui_core                               : latest=2.7.5, local=2.7.5
qt_gui_cpp                                : latest=2.7.5, local=2.7.5
pluginlib                                 : latest=5.4.2, local=5.4.2
tinyxml2_vendor                           : latest=0.9.1, local=0.9.1
test_tracetools_launch                    : latest=N/A, local=8.2.3
test_security                             : latest=N/A, local=0.20.2
test_ros2trace                            : latest=N/A, local=8.2.3
test_rmw_implementation                   : latest=N/A, local=2.15.5
test_rclcpp                               : latest=N/A, local=0.20.2
test_quality_of_service                   : latest=N/A, local=0.20.2
test_launch_testing                       : latest=N/A, local=3.4.3
test_launch_ros                           : latest=N/A, local=0.26.6
test_interface_files                      : latest=0.11.0, local=0.11.0
test_cli_remapping                        : latest=N/A, local=0.20.2
test_cli                                  : latest=N/A, local=0.20.2
qt_gui_app                                : latest=2.7.5, local=2.7.5
qt_gui                                    : latest=2.7.5, local=2.7.5
tango_icons_vendor                        : latest=0.3.0, local=0.3.0
sqlite3_vendor                            : latest=0.26.6, local=0.26.6
rcl_logging_spdlog                        : latest=3.1.1, local=3.1.1
spdlog_vendor                             : latest=1.6.1, local=1.6.1
shared_queues_vendor                      : latest=0.26.6, local=0.26.6
rviz_rendering_tests                      : latest=14.1.8, local=14.1.7
rviz_rendering                            : latest=14.1.8, local=14.1.7
rviz_ogre_vendor                          : latest=14.1.8, local=14.1.7
rviz_assimp_vendor                        : latest=14.1.8, local=14.1.7
rttest                                    : latest=0.17.1, local=0.17.0
rti_connext_dds_cmake_module              : latest=0.22.1, local=0.22.1
lttngpy                                   : latest=8.2.3, local=8.2.3
rpyutils                                  : latest=0.4.1, local=0.4.1
rosidl_typesupport_tests                  : latest=N/A, local=3.2.2
rosidl_typesupport_introspection_tests    : latest=N/A, local=4.6.5
rosidl_typesupport_cpp                    : latest=3.2.2, local=3.2.2
rosidl_typesupport_introspection_cpp      : latest=4.6.5, local=4.6.5
rosidl_typesupport_c                      : latest=3.2.2, local=3.2.2
rosidl_typesupport_introspection_c        : latest=4.6.5, local=4.6.5
rosidl_typesupport_fastrtps_c             : latest=3.6.1, local=3.6.1
rosidl_typesupport_fastrtps_cpp           : latest=3.6.1, local=3.6.1
rosidl_generator_cpp                      : latest=4.6.5, local=4.6.5
rosidl_runtime_cpp                        : latest=4.6.5, local=4.6.5
rosidl_dynamic_typesupport_fastrtps       : latest=0.1.0, local=0.1.0
rcl_yaml_param_parser                     : latest=9.2.5, local=9.2.4
rmw                                       : latest=7.3.2, local=7.3.2
rosidl_dynamic_typesupport                : latest=0.1.2, local=0.1.2
rosidl_runtime_c                          : latest=4.6.5, local=4.6.5
rosidl_generator_c                        : latest=4.6.5, local=4.6.5
rosidl_typesupport_interface              : latest=4.6.5, local=4.6.5
rosidl_runtime_py                         : latest=0.13.1, local=0.13.1
rosidl_generator_dds_idl                  : latest=0.11.1, local=0.11.1
rosidl_cmake                              : latest=4.6.5, local=4.6.5
rosidl_pycommon                           : latest=4.6.5, local=4.6.5
rosidl_generator_type_description         : latest=4.6.5, local=4.6.5
rosidl_parser                             : latest=4.6.5, local=4.6.5
rosidl_generator_tests                    : latest=N/A, local=4.6.5
rosidl_adapter                            : latest=4.6.5, local=4.6.5
rosidl_cli                                : latest=4.6.5, local=4.6.5
rosbag2_tests                             : latest=0.26.6, local=0.26.6
rosbag2_test_msgdefs                      : latest=0.26.6, local=0.26.6
ros_environment                           : latest=4.2.1, local=4.2.1
rmw_implementation_cmake                  : latest=7.3.2, local=7.3.2
resource_retriever                        : latest=3.4.3, local=3.4.3
class_loader                              : latest=2.7.0, local=2.7.0
rcpputils                                 : latest=2.11.2, local=2.11.1
rcl_logging_noop                          : latest=3.1.1, local=3.1.1
rcl_logging_interface                     : latest=3.1.1, local=3.1.1
rcutils                                   : latest=6.7.2, local=6.7.2
qt_gui_py_common                          : latest=2.7.5, local=2.7.5
qt_dotgraph                               : latest=2.7.5, local=2.7.5
python_qt_binding                         : latest=2.2.1, local=2.2.1
python_orocos_kdl_vendor                  : latest=0.5.1, local=0.5.1
launch_testing_ament_cmake                : latest=3.4.4, local=3.4.3
python_cmake_module                       : latest=0.11.1, local=0.11.1
pybind11_vendor                           : latest=3.1.3, local=3.1.2
performance_test_fixture                  : latest=0.2.1, local=0.2.1
launch_pytest                             : latest=3.4.4, local=3.4.3
launch_testing                            : latest=3.4.4, local=3.4.3
launch_yaml                               : latest=3.4.4, local=3.4.3
launch_xml                                : latest=3.4.4, local=3.4.3
launch                                    : latest=3.4.4, local=3.4.3
osrf_pycommon                             : latest=2.1.5, local=2.1.5
orocos_kdl_vendor                         : latest=0.5.1, local=0.5.1
mimick_vendor                             : latest=0.6.2, local=0.6.2
libyaml_vendor                            : latest=1.6.3, local=1.6.3
liblz4_vendor                             : latest=0.26.6, local=0.26.6
libcurl_vendor                            : latest=3.4.3, local=3.4.3
keyboard_handler                          : latest=0.3.1, local=0.3.1
gz_math_vendor                            : latest=0.0.8, local=0.0.8
gz_utils_vendor                           : latest=0.0.5, local=0.0.5
gz_cmake_vendor                           : latest=0.0.9, local=0.0.9
ament_cmake_ros                           : latest=0.12.0, local=0.12.0
ament_cmake_auto                          : latest=2.5.3, local=2.5.3
ament_cmake_gmock                         : latest=2.5.3, local=2.5.3
ament_cmake_gtest                         : latest=2.5.3, local=2.5.3
ament_cmake_google_benchmark              : latest=2.5.3, local=2.5.3
fastrtps_cmake_module                     : latest=3.6.1, local=3.6.1
eigen3_cmake_module                       : latest=0.3.0, local=0.3.0
domain_coordinator                        : latest=0.12.0, local=0.12.0
console_bridge_vendor                     : latest=1.7.1, local=1.7.1
ament_cmake_xmllint                       : latest=0.17.2, local=0.17.1
ament_xmllint                             : latest=0.17.2, local=0.17.1
ament_cmake_pyflakes                      : latest=0.17.2, local=0.17.1
ament_pyflakes                            : latest=0.17.2, local=0.17.1
ament_cmake_pycodestyle                   : latest=0.17.2, local=0.17.1
ament_pycodestyle                         : latest=0.17.2, local=0.17.1
ament_cmake_pep257                        : latest=0.17.2, local=0.17.1
ament_pep257                              : latest=0.17.2, local=0.17.1
ament_cmake_pclint                        : latest=0.17.2, local=0.17.1
ament_pclint                              : latest=0.17.2, local=0.17.1
ament_lint_auto                           : latest=0.17.2, local=0.17.1
ament_cmake                               : latest=2.5.3, local=2.5.3
ament_cmake_version                       : latest=2.5.3, local=2.5.3
ament_cmake_vendor_package                : latest=2.5.3, local=2.5.3
ament_cmake_pytest                        : latest=2.5.3, local=2.5.3
ament_cmake_mypy                          : latest=0.17.2, local=0.17.1
ament_cmake_lint_cmake                    : latest=0.17.2, local=0.17.1
ament_cmake_flake8                        : latest=0.17.2, local=0.17.1
ament_cmake_cpplint                       : latest=0.17.2, local=0.17.1
ament_cmake_cppcheck                      : latest=0.17.2, local=0.17.1
ament_cmake_copyright                     : latest=0.17.2, local=0.17.1
ament_cmake_clang_tidy                    : latest=0.17.2, local=0.17.1
ament_cmake_clang_format                  : latest=0.17.2, local=0.17.1
ament_cmake_test                          : latest=2.5.3, local=2.5.3
ament_cmake_target_dependencies           : latest=2.5.3, local=2.5.3
ament_cmake_python                        : latest=2.5.3, local=2.5.3
ament_cmake_export_dependencies           : latest=2.5.3, local=2.5.3
ament_cmake_libraries                     : latest=2.5.3, local=2.5.3
ament_cmake_include_directories           : latest=2.5.3, local=2.5.3
ament_cmake_gen_version_h                 : latest=2.5.3, local=2.5.3
ament_cmake_export_targets                : latest=2.5.3, local=2.5.3
ament_cmake_export_link_flags             : latest=2.5.3, local=2.5.3
ament_cmake_export_interfaces             : latest=2.5.3, local=2.5.3
ament_cmake_export_libraries              : latest=2.5.3, local=2.5.3
ament_cmake_export_include_directories    : latest=2.5.3, local=2.5.3
ament_cmake_export_definitions            : latest=2.5.3, local=2.5.3
ament_cmake_core                          : latest=2.5.3, local=2.5.3
ament_package                             : latest=0.16.3, local=0.16.3
ament_mypy                                : latest=0.17.2, local=0.17.1
ament_lint_cmake                          : latest=0.17.2, local=0.17.1
ament_flake8                              : latest=0.17.2, local=0.17.1
ament_copyright                           : latest=0.17.2, local=0.17.1
ament_lint                                : latest=0.17.2, local=0.17.1
ament_index_python                        : latest=1.8.1, local=1.8.1
ament_index_cpp                           : latest=1.8.1, local=1.8.1
ament_cpplint                             : latest=0.17.2, local=0.17.1
ament_cppcheck                            : latest=0.17.2, local=0.17.1
ament_clang_tidy                          : latest=0.17.2, local=0.17.1
ament_clang_format                        : latest=0.17.2, local=0.17.1

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

   QOS COMPATIBILITY LIST
compatibility status    : No publisher/subscriber pairs found

   RMW MIDDLEWARE
middleware name    : rmw_fastrtps_cpp

   ROS 2 INFORMATION
distribution name      : jazzy
distribution type      : ros2
distribution status    : active
release platforms      : {'debian': ['bookworm'], 'rhel': ['9'], 'ubuntu': ['noble']}

   TOPIC LIST
topic               : none
publisher count     : 0
subscriber count    : 0

Steps to reproduce issue

Environment

OS Version: Ubuntu 24.04
rviz version: ros2 jazzy
Compiler name and version number: Ubuntu clang version 18.1.3
Source or binary build?
source build
build options: --mixin asan-gcc

Description

The function rviz_default_plugins::ogreQuaternionAngularDistance is designed to compute the angular distance between two quaternions. However, the implementation implicitly assumes that the input quaternions are normalized (unit quaternions), without validating or normalizing them internally. As a result, if non-normalized quaternions are passed as input, the function may return incorrect angle values.

Test Case

#include <gmock/gmock.h>
#include <OgreMatrix3.h>
#include <OgreQuaternion.h>
#include <OgreVector.h>
#include "../../../../src/rviz_default_plugins/displays/odometry/quaternion_helper.hpp"
using namespace ::testing;  // NOLINT

TEST(QuaternionHelper, ogreQuaternionHelper_validates_non_standardized_inputs) {
  Ogre::Quaternion quaternion1(2.0f, 0.0f, 0.0f, 0.0f);
  Ogre::Quaternion quaternion2(0.0f, 2.0f, 2.0f, 0.0f);
  quaternion2.normalise();
  EXPECT_THAT(
    rviz_default_plugins::ogreQuaternionAngularDistance(quaternion1, quaternion2),
    FloatNear(Ogre::Math::HALF_PI, 0.001f));
}

Output

[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from QuaternionHelper
[ RUN      ] QuaternionHelper.ogreQuaternionHelper_validates_non_standardized_inputs
/home/shangzh/ros2_jazzy/src/ros2/rviz/rviz_default_plugins/test/rviz_default_plugins/displays/odometry/quaternion_helper_test.cpp:12: Failure
Value of: rviz_default_plugins::ogreQuaternionAngularDistance(quaternion1, quaternion2)
Expected: is approximately 1.5707964 (absolute error <= 0.001)
  Actual: 3.14159274 (of type float), which is 1.5708 from 1.5708

[  FAILED  ] QuaternionHelper.ogreQuaternionHelper_validates_non_standardized_inputs (0 ms)
[----------] 1 test from QuaternionHelper (0 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (0 ms total)
[  PASSED  ] 0 tests.
[  FAILED  ] 1 test, listed below:
[  FAILED  ] QuaternionHelper.ogreQuaternionHelper_validates_non_standardized_inputs

 1 FAILED TEST

Expected behavior

The function should return the correct angular distance even when the input quaternions are not normalized, or it should explicitly enforce that the inputs must be unit quaternions (either via normalization or input validation).

Actual behavior

The function does not normalize or validate the input quaternions before performing the computation. As a result, when non-unit quaternions are used, the calculated angle is incorrect. In the test case above, the result is π instead of the expected π/2.

Additional information

Suggested Fixes

1.Automatically normalize inputs (recommended):
Add normalization inside the function to ensure valid input:

first.normalise();
second.normalise();

2.Add input validation:
If normalization is avoided for performance reasons, assert or throw an error when non-normalized inputs are detected:

assert(fabs(first.normalise() - 1.0f) < 1e-3 && "Input quaternion must be normalized.");
@zhihaoshang zhihaoshang added the bug Something isn't working label Apr 5, 2025
@mjcarroll
Copy link
Member

Thanks for the report, this does look like a bug, would you care to open a PR?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
bug Something isn't working
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@mjcarroll @zhihaoshang