Commit f558c31f authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added support for the global localization sign.

parent 372518e4
Pipeline #6820 canceled with stages
......@@ -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
......@@ -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;
......
Supports Markdown
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