diff --git a/include/adc_circuit.h b/include/adc_circuit.h
index afa5cb63fb85fdb73ba02eb1f26a31fdbeace8a2..05dfe7164d552b39ce7039dbacfa2ef22fee4c55 100644
--- a/include/adc_circuit.h
+++ b/include/adc_circuit.h
@@ -13,6 +13,7 @@
#define SIGNS_LAUNCH_FILE "spawn_signs.launch"
#define OBJECTS_LAUNCH_FILE "spawn_objects.launch"
+#define ROAD_SCALE 10
/**
* \class CAdcCircuit
@@ -94,7 +95,7 @@ class CAdcCircuit
* It follows the same steps to generate the objects launch file
* calling CAdcRoad::append_objects_spwan.
*
- * \param dir The route to the output .launch file.
+ * \param dir The route to the output launch directory with the base name for both launch files.
*
* \throws CException If the output file can't be opened.
*/
diff --git a/include/adc_geometries.h b/include/adc_geometries.h
index e398c7c792a6b56fd44e72a7d8733b9c0c9b9912..a2ec97bc9778276918e069a2dbaa2afcdeefd54b 100644
--- a/include/adc_geometries.h
+++ b/include/adc_geometries.h
@@ -133,10 +133,12 @@ class CAdcGeometry
*
* \param s Track coordenate "s" of the element.
*
+ * \param last_road_geometry Bool to set the end of the road on range.
+ *
* \return If belongs or not.
*
*/
- bool on_range(double s);
+ bool on_range(double s, bool last_road_geometry);
/**
* \brief Function to print its info.
diff --git a/include/adc_object.h b/include/adc_object.h
index 65492234afd4270a9af65842a5f659b65dc66883..634bfe697eccabe7c4f36cfe1e628fb4a582668c 100644
--- a/include/adc_object.h
+++ b/include/adc_object.h
@@ -267,8 +267,10 @@ class CAdcObject
*
* \param filename The route to the output .launch file.
*
+ * \param road_scale The scale of the road to divide by the objects poses.
+ *
*/
- void append_objects_spawn(std::string &filename);
+ void append_objects_spawn(std::string &filename, double road_scale);
/**
* \brief Function to load the .xodr info.
diff --git a/include/adc_road.h b/include/adc_road.h
index b19be20fc2c9de7b0b76b7ba0817ec29c592d162..37f8e7cd7250d652c6dad273523725b4064580e6 100644
--- a/include/adc_road.h
+++ b/include/adc_road.h
@@ -187,7 +187,7 @@ class CAdcRoad
* \param filename The route to the output .launch file.
*
*/
- void append_signs_spawn(std::string &filename);
+ void append_signs_spawn(std::string &filename, double road_scale);
/**
* \brief Function to append each object spwan lines on a .launch file.
@@ -196,8 +196,10 @@ class CAdcRoad
*
* \param filename The route to the output .launch file.
*
+ * \param road_scale The sclae of the road to divide by the objects and signals poses.
+ *
*/
- void append_objects_spawn(std::string &filename);
+ void append_objects_spawn(std::string &filename, double road_scale);
/**
* \brief Function to load the .xodr info.
diff --git a/include/adc_signals.h b/include/adc_signals.h
index 5e02050aadd74333e9d1d89f1d71be0a85ad885e..7b37d80ac12215f1959866bd1693501c44789e46 100644
--- a/include/adc_signals.h
+++ b/include/adc_signals.h
@@ -196,8 +196,10 @@ class CAdcSignals
*
* \param filename The route to the output .launch file.
*
+ * \param road_scale The scale of the road to divide by the objects poses.
+ *
*/
- void append_sign_spawn(std::string &filename);
+ void append_sign_spawn(std::string &filename, double road_scale);
/**
* \brief Function to load the .xodr info.
diff --git a/include/opendrive_to_gazebo.h b/include/opendrive_to_gazebo.h
index a7f108c8b5a4ce44a6e49fdca828fe9379c2ddad..93f3239e241f9d1abb4163753cf518cfe4911024 100644
--- a/include/opendrive_to_gazebo.h
+++ b/include/opendrive_to_gazebo.h
@@ -61,7 +61,7 @@ class COpenDriveFormat
*
* It calls CAdcCircuit::generate_spawn_launch_files function.
*
- * \param dir The route to the output launch directory.
+ * \param dir The route to the output launch directory with the base name for both launch files.
*
*/
void generate_spawn_launch_files(std::string &dir);
diff --git a/src/adc_circuit.cpp b/src/adc_circuit.cpp
index cf7b6a08841825c7b78c789fb594c9e12561b9b2..037482bc154cba24e6fef2bb31111a06d0deccfc 100644
--- a/src/adc_circuit.cpp
+++ b/src/adc_circuit.cpp
@@ -50,8 +50,8 @@ void CAdcCircuit::get_signals(std::vector<CAdcSignals*> &signals)
void CAdcCircuit::generate_spawn_launch_files(std::string &dir)
{
- std::string signs_filename = dir + SIGNS_LAUNCH_FILE;
- std::string objects_filename = dir + OBJECTS_LAUNCH_FILE;
+ std::string signs_filename = dir + "_" + SIGNS_LAUNCH_FILE;
+ std::string objects_filename = dir + "_" + OBJECTS_LAUNCH_FILE;
std::remove(signs_filename.c_str());
std::remove(objects_filename.c_str());
@@ -77,11 +77,11 @@ void CAdcCircuit::generate_spawn_launch_files(std::string &dir)
//sign info
for (unsigned int i = 0; i < this->roads.size(); i++)
- this->roads[i]->append_signs_spawn(signs_filename);
+ this->roads[i]->append_signs_spawn(signs_filename, ROAD_SCALE);
//object info
for (unsigned int i = 0; i < this->roads.size(); i++)
- this->roads[i]->append_objects_spawn(objects_filename);
+ this->roads[i]->append_objects_spawn(objects_filename, ROAD_SCALE);
//End
oss.str("");
diff --git a/src/adc_geometries.cpp b/src/adc_geometries.cpp
index 3a351000ff956107c28427fa0f731103191ca82a..32fafeb7fede3c179551c5346df58d557b842ea4 100644
--- a/src/adc_geometries.cpp
+++ b/src/adc_geometries.cpp
@@ -61,9 +61,9 @@ bool CAdcGeometry::get_world_pose(double s, double t, double heading, double &x,
return false;
}
-bool CAdcGeometry::on_range(double s)
+bool CAdcGeometry::on_range(double s, bool last_road_geometry)
{
- if ((s < this->max_s) && (s >= this->min_s))
+ if ((last_road_geometry ? (s <= this->max_s): (s < this->max_s)) && (s >= this->min_s))
return true;
return false;
}
diff --git a/src/adc_object.cpp b/src/adc_object.cpp
index 65419ffd944bb6a563cd3f45753aaed282ba6c05..a9a114c180b0e9536240626a65c8a04775c6f1d4 100644
--- a/src/adc_object.cpp
+++ b/src/adc_object.cpp
@@ -280,7 +280,7 @@ void CAdcObject::debug(bool world_pose)
}
}
-void CAdcObject::append_objects_spawn(std::string &filename)
+void CAdcObject::append_objects_spawn(std::string &filename, double road_scale)
{
if (this->repeat || this->outline)
{
@@ -289,20 +289,20 @@ void CAdcObject::append_objects_spawn(std::string &filename)
}
std::ostringstream oss;
oss << std::endl << " <include file=\"$(find iri_object_description)/launch/spawn_object.launch\">" << std::endl;
- oss << " <arg name=\"name\" value=\"" << this->name << this->id << "\"/>" << std::endl;
+ oss << " <arg name=\"name\" value=\"" << this->name << "_" << this->id << "\"/>" << std::endl;
oss << " <arg name=\"model\" value=\"" << (this->radius != 0.0 ? "cyl": "box") << "\"/>" << std::endl;
if (this->radius != 0.0)
- oss << " <arg name=\"radius\" value=\"" << this->radius << "\"/>" << std::endl;
+ oss << " <arg name=\"radius\" value=\"" << this->radius/road_scale << "\"/>" << std::endl;
else
{
- oss << " <arg name=\"length\" value=\"" << this->length << "\"/>" << std::endl;
- oss << " <arg name=\"width\" value=\"" << this->width << "\"/>" << std::endl;
+ oss << " <arg name=\"length\" value=\"" << this->length/road_scale << "\"/>" << std::endl;
+ oss << " <arg name=\"width\" value=\"" << this->width/road_scale << "\"/>" << std::endl;
}
- oss << " <arg name=\"height\" value=\"" << this->height << "\"/>" << std::endl;
- oss << " <arg name=\"x\" value=\"" << this->world_x << "\"/>" << std::endl;
- oss << " <arg name=\"y\" value=\"" << this->world_y << "\"/>" << std::endl;
+ oss << " <arg name=\"height\" value=\"" << this->height/road_scale << "\"/>" << std::endl;
+ oss << " <arg name=\"x\" value=\"" << this->world_x/road_scale << "\"/>" << std::endl;
+ oss << " <arg name=\"y\" value=\"" << this->world_y/road_scale << "\"/>" << std::endl;
oss << " <arg name=\"yaw\" value=\"" << this->world_yaw << "\"/>" << std::endl;
oss << " <arg name=\"parent\" value=\"$(arg parent)\"/>" << std::endl;
diff --git a/src/adc_road.cpp b/src/adc_road.cpp
index 230f438a12fda7435c431b0f009ab0de69f29965..c32a333526be2a0e3c248cfaeb5ba7eae8dcd14f 100644
--- a/src/adc_road.cpp
+++ b/src/adc_road.cpp
@@ -124,15 +124,22 @@ void CAdcRoad::calculate_signals_pose(bool debug)
for (unsigned int i = 0; i < this->signals.size(); i++)
{
this->signals[i]->get_pose(s, t, heading);
- for (unsigned int j = 0; j < this->geometries.size(); j++)
+ unsigned int j = 0;
+ for (; j < this->geometries.size(); j++)
{
- if (this->geometries[j]->on_range(s))
+ if (this->geometries[j]->on_range(s, j==this->geometries.size()-1))
{
this->geometries[j]->get_world_pose(s, t, heading, x, y, yaw);
this->signals[i]->set_world_pose(x, y, yaw);
break;
}
}
+ if (j == this->geometries.size())
+ {
+ std::cout << "WARNING: Sign out of road: " << std::endl;
+ std::cout << "Road " << this->name << " id = " << this->id << "; length = " << this->length << std::endl;
+ this->signals[i]->debug(true);
+ }
if (debug)
{
std::cout << "Road " << this->name << " id = " << this->id << "; length = " << this->length << std::endl;
@@ -154,15 +161,22 @@ void CAdcRoad::calculate_objects_pose(bool debug)
{
for (unsigned int l = 0; l < corners[j].size(); l++)
{
- for (unsigned int k = 0; k < this->geometries.size(); k++)
+ unsigned int k = 0;
+ for (; k < this->geometries.size(); k++)
{
- if (this->geometries[k]->on_range(corners[j][l].s))
+ if (this->geometries[k]->on_range(corners[j][l].s, k==this->geometries.size()-1))
{
this->geometries[k]->get_world_pose(corners[j][l].s, corners[j][l].t, 0.0, x, y, yaw);
this->objects[i]->add_corner_world_pose(j, l, x, y);
break;
}
}
+ if (k == this->geometries.size())
+ {
+ std::cout << "WARNING: Object out of road: " << std::endl;
+ std::cout << "Road " << this->name << " id = " << this->id << "; length = " << this->length << std::endl;
+ this->objects[i]->debug(true);
+ }
}
}
}
@@ -175,15 +189,22 @@ void CAdcRoad::calculate_objects_pose(bool debug)
for (unsigned int l = 0; l < repeat_info[j].n; l++)
{
double s = repeat_info[j].s + repeat_info[j].distance*l;
- for (unsigned int k = 0; k < this->geometries.size(); k++)
+ unsigned int k = 0;
+ for (; k < this->geometries.size(); k++)
{
- if (this->geometries[k]->on_range(s))
+ if (this->geometries[k]->on_range(s, k==this->geometries.size()-1))
{
this->geometries[k]->get_world_pose(s, repeat_info[j].t, repeat_info[j].heading, x, y, yaw);
this->objects[i]->add_repeat_info_world_pose(j, x, y, yaw);
break;
}
}
+ if (k == this->geometries.size())
+ {
+ std::cout << "WARNING: Object out of road: " << std::endl;
+ std::cout << "Road " << this->name << " id = " << this->id << "; length = " << this->length << std::endl;
+ this->objects[i]->debug(true);
+ }
}
}
}
@@ -191,15 +212,22 @@ void CAdcRoad::calculate_objects_pose(bool debug)
{
double s, t, heading;
this->objects[i]->get_pose(s, t, heading);
- for (unsigned int j = 0; j < this->geometries.size(); j++)
+ unsigned int j = 0;
+ for (; j < this->geometries.size(); j++)
{
- if (this->geometries[j]->on_range(s))
+ if (this->geometries[j]->on_range(s, j==this->geometries.size()-1))
{
this->geometries[j]->get_world_pose(s, t, heading, x, y, yaw);
this->objects[i]->set_world_pose(x, y, yaw);
break;
}
}
+ if (j == this->geometries.size())
+ {
+ std::cout << "WARNING: Object out of road: " << std::endl;
+ std::cout << "Road " << this->name << " id = " << this->id << "; length = " << this->length << std::endl;
+ this->objects[i]->debug(true);
+ }
}
if (debug)
{
@@ -215,16 +243,16 @@ void CAdcRoad::get_signals(std::vector<CAdcSignals*> &signals)
signals.push_back(this->signals[i]);
}
-void CAdcRoad::append_signs_spawn(std::string &filename)
+void CAdcRoad::append_signs_spawn(std::string &filename, double road_scale)
{
for (unsigned int i = 0; i < this->signals.size(); i++)
- this->signals[i]->append_sign_spawn(filename);
+ this->signals[i]->append_sign_spawn(filename, road_scale);
}
-void CAdcRoad::append_objects_spawn(std::string &filename)
+void CAdcRoad::append_objects_spawn(std::string &filename, double road_scale)
{
for (unsigned int i = 0; i < this->objects.size(); i++)
- this->objects[i]->append_objects_spawn(filename);
+ this->objects[i]->append_objects_spawn(filename,road_scale);
}
void CAdcRoad::load(std::unique_ptr<OpenDRIVE::road_type> &road_info)
diff --git a/src/adc_signals.cpp b/src/adc_signals.cpp
index 0bbee83ebc0782fa41239becf229b7333fac19e5..51e09a35b5d4ea4adaadd5ecdac0de2391d669d5 100644
--- a/src/adc_signals.cpp
+++ b/src/adc_signals.cpp
@@ -179,7 +179,7 @@ void CAdcSignals::debug(bool world_pose)
std::cout << " world pose: x = " << this->world_x << ", y = " << this->world_y << ", yaw = " << this->world_yaw << std::endl;
}
-void CAdcSignals::append_sign_spawn(std::string &filename)
+void CAdcSignals::append_sign_spawn(std::string &filename, double road_scale)
{
std::map<std::string, Sign_urdf_info>::iterator it = urdf_map.end();
@@ -200,12 +200,12 @@ void CAdcSignals::append_sign_spawn(std::string &filename)
std::ostringstream oss;
oss << std::endl << " <include file=\"$(find sign_description)/launch/spawn_sign.launch\">" << std::endl;
- oss << " <arg name=\"name\" value=\"" << it->second.type << this->id << "\"/>" << std::endl;
+ oss << " <arg name=\"name\" value=\"" << it->second.type << "_" << this->id << "\"/>" << std::endl;
oss << " <arg name=\"model\" value=\"" << (this->type == SEMAPHORE_TYPE ? "semaphore": "sign") << "\"/>" << std::endl;
oss << " <arg name=\"tag\" value=\"" << it->second.marker << "\"/>" << std::endl;
oss << " <arg name=\"type\" value=\"" << it->second.type << "\"/>" << std::endl;
- oss << " <arg name=\"x\" value=\"" << this->world_x << "\"/>" << std::endl;
- oss << " <arg name=\"y\" value=\"" << this->world_y << "\"/>" << std::endl;
+ oss << " <arg name=\"x\" value=\"" << this->world_x/road_scale << "\"/>" << std::endl;
+ oss << " <arg name=\"y\" value=\"" << this->world_y/road_scale << "\"/>" << std::endl;
oss << " <arg name=\"yaw\" value=\"" << this->world_yaw << "\"/>" << std::endl;
oss << " <arg name=\"parent\" value=\"$(arg parent)\"/>" << std::endl;
oss << " </include>" << std::endl;
diff --git a/src/examples/opendrive_to_gazebo_test.cpp b/src/examples/opendrive_to_gazebo_test.cpp
index ceb67157d8205306d2ebe3e7a769716570a24dac..ae20ae6358e2c513b7927152fd7eabcfb301b0e3 100644
--- a/src/examples/opendrive_to_gazebo_test.cpp
+++ b/src/examples/opendrive_to_gazebo_test.cpp
@@ -6,10 +6,22 @@
int main(int argc, char *argv[])
{
+ if (argc != 3)
+ {
+ std::cerr << "ERROR: Missing input arguments.The usage of this program is:" << std::endl;
+ std::cerr << "opendrive_to_gazebo_test <xodr input file path> <output launch file direcctory>" << std::endl;
+ return -1;
+ }
COpenDriveFormat open_drive_format;
- std::string xml_file = "../src/xml/atlatec_generic.xodr";
- std::string launch_dir = "../../../../iri_ws/src/model_car_simulator/sign_description/launch/";
- // std::string xml_file = "../src/xml/atlatec_vires.xodr";
+ std::string xml_file = argv[1];
+ std::string launch_dir = argv[2];
+ int slash_index, point_index;
+ slash_index = xml_file.find_last_of("/");
+ point_index = xml_file.find_last_of(".");
+ std::string file_name = xml_file.substr(xml_file.find_last_of("/")+1, point_index - slash_index -1);
+ std::cout << "file name " << file_name << std::endl;
+ std::string launch_base_name = launch_dir + file_name;
+
try
{
open_drive_format.load(xml_file, false);
@@ -19,7 +31,7 @@ int main(int argc, char *argv[])
// open_drive_format.get_signals(signals);
// for (unsigned int i = 0; i < signals.size(); i++)
// signals[i]->debug(true);
- open_drive_format.generate_spawn_launch_files(launch_dir);
+ open_drive_format.generate_spawn_launch_files(launch_base_name);
}
catch (CException &e)
{