Release 2.2.0 (#20)

* Release 2.2.0

* update travis file for pyplusplus installation

Change-Id: Id46adebc141f65861225baa3e38d61b66bbcc053

.travis.yml

@@ -41,7 +41,7 @@ before_install:
   - if [ "${EXTRA_PACKAGES}" != "" ]; then sudo apt-get -f -y install ${EXTRA_PACKAGES}; fi
   - pip3 install --user -U colcon-common-extensions
   - pip3 install --user xmlrunner
-  - pip2 install --user pygccxml https://bitbucket.org/ompl/pyplusplus/get/1.8.1.zip xmlrunner
+  - pip2 install --user pygccxml pyplusplus xmlrunner
 
 env:
   global:

ad_map_access/generated/CMakeLists.txt

@@ -14,7 +14,7 @@
 ##
 
 cmake_minimum_required(VERSION 3.5)
-project(ad_map_access VERSION 2.1.0)
+project(ad_map_access VERSION 2.2.0)
 
 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.1.0 REQUIRED CONFIG)
+find_package(ad_physics 2.2.0 REQUIRED CONFIG)
 find_package(spdlog REQUIRED CONFIG)
 
 add_library(${PROJECT_NAME}

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.1.0)
+find_dependency(ad_physics 2.2.0)
 find_dependency(spdlog)
 
 include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@Targets.cmake")

ad_map_access/generated/include/ad/map/landmark/Landmark.hpp

@@ -46,7 +46,7 @@ namespace landmark {
 /*!
  * \brief DataType Landmark
  *
- * Landmark description in ENU coordiante frame.
+ * Landmark description in ECEF coordiante frame.
  */
 struct Landmark
 {

ad_map_access/generated/include/ad_map_access/Version.hpp

@@ -25,7 +25,7 @@
 /*!
  * The minor version of ad_map_access
  */
-#define AD_MAP_ACCESS_VERSION_MINOR 1
+#define AD_MAP_ACCESS_VERSION_MINOR 2
 
 /*!
  * The revision of ad_map_access
@@ -35,4 +35,4 @@
 /*!
  * The version of ad_map_access as string
  */
-#define AD_MAP_ACCESS_VERSION_STRING "2.1.0"
+#define AD_MAP_ACCESS_VERSION_STRING "2.2.0"

ad_map_access/impl/include/ad/map/lane/LaneOperation.hpp

@@ -93,6 +93,13 @@ point::ENUHeading getLaneENUHeading(point::ParaPoint const &paraPoint, point::Ge
 */
 LaneId uniqueLaneId(point::GeoPoint const &point);
 
+/**
+* @return parametric point at given location
+*
+* Throws if there is more than one lane at the given position
+*/
+point::ParaPoint uniqueParaPoint(point::GeoPoint const &point);
+
 /**
  * @brief Checks if vehicle fits the lanes restriction criteria.
  * @param[in] lane the lane.

ad_map_access/impl/include/ad/map/match/AdMapMatching.hpp

@@ -155,7 +155,7 @@ class AdMapMatching
    */
   MapMatchedPositionConfidenceList getMapMatchedPositions(point::GeoPoint const &geoPoint,
                                                           physics::Distance const &distance,
-                                                          physics::Probability const &minProbability);
+                                                          physics::Probability const &minProbability) const;
 
   /**
    * @brief get the map matched positions
@@ -170,7 +170,7 @@ class AdMapMatching
   MapMatchedPositionConfidenceList getMapMatchedPositions(point::ENUPoint const &enuPoint,
                                                           point::GeoPoint const &enuReferencePoint,
                                                           physics::Distance const &distance,
-                                                          physics::Probability const &minProbability);
+                                                          physics::Probability const &minProbability) const;
 
   /**
    * @brief get the map matched positions
@@ -186,7 +186,7 @@ class AdMapMatching
    */
   MapMatchedPositionConfidenceList getMapMatchedPositions(point::ENUPoint const &enuPoint,
                                                           physics::Distance const &distance,
-                                                          physics::Probability const &minProbability);
+                                                          physics::Probability const &minProbability) const;
 
   /**
    * @brief get the map matched positions
@@ -225,7 +225,7 @@ class AdMapMatching
    */
   MapMatchedObjectBoundingBox getMapMatchedBoundingBox(ENUObjectPosition const &enuObjectPosition,
                                                        physics::Distance const &samplingDistance
-                                                       = physics::Distance(1.));
+                                                       = physics::Distance(1.)) const;
 
   /**
    * @brief get the lane occupied regions from a list of ENUObjectPositionList
@@ -243,12 +243,13 @@ class AdMapMatching
    * @returns the map matched bounding box of the object
    */
   LaneOccupiedRegionList getLaneOccupiedRegions(ENUObjectPositionList enuObjectPositionList,
-                                                physics::Distance const &samplingDistance = physics::Distance(1.));
+                                                physics::Distance const &samplingDistance
+                                                = physics::Distance(1.)) const;
 
   /**
    * @brief Method to be called to retrieve the lane heading at a mapMatchedPosition
    */
-  point::ENUHeading getLaneENUHeading(MapMatchedPosition const &mapMatchedPosition);
+  point::ENUHeading getLaneENUHeading(MapMatchedPosition const &mapMatchedPosition) const;
 
   /**
    * @brief Spatial Lane Search.
@@ -303,14 +304,14 @@ class AdMapMatching
    */
 
   void addLaneRegions(LaneOccupiedRegionList &laneOccupiedRegions,
-                      MapMatchedPositionConfidenceList const &mapMatchedPositions);
+                      MapMatchedPositionConfidenceList const &mapMatchedPositions) const;
   void addLaneRegions(LaneOccupiedRegionList &laneOccupiedRegions,
-                      LaneOccupiedRegionList const &otherLaneOccupiedRegions);
+                      LaneOccupiedRegionList const &otherLaneOccupiedRegions) const;
 
   MapMatchedPositionConfidenceList considerMapMatchingHints(MapMatchedPositionConfidenceList const &mapMatchedPositions,
-                                                            physics::Probability const &minProbability);
-  bool isLanePartOfRouteHints(lane::LaneId const &laneId);
-  double getHeadingFactor(MapMatchedPosition const &matchedPosition);
+                                                            physics::Probability const &minProbability) const;
+  bool isLanePartOfRouteHints(lane::LaneId const &laneId) const;
+  double getHeadingFactor(MapMatchedPosition const &matchedPosition) const;
 
   std::list<point::ECEFHeading> mHeadingHints;
   double mHeadingHintFactor{2.};

ad_map_access/impl/include/ad/map/route/Planning.hpp

@@ -350,46 +350,81 @@ std::vector<route::FullRoute> predictRoutes(const match::MapMatchedObjectBoundin
                                             RouteCreationMode const routeCreationMode
                                             = RouteCreationMode::SameDrivingDirection);
 
+/**
+ * @brief Filter duplicated routes from a list of routes
+ *
+ * If one of the routes is a real sub-route of the other, the longer version of the route is kept, the shorter dropped
+ *
+ * @param[in] fullRoutes list of full routes to be filtered
+ *
+ * @returns the filtered list of routes
+ */
+std::vector<FullRoute> filterDuplicatedRoutes(const std::vector<FullRoute> fullRoutes);
+
 /**
  * @brief Calculate the connecting route between the the two objects
  *
  * For route calculations the route type core::Route::Type::SHORTEST_IGNORE_DIRECTION is used.
+ * The prediction hints are taken into account if no direct connecting route can be found in search of a merge route.
+ * If no prediction hints are given, respective route predictions are calculated internally.
  *
  * @param[in] startObject object at starting position
  * @param[in] destObject object at destination position
  * @param[in] maxDistance distance when the search can be stopped.
  * @param[in] maxDuration duration when the search can be stopped.
+ * @param[in] startObjectPredictionHints route prediction hints for start object (optional)
+ * @param[in] destObjectPredictionHints route prediction hints for dest object (optional)
  */
 ConnectingRoute calculateConnectingRoute(const match::Object &startObject,
                                          const match::Object &destObject,
                                          physics::Distance const &maxDistance,
-                                         physics::Duration const &maxDuration);
+                                         physics::Duration const &maxDuration,
+                                         std::vector<route::FullRoute> const &startObjectPredictionHints
+                                         = std::vector<route::FullRoute>(),
+                                         std::vector<route::FullRoute> const &destObjectPredictionHints
+                                         = std::vector<route::FullRoute>());
 
 /**
  * @brief Calculate the connecting route between the the two objects
  *
  * For route calculations the route type core::Route::Type::SHORTEST_IGNORE_DIRECTION is used.
+ * The prediction hints are taken into account if no direct connecting route can be found in search of a merge route.
+ * If no prediction hints are given, respective route predictions are calculated internally.
  *
  * @param[in] startObject object at starting position
  * @param[in] destObject object at destination position
  * @param[in] maxDistance distance when the search can be stopped.
+ * @param[in] startObjectPredictionHints route prediction hints for start object (optional)
+ * @param[in] destObjectPredictionHints route prediction hints for dest object (optional)
  */
 ConnectingRoute calculateConnectingRoute(const match::Object &startObject,
                                          const match::Object &destObject,
-                                         physics::Distance const &maxDistance);
+                                         physics::Distance const &maxDistance,
+                                         std::vector<route::FullRoute> const &startObjectPredictionHints
+                                         = std::vector<route::FullRoute>(),
+                                         std::vector<route::FullRoute> const &destObjectPredictionHints
+                                         = std::vector<route::FullRoute>());
 
 /**
  * @brief Calculate the connecting route between the the two objects
  *
  * For route calculations the route type core::Route::Type::SHORTEST_IGNORE_DIRECTION is used.
+ * The prediction hints are taken into account if no direct connecting route can be found in search of a merge route.
+ * If no prediction hints are given, respective route predictions are calculated internally.
  *
  * @param[in] startObject object at starting position
  * @param[in] destObject object at destination position
  * @param[in] maxDuration duration when the search can be stopped.
+ * @param[in] startObjectPredictionHints route prediction hints for start object (optional)
+ * @param[in] destObjectPredictionHints route prediction hints for dest object (optional)
  */
 ConnectingRoute calculateConnectingRoute(const match::Object &startObject,
                                          const match::Object &destObject,
-                                         physics::Duration const &maxDuration);
+                                         physics::Duration const &maxDuration,
+                                         std::vector<route::FullRoute> const &startObjectPredictionHints
+                                         = std::vector<route::FullRoute>(),
+                                         std::vector<route::FullRoute> const &destObjectPredictionHints
+                                         = std::vector<route::FullRoute>());
 
 /**
  * @brief update route planning counters of the route

ad_map_access/impl/include/ad/map/route/RouteOperation.hpp

@@ -12,6 +12,7 @@
 #include "ad/map/match/Types.hpp"
 #include "ad/map/point/Types.hpp"
 #include "ad/map/route/LaneIntervalOperation.hpp"
+#include "ad/map/route/Routing.hpp"
 #include "ad/map/route/Types.hpp"
 #include "ad/physics/Types.hpp"
 
@@ -738,6 +739,40 @@ bool extendRouteToDistance(route::FullRoute &route,
                            physics::Distance const &length,
                            std::vector<route::FullRoute> &additionalRoutes);
 
+/**
+ * @brief extends route with the given list of destinations
+ *
+ * @param[in/out] route the route to check and to extend
+ * @param[in] dest Vector with supporting points as routing parametric points to be visited on the route. Last point in
+ * the list is the
+ * actual destination point.
+ *
+ * @returns @c false is returned if the route is/was empty/degenerated
+ */
+bool extendRouteToDestinations(route::FullRoute &route, const std::vector<route::planning::RoutingParaPoint> &dest);
+
+/**
+ * @brief extends route with the given list of destinations
+ *
+ * @param[in/out] route the route to check and to extend
+ * @param[in] dest Vector with supporting points as geo points to be visited on the route. Last point in the list is the
+ * actual destination point.
+ *
+ * @returns @c false is returned if the route is/was empty/degenerated
+ */
+bool extendRouteToDestinations(route::FullRoute &route, const std::vector<point::GeoPoint> &dest);
+
+/**
+ * @brief extends route with the given list of destinations
+ *
+ * @param[in/out] route the route to check and to extend
+ * @param[in] dest Vector with supporting points as ENU points to be visited on the route. Last point in the list is the
+ * actual destination point.
+ *
+ * @returns @c false is returned if the route is/was empty/degenerated
+ */
+bool extendRouteToDestinations(route::FullRoute &route, const std::vector<point::ENUPoint> &dest);
+
 /** @brief function to append a new lane interval to a road segment list
  *
  * This function is mainly used while creation of a FullRoute object.

ad_map_access/impl/include/ad/map/serialize/ChecksumCRC32.hpp

@@ -1,6 +1,6 @@
 // ----------------- BEGIN LICENSE BLOCK ---------------------------------
 //
-// Copyright (C) 2018-2019 Intel Corporation
+// Copyright (C) 2018-2020 Intel Corporation
 //
 // SPDX-License-Identifier: MIT
 //
@@ -20,6 +20,11 @@ namespace map {
 /** @brief namespace serialize */
 namespace serialize {
 
+/**
+ * @brief Standalone checksum CRC32 calculation
+ */
+uint32_t calculateCRC32Checksum(const void *x, size_t bytes);
+
 /**
  * @brief Checksum CRC32 calculation implementation
  */

ad_map_access/impl/src/access/AdMapAccess.cpp

@@ -102,11 +102,21 @@ bool AdMapAccess::initializeFromOpenDriveContent(std::string const &openDriveCon
     return false;
   }
 
+  auto const openDriveContentChecksum
+    = serialize::calculateCRC32Checksum(openDriveContent.c_str(), openDriveContent.size());
   if (mInitializedFromStore)
   {
-    mLogger->error(
-      "AdMapAccess::initializeFromOpenDriveContent() failed; already initialized with different store object");
-    return false;
+    if (openDriveContentChecksum == mInitializeFromOpenDriveContentChecksum)
+    {
+      // initialize twice from same store object is ok.
+      return true;
+    }
+    else
+    {
+      mLogger->error("AdMapAccess::initializeFromOpenDriveContent() failed; already initialized with different store "
+                     "object or content");
+      return false;
+    }
   }
 
   auto store = std::make_shared<Store>();
@@ -116,6 +126,7 @@ bool AdMapAccess::initializeFromOpenDriveContent(std::string const &openDriveCon
     = factory.createAdMapFromString(openDriveContent, overlapMargin, defaultIntersectionType, defaultTrafficLightType);
   if (result)
   {
+    mInitializeFromOpenDriveContentChecksum = openDriveContentChecksum;
     mInitializedFromStore = true;
     mStore = store;
   }

ad_map_access/impl/src/access/AdMapAccess.hpp

@@ -57,16 +57,17 @@ class AdMapAccess
   /**
    * @brief initialize singleton with OpenDRIVE content string
    *
+   * If previously initialized with the same opendrive content the function performs nothing, but returns true.
+   *
    * The string contains the full OpenDRIVE data
-   * @return true if initialization was successful (i.e. config file exists or singleton was already initialized)
+   * @return true if initialization was successful.
    * @return false if OpenDRIVE data wasn't valid or the singleton was already initialized differently
    */
   bool initializeFromOpenDriveContent(std::string const &openDriveContent,
                                       double const overlapMargin,
                                       intersection::IntersectionType const defaultIntersectionType,
                                       landmark::TrafficLightType const defaultTrafficLightType
                                       = landmark::TrafficLightType::SOLID_RED_YELLOW_GREEN);
-
   /**
    * @brief initialize singleton with given store
    *
@@ -91,6 +92,8 @@ class AdMapAccess
 
   bool mInitializedFromStore{false};
 
+  uint32_t mInitializeFromOpenDriveContentChecksum{0u};
+
   std::shared_ptr<spdlog::logger> mLogger;
 
   //! protect access to mLoadedMaps and used while loading a map

ad_map_access/impl/src/lane/LaneOperation.cpp

@@ -114,7 +114,7 @@ point::ENUHeading getLaneENUHeading(point::ParaPoint const &paraPoint, point::Ge
   return point::createENUHeading(getLaneECEFHeading(paraPoint), gnssReference);
 }
 
-LaneId uniqueLaneId(point::GeoPoint const &geoPoint)
+point::ParaPoint uniqueParaPoint(point::GeoPoint const &geoPoint)
 {
   match::AdMapMatching mapMatching;
   auto mapMatchingResult
@@ -125,7 +125,12 @@ LaneId uniqueLaneId(point::GeoPoint const &geoPoint)
     throw std::runtime_error("uniqueLaneId: position matches multiple lanes");
   }
 
-  return mapMatchingResult[0].lanePoint.paraPoint.laneId;
+  return mapMatchingResult[0].lanePoint.paraPoint;
+}
+
+LaneId uniqueLaneId(point::GeoPoint const &geoPoint)
+{
+  return uniqueParaPoint(geoPoint).laneId;
 }
 
 physics::Distance getWidth(Lane const &lane, physics::ParametricValue const &longitudinalOffset)