diff --git a/include/generate_launch.h b/include/generate_launch.h index 712fe860a7fb6f4926311ea29f49e5fe0e454748..28c600afb3f0c6d3a7c80bed1832a13a1919c77f 100644 --- a/include/generate_launch.h +++ b/include/generate_launch.h @@ -58,16 +58,17 @@ void generate_parkings_yaml(std::string &path,std::string &parkings_file,COpendr #define SEMAPHORE_TYPE "1000001" #define SEMAPHORE_MARKER "alvar1" // #define SEMAPHORE_MARKER "alvar15" -#define LOC_SIGN_TRIANGLE_TYPE "" -#define LOC_SIGN_TRIANGLE_MARKER "alvar2_neg_1" +#define GENERIC_LOC_SIGN_TRIANGLE_TYPE "1000" +#define GENERIC_LOC_SIGN_TRIANGLE_MARKER "alvar2_neg_1" +#define GLOBAL_LOC_TYPE "1001" +#define GLOBAL_LOC_MARKER "alvar3_neg_1" -#define LOC_SIGN_TRIANGLE_WIDTH 0.074 -#define LOC_SIGN_TRIANGLE_ANGLE M_PI/2 -#define LOC_SIGN_ID_OFFSET 100 +#define GENERIC_LOC_SIGN_TRIANGLE_WIDTH 0.074 +#define GENERIC_LOC_SIGN_TRIANGLE_ANGLE M_PI/2 +#define GENERIC_LOC_SIGN_ID_OFFSET 100 #define PARKED_CAR_LENGTH "0.6" #define PARKED_CAR_WIDTH "0.25" #define PARKED_CAR_HEIGHT "0.15" - #endif diff --git a/src/generate_launch.cpp b/src/generate_launch.cpp index 20f1f9dbae5e4cb66e81fa64ff2c49f49f62593d..53fcbf059ad756f7a50a5f6e79e04603b04308d8 100644 --- a/src/generate_launch.cpp +++ b/src/generate_launch.cpp @@ -107,10 +107,15 @@ void get_signal_type_info(std::string &type,std::string &subtype,int value,std:: name="semaphore"; marker=SEMAPHORE_MARKER; } - else if (type==LOC_SIGN_TRIANGLE_TYPE) + else if (type==GENERIC_LOC_SIGN_TRIANGLE_TYPE) { - name="locsign"; - marker=LOC_SIGN_TRIANGLE_MARKER; + name="generic_loc"; + marker=GENERIC_LOC_SIGN_TRIANGLE_MARKER; + } + else if (type==GLOBAL_LOC_TYPE) + { + name="global_loc"; + marker=GLOBAL_LOC_MARKER; } else throw CException(_HERE_,"Unsupported signal " + type); @@ -152,17 +157,17 @@ void generate_signals_launch(std::string &path,std::string &signals_file,COpendr signal.get_type(type,subtype); get_signal_type_info(type,subtype,signal.get_value(),name,marker); out_file << std::endl; - if (type == LOC_SIGN_TRIANGLE_TYPE) + if (type == GENERIC_LOC_SIGN_TRIANGLE_TYPE) { double loc_x, loc_y, loc_yaw, world_x, world_y, world_yaw; out_file << " <include file=\"$(find iri_sign_description)/launch/spawn_localization_sign.launch\">" << std::endl; - out_file << " <arg name=\"name\" value=\"" << name << "_" << (signal.get_id()%LOC_SIGN_ID_OFFSET)*2-1 << "\"/>" << std::endl; + out_file << " <arg name=\"name\" value=\"" << name << "_" << (signal.get_id()%GENERIC_LOC_SIGN_ID_OFFSET)*2-1 << "\"/>" << std::endl; out_file << " <arg name=\"tag\" value=\"" << marker << "\"/>" << std::endl; world=signal.get_world_pose(); - loc_x = -LOC_SIGN_TRIANGLE_WIDTH*std::sin(LOC_SIGN_TRIANGLE_ANGLE/2)/2; - loc_y = -LOC_SIGN_TRIANGLE_WIDTH*std::cos(LOC_SIGN_TRIANGLE_ANGLE/2)/2; - loc_yaw = M_PI + LOC_SIGN_TRIANGLE_ANGLE/2; + loc_x = -GENERIC_LOC_SIGN_TRIANGLE_WIDTH*std::sin(GENERIC_LOC_SIGN_TRIANGLE_ANGLE/2)/2; + loc_y = -GENERIC_LOC_SIGN_TRIANGLE_WIDTH*std::cos(GENERIC_LOC_SIGN_TRIANGLE_ANGLE/2)/2; + loc_yaw = M_PI + GENERIC_LOC_SIGN_TRIANGLE_ANGLE/2; 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; @@ -178,12 +183,12 @@ void generate_signals_launch(std::string &path,std::string &signals_file,COpendr out_file << " </include>" << std::endl; out_file << " <include file=\"$(find iri_sign_description)/launch/spawn_localization_sign.launch\">" << std::endl; - out_file << " <arg name=\"name\" value=\"" << name << "_" << (signal.get_id()%LOC_SIGN_ID_OFFSET)*2 << "\"/>" << std::endl; + out_file << " <arg name=\"name\" value=\"" << name << "_" << (signal.get_id()%GENERIC_LOC_SIGN_ID_OFFSET)*2 << "\"/>" << std::endl; out_file << " <arg name=\"tag\" value=\"" << marker << "\"/>" << std::endl; - loc_x = LOC_SIGN_TRIANGLE_WIDTH*std::sin(LOC_SIGN_TRIANGLE_ANGLE/2)/2; - loc_y = -LOC_SIGN_TRIANGLE_WIDTH*std::cos(LOC_SIGN_TRIANGLE_ANGLE/2)/2; - loc_yaw = -LOC_SIGN_TRIANGLE_ANGLE/2; + loc_x = GENERIC_LOC_SIGN_TRIANGLE_WIDTH*std::sin(GENERIC_LOC_SIGN_TRIANGLE_ANGLE/2)/2; + loc_y = -GENERIC_LOC_SIGN_TRIANGLE_WIDTH*std::cos(GENERIC_LOC_SIGN_TRIANGLE_ANGLE/2)/2; + loc_yaw = -GENERIC_LOC_SIGN_TRIANGLE_ANGLE/2; 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; @@ -215,6 +220,8 @@ void generate_signals_launch(std::string &path,std::string &signals_file,COpendr out_file << " <arg name=\"model\" value=\"sign\"/>" << std::endl; out_file << " <arg name=\"type\" value=\"" << name << "\"/>" << std::endl; } + if(type==GLOBAL_LOC_TYPE) + out_file << " <arg name=\"tag\" value=\"" << "alvar3_neg_1" << "\"/>" << std::endl; world=signal.get_world_pose(); out_file << " <arg name=\"x\" value=\"" << world.x << "\"/>" << std::endl; out_file << " <arg name=\"y\" value=\"" << world.y << "\"/>" << std::endl;