Adds SimpleObjectQuery implementation.

maliput_object/CMakeLists.txt

@@ -71,4 +71,5 @@ install(
 ament_export_include_directories(include)
 
 ament_export_dependencies(ament_cmake)
+ament_export_interfaces(${PROJECT_NAME}-targets HAS_LIBRARY_TARGET)
 ament_package()

maliput_object/include/maliput_object/base/simple_object_query.h

@@ -0,0 +1,44 @@
+// Copyright 2022 Toyota Research Institute
+#pragma once
+
+#include <optional>
+#include <vector>
+
+#include <maliput/api/road_network.h>
+#include <maliput/common/maliput_copyable.h>
+#include <maliput/math/vector.h>
+
+#include "maliput_object/api/object.h"
+#include "maliput_object/api/object_book.h"
+#include "maliput_object/api/object_query.h"
+#include "maliput_object/api/overlapping_type.h"
+
+namespace maliput {
+namespace object {
+
+/// api::ObjectQuery Implementation.
+/// The implementation uses maliput's api for finding the lanes.
+/// Methods like ToRoadPosition or FindRoadPositions are extensively used.
+class SimpleObjectQuery : public api::ObjectQuery {
+ public:
+  MALIPUT_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(SimpleObjectQuery)
+  SimpleObjectQuery(const maliput::api::RoadNetwork* road_network,
+                    const api::ObjectBook<maliput::math::Vector3>* object_book);
+  ~SimpleObjectQuery() = default;
+
+ private:
+  std::vector<const maliput::api::Lane*> DoFindOverlappingLanesIn(
+      const api::Object<maliput::math::Vector3>* object) const;
+  std::vector<const maliput::api::Lane*> DoFindOverlappingLanesIn(const api::Object<maliput::math::Vector3>* object,
+                                                                  const api::OverlappingType& overlapping_type) const;
+  std::optional<const maliput::api::LaneSRoute> DoRoute(const api::Object<maliput::math::Vector3>* origin,
+                                                        const api::Object<maliput::math::Vector3>* target) const;
+  const api::ObjectBook<maliput::math::Vector3>* do_object_book() const;
+  const maliput::api::RoadNetwork* do_road_network() const;
+
+  const maliput::api::RoadNetwork* road_network_;
+  const api::ObjectBook<maliput::math::Vector3>* object_book_;
+};
+
+}  // namespace object
+}  // namespace maliput

maliput_object/src/base/CMakeLists.txt

@@ -4,6 +4,7 @@
 
 set(BASE_SOURCES
   bounding_box.cc
+  simple_object_query.cc
 )
 
 add_library(base ${BASE_SOURCES})
@@ -22,8 +23,11 @@ target_include_directories(base
 
 target_link_libraries(base
   PUBLIC
+  maliput::api
   maliput::common
   maliput::math
+  maliput::routing
+  maliput_object::api
 )
 
 ##############################################################################

maliput_object/src/base/simple_object_query.cc

@@ -0,0 +1,106 @@
+// Copyright 2022 Toyota Research Institute
+#include "maliput_object/base/simple_object_query.h"
+
+#include <algorithm>
+
+#include <maliput/common/maliput_throw.h>
+#include <maliput/routing/derive_lane_s_routes.h>
+
+#include "maliput_object/base/bounding_box.h"
+
+namespace maliput {
+namespace object {
+
+SimpleObjectQuery::SimpleObjectQuery(const maliput::api::RoadNetwork* road_network,
+                                     const api::ObjectBook<maliput::math::Vector3>* object_book)
+    : road_network_(road_network), object_book_(object_book) {
+  MALIPUT_THROW_UNLESS(road_network_ != nullptr);
+  MALIPUT_THROW_UNLESS(object_book != nullptr);
+}
+
+std::vector<const maliput::api::Lane*> SimpleObjectQuery::DoFindOverlappingLanesIn(
+    const api::Object<maliput::math::Vector3>* object) const {
+  MALIPUT_THROW_UNLESS(object != nullptr);
+  return DoFindOverlappingLanesIn(object, api::OverlappingType::kIntersected);
+}
+std::vector<const maliput::api::Lane*> SimpleObjectQuery::DoFindOverlappingLanesIn(
+    const api::Object<maliput::math::Vector3>* object, const api::OverlappingType& overlapping_type) const {
+  MALIPUT_THROW_UNLESS(object != nullptr);
+  std::vector<const maliput::api::Lane*> overlapping_lanes;
+
+  // TODO(francocipollone): BoundingRegion should provide a method to obtain the vertices of the bounding region to
+  // avoid casting.
+  const auto bb = static_cast<const BoundingBox&>(object->bounding_region());
+  const auto vertices = bb.get_vertices();
+
+  for (const auto& vertex : vertices) {
+    // FindRoadPosition at each vertex of the bounding box using a radius large enough to include the center of the
+    // bounding box.
+    const double radius = (object->position() - vertex).norm();
+    const std::vector<maliput::api::RoadPositionResult> road_position_results =
+        road_network_->road_geometry()->FindRoadPositions(maliput::api::InertialPosition::FromXyz(vertex), radius);
+    // The RoadPositionResults contain the closest points to the lanes contained in the sphere,
+    // There could be lanes that even though overlapp with the object, their closest point to the vertex is outside the
+    // bounding region, leading to not tracking those lanes. Therefore, once the lanes located in the sphere are
+    // obtained, they are queried to obtain the closest RoadPosition to the center of bounding region.
+    for (const auto& road_position_result : road_position_results) {
+      // Lane could have been already added by other road_position_result.
+      if (std::find(overlapping_lanes.begin(), overlapping_lanes.end(), road_position_result.road_position.lane) !=
+          overlapping_lanes.end()) {
+        continue;
+      }
+      const maliput::api::Lane* lane = road_position_result.road_position.lane;
+      const maliput::api::LanePositionResult closest_lane_position_result =
+          lane->ToLanePosition(maliput::api::InertialPosition::FromXyz(object->position()));
+      // The lane position result will contain lane position that could lay outside the lane boundary(always within the
+      // segment bounds). Therefore, the lane position is compared with the lane bounds of the lane. In case the lane
+      // position is outside the lane bounds, the lane position and nearest position are recomputed with the boundary
+      // r-coordinate.
+      maliput::api::LanePosition lane_pos = closest_lane_position_result.lane_position;
+      maliput::api::InertialPosition neareast_pos = closest_lane_position_result.nearest_position;
+      const auto lane_bounds = lane->lane_bounds(lane_pos.s());
+      if (lane_pos.r() > lane_bounds.max() || lane_pos.r() < lane_bounds.min()) {
+        lane_pos.set_r(std::clamp(lane_pos.r(), lane_bounds.min(), lane_bounds.max()));
+        neareast_pos = lane->ToInertialPosition(lane_pos);
+      }
+
+      // Check if the lane's closest point is within the bounding region.
+      if (object->bounding_region().Contains(neareast_pos.xyz())) {
+        overlapping_lanes.push_back(lane);
+      }
+    }
+  }
+
+  if (overlapping_type == api::OverlappingType::kIntersected) {
+    return overlapping_lanes;
+  } else if (overlapping_type == api::OverlappingType::kContained) {
+    MALIPUT_THROW_MESSAGE("Not implemented yet for kContained overlapping type.");
+  } else {
+    MALIPUT_THROW_MESSAGE("Not implemented yet for kDisjointed overlapping type.");
+  }
+}
+
+std::optional<const maliput::api::LaneSRoute> SimpleObjectQuery::DoRoute(
+    const api::Object<maliput::math::Vector3>* origin, const api::Object<maliput::math::Vector3>* target) const {
+  const auto origin_road_pos_result =
+      road_network_->road_geometry()->ToRoadPosition(maliput::api::InertialPosition::FromXyz(origin->position()));
+  const auto target_road_pos_result =
+      road_network_->road_geometry()->ToRoadPosition(maliput::api::InertialPosition::FromXyz(target->position()));
+  const auto lane_s_route =
+      maliput::routing::DeriveLaneSRoutes(origin_road_pos_result.road_position, target_road_pos_result.road_position,
+                                          std::numeric_limits<double>::infinity());
+  if (lane_s_route.empty()) {
+    return std::nullopt;
+  }
+  const auto min_route =
+      std::min_element(lane_s_route.begin(), lane_s_route.end(),
+                       [](const maliput::api::LaneSRoute& left, const maliput::api::LaneSRoute& right) {
+                         return left.length() < right.length();
+                       });
+  return std::make_optional(*min_route);
+}
+const api::ObjectBook<maliput::math::Vector3>* SimpleObjectQuery::do_object_book() const { return object_book_; }
+const maliput::api::RoadNetwork* SimpleObjectQuery::do_road_network() const { return {road_network_}; }
+
+}  // namespace object
+}  // namespace maliput

maliput_object/test/base/CMakeLists.txt

@@ -1,3 +1,4 @@
+ament_add_gmock(simple_object_query_test simple_object_query_test.cc)
 ament_add_gtest(bounding_box_test bounding_box_test.cc)
 
 macro(add_dependencies_to_test target)
@@ -11,11 +12,15 @@ macro(add_dependencies_to_test target)
       )
 
       target_link_libraries(${target}
+        maliput::api
         maliput::common
+        maliput::test_utilities
+        maliput_object::api
         maliput_object::base
       )
 
     endif()
 endmacro()
 
 add_dependencies_to_test(bounding_box_test)
+add_dependencies_to_test(simple_object_query_test)

maliput_object/test/base/simple_object_query_test.cc

@@ -0,0 +1,43 @@
+#include "maliput_object/base/simple_object_query.h"
+
+#include <memory>
+
+#include <gtest/gtest.h>
+#include <maliput/api/road_network.h>
+#include <maliput/common/assertion_error.h>
+#include <maliput/test_utilities/mock.h>
+
+#include "maliput_object/api/object.h"
+#include "maliput_object/test_utilities/mock.h"
+
+namespace maliput {
+namespace object {
+namespace test {
+namespace {
+
+class SimpleObjectQueryTest : public ::testing::Test {
+ public:
+  std::unique_ptr<maliput::api::RoadNetwork> road_network_ = maliput::api::test::CreateRoadNetwork();
+  std::unique_ptr<api::ObjectBook<maliput::math::Vector3>> object_book_ =
+      std::make_unique<test_utilities::MockObjectBook<maliput::math::Vector3>>();
+};
+
+TEST_F(SimpleObjectQueryTest, Constructor) {
+  EXPECT_THROW(SimpleObjectQuery(nullptr, object_book_.get()), maliput::common::assertion_error);
+  EXPECT_THROW(SimpleObjectQuery(road_network_.get(), nullptr), maliput::common::assertion_error);
+  EXPECT_NO_THROW(SimpleObjectQuery(road_network_.get(), object_book_.get()));
+}
+
+TEST_F(SimpleObjectQueryTest, Getters) {
+  SimpleObjectQuery dut(road_network_.get(), object_book_.get());
+  EXPECT_EQ(road_network_.get(), dut.road_network());
+  EXPECT_EQ(object_book_.get(), dut.object_book());
+}
+
+// Route and FindOverlappingIn methods are easier to test via integration tests. They are tested at
+// maliput_integration_tests package.
+
+}  // namespace
+}  // namespace test
+}  // namespace object
+}  // namespace maliput