p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fix transform null value in rviz

This commit is contained in:
LuisPoveda 2023-11-07 17:35:41 +01:00 committed by bernat
parent 406f5e1460
commit 8d63885767
1 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
LibCarla/source/carla/ros2/publishers/CarlaTransformPublisher.cpp
View File

@ -87,8 +87,29 @@ 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;