Skip to content
Snippets Groups Projects
Commit 372518e4 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Normalized angles

parent 6722f71e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 8 additions and 0 deletions
src/generate_launch.cpp
+ 8
− 0
View file @ 372518e4
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment