Fix OpenDrive GeometryParamPoly3 calculation

Update version numbers to 2.4.7

* Fix OpenDrive GeometryParamPoly3 calculation
* Cleanup documentation
* Remove outdated and complicated plain cmake handling from docu
* Removed some left-over from python-binding.cmake
* Build QGIS plugin only if python is enabled
* Update ad_map_access_qgis docu and release workflow

.github/workflows/release_qgis_plugin.yml

@@ -11,17 +11,8 @@ on:
 
 jobs:
   publish:
-    name: Publish for ${{ matrix.os }}
-    runs-on: ${{ matrix.os }}
-    strategy:
-      matrix:
-        include:
-          - os: ubuntu-18.04
-            artifact_name: ad_map_access_qgis_ubuntu1804.zip
-            PYTHON_BINDING_VERSION: "3.6"
-          - os: ubuntu-20.04
-            artifact_name: ad_map_access_qgis_ubuntu2004.zip
-            PYTHON_BINDING_VERSION: "3.8"
+    name: Publish AD-Map QGIS Plugin
+    runs-on: ubuntu-20.04
 
     steps:
     - uses: actions/checkout@v2
@@ -32,23 +23,19 @@ jobs:
       run: bash .github/workflows/install_dependencies.sh
 
     - name: Build Libraries
-      env:
-          PYTHON_BINDING_VERSION: ${{ matrix.PYTHON_BINDING_VERSION }}
       run: |
-        colcon build --event-handlers console_direct+ --executor sequential --metas colcon_python.meta --build-base build-static --install-base install-static --cmake-args -DBUILD_SHARED_LIBS=OFF -DCMAKE_POSITION_INDEPENDENT_CODE=ON -DPYTHON_BINDING_VERSION=${PYTHON_BINDING_VERSION}
+        colcon build --event-handlers console_direct+ --executor sequential --metas colcon_python.meta --packages-up-to ad_map_access_qgis
 
     - name: Build Package
       run: |
-        cd install-static
-        cp -r ad_map_access/lib/python3.*/site-packages/ad_map_access ad_map_access_qgis/share/qgis/python/plugins/ad_map_access_qgis/.
-        cd ad_map_access_qgis/share/qgis/python/plugins/
-        zip -r ${{ matrix.artifact_name }} ad_map_access_qgis/
-        mv ${{ matrix.artifact_name }} ../../../../../../
+        cd install/ad_map_access_qgis/share/qgis/python/plugins/
+        zip -r ad_map_access_qgis.zip ad_map_access_qgis/
+        mv ad_map_access_qgis.zip ../../../../../../
 
     - name: Upload binaries to release
       uses: svenstaro/upload-release-action@v2
       if: ${{ github.event_name == 'release'}}
       with:
         repo_token: ${{ secrets.GITHUB_TOKEN }}
-        file: ${{ matrix.artifact_name }}
+        file: ad_map_access_qgis.zip
         tag: ${{ github.ref }}

ad_map_access/generated/CMakeLists.txt

@@ -14,7 +14,7 @@
 ##
 
 cmake_minimum_required(VERSION 3.5)
-project(ad_map_access VERSION 2.4.0)
+project(ad_map_access VERSION 2.4.7)
 
 include(GNUInstallDirs)
 include(CMakePackageConfigHelpers)
@@ -47,7 +47,7 @@ get_target_property(AD_MAP_OPENDRIVE_READER_INTERFACE_INCLUDE_DIRS ad_map_opendr
 list(APPEND INCLUDE_DIRS ${AD_MAP_OPENDRIVE_READER_INTERFACE_INCLUDE_DIRS})
 list(APPEND LIBRARIES ad_map_opendrive_reader)
 
-find_package(ad_physics 2.4.0 REQUIRED CONFIG)
+find_package(ad_physics 2.4.7 REQUIRED CONFIG)
 find_package(spdlog REQUIRED CONFIG)
 
 add_library(${PROJECT_NAME}
@@ -155,4 +155,3 @@ endif()
 if (ad_map_access_TOOLS_DIR)
   add_subdirectory(${ad_map_access_TOOLS_DIR} ad_map_access_tools)
 endif()
-

ad_map_access/generated/cmake/Config.cmake.in

@@ -30,7 +30,7 @@ get_target_property(AD_MAP_OPENDRIVE_READER_INTERFACE_INCLUDE_DIRS ad_map_opendr
 list(APPEND INCLUDE_DIRS ${AD_MAP_OPENDRIVE_READER_INTERFACE_INCLUDE_DIRS})
 list(APPEND LIBRARIES ad_map_opendrive_reader)
 
-find_dependency(ad_physics 2.4.0)
+find_dependency(ad_physics 2.4.7)
 find_dependency(spdlog)
 
 include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake")

ad_map_access/generated/include/ad_map_access/Version.hpp

@@ -30,9 +30,9 @@
 /*!
  * The revision of ad_map_access
  */
-#define AD_MAP_ACCESS_VERSION_REVISION 0
+#define AD_MAP_ACCESS_VERSION_REVISION 7
 
 /*!
  * The version of ad_map_access as string
  */
-#define AD_MAP_ACCESS_VERSION_STRING "2.4.0"
+#define AD_MAP_ACCESS_VERSION_STRING "2.4.7"

ad_map_access/impl/src/opendrive/AdMapFactory.cpp

@@ -190,8 +190,20 @@ bool AdMapFactory::addLane(::opendrive::Lane const &lane)
   bool ok = true;
   if ((lane.leftEdge.size() < 2) || (lane.rightEdge.size() < 2))
   {
-    // Invalid number of points for lane"
-    access::getLogger()->error("Invalid number of points for lane {}", lane.id);
+    access::getLogger()->error("Invalid number of points for lane {}. Skip lane.", lane.id);
+    return false;
+  }
+
+  point::Geometry leftEcefEdge;
+  point::Geometry rightEcefEdge;
+  try
+  {
+    leftEcefEdge = toGeometry(lane.leftEdge);
+    rightEcefEdge = toGeometry(lane.rightEdge);
+  }
+  catch (...)
+  {
+    access::getLogger()->error("Invalid points for lane {}. Skip lane.", lane.id);
     return false;
   }
 
@@ -212,8 +224,6 @@ bool AdMapFactory::addLane(::opendrive::Lane const &lane)
   {
     ok = false;
   }
-  auto const leftEcefEdge = toGeometry(lane.leftEdge);
-  auto const rightEcefEdge = toGeometry(lane.rightEdge);
   if (!set(laneId, leftEcefEdge, rightEcefEdge))
   {
     ok = false;

ad_map_access/python/setup.py

@@ -24,7 +24,7 @@ def finalize_options(self):
     packages=["ad_map_access"],
     package_dir={"": sys.argv.pop(-1)},
     package_data={"ad_map_access": ["*.so"]},
-    version="2.4.5",
+    version="2.4.7",
     author='CARLA Simulator Team',
     author_email='carla.simulator@gmail.com',
     license="MIT",
@@ -48,7 +48,7 @@ def finalize_options(self):
 \
 See [project webpage](https://ad-map-access.readthedocs.io/en/latest/) or [doxygen docu](https://ad-map-access.readthedocs.io/en/latest/ad_map_access/apidoc/html/index.html) for a full interface description.",
     long_description_content_type="text/markdown",
-    install_requires=["ad_physics>=2.4.5"],
+    install_requires=["ad_physics>=2.4.7"],
     distclass=BinaryDistribution,
     cmdclass={'install': Install}
 )

ad_map_access_test_support/generated/CMakeLists.txt

@@ -14,7 +14,7 @@
 ##
 
 cmake_minimum_required(VERSION 3.5)
-project(ad_map_access_test_support VERSION 2.4.0)
+project(ad_map_access_test_support VERSION 2.4.7)
 
 include(GNUInstallDirs)
 include(CMakePackageConfigHelpers)
@@ -31,7 +31,7 @@ if ((NOT ad_map_access_test_support_SOURCES) OR (NOT ad_map_access_test_support_
   message(FATAL_ERROR "${PROJECT_NAME}: Variable ad_map_access_test_support_SOURCES or ad_map_access_test_support_INCLUDE_DIRS pointing to the generator managed library not set!")
 endif()
 
-find_package(ad_map_access 2.4.0 REQUIRED CONFIG)
+find_package(ad_map_access 2.4.7 REQUIRED CONFIG)
 
 add_library(${PROJECT_NAME}
   ${ad_map_access_test_support_SOURCES}
@@ -109,4 +109,3 @@ endif()
 if (ad_map_access_test_support_TOOLS_DIR)
   add_subdirectory(${ad_map_access_test_support_TOOLS_DIR} ad_map_access_test_support_tools)
 endif()
-

ad_map_access_test_support/generated/cmake/Config.cmake.in

@@ -17,7 +17,7 @@
 
 include(CMakeFindDependencyMacro)
 
-find_dependency(ad_map_access 2.4.0)
+find_dependency(ad_map_access 2.4.7)
 
 include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake")
 

ad_map_access_test_support/generated/include/ad_map_access_test_support/Version.hpp

@@ -30,9 +30,9 @@
 /*!
  * The revision of ad_map_access_test_support
  */
-#define AD_MAP_ACCESS_TEST_SUPPORT_VERSION_REVISION 0
+#define AD_MAP_ACCESS_TEST_SUPPORT_VERSION_REVISION 7
 
 /*!
  * The version of ad_map_access_test_support as string
  */
-#define AD_MAP_ACCESS_TEST_SUPPORT_VERSION_STRING "2.4.0"
+#define AD_MAP_ACCESS_TEST_SUPPORT_VERSION_STRING "2.4.7"

ad_map_opendrive_reader/CMakeLists.txt

@@ -19,7 +19,7 @@ include(../cmake/apidoc.cmake)
 #convert list to string
 string (REPLACE ";" " " TARGET_LINK_FLAGS "${TARGET_LINK_OPTIONS}")
 
-project(ad_map_opendrive_reader VERSION 3.0.0)
+project(ad_map_opendrive_reader VERSION 2.4.7)
 
 include(GNUInstallDirs)
 include(CMakePackageConfigHelpers)
@@ -150,8 +150,8 @@ write_basic_package_version_file(${CMAKE_CURRENT_BINARY_DIR}/install/${PROJECT_N
   COMPATIBILITY SameMajorVersion
 )
 
-install(FILES 
+install(FILES
   "${CMAKE_CURRENT_BINARY_DIR}/install/${PROJECT_NAME}Config.cmake"
   "${CMAKE_CURRENT_BINARY_DIR}/install/${PROJECT_NAME}ConfigVersion.cmake"
   DESTINATION ${CMAKECONFIG_INSTALL_DIR}
-)
+)

ad_map_opendrive_reader/include/opendrive/types.hpp

@@ -17,6 +17,7 @@
 #include <map>
 #include <memory>
 #include <set>
+#include <spdlog/spdlog.h>
 #include <string>
 #include <unordered_map>
 #include <vector>
@@ -116,45 +117,78 @@ struct Point
 
   bool operator==(const Point &rhs) const
   {
+    ensureValid();
+    rhs.ensureValid();
     // Points are treated as equal when below 1 mm
     return (std::fabs(x - rhs.x) < cCoordinateTolerance) && (std::fabs(y - rhs.y) < cCoordinateTolerance)
       && (std::fabs(z - rhs.z) < cCoordinateTolerance);
   }
   bool operator!=(const Point &rhs) const
   {
+    ensureValid();
+    rhs.ensureValid();
     return !operator==(rhs);
   }
   double normSquared() const
   {
+    ensureValid();
     return x * x + y * y + z * z;
   }
 
   double norm() const
   {
+    ensureValid();
     return sqrt(normSquared());
   }
 
   double dot(const Point &other) const
   {
+    ensureValid();
+    other.ensureValid();
     return x * other.x + y * other.y + z * other.z;
   }
+
+  void ensureValid() const
+  {
+    if (!isValid())
+    {
+      spdlog::error("ensureValid(Point)>> {},{},{} value out of range", x, y, z);
+      throw std::out_of_range("Point value out of range");
+    }
+  }
+
+  bool isValid() const
+  {
+    auto const valueClassX = std::fpclassify(x);
+    auto const valueClassY = std::fpclassify(y);
+    auto const valueClassZ = std::fpclassify(z);
+    return ((valueClassX == FP_NORMAL) || (valueClassX == FP_ZERO))
+      && ((valueClassY == FP_NORMAL) || (valueClassY == FP_ZERO))
+      && ((valueClassZ == FP_NORMAL) || (valueClassZ == FP_ZERO));
+  }
 };
 
 inline Point operator-(const Point &left, const Point &right)
 {
+  left.ensureValid();
+  right.ensureValid();
   return Point(left.x - right.x, left.y - right.y, left.z - right.z);
 }
 inline Point operator+(const Point &left, const Point &right)
 {
+  left.ensureValid();
+  right.ensureValid();
   return Point(left.x + right.x, left.y + right.y, left.z + right.z);
 }
 inline Point operator*(const double &scalar, const Point &point)
 {
+  point.ensureValid();
   return Point(scalar * point.x, scalar * point.y, scalar * point.z);
 }
 
 inline Point operator*(const Point &point, const double &scalar)
 {
+  point.ensureValid();
   return operator*(scalar, point);
 }
 

ad_map_opendrive_reader/src/geometry/Geometry.cpp

@@ -197,7 +197,7 @@ const DirectedPoint GeometryParamPoly3::PosFromDist(const double dist) const
   double p = dist;
   if (_p_range_is_normalized)
   {
-    std::min(1.0, dist / _length);
+    p = std::min(1.0, dist / _length);
   }
 
   auto polyU = boost::array<double, 4>{{_aU, _bU, _cU, _dU}};

ad_map_opendrive_reader/src/geometry/GeometryGenerator.cpp

@@ -966,25 +966,36 @@ bool convertToGeoPoints(opendrive::OpenDriveData &mapData)
   }
 
   auto convertENUToGeo = [&projPtr](Point &point) {
+    point.ensureValid();
     projXY enuPoint;
     enuPoint.u = point.x;
     enuPoint.v = point.y;
 
     auto geoPoint = pj_inv(enuPoint, projPtr);
     point.x = geoPoint.u * RAD_TO_DEG;
     point.y = geoPoint.v * RAD_TO_DEG;
+    point.ensureValid();
   };
 
   for (auto &element : mapData.laneMap)
   {
     auto &lane = element.second;
-    for (auto &point : lane.leftEdge)
+    try
     {
-      convertENUToGeo(point);
+      for (auto &point : lane.leftEdge)
+      {
+        convertENUToGeo(point);
+      }
+      for (auto &point : lane.rightEdge)
+      {
+        convertENUToGeo(point);
+      }
     }
-    for (auto &point : lane.rightEdge)
+    catch (...)
     {
-      convertENUToGeo(point);
+      spdlog::error("convertToGeoPoints>> error converting ENU points of lane {} to Geo Points", lane.id);
+      lane.leftEdge.resize(0);
+      lane.rightEdge.resize(0);
     }
   }
 

ad_physics/generated/CMakeLists.txt

@@ -14,7 +14,7 @@
 ##
 
 cmake_minimum_required(VERSION 3.5)
-project(ad_physics VERSION 2.4.0)
+project(ad_physics VERSION 2.4.7)
 
 include(GNUInstallDirs)
 include(CMakePackageConfigHelpers)
@@ -123,4 +123,3 @@ endif()
 if (ad_physics_TOOLS_DIR)
   add_subdirectory(${ad_physics_TOOLS_DIR} ad_physics_tools)
 endif()
-

ad_physics/generated/include/ad_physics/Version.hpp

@@ -30,9 +30,9 @@
 /*!
  * The revision of ad_physics
  */
-#define AD_PHYSICS_VERSION_REVISION 0
+#define AD_PHYSICS_VERSION_REVISION 7
 
 /*!
  * The version of ad_physics as string
  */
-#define AD_PHYSICS_VERSION_STRING "2.4.0"
+#define AD_PHYSICS_VERSION_STRING "2.4.7"

ad_physics/python/setup.py

@@ -24,7 +24,7 @@ def finalize_options(self):
     packages=["ad_physics"],
     package_dir={"": sys.argv.pop(-1)},
     package_data={"ad_physics": ["*.so"]},
-    version="2.4.5",
+    version="2.4.7",
     author='CARLA Simulator Team',
     author_email='carla.simulator@gmail.com',
     license="MIT",

cmake/python-binding.cmake

@@ -15,11 +15,6 @@ function(generate_python_binding_source_code WORKING_DIR)
   message(STATUS "Generating python binding source code at ${WORKING_DIR}" )
   configure_file(${PYTHON_WRAPPER_HELPER_DIR}/python_wrapper_helper.py.in python_wrapper_helper.py @ONLY)
 
-  set(PYTHON_CMD python)
-  if ( PYTHON_BINDINGS )
-    list(GET PYTHON_BINDINGS 0 PYTHON_CMD)
-  endif()
-
   execute_process(
     COMMAND ${PYTHON_EXECUTABLE} generate_python_lib.py
     WORKING_DIRECTORY ${WORKING_DIR}