Added support for the global localization sign.

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

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;