ros conversion: ROS: x pointing forward, ArmarX: y pointing forward

source/armarx/navigation/local_planning/ros_conversions.cpp

 #include "ros_conversions.h"
+
+#include <cmath>
+
 #include "ArmarXCore/core/exceptions/local/ExpressionException.h"
 #include "ArmarXCore/core/time/Clock.h"
 #include "ArmarXCore/core/time/Duration.h"
@@ -6,7 +9,6 @@
 #include "armarx/navigation/core/Trajectory.h"
 #include "armarx/navigation/core/basic_types.h"
 #include <armarx/navigation/conversions/eigen.h>
-
 #include <range/v3/view/drop.hpp>
 #include <range/v3/view/zip.hpp>
 
@@ -25,6 +27,10 @@ namespace armarx::navigation::conv
         Eigen::Vector2d pos = pose.translation().cast<double>();
         double theta = Eigen::Rotation2Df(pose.linear()).angle();
 
+        // we change it such that x is pointing forward.
+        // ROS: x pointing forward, ArmarX: y pointing forward
+        theta += M_PI_2;
+
         return {pos / 1000., theta}; // [mm] to [m]
     }
 
@@ -35,6 +41,11 @@ namespace armarx::navigation::conv
         Eigen::Vector2d pos = pose.position() * 1000; // [m] to [mm]
         double theta = pose.theta();
 
+        // we change it such that x is pointing forward.
+        // ROS: x pointing forward, ArmarX: y pointing forward
+        theta -= M_PI_2;
+
+
         core::Pose2D ret;
         ret.translation() = pos.cast<float>();
         ret.linear() = Eigen::Rotation2Df(theta).toRotationMatrix();
@@ -46,8 +57,12 @@ namespace armarx::navigation::conv
     toRos(const core::Twist& velocity)
     {
         geometry_msgs::Twist ret;
-        ret.linear.x = velocity.linear.x() / 1000; // [mm] to [m]
-        ret.linear.y = velocity.linear.y() / 1000; // [mm] to [m]
+
+        // we change it such that x is pointing forward.
+        // ROS: x pointing forward, ArmarX: y pointing forward
+
+        ret.linear.x = velocity.linear.y() / 1000; // [mm] to [m]
+        ret.linear.y = -velocity.linear.x() / 1000; // [mm] to [m]
         ret.angular.z = velocity.angular.z();
 
         return ret;
@@ -85,7 +100,7 @@ namespace armarx::navigation::conv
     {
         const auto& tebPoses = teb.poses();
 
-        if(tebPoses.empty())
+        if (tebPoses.empty())
         {
             return {};
         }
@@ -96,19 +111,22 @@ namespace armarx::navigation::conv
         // TODO this timestamp should be given via the argument list
         DateTime timestamp = Clock::Now();
 
-        ARMARX_CHECK_EQUAL(tebPoses.size()-1, teb.timediffs().size());
+        ARMARX_CHECK_EQUAL(tebPoses.size() - 1, teb.timediffs().size());
 
         // add the first pose at this timestamp
-        trajectory.push_back(core::LocalTrajectoryPoint{.timestamp = timestamp, .pose = {fromRos(tebPoses.front()->pose())}});
+        trajectory.push_back(core::LocalTrajectoryPoint{
+            .timestamp = timestamp, .pose = {fromRos(tebPoses.front()->pose())}});
 
         // all consecutive poses
-        for (const auto& [poseVertex, timediff]: ranges::views::zip(tebPoses | ranges::views::drop(1), teb.timediffs()))
+        for (const auto& [poseVertex, timediff] :
+             ranges::views::zip(tebPoses | ranges::views::drop(1), teb.timediffs()))
         {
             const core::Pose pose = fromRos(poseVertex->pose());
             const Duration dt = Duration::SecondsDouble(timediff->dt());
 
-            timestamp += dt; 
-            trajectory.push_back(core::LocalTrajectoryPoint{.timestamp = timestamp, .pose = {pose}});
+            timestamp += dt;
+            trajectory.push_back(
+                core::LocalTrajectoryPoint{.timestamp = timestamp, .pose = {pose}});
         }
 
         ARMARX_CHECK_EQUAL(trajectory.size(), tebPoses.size());