Move third-party code in LibCarla to a third-party folder
This commit is contained in:
parent
212281d22b
commit
ab2f6f5bb6
|
@ -10,8 +10,10 @@ message(STATUS "Build release: ${LIBCARLA_BUILD_RELEASE}")
|
|||
message(STATUS "Build test: ${LIBCARLA_BUILD_TEST}")
|
||||
|
||||
set(libcarla_source_path "${PROJECT_SOURCE_DIR}/../source")
|
||||
set(libcarla_source_thirdparty_path "${libcarla_source_path}/third-party")
|
||||
|
||||
include_directories(${libcarla_source_path})
|
||||
include_directories(${libcarla_source_thirdparty_path})
|
||||
|
||||
if (CARLA_VERSION)
|
||||
configure_file(${libcarla_source_path}/carla/Version.h.in ${libcarla_source_path}/carla/Version.h)
|
||||
|
|
|
@ -72,12 +72,11 @@ file(GLOB libcarla_carla_opendrive_parser_sources
|
|||
set(libcarla_sources "${libcarla_sources};${libcarla_carla_opendrive_parser_sources}")
|
||||
install(FILES ${libcarla_carla_opendrive_parser_sources} DESTINATION include/carla/opendrive/parser)
|
||||
|
||||
file(GLOB libcarla_carla_opendrive_parser_pugixml_sources
|
||||
"${libcarla_source_path}/carla/opendrive/parser/pugixml/*.cpp"
|
||||
"${libcarla_source_path}/carla/opendrive/parser/pugixml/*.hpp"
|
||||
"${libcarla_source_path}/carla/opendrive/parser/pugixml/*.h")
|
||||
set(libcarla_sources "${libcarla_sources};${libcarla_carla_opendrive_parser_pugixml_sources}")
|
||||
install(FILES ${libcarla_carla_opendrive_parser_pugixml_sources} DESTINATION include/carla/opendrive/parser/pugixml)
|
||||
file(GLOB libcarla_pugixml_sources
|
||||
"${libcarla_source_thirdparty_path}/pugixml/*.cpp"
|
||||
"${libcarla_source_thirdparty_path}/pugixml/*.hpp"
|
||||
"${libcarla_source_thirdparty_path}/pugixml/*.h")
|
||||
set(libcarla_sources "${libcarla_sources};${libcarla_pugixml_sources}")
|
||||
|
||||
file(GLOB libcarla_carla_pointcloud_sources
|
||||
"${libcarla_source_path}/carla/pointcloud/*.cpp"
|
||||
|
@ -101,11 +100,10 @@ file(GLOB libcarla_carla_road_element_sources
|
|||
set(libcarla_sources "${libcarla_sources};${libcarla_carla_road_element_sources}")
|
||||
install(FILES ${libcarla_carla_road_element_sources} DESTINATION include/carla/road/element)
|
||||
|
||||
file(GLOB libcarla_carla_road_element_cephes_sources
|
||||
"${libcarla_source_path}/carla/road/element/cephes/*.cpp"
|
||||
"${libcarla_source_path}/carla/road/element/cephes/*.h")
|
||||
set(libcarla_sources "${libcarla_sources};${libcarla_carla_road_element_cephes_sources}")
|
||||
install(FILES ${libcarla_carla_road_element_cephes_sources} DESTINATION include/carla/road/element/cephes)
|
||||
file(GLOB libcarla_carla_cephes_sources
|
||||
"${libcarla_source_thirdparty_path}/cephes/*.cpp"
|
||||
"${libcarla_source_thirdparty_path}/cephes/*.h")
|
||||
set(libcarla_sources "${libcarla_sources};${libcarla_carla_cephes_sources}")
|
||||
|
||||
file(GLOB libcarla_carla_road_general_sources
|
||||
"${libcarla_source_path}/carla/road/general/*.cpp"
|
||||
|
@ -174,7 +172,7 @@ set(libcarla_sources "${libcarla_sources};${libcarla_carla_streaming_low_level_s
|
|||
install(FILES ${libcarla_carla_streaming_low_level_sources} DESTINATION include/carla/streaming/low_level)
|
||||
|
||||
file(GLOB libcarla_moodycamel_sources
|
||||
"${libcarla_source_path}/moodycamel/*.h")
|
||||
"${libcarla_source_thirdparty_path}/moodycamel/*.h")
|
||||
set(libcarla_sources "${libcarla_sources};${libcarla_moodycamel_sources}")
|
||||
install(FILES ${libcarla_moodycamel_sources} DESTINATION include/moodycamel)
|
||||
|
||||
|
|
|
@ -26,9 +26,6 @@ install(FILES ${libcarla_carla_opendrive} DESTINATION include/carla/opendrive)
|
|||
file(GLOB libcarla_carla_opendrive_parser "${libcarla_source_path}/carla/opendrive/parser/*.h")
|
||||
install(FILES ${libcarla_carla_opendrive_parser} DESTINATION include/carla/opendrive/parser)
|
||||
|
||||
file(GLOB libcarla_carla_opendrive_parser_pugixml "${libcarla_source_path}/carla/opendrive/parser/pugixml/*.hpp")
|
||||
install(FILES ${libcarla_carla_opendrive_parser_pugixml} DESTINATION include/carla/opendrive/parser/pugixml)
|
||||
|
||||
file(GLOB libcarla_carla_profiler_headers "${libcarla_source_path}/carla/profiler/*.h")
|
||||
install(FILES ${libcarla_carla_profiler_headers} DESTINATION include/carla/profiler)
|
||||
|
||||
|
@ -38,9 +35,6 @@ install(FILES ${libcarla_carla_road_headers} DESTINATION include/carla/road)
|
|||
file(GLOB libcarla_carla_road_element_headers "${libcarla_source_path}/carla/road/element/*.h")
|
||||
install(FILES ${libcarla_carla_road_element_headers} DESTINATION include/carla/road/element)
|
||||
|
||||
file(GLOB libcarla_carla_road_element_cephes_headers "${libcarla_source_path}/carla/road/element/cephes/*.h")
|
||||
install(FILES ${libcarla_carla_road_element_cephes_headers} DESTINATION include/carla/road/element/cephes)
|
||||
|
||||
file(GLOB libcarla_carla_road_general_headers "${libcarla_source_path}/carla/road/general/*.h")
|
||||
install(FILES ${libcarla_carla_road_general_headers} DESTINATION include/carla/road/general)
|
||||
|
||||
|
@ -96,14 +90,10 @@ file(GLOB libcarla_server_sources
|
|||
"${libcarla_source_path}/carla/opendrive/*.h"
|
||||
"${libcarla_source_path}/carla/opendrive/parser/*.cpp"
|
||||
"${libcarla_source_path}/carla/opendrive/parser/*.h"
|
||||
"${libcarla_source_path}/carla/opendrive/parser/pugixml/*.cpp"
|
||||
"${libcarla_source_path}/carla/opendrive/parser/pugixml/*.hpp"
|
||||
"${libcarla_source_path}/carla/road/*.cpp"
|
||||
"${libcarla_source_path}/carla/road/*.h"
|
||||
"${libcarla_source_path}/carla/road/element/*.cpp"
|
||||
"${libcarla_source_path}/carla/road/element/*.h"
|
||||
"${libcarla_source_path}/carla/road/element/cephes/*.cpp"
|
||||
"${libcarla_source_path}/carla/road/element/cephes/*.h"
|
||||
"${libcarla_source_path}/carla/road/general/*.cpp"
|
||||
"${libcarla_source_path}/carla/road/general/*.h"
|
||||
"${libcarla_source_path}/carla/road/object/*.cpp"
|
||||
|
@ -120,7 +110,13 @@ file(GLOB libcarla_server_sources
|
|||
"${libcarla_source_path}/carla/streaming/detail/*.h"
|
||||
"${libcarla_source_path}/carla/streaming/detail/tcp/*.cpp"
|
||||
"${libcarla_source_path}/carla/streaming/detail/tcp/*.h"
|
||||
"${libcarla_source_path}/carla/streaming/low_level/*.h")
|
||||
"${libcarla_source_path}/carla/streaming/low_level/*.h"
|
||||
"${libcarla_source_thirdparty_path}/cephes/*.cpp"
|
||||
"${libcarla_source_thirdparty_path}/cephes/*.h"
|
||||
"${libcarla_source_thirdparty_path}/moodycamel/*.cpp"
|
||||
"${libcarla_source_thirdparty_path}/moodycamel/*.h"
|
||||
"${libcarla_source_thirdparty_path}/pugixml/*.cpp"
|
||||
"${libcarla_source_thirdparty_path}/pugixml/*.hpp")
|
||||
|
||||
# ==============================================================================
|
||||
# Create targets for debug and release in the same build type.
|
||||
|
|
|
@ -16,9 +16,10 @@
|
|||
#include "carla/opendrive/parser/RoadParser.h"
|
||||
#include "carla/opendrive/parser/SignalParser.h"
|
||||
#include "carla/opendrive/parser/TrafficGroupParser.h"
|
||||
#include "carla/opendrive/parser/pugixml/pugixml.hpp"
|
||||
#include "carla/road/MapBuilder.h"
|
||||
|
||||
#include <pugixml/pugixml.hpp>
|
||||
|
||||
namespace carla {
|
||||
namespace opendrive {
|
||||
|
||||
|
|
|
@ -9,9 +9,10 @@
|
|||
#include "carla/Logging.h"
|
||||
#include "carla/StringUtil.h"
|
||||
#include "carla/geom/GeoLocation.h"
|
||||
#include "carla/opendrive/parser/pugixml/pugixml.hpp"
|
||||
#include "carla/road/MapBuilder.h"
|
||||
|
||||
#include <pugixml/pugixml.hpp>
|
||||
|
||||
#include <limits>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
|
|
@ -6,10 +6,10 @@
|
|||
|
||||
#include "carla/opendrive/parser/GeometryParser.h"
|
||||
|
||||
#include "carla/opendrive/parser/pugixml/pugixml.hpp"
|
||||
|
||||
#include "carla/road/MapBuilder.h"
|
||||
|
||||
#include <pugixml/pugixml.hpp>
|
||||
|
||||
namespace carla {
|
||||
namespace opendrive {
|
||||
namespace parser {
|
||||
|
|
|
@ -6,10 +6,10 @@
|
|||
|
||||
#include "carla/opendrive/parser/JunctionParser.h"
|
||||
|
||||
#include "carla/opendrive/parser/pugixml/pugixml.hpp"
|
||||
|
||||
#include "carla/road/MapBuilder.h"
|
||||
|
||||
#include <pugixml/pugixml.hpp>
|
||||
|
||||
namespace carla {
|
||||
namespace opendrive {
|
||||
namespace parser {
|
||||
|
|
|
@ -6,10 +6,10 @@
|
|||
|
||||
#include "carla/opendrive/parser/LaneParser.h"
|
||||
|
||||
#include "carla/opendrive/parser/pugixml/pugixml.hpp"
|
||||
|
||||
#include "carla/road/MapBuilder.h"
|
||||
|
||||
#include <pugixml/pugixml.hpp>
|
||||
|
||||
namespace carla {
|
||||
namespace opendrive {
|
||||
namespace parser {
|
||||
|
|
|
@ -6,10 +6,10 @@
|
|||
|
||||
#include "carla/opendrive/parser/ObjectParser.h"
|
||||
|
||||
#include "carla/opendrive/parser/pugixml/pugixml.hpp"
|
||||
|
||||
#include "carla/road/MapBuilder.h"
|
||||
|
||||
#include <pugixml/pugixml.hpp>
|
||||
|
||||
namespace carla {
|
||||
namespace opendrive {
|
||||
namespace parser {
|
||||
|
|
|
@ -6,10 +6,10 @@
|
|||
|
||||
#include "carla/opendrive/parser/ProfilesParser.h"
|
||||
|
||||
#include "carla/opendrive/parser/pugixml/pugixml.hpp"
|
||||
|
||||
#include "carla/road/MapBuilder.h"
|
||||
|
||||
#include <pugixml/pugixml.hpp>
|
||||
|
||||
namespace carla {
|
||||
namespace opendrive {
|
||||
namespace parser {
|
||||
|
|
|
@ -8,10 +8,11 @@
|
|||
|
||||
#include "carla/Logging.h"
|
||||
#include "carla/StringUtil.h"
|
||||
#include "carla/opendrive/parser/pugixml/pugixml.hpp"
|
||||
#include "carla/road/MapBuilder.h"
|
||||
#include "carla/road/RoadTypes.h"
|
||||
|
||||
#include <pugixml/pugixml.hpp>
|
||||
|
||||
namespace carla {
|
||||
namespace opendrive {
|
||||
namespace parser {
|
||||
|
|
|
@ -6,10 +6,10 @@
|
|||
|
||||
#include "carla/opendrive/parser/SignalParser.h"
|
||||
|
||||
#include "carla/opendrive/parser/pugixml/pugixml.hpp"
|
||||
|
||||
#include "carla/road/MapBuilder.h"
|
||||
|
||||
#include <pugixml/pugixml.hpp>
|
||||
|
||||
namespace carla {
|
||||
namespace opendrive {
|
||||
namespace parser {
|
||||
|
|
|
@ -6,10 +6,10 @@
|
|||
|
||||
#include "carla/opendrive/parser/TrafficGroupParser.h"
|
||||
|
||||
#include "carla/opendrive/parser/pugixml/pugixml.hpp"
|
||||
|
||||
#include "carla/road/MapBuilder.h"
|
||||
|
||||
#include <pugixml/pugixml.hpp>
|
||||
|
||||
namespace carla {
|
||||
namespace opendrive {
|
||||
namespace parser {
|
||||
|
|
|
@ -0,0 +1,85 @@
|
|||
// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma
|
||||
// de Barcelona (UAB).
|
||||
//
|
||||
// This work is licensed under the terms of the MIT license.
|
||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||
|
||||
#include "carla/road/element/Geometry.h"
|
||||
|
||||
#include "carla/Debug.h"
|
||||
#include "carla/Exception.h"
|
||||
#include "carla/geom/Location.h"
|
||||
#include "carla/geom/Math.h"
|
||||
|
||||
#include <cephes/fresnel.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace carla {
|
||||
namespace road {
|
||||
namespace element {
|
||||
|
||||
void DirectedPoint::ApplyLateralOffset(float lateral_offset) {
|
||||
/// @todo Z axis??
|
||||
auto normal_x = std::sin(static_cast<float>(tangent));
|
||||
auto normal_y = -std::cos(static_cast<float>(tangent));
|
||||
location.x += lateral_offset * normal_x;
|
||||
location.y += lateral_offset * normal_y;
|
||||
}
|
||||
|
||||
DirectedPoint GeometryLine::PosFromDist(double dist) const {
|
||||
DEBUG_ASSERT(_length > 0.0);
|
||||
dist = geom::Math::Clamp(dist, 0.0, _length);
|
||||
DirectedPoint p(_start_position, _heading);
|
||||
p.location.x += static_cast<float>(dist * std::cos(p.tangent));
|
||||
p.location.y += static_cast<float>(dist * std::sin(p.tangent));
|
||||
return p;
|
||||
}
|
||||
|
||||
DirectedPoint GeometryArc::PosFromDist(double dist) const {
|
||||
dist = geom::Math::Clamp(dist, 0.0, _length);
|
||||
DEBUG_ASSERT(_length > 0.0);
|
||||
DEBUG_ASSERT(std::fabs(_curvature) > 1e-15);
|
||||
const double radius = 1.0 / _curvature;
|
||||
constexpr double pi_half = geom::Math::Pi<double>() / 2.0;
|
||||
DirectedPoint p(_start_position, _heading);
|
||||
p.location.x += static_cast<float>(radius * std::cos(p.tangent + pi_half));
|
||||
p.location.y += static_cast<float>(radius * std::sin(p.tangent + pi_half));
|
||||
p.tangent += dist * _curvature;
|
||||
p.location.x -= static_cast<float>(radius * std::cos(p.tangent + pi_half));
|
||||
p.location.y -= static_cast<float>(radius * std::sin(p.tangent + pi_half));
|
||||
return p;
|
||||
}
|
||||
|
||||
DirectedPoint GeometrySpiral::PosFromDist(double dist) const {
|
||||
// not working yet with negative values
|
||||
dist = geom::Math::Clamp(dist, 0.0, _length);
|
||||
DEBUG_ASSERT(_length > 0.0);
|
||||
DEBUG_ASSERT(std::fabs(_curve_end) > 1e-15);
|
||||
const double radius = 1.0 / _curve_end;
|
||||
const double extra_norm = 1.0 / std::sqrt(geom::Math::Pi<double>() / 2.0);
|
||||
const double norm = 1.0 / std::sqrt(2.0 * radius * _length);
|
||||
const double length = dist * norm;
|
||||
double S, C;
|
||||
fresnl(length * extra_norm, &S, &C);
|
||||
S /= (norm * extra_norm);
|
||||
C /= (norm * extra_norm);
|
||||
DirectedPoint p(_start_position, _heading);
|
||||
const double cos_a = std::cos(p.tangent);
|
||||
const double sin_a = std::sin(p.tangent);
|
||||
p.location.x += static_cast<float>(C * cos_a - S * sin_a);
|
||||
p.location.y += static_cast<float>(S * cos_a + C * sin_a);
|
||||
p.tangent += length * length;
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
/// @todo
|
||||
std::pair<float, float> GeometrySpiral::DistanceTo(const geom::Location &) const {
|
||||
throw_exception(std::runtime_error("not implemented"));
|
||||
}
|
||||
|
||||
} // namespace element
|
||||
} // namespace road
|
||||
} // namespace carla
|
|
@ -6,14 +6,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "carla/Debug.h"
|
||||
#include "carla/Exception.h"
|
||||
#include "carla/geom/Location.h"
|
||||
#include "carla/geom/Math.h"
|
||||
#include "carla/road/element/cephes/fresnel.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace carla {
|
||||
namespace road {
|
||||
|
@ -41,13 +35,7 @@ namespace element {
|
|||
double tangent = 0.0; // [radians]
|
||||
double pitch = 0.0; // [radians]
|
||||
|
||||
void ApplyLateralOffset(float lateral_offset) {
|
||||
/// @todo Z axis??
|
||||
auto normal_x = std::sin(static_cast<float>(tangent));
|
||||
auto normal_y = -std::cos(static_cast<float>(tangent));
|
||||
location.x += lateral_offset * normal_x;
|
||||
location.y += lateral_offset * normal_y;
|
||||
}
|
||||
void ApplyLateralOffset(float lateral_offset);
|
||||
|
||||
friend bool operator==(const DirectedPoint &lhs, const DirectedPoint &rhs) {
|
||||
return (lhs.location == rhs.location) && (lhs.tangent == rhs.tangent);
|
||||
|
@ -116,14 +104,7 @@ namespace element {
|
|||
const geom::Location &start_pos)
|
||||
: Geometry(GeometryType::LINE, start_offset, length, heading, start_pos) {}
|
||||
|
||||
DirectedPoint PosFromDist(double dist) const override {
|
||||
DEBUG_ASSERT(_length > 0.0);
|
||||
dist = geom::Math::Clamp(dist, 0.0, _length);
|
||||
DirectedPoint p(_start_position, _heading);
|
||||
p.location.x += static_cast<float>(dist * std::cos(p.tangent));
|
||||
p.location.y += static_cast<float>(dist * std::sin(p.tangent));
|
||||
return p;
|
||||
}
|
||||
DirectedPoint PosFromDist(double dist) const override;
|
||||
|
||||
/// Returns a pair containing:
|
||||
/// - @b first: distance to the nearest point in this line from the
|
||||
|
@ -152,20 +133,7 @@ namespace element {
|
|||
: Geometry(GeometryType::ARC, start_offset, length, heading, start_pos),
|
||||
_curvature(curv) {}
|
||||
|
||||
DirectedPoint PosFromDist(double dist) const override {
|
||||
dist = geom::Math::Clamp(dist, 0.0, _length);
|
||||
DEBUG_ASSERT(_length > 0.0);
|
||||
DEBUG_ASSERT(std::fabs(_curvature) > 1e-15);
|
||||
const double radius = 1.0 / _curvature;
|
||||
constexpr double pi_half = geom::Math::Pi<double>() / 2.0;
|
||||
DirectedPoint p(_start_position, _heading);
|
||||
p.location.x += static_cast<float>(radius * std::cos(p.tangent + pi_half));
|
||||
p.location.y += static_cast<float>(radius * std::sin(p.tangent + pi_half));
|
||||
p.tangent += dist * _curvature;
|
||||
p.location.x -= static_cast<float>(radius * std::cos(p.tangent + pi_half));
|
||||
p.location.y -= static_cast<float>(radius * std::sin(p.tangent + pi_half));
|
||||
return p;
|
||||
}
|
||||
DirectedPoint PosFromDist(double dist) const override;
|
||||
|
||||
/// Returns a pair containing:
|
||||
/// - @b first: distance to the nearest point in this arc from the
|
||||
|
@ -212,33 +180,9 @@ namespace element {
|
|||
return _curve_end;
|
||||
}
|
||||
|
||||
DirectedPoint PosFromDist(double dist) const override {
|
||||
// not working yet with negative values
|
||||
dist = geom::Math::Clamp(dist, 0.0, _length);
|
||||
DEBUG_ASSERT(_length > 0.0);
|
||||
DEBUG_ASSERT(std::fabs(_curve_end) > 1e-15);
|
||||
const double radius = 1.0 / _curve_end;
|
||||
const double extra_norm = 1.0 / std::sqrt(geom::Math::Pi<double>() / 2.0);
|
||||
const double norm = 1.0 / std::sqrt(2.0 * radius * _length);
|
||||
const double length = dist * norm;
|
||||
double S, C;
|
||||
fresnl(length * extra_norm, &S, &C);
|
||||
S /= (norm * extra_norm);
|
||||
C /= (norm * extra_norm);
|
||||
DirectedPoint p(_start_position, _heading);
|
||||
const double cos_a = std::cos(p.tangent);
|
||||
const double sin_a = std::sin(p.tangent);
|
||||
p.location.x += static_cast<float>(C * cos_a - S * sin_a);
|
||||
p.location.y += static_cast<float>(S * cos_a + C * sin_a);
|
||||
p.tangent += length * length;
|
||||
DirectedPoint PosFromDist(double dist) const override;
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
/// @todo
|
||||
std::pair<float, float> DistanceTo(const geom::Location &) const override {
|
||||
throw_exception(std::runtime_error("not implemented"));
|
||||
}
|
||||
std::pair<float, float> DistanceTo(const geom::Location &) const override;
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
@ -13,13 +13,14 @@
|
|||
#include <carla/geom/Location.h>
|
||||
#include <carla/geom/Math.h>
|
||||
#include <carla/opendrive/OpenDriveParser.h>
|
||||
#include <carla/opendrive/parser/pugixml/pugixml.hpp>
|
||||
#include <carla/road/MapBuilder.h>
|
||||
#include <carla/road/element/RoadInfoElevation.h>
|
||||
#include <carla/road/element/RoadInfoGeometry.h>
|
||||
#include <carla/road/element/RoadInfoMarkRecord.h>
|
||||
#include <carla/road/element/RoadInfoVisitor.h>
|
||||
|
||||
#include <pugixml/pugixml.hpp>
|
||||
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
|
||||
|
|
Loading…
Reference in New Issue