diff --git a/include/generate_launch.h b/include/generate_launch.h
index 3dbbda01229fa82d2d504f244b7fe5fb914e972a..712fe860a7fb6f4926311ea29f49e5fe0e454748 100644
--- a/include/generate_launch.h
+++ b/include/generate_launch.h
@@ -6,6 +6,7 @@
#include "opendrive_signal.h"
#include <boost/filesystem.hpp>
+#include <cmath>
std::string get_full_name(std::string &path,std::string &file_name,const std::string &ext);
void generate_signals_launch(std::string &path,std::string &signals_file,COpendriveRoad &road);
@@ -57,6 +58,12 @@ 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 LOC_SIGN_TRIANGLE_WIDTH 0.074
+#define LOC_SIGN_TRIANGLE_ANGLE M_PI/2
+#define LOC_SIGN_ID_OFFSET 100
#define PARKED_CAR_LENGTH "0.6"
#define PARKED_CAR_WIDTH "0.25"
diff --git a/src/generate_launch.cpp b/src/generate_launch.cpp
index 95cc3822f7181f5a955c98fc5064c830665d2d93..bf4f5de1f1afe8d07f66060faea7f087a67ab93d 100644
--- a/src/generate_launch.cpp
+++ b/src/generate_launch.cpp
@@ -107,6 +107,11 @@ 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)
+ {
+ name="locsign";
+ marker=LOC_SIGN_TRIANGLE_MARKER;
+ }
else
throw CException(_HERE_,"Unsupported signal " + type);
}
@@ -137,7 +142,7 @@ void generate_signals_launch(std::string &path,std::string &signals_file,COpendr
{
out_file << "<?xml version=\"1.0\"?>" << std::endl << std::endl;
out_file << "<launch>" << std::endl;
- out_file << " <arg name=\"parent\" default=\"map\"/>" << std::endl;
+ out_file << " <arg name=\"parent\" default=\"map\"/>" << std::endl;//opendrive instead of map??
for(unsigned int i=0;i<road.get_num_segments();i++)
{
const COpendriveRoadSegment &segment=road.get_segment(i);
@@ -147,27 +152,68 @@ 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==SEMAPHORE_TYPE)
- out_file << " <include file=\"$(find iri_sign_description)/launch/spawn_semaphore.launch\">" << std::endl;
- else
- out_file << " <include file=\"$(find iri_sign_description)/launch/spawn_sign.launch\">" << std::endl;
- out_file << " <arg name=\"name\" value=\"" << name << "_" << signal.get_id() << "\"/>" << std::endl;
- if(type==SEMAPHORE_TYPE)
+ if (type == LOC_SIGN_TRIANGLE_TYPE)
{
- out_file << " <arg name=\"model\" value=\"semaphore\"/>" << std::endl;
- out_file << " <arg name=\"initial_state\" value=\"True\"/>" << std::endl;
+ 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=\"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;
+ 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;
+
+ out_file << " <arg name=\"x\" value=\"" << world_x << "\"/>" << std::endl;
+ out_file << " <arg name=\"y\" value=\"" << world_y << "\"/>" << std::endl;
+ out_file << " <arg name=\"yaw\" value=\"" << world_yaw << "\"/>" << std::endl;
+ out_file << " <arg name=\"parent\" value=\"$(arg parent)\"/>" << std::endl;
+ 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=\"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;
+ 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;
+
+ out_file << " <arg name=\"x\" value=\"" << world_x << "\"/>" << std::endl;
+ out_file << " <arg name=\"y\" value=\"" << world_y << "\"/>" << std::endl;
+ out_file << " <arg name=\"yaw\" value=\"" << world_yaw << "\"/>" << std::endl;
+ out_file << " <arg name=\"parent\" value=\"$(arg parent)\"/>" << std::endl;
+ out_file << " </include>" << std::endl;
}
- else
+ else
{
- out_file << " <arg name=\"model\" value=\"sign\"/>" << std::endl;
- out_file << " <arg name=\"type\" value=\"" << name << "\"/>" << std::endl;
+ if(type==SEMAPHORE_TYPE)
+ out_file << " <include file=\"$(find iri_sign_description)/launch/spawn_semaphore.launch\">" << std::endl;
+ else
+ out_file << " <include file=\"$(find iri_sign_description)/launch/spawn_sign.launch\">" << std::endl;
+ out_file << " <arg name=\"name\" value=\"" << name << "_" << signal.get_id() << "\"/>" << std::endl;
+ if(type==SEMAPHORE_TYPE)
+ {
+ out_file << " <arg name=\"model\" value=\"semaphore\"/>" << std::endl;
+ out_file << " <arg name=\"initial_state\" value=\"True\"/>" << std::endl;
+ }
+ else
+ {
+ out_file << " <arg name=\"model\" value=\"sign\"/>" << std::endl;
+ out_file << " <arg name=\"type\" value=\"" << name << "\"/>" << 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;
+ out_file << " <arg name=\"yaw\" value=\"" << world.heading << "\"/>" << std::endl;
+ out_file << " <arg name=\"parent\" value=\"$(arg parent)\"/>" << std::endl;
+ out_file << " </include>" << 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;
- out_file << " <arg name=\"yaw\" value=\"" << world.heading << "\"/>" << std::endl;
- out_file << " <arg name=\"parent\" value=\"$(arg parent)\"/>" << std::endl;
- out_file << " </include>" << std::endl;
}
}
out_file << "</launch>" << std::endl;