From c7360e002b41fae0f7f914c6fe41f58f99cfa001 Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Fri, 13 Dec 2024 16:06:34 +0300 Subject: [PATCH 1/2] Fix error application on odom rot --- Runtime/Tx/Msgs/Nav/OdometryTx.cs | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/Runtime/Tx/Msgs/Nav/OdometryTx.cs b/Runtime/Tx/Msgs/Nav/OdometryTx.cs index 522377e..1bfd350 100644 --- a/Runtime/Tx/Msgs/Nav/OdometryTx.cs +++ b/Runtime/Tx/Msgs/Nav/OdometryTx.cs @@ -79,10 +79,13 @@ protected override ProBridge.Msg GetMsg(TimeSpan ts) data.pose.pose.position.y += GaussianNoise.Generate(positionNoiseStdDev); data.pose.pose.position.z += GaussianNoise.Generate(positionNoiseStdDev); - data.pose.pose.orientation.x += GaussianNoise.Generate(rotationNoiseStdDev); - data.pose.pose.orientation.y += GaussianNoise.Generate(rotationNoiseStdDev); - data.pose.pose.orientation.z += GaussianNoise.Generate(rotationNoiseStdDev); - data.pose.pose.orientation.w += GaussianNoise.Generate(rotationNoiseStdDev); + var euler = data.pose.pose.orientation.FromRos().eulerAngles; + + euler.x += GaussianNoise.Generate(rotationNoiseStdDev); + euler.y += GaussianNoise.Generate(rotationNoiseStdDev); + euler.z += GaussianNoise.Generate(rotationNoiseStdDev); + + data.pose.pose.orientation = Quaternion.Euler(euler).ToRos(); data.twist.twist.linear.x += GaussianNoise.Generate(linearVelocityNoiseStdDev); data.twist.twist.linear.y += GaussianNoise.Generate(linearVelocityNoiseStdDev); From 7a876c21053cf917f077ec933619e5dc8e363276 Mon Sep 17 00:00:00 2001 From: Sherif Nafee Date: Thu, 9 Jan 2025 12:57:32 +0300 Subject: [PATCH 2/2] Apply orientation error by multiplication --- Runtime/Tx/Msgs/Nav/OdometryTx.cs | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/Runtime/Tx/Msgs/Nav/OdometryTx.cs b/Runtime/Tx/Msgs/Nav/OdometryTx.cs index 1bfd350..3703235 100644 --- a/Runtime/Tx/Msgs/Nav/OdometryTx.cs +++ b/Runtime/Tx/Msgs/Nav/OdometryTx.cs @@ -78,14 +78,16 @@ protected override ProBridge.Msg GetMsg(TimeSpan ts) data.pose.pose.position.x += GaussianNoise.Generate(positionNoiseStdDev); data.pose.pose.position.y += GaussianNoise.Generate(positionNoiseStdDev); data.pose.pose.position.z += GaussianNoise.Generate(positionNoiseStdDev); + + var qNoise = Quaternion.Euler( + GaussianNoise.Generate(rotationNoiseStdDev), + GaussianNoise.Generate(rotationNoiseStdDev), + GaussianNoise.Generate(rotationNoiseStdDev) + ); + + var orientationWithNoise = data.pose.pose.orientation.FromRos() * qNoise; - var euler = data.pose.pose.orientation.FromRos().eulerAngles; - - euler.x += GaussianNoise.Generate(rotationNoiseStdDev); - euler.y += GaussianNoise.Generate(rotationNoiseStdDev); - euler.z += GaussianNoise.Generate(rotationNoiseStdDev); - - data.pose.pose.orientation = Quaternion.Euler(euler).ToRos(); + data.pose.pose.orientation = orientationWithNoise.ToRos(); data.twist.twist.linear.x += GaussianNoise.Generate(linearVelocityNoiseStdDev); data.twist.twist.linear.y += GaussianNoise.Generate(linearVelocityNoiseStdDev);