From 116edcdc695c19397c3e78c8da5906864e7061e5 Mon Sep 17 00:00:00 2001 From: LuisPoveda Date: Wed, 8 Nov 2023 11:46:34 +0100 Subject: [PATCH] Change previous solution, now quaternions are created with the unit values --- .../publishers/CarlaTransformPublisher.cpp | 21 ------------------- .../source/carla/ros2/types/Quaternion.cpp | 2 +- 2 files changed, 1 insertion(+), 22 deletions(-) diff --git a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp index b5560decb..caf9d5339 100644 --- a/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp +++ b/LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp @@ -87,29 +87,8 @@ namespace ros2 { return true; } - static bool CompareDouble(double value, double cmp_value) { - const double result = abs(value) - abs(cmp_value); - if (result < 0.001) - return true; - return false; - } - bool CarlaTransformPublisher::Publish() { eprosima::fastrtps::rtps::InstanceHandle_t instance_handle; - - const double qx = _impl->_transform.transforms()[0].transform().rotation().x(); - const double qy = _impl->_transform.transforms()[0].transform().rotation().y(); - const double qz = _impl->_transform.transforms()[0].transform().rotation().z(); - const double qw = _impl->_transform.transforms()[0].transform().rotation().w(); - - bool q1 = CompareDouble(qx, 0.0); - bool q2 = CompareDouble(qy, 0.0); - bool q3 = CompareDouble(qz, 0.0); - bool q4 = CompareDouble(qw, 0.0); - - if (q1 && q2 && q3 && q4) - _impl->_transform.transforms()[0].transform().rotation().w(1.0); - eprosima::fastrtps::types::ReturnCode_t rcode = _impl->_datawriter->write(&_impl->_transform, instance_handle); if (rcode == erc::ReturnCodeValue::RETCODE_OK) { return true; diff --git a/LibCarla/source/carla/ros2/types/Quaternion.cpp b/LibCarla/source/carla/ros2/types/Quaternion.cpp index a7c47c126..4858a84b2 100644 --- a/LibCarla/source/carla/ros2/types/Quaternion.cpp +++ b/LibCarla/source/carla/ros2/types/Quaternion.cpp @@ -46,7 +46,7 @@ geometry_msgs::msg::Quaternion::Quaternion() // double m_z m_z = 0.0; // double m_w - m_w = 0.0; + m_w = 1.0; } geometry_msgs::msg::Quaternion::~Quaternion()