From 94d51d91db17dd508ca06ab175dc8e51f65e03a1 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Tue, 21 Oct 2025 14:06:18 +0200 Subject: [PATCH 1/3] Edit orientation on spawning Signed-off-by: Jan Hanca --- src/sim_widget.ui | 24 ++++++++++++++++++++++++ src/simulation_widget.cpp | 10 ++++++++++ 2 files changed, 34 insertions(+) diff --git a/src/sim_widget.ui b/src/sim_widget.ui index 0b8334e..2e3c984 100644 --- a/src/sim_widget.ui +++ b/src/sim_widget.ui @@ -405,6 +405,30 @@ + + + + + + RotVector + + + + + + + + + + RotAngle [deg] + + + + + + + + diff --git a/src/simulation_widget.cpp b/src/simulation_widget.cpp index 07e8495..b1029ab 100644 --- a/src/simulation_widget.cpp +++ b/src/simulation_widget.cpp @@ -736,6 +736,16 @@ namespace q_simulation_interfaces request.initial_pose.pose.position.y = ui_->doubleSpinBoxY->value(); request.initial_pose.pose.position.z = ui_->doubleSpinBoxZ->value(); + const QString vectorStr = ui_->spawnRotVectorEdit->text(); + const double angle = qDegreesToRadians(ui_->spawnRotAngleBox->value()); + const auto vector = QStringToVector(vectorStr); + tf2::Quaternion q(vector, angle); + + request.initial_pose.pose.orientation.x = q.x(); + request.initial_pose.pose.orientation.y = q.y(); + request.initial_pose.pose.orientation.z = q.z(); + request.initial_pose.pose.orientation.w = q.w(); + auto cb = [this](auto response) { ProduceWarningIfProblem(this, "SpawnEntity", response); From 33761b938b479826b6e7a8a0e166fa5e0beccd5b Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Tue, 21 Oct 2025 15:31:19 +0200 Subject: [PATCH 2/3] Self code-review: fix empty axis Signed-off-by: Jan Hanca --- src/quaternion_utils.h | 42 +++++++++++++++++++++++++++++++++++++++ src/simulation_widget.cpp | 11 +++++----- 2 files changed, 48 insertions(+), 5 deletions(-) create mode 100644 src/quaternion_utils.h diff --git a/src/quaternion_utils.h b/src/quaternion_utils.h new file mode 100644 index 0000000..fe96efd --- /dev/null +++ b/src/quaternion_utils.h @@ -0,0 +1,42 @@ +/* Copyright 2025, Robotec.ai sp. z o.o. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include +#include + +inline tf2::Quaternion AxisAngleToQuaternion(const tf2::Vector3& axis, const double angle) +{ + tf2::Quaternion q; + + if (axis.length() > 1e-6) + { + q = tf2::Quaternion(axis, angle); + q.normalize(); + } + else + { + // Default to no rotation if vector is zero + q.setValue(0.0, 0.0, 0.0, 1.0); + } + + if (std::isnan(q.x()) || std::isnan(q.y()) || std::isnan(q.z()) || std::isnan(q.w())) + { + q.setValue(0.0, 0.0, 0.0, 1.0); + } + + return q; +} diff --git a/src/simulation_widget.cpp b/src/simulation_widget.cpp index b1029ab..4d69041 100644 --- a/src/simulation_widget.cpp +++ b/src/simulation_widget.cpp @@ -21,6 +21,7 @@ #include #include #include +#include "quaternion_utils.h" #include "service.h" #include "string_to_keys.h" #include "ui_sim_widget.h" @@ -697,8 +698,9 @@ namespace q_simulation_interfaces const QString vectorStr = ui_->RotVector->text(); const double angle = qDegreesToRadians(ui_->RotAngle->value()); - const auto vector = QStringToVector(vectorStr); - tf2::Quaternion q(vector, angle); + const auto axis = QStringToVector(vectorStr); + const auto q = AxisAngleToQuaternion(axis, angle); + request.state.header.frame_id = ui_->frameStateLineEdit->text().toStdString(); request.state.pose.orientation.x = q.x(); request.state.pose.orientation.y = q.y(); @@ -716,7 +718,6 @@ namespace q_simulation_interfaces setEntityStateService_->call_service_async(cb, request); } - void SimulationWidget::SpawnButton() { if (!spawnEntityService_) @@ -738,8 +739,8 @@ namespace q_simulation_interfaces const QString vectorStr = ui_->spawnRotVectorEdit->text(); const double angle = qDegreesToRadians(ui_->spawnRotAngleBox->value()); - const auto vector = QStringToVector(vectorStr); - tf2::Quaternion q(vector, angle); + const auto axis = QStringToVector(vectorStr); + const auto q = AxisAngleToQuaternion(axis, angle); request.initial_pose.pose.orientation.x = q.x(); request.initial_pose.pose.orientation.y = q.y(); From 6bc64ad216b18ea610b0130302bd0e4a35603182 Mon Sep 17 00:00:00 2001 From: Jan Hanca Date: Tue, 21 Oct 2025 18:50:35 +0200 Subject: [PATCH 3/3] Code review: comment on epsilon Signed-off-by: Jan Hanca --- src/quaternion_utils.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/quaternion_utils.h b/src/quaternion_utils.h index fe96efd..6174774 100644 --- a/src/quaternion_utils.h +++ b/src/quaternion_utils.h @@ -22,7 +22,9 @@ inline tf2::Quaternion AxisAngleToQuaternion(const tf2::Vector3& axis, const dou { tf2::Quaternion q; - if (axis.length() > 1e-6) + // Threshold for considering axis as non-zero + constexpr double kAxisEpsilon = 1e-6; + if (axis.length() > kAxisEpsilon) { q = tf2::Quaternion(axis, angle); q.normalize();