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

Adding geometry parser

This commit is contained in:
Axel 2020-01-22 14:17:09 +01:00 committed by Marc Garcia Puig
parent 26d44172a0
commit 1792c68546
4 changed files with 106 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
LibCarla/source/carla/road/Map.cpp
View File

@ -352,12 +352,16 @@ namespace road {
lane_tangent = static_cast<float>(computed_width.second);
}
//log_warning("Got to computetransform, road: ", road.GetId(), " ", road.GetLength());
// get a directed point in s and apply the computed lateral offet
DirectedPoint dp = road.GetDirectedPointIn(waypoint.s);
// compute the tangent of the laneOffset
const auto lane_offset_info = road.GetInfo<RoadInfoLaneOffset>(waypoint.s);
const auto lane_offset_tangent = static_cast<float>(lane_offset_info->GetPolynomial().Tangent(waypoint.s));
float lane_offset_tangent = 0;
if(lane_offset_info){
lane_offset_tangent = static_cast<float>(lane_offset_info->GetPolynomial().Tangent(waypoint.s));
}
lane_tangent -= lane_offset_tangent;

12
LibCarla/source/carla/road/Map.h
View File

@ -14,6 +14,9 @@
#include "carla/road/element/RoadInfoMarkRecord.h"
#include "carla/road/element/Waypoint.h"
//#include <boost/geometry.hpp>
//#include <boost/geometry/geometries/point.hpp>
//#include <boost/geometry/index/rtree.hpp>
#include <boost/optional.hpp>
#include <vector>
@ -132,6 +135,15 @@ private:
friend MapBuilder;
MapData _data;
//namespace bg = boost::geometry;
//namespace bgi = boost::geometry::index;
/*struct rtreecontent{
LaneId lid;
RoadId rid;
};*/
//bgi::rtree rtree;
};
} // namespace road

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

@ -390,48 +390,97 @@ namespace road {
}
void MapBuilder::AddRoadGeometrySpiral(
carla::road::Road * /*road*/,
const double /*s*/,
const double /*x*/,
const double /*y*/,
const double /*hdg*/,
const double /*length*/,
const double /*curvStart*/,
const double /*curvEnd*/) {
throw_exception(std::runtime_error("geometry spiral not supported"));
carla::road::Road * road,
const double s,
const double x,
const double y,
const double hdg,
const double length,
const double curvStart,
const double curvEnd) {
//throw_exception(std::runtime_error("geometry spiral not supported"));
DEBUG_ASSERT(road != nullptr);
const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
auto spiral_geometry = std::make_unique<GeometrySpiral>(
s,
length,
hdg,
location,
curvStart,
curvEnd);
_temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
std::move(spiral_geometry))));
}
void MapBuilder::AddRoadGeometryPoly3(
carla::road::Road * /*road*/,
const double /*s*/,
const double /*x*/,
const double /*y*/,
const double /*hdg*/,
const double /*length*/,
const double /*a*/,
const double /*b*/,
const double /*c*/,
const double /*d*/) {
throw_exception(std::runtime_error("geometry poly3 not supported"));
carla::road::Road * road,
const double s,
const double x,
const double y,
const double hdg,
const double length,
const double a,
const double b,
const double c,
const double d) {
//throw_exception(std::runtime_error("geometry poly3 not supported"));
DEBUG_ASSERT(road != nullptr);
const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
auto poly3_geometry = std::make_unique<GeometryPoly3>(
s,
length,
hdg,
location,
a,
b,
c,
d);
_temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
std::move(poly3_geometry))));
}
void MapBuilder::AddRoadGeometryParamPoly3(
carla::road::Road * /*road*/,
const double /*s*/,
const double /*x*/,
const double /*y*/,
const double /*hdg*/,
const double /*length*/,
const double /*aU*/,
const double /*bU*/,
const double /*cU*/,
const double /*dU*/,
const double /*aV*/,
const double /*bV*/,
const double /*cV*/,
const double /*dV*/,
const std::string /*p_range*/) {
throw_exception(std::runtime_error("geometry poly3 not supported"));
carla::road::Road * road,
const double s,
const double x,
const double y,
const double hdg,
const double length,
const double aU,
const double bU,
const double cU,
const double dU,
const double aV,
const double bV,
const double cV,
const double dV,
const std::string p_range) {
//throw_exception(std::runtime_error("geometry poly3 not supported"));
bool arcLength;
if(p_range == "arcLength"){
arcLength = true;
}else{
arcLength = false;
}
DEBUG_ASSERT(road != nullptr);
const geom::Location location(static_cast<float>(x), static_cast<float>(y), 0.0f);
auto parampoly3_geometry = std::make_unique<GeometryParamPoly3>(
s,
length,
hdg,
location,
aU,
bU,
cU,
dU,
aV,
bV,
cV,
dV,
arcLength);
_temp_road_info_container[road].emplace_back(std::unique_ptr<RoadInfo>(new RoadInfoGeometry(s,
std::move(parampoly3_geometry))));
}
void MapBuilder::AddJunction(const int32_t id, const std::string name) {

6
LibCarla/source/carla/road/Road.cpp
View File

@ -184,8 +184,10 @@ namespace road {
const auto geometry = _info.GetInfo<element::RoadInfoGeometry>(clamped_s);
const auto lane_offset = _info.GetInfo<element::RoadInfoLaneOffset>(clamped_s);
const auto offset = static_cast<float>(lane_offset->GetPolynomial().Evaluate(clamped_s));
float offset = 0;
if(lane_offset){
offset = static_cast<float>(lane_offset->GetPolynomial().Evaluate(clamped_s));
}
// Apply road's lane offset record
element::DirectedPoint p = geometry->GetGeometry().PosFromDist(clamped_s - geometry->GetDistance());
// Unreal's Y axis hack (the minus on the offset)