p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added inertial position to parser. Updated osm2odr version.

This commit is contained in:
Axel 2021-04-16 18:33:35 +02:00 committed by bernat
parent e6a23ff101
commit 84b469b59e
4 changed files with 116 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
LibCarla/source/carla/opendrive/parser/SignalParser.cpp
View File

@ -112,6 +112,18 @@ namespace parser {
log_debug("Added dependency to signal ", signal_id, ":", dependency_id, dependency_type);
map_builder.AddDependencyToSignal(signal_id, dependency_id, dependency_type);
}
for (pugi::xml_node position_node : signal_node.children("positionInertial")) {
const double x = position_node.attribute("x").as_double();
const double y = position_node.attribute("y").as_double();
const double z = position_node.attribute("z").as_double();
const double hdg = position_node.attribute("hdg").as_double();
const double inertial_pitch = position_node.attribute("pitch").as_double();
const double inertial_roll = position_node.attribute("roll").as_double();
map_builder.AddSignalPositionInertial(
signal_id,
x, y, z,
hdg, inertial_pitch, inertial_roll);
}
}
for (pugi::xml_node signal_reference_node : signals_node.children("signalReference")) {
const double s_position = signal_reference_node.attribute("s").as_double();

127
LibCarla/source/carla/road/MapBuilder.cpp
View File

@ -235,49 +235,85 @@ namespace road {
}
element::RoadInfoSignal* MapBuilder::AddSignal(
Road* road,
const SignId signal_id,
const double s,
const double t,
const std::string name,
const std::string dynamic,
const std::string orientation,
const double zOffset,
const std::string country,
const std::string type,
const std::string subtype,
const double value,
const std::string unit,
const double height,
const double width,
const std::string text,
const double hOffset,
const double pitch,
const double roll) {
_temp_signal_container[signal_id] = std::make_unique<Signal>(
road->GetId(),
signal_id,
s,
t,
name,
dynamic,
orientation,
zOffset,
country,
type,
subtype,
value,
unit,
height,
width,
text,
hOffset,
pitch,
roll);
element::RoadInfoSignal* MapBuilder::AddSignal(
Road* road,
const SignId signal_id,
const double s,
const double t,
const std::string name,
const std::string dynamic,
const std::string orientation,
const double zOffset,
const std::string country,
const std::string type,
const std::string subtype,
const double value,
const std::string unit,
const double height,
const double width,
const std::string text,
const double hOffset,
const double pitch,
const double roll) {
_temp_signal_container[signal_id] = std::make_unique<Signal>(
road->GetId(),
signal_id,
s,
t,
name,
dynamic,
orientation,
zOffset,
country,
type,
subtype,
value,
unit,
height,
width,
text,
hOffset,
pitch,
roll);
return AddSignalReference(road, signal_id, s, t, orientation);
}
return AddSignalReference(road, signal_id, s, t, orientation);
}
void MapBuilder::AddSignalPositionInertial(
const SignId signal_id,
const double x,
const double y,
const double z,
const double hdg,
const double pitch,
const double roll) {
std::unique_ptr<Signal> &signal = _temp_signal_container[signal_id];
signal->_using_inertial_position = true;
geom::Location location = geom::Location(x, -y, z);
signal->_transform = geom::Transform (location, geom::Rotation(
geom::Math::ToDegrees(static_cast<float>(pitch)),
geom::Math::ToDegrees(static_cast<float>(-hdg)),
geom::Math::ToDegrees(static_cast<float>(roll))));
}
void MapBuilder::AddSignalPositionRoad(
const SignId signal_id,
const RoadId road_id,
const double s,
const double t,
const double zOffset,
const double hOffset,
const double pitch,
const double roll) {
std::unique_ptr<Signal> &signal = _temp_signal_container[signal_id];
signal->_road_id = road_id;
signal->_s = s;
signal->_t = t;
signal->_zOffset = zOffset;
signal->_hOffset = hOffset;
signal->_pitch = pitch;
signal->_roll = roll;
}
element::RoadInfoSignal* MapBuilder::AddSignalReference(
Road* road,
@ -769,8 +805,10 @@ namespace road {
for(auto& signal_pair : _temp_signal_container) {
auto& signal = signal_pair.second;
if (signal->_using_inertial_position) {
continue;
}
auto transform = ComputeSignalTransform(signal, _map_data);
// Hack: compensate RoadRunner displacement (25cm) due to lightbox size
if (SignalType::IsTrafficLight(signal->GetType())) {
transform.location = transform.location +
geom::Location(transform.GetForwardVector()*0.25);
@ -1007,7 +1045,8 @@ void MapBuilder::CreateController(
// workarround to not move speed signals
if (signal->GetName().substr(0, 6) == "Speed_" ||
signal->GetName().substr(0, 6) == "speed_" ||
signal->GetName().find("Stencil_STOP") != std::string::npos) {
signal->GetName().find("Stencil_STOP") != std::string::npos ||
signal->_using_inertial_position) {
continue;
}
if(closest_waypoint_to_signal) {

19
LibCarla/source/carla/road/MapBuilder.h
View File

@ -172,6 +172,25 @@ namespace road {
const double pitch,
const double roll);
void AddSignalPositionInertial(
const SignId signal_id,
const double x,
const double y,
const double z,
const double hdg,
const double pitch,
const double roll);
void AddSignalPositionRoad(
const SignId signal_id,
const RoadId road_id,
const double s,
const double t,
const double zOffset,
const double hOffset,
const double pitch,
const double roll);
element::RoadInfoSignal* AddSignalReference(
Road* road,
const SignId signal_id,

2
LibCarla/source/carla/road/Signal.h
View File

@ -223,6 +223,8 @@ namespace road {
geom::Transform _transform;
std::set<ContId> _controllers;
bool _using_inertial_position = false;
};
} // road