Edit orientation on spawning

src/quaternion_utils.h

@@ -0,0 +1,44 @@
+/* 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 <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Vector3.h>
+
+inline tf2::Quaternion AxisAngleToQuaternion(const tf2::Vector3& axis, const double angle)
+{
+    tf2::Quaternion q;
+
+    // Threshold for considering axis as non-zero
+    constexpr double kAxisEpsilon = 1e-6;
+    if (axis.length() > kAxisEpsilon)
+    {
+        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;
+}

src/sim_widget.ui

@@ -405,6 +405,30 @@
          </item>
         </layout>
        </item>
+       <item>
+        <layout class="QHBoxLayout" name="horizontalLayout_6">
+         <item>
+          <widget class="QLabel" name="spawnRotVectorLabel">
+           <property name="text">
+            <string>RotVector </string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QLineEdit" name="spawnRotVectorEdit"/>
+         </item>
+         <item>
+          <widget class="QLabel" name="spawnRotAngleLabel">
+           <property name="text">
+            <string>RotAngle [deg] </string>
+           </property>
+          </widget>
+         </item>
+         <item>
+          <widget class="QDoubleSpinBox" name="spawnRotAngleBox"/>
+         </item>
+        </layout>
+       </item>
        <item>
         <layout class="QHBoxLayout" name="horizontalLayout">
          <item>

src/simulation_widget.cpp

@@ -21,6 +21,7 @@
 #include <rviz_common/display_context.hpp>
 #include <simulation_interfaces/action/simulate_steps.hpp>
 #include <tf2/LinearMath/Quaternion.h>
+#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_)
@@ -736,6 +737,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 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();
+        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);