From 372518e403f772033ec9e26850c5eec7748713da Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Tue, 5 Oct 2021 10:55:43 +0200
Subject: [PATCH] Normalized angles
---
src/generate_launch.cpp | 8 ++++++++
1 file changed, 8 insertions(+)
diff --git a/src/generate_launch.cpp b/src/generate_launch.cpp
index bf4f5de..20f1f9d 100644
--- a/src/generate_launch.cpp
+++ b/src/generate_launch.cpp
@@ -166,6 +166,10 @@ void generate_signals_launch(std::string &path,std::string &signals_file,COpendr
world_x = world.x + loc_x*std::cos(world.heading) - loc_y*std::sin(world.heading);
world_y = world.y + loc_x*std::sin(world.heading) + loc_y*std::cos(world.heading);
world_yaw = loc_yaw + world.heading;
+ while (world_yaw >= M_PI)
+ world_yaw -= 2*M_PI;
+ while (world_yaw < -M_PI)
+ world_yaw += 2*M_PI;
out_file << " <arg name=\"x\" value=\"" << world_x << "\"/>" << std::endl;
out_file << " <arg name=\"y\" value=\"" << world_y << "\"/>" << std::endl;
@@ -183,6 +187,10 @@ void generate_signals_launch(std::string &path,std::string &signals_file,COpendr
world_x = world.x + loc_x*std::cos(world.heading) - loc_y*std::sin(world.heading);
world_y = world.y + loc_x*std::sin(world.heading) + loc_y*std::cos(world.heading);
world_yaw = loc_yaw + world.heading;
+ while (world_yaw >= M_PI)
+ world_yaw -= 2*M_PI;
+ while (world_yaw < -M_PI)
+ world_yaw += 2*M_PI;
out_file << " <arg name=\"x\" value=\"" << world_x << "\"/>" << std::endl;
out_file << " <arg name=\"y\" value=\"" << world_y << "\"/>" << std::endl;
--
GitLab