diff --git a/src/generate_launch.cpp b/src/generate_launch.cpp
index bf4f5de1f1afe8d07f66060faea7f087a67ab93d..20f1f9dbae5e4cb66e81fa64ff2c49f49f62593d 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;