From 02468310479a329d27184d0e61ba25d7c2053321 Mon Sep 17 00:00:00 2001 From: Agustin Alba Chicar Date: Thu, 12 Jan 2023 11:07:30 -0300 Subject: [PATCH] Add get junction service (#14) * Adds new mocks and standardizes the macros. * Adds test to MaliputQuery. * Adds the junction service call and its tests. Signed-off-by: Agustin Alba Chicar --- .../include/maliput_ros/ros/maliput_query.h | 8 ++ .../maliput_ros/ros/maliput_query_node.h | 12 ++ .../src/maliput_ros/ros/maliput_query_node.cc | 22 +++ .../test/maliput_ros/ros/maliput_mock.h | 76 ++++++++++ .../ros/maliput_query_node_test.cc | 121 ++++++++++++++-- .../maliput_ros/ros/maliput_query_test.cc | 14 ++ .../maliput_ros_translation/convert_test.cc | 134 ++++++++++-------- 7 files changed, 321 insertions(+), 66 deletions(-) diff --git a/maliput_ros/include/maliput_ros/ros/maliput_query.h b/maliput_ros/include/maliput_ros/ros/maliput_query.h index 8a45b24..9fd7d4b 100644 --- a/maliput_ros/include/maliput_ros/ros/maliput_query.h +++ b/maliput_ros/include/maliput_ros/ros/maliput_query.h @@ -32,6 +32,7 @@ #include #include +#include #include #include #include @@ -53,6 +54,13 @@ class MaliputQuery final { /// @return The maliput::api::RoadGeometry. inline const maliput::api::RoadGeometry* road_geometry() const { return road_network_->road_geometry(); } + /// Finds a maliput::api::Junction by its ID. + /// @param[in] id The maliput::api::JunctionId. + /// @return A maliput::api::Junction when @p id refers to a valid maliput::api::Junction. Otherwise, nullptr.. + inline const maliput::api::Junction* GetJunctionBy(const maliput::api::JunctionId& id) const { + return road_network_->road_geometry()->ById().GetJunction(id); + } + private: std::unique_ptr road_network_{}; }; diff --git a/maliput_ros/include/maliput_ros/ros/maliput_query_node.h b/maliput_ros/include/maliput_ros/ros/maliput_query_node.h index c347b9d..c94322c 100644 --- a/maliput_ros/include/maliput_ros/ros/maliput_query_node.h +++ b/maliput_ros/include/maliput_ros/ros/maliput_query_node.h @@ -34,6 +34,7 @@ #include #include +#include #include #include #include @@ -67,6 +68,7 @@ namespace ros { /// - MaliputQueryNode::on_cleanup(): the maliput::api::RoadNetwork, the MaliputQuery, and the services are torn down. /// /// This query server offers: +/// - /junction: looks for a maliput::api::Junction by its ID. /// - /road_geometry: responds the maliput::api::RoadGeometry configuration. class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode { public: @@ -81,6 +83,7 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode { const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); private: + static constexpr const char* kJunctionServiceName = "junction"; static constexpr const char* kRoadGeometryServiceName = "road_geometry"; static constexpr const char* kYamlConfigurationPath = "yaml_configuration_path"; static constexpr const char* kYamlConfigurationPathDescription = @@ -89,6 +92,13 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode { // @return The path to the YAMl file containing the maliput plugin configuration from the node parameter. std::string GetMaliputYamlFilePath() const; + // @brief Responds the maliput::api::Junction configuration. + // @pre The node must be in the ACTIVE state. + // @param[in] request Holds the maliput::api::JunctionId to query. + // @param[out] response Loads the maliput::api::Junction description. + void JunctionCallback(const std::shared_ptr request, + std::shared_ptr response) const; + // @brief Responds the maliput::api::RoadGeometry configuration. // @pre The node must be in the ACTIVE state. // @param[in] request Unused. @@ -142,6 +152,8 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode { // Works as a barrier to all service callbacks. When it is true, callbacks can operate. std::atomic is_active_; + // /junction service. + rclcpp::Service::SharedPtr junction_srv_; // /road_geometry service. rclcpp::Service::SharedPtr road_geometry_srv_; // Proxy to a maliput::api::RoadNetwork queries. diff --git a/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc b/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc index 2de3b45..3f49863 100644 --- a/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc +++ b/maliput_ros/src/maliput_ros/ros/maliput_query_node.cc @@ -32,6 +32,7 @@ #include #include +#include #include #include #include @@ -66,6 +67,22 @@ void MaliputQueryNode::RoadGeometryCallback( response->road_geometry = maliput_ros_translation::ToRosMessage(maliput_query_->road_geometry()); } +void MaliputQueryNode::JunctionCallback( + const std::shared_ptr request, + std::shared_ptr response) const { + RCLCPP_INFO(get_logger(), "JunctionCallback"); + if (!is_active_.load()) { + RCLCPP_WARN(get_logger(), "The node is not active yet."); + return; + } + if (request->id.id.empty()) { + RCLCPP_ERROR(get_logger(), "Request /junction with invalid value for JunctionId."); + return; + } + response->junction = maliput_ros_translation::ToRosMessage( + maliput_query_->GetJunctionBy(maliput_ros_translation::FromRosMessage(request->id))); +} + std::string MaliputQueryNode::GetMaliputYamlFilePath() const { return this->get_parameter(kYamlConfigurationPath).get_parameter_value().get(); } @@ -93,6 +110,10 @@ void MaliputQueryNode::TearDownMaliputQuery() { bool MaliputQueryNode::InitializeAllServices() { RCLCPP_INFO(get_logger(), "InitializeAllServices"); + + junction_srv_ = this->create_service( + kJunctionServiceName, + std::bind(&MaliputQueryNode::JunctionCallback, this, std::placeholders::_1, std::placeholders::_2)); road_geometry_srv_ = this->create_service( kRoadGeometryServiceName, std::bind(&MaliputQueryNode::RoadGeometryCallback, this, std::placeholders::_1, std::placeholders::_2)); @@ -102,6 +123,7 @@ bool MaliputQueryNode::InitializeAllServices() { void MaliputQueryNode::TearDownAllServices() { RCLCPP_INFO(get_logger(), "TearDownAllServices"); road_geometry_srv_.reset(); + junction_srv_.reset(); } MaliputQueryNode::LifecyleNodeCallbackReturn MaliputQueryNode::on_activate(const rclcpp_lifecycle::State&) { diff --git a/maliput_ros/test/maliput_ros/ros/maliput_mock.h b/maliput_ros/test/maliput_ros/ros/maliput_mock.h index 91a4773..7dd79f1 100644 --- a/maliput_ros/test/maliput_ros/ros/maliput_mock.h +++ b/maliput_ros/test/maliput_ros/ros/maliput_mock.h @@ -29,10 +29,14 @@ #pragma once #include +#include #include #include +#include #include +#include +#include #include #include #include @@ -45,6 +49,7 @@ #include #include #include +#include namespace maliput_ros { namespace ros { @@ -164,6 +169,77 @@ class RangeValueRuleStateProviderMock final : public maliput::api::rules::RangeV (const maliput::api::RoadPosition&, const maliput::api::rules::Rule::TypeId&, double), (const)); }; +/// @brief Google mock maliput::api::BranchPoint. +class BranchPointMock final : public maliput::api::BranchPoint { + public: + MOCK_METHOD(maliput::api::BranchPointId, do_id, (), (const)); + MOCK_METHOD(const maliput::api::RoadGeometry*, do_road_geometry, (), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetConfluentBranches, (const maliput::api::LaneEnd& end), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetOngoingBranches, (const maliput::api::LaneEnd& end), (const)); + MOCK_METHOD(std::optional, DoGetDefaultBranch, (const maliput::api::LaneEnd& end), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetASide, (), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetBSide, (), (const)); +}; + +/// @brief Google mock maliput::api::Lane. +class LaneMock final : public maliput::api::Lane { + public: + MOCK_METHOD(maliput::api::LaneId, do_id, (), (const)); + MOCK_METHOD(int, do_index, (), (const)); + MOCK_METHOD(const maliput::api::Segment*, do_segment, (), (const)); + MOCK_METHOD(const maliput::api::Lane*, do_to_left, (), (const)); + MOCK_METHOD(const maliput::api::Lane*, do_to_right, (), (const)); + MOCK_METHOD(double, do_length, (), (const)); + MOCK_METHOD(const maliput::api::BranchPoint*, DoGetBranchPoint, (const maliput::api::LaneEnd::Which), (const)); + MOCK_METHOD(std::optional, DoGetDefaultBranch, (const maliput::api::LaneEnd::Which), (const)); + MOCK_METHOD(maliput::api::RBounds, do_lane_bounds, (double), (const)); + MOCK_METHOD(maliput::api::RBounds, do_segment_bounds, (double), (const)); + MOCK_METHOD(maliput::api::HBounds, do_elevation_bounds, (double, double), (const)); + MOCK_METHOD(maliput::api::InertialPosition, DoToInertialPosition, (const maliput::api::LanePosition&), (const)); + MOCK_METHOD(maliput::api::Rotation, DoGetOrientation, (const maliput::api::LanePosition&), (const)); + MOCK_METHOD(maliput::api::LanePosition, DoEvalMotionDerivatives, + (const maliput::api::LanePosition&, const maliput::api::IsoLaneVelocity&), (const)); + MOCK_METHOD(maliput::api::LanePositionResult, DoToLanePosition, (const maliput::api::InertialPosition&), (const)); + MOCK_METHOD(maliput::api::LanePositionResult, DoToSegmentPosition, (const maliput::api::InertialPosition&), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetConfluentBranches, (const maliput::api::LaneEnd::Which), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetOngoingBranches, (const maliput::api::LaneEnd::Which), (const)); +}; + +/// @brief Google mock maliput::api::LaneEndSet. +class LaneEndSetMock final : public maliput::api::LaneEndSet { + public: + MOCK_METHOD(int, do_size, (), (const)); + MOCK_METHOD(const maliput::api::LaneEnd&, do_get, (int), (const)); +}; + +/// @brief Google mock maliput::api::Segment. +class SegmentMock final : public maliput::api::Segment { + public: + MOCK_METHOD(maliput::api::SegmentId, do_id, (), (const)); + MOCK_METHOD(const maliput::api::Junction*, do_junction, (), (const)); + MOCK_METHOD(int, do_num_lanes, (), (const)); + MOCK_METHOD(const maliput::api::Lane*, do_lane, (int), (const)); +}; + +/// @brief Google mock maliput::api::Junction. +class JunctionMock final : public maliput::api::Junction { + public: + MOCK_METHOD(maliput::api::JunctionId, do_id, (), (const)); + MOCK_METHOD(const maliput::api::RoadGeometry*, do_road_geometry, (), (const)); + MOCK_METHOD(int, do_num_segments, (), (const)); + MOCK_METHOD(const maliput::api::Segment*, do_segment, (int), (const)); +}; + +/// @brief Google mock maliput::api::RoadGeometry::IdIndex. +class IdIndexMock final : public maliput::api::RoadGeometry::IdIndex { + public: + MOCK_METHOD(const maliput::api::Lane*, DoGetLane, (const maliput::api::LaneId&), (const)); + MOCK_METHOD((const std::unordered_map&), DoGetLanes, (), (const)); + MOCK_METHOD(const maliput::api::Segment*, DoGetSegment, (const maliput::api::SegmentId&), (const)); + MOCK_METHOD(const maliput::api::Junction*, DoGetJunction, (const maliput::api::JunctionId&), (const)); + MOCK_METHOD(const maliput::api::BranchPoint*, DoGetBranchPoint, (const maliput::api::BranchPointId&), (const)); +}; + } // namespace test } // namespace ros } // namespace maliput_ros diff --git a/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc b/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc index 6f9a533..2294037 100644 --- a/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc +++ b/maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -50,6 +51,7 @@ #include "maliput_ros/ros/maliput_mock.h" #include "maliput_ros/ros/maliput_plugin_config_test.h" +#include "maliput_ros_translation/convert.h" namespace maliput_ros { namespace ros { @@ -57,6 +59,7 @@ namespace test { namespace { using ::testing::Return; +using ::testing::ReturnRef; // @return A maliput::api::RoadNetwork populated with gmock versions of it, except maliput::api::rules::RuleRegistry. std::unique_ptr MakeRoadNetworkMock() { @@ -177,6 +180,8 @@ class MaliputQueryNodeAfterConfigurationTest : public MaliputQueryNodeTest { static constexpr const char* kRoadNetowrkMockPluginPath = TEST_MALIPUT_PLUGIN_LIBDIR; static constexpr const char* kRoadGeometryServiceName = "/my_namespace/road_geometry"; static constexpr const char* kRoadGeometryServiceType = "maliput_ros_interfaces/srv/RoadGeometry"; + static constexpr const char* kJunctionServiceName = "/my_namespace/junction"; + static constexpr const char* kJunctionServiceType = "maliput_ros_interfaces/srv/Junction"; const std::string kYamlFilePath{TEST_YAML_CONFIGURATION_PLUGIN_INSTALL_PATH}; const std::chrono::nanoseconds kTimeout = std::chrono::seconds(1); const std::chrono::nanoseconds kTimeoutServiceCall = std::chrono::seconds(1); @@ -277,22 +282,38 @@ TEST_F(MaliputQueryNodeAfterConfigurationTest, ConfigureStateAdvertisesServices) auto service_names_and_types = dut_->get_service_names_and_types(); ASSERT_STREQ(service_names_and_types[kRoadGeometryServiceName][0].c_str(), kRoadGeometryServiceType); + ASSERT_STREQ(service_names_and_types[kJunctionServiceName][0].c_str(), kJunctionServiceType); } // Makes sure services don't process the request when the node is not ACTIVE. TEST_F(MaliputQueryNodeAfterConfigurationTest, CallingServiceBeforeActiveYieldsToFailure) { AddNodeToExecutorAndSpin(dut_); TransitionToConfigureFromUnconfigured(); - ASSERT_TRUE(WaitForService(dut_, kRoadGeometryServiceName, kTimeout, kSleepPeriod)); + { + ASSERT_TRUE(WaitForService(dut_, kRoadGeometryServiceName, kTimeout, kSleepPeriod)); + + auto road_geometry_service = + dut_->create_client(kRoadGeometryServiceName); + auto request = std::make_shared(); + auto future_result = road_geometry_service->async_send_request(request); + const auto future_status = future_result.wait_for(kTimeoutServiceCall); + + ASSERT_TRUE(future_status == std::future_status::ready); + const auto response = future_result.get(); + ASSERT_TRUE(response->road_geometry.id.id.empty()); + } + { + ASSERT_TRUE(WaitForService(dut_, kJunctionServiceName, kTimeout, kSleepPeriod)); - auto road_geometry_service = dut_->create_client(kRoadGeometryServiceName); - auto request = std::make_shared(); - auto future_result = road_geometry_service->async_send_request(request); - const auto future_status = future_result.wait_for(kTimeoutServiceCall); + auto service = dut_->create_client(kJunctionServiceName); + auto request = std::make_shared(); + auto future_result = service->async_send_request(request); + const auto future_status = future_result.wait_for(kTimeoutServiceCall); - ASSERT_TRUE(future_status == std::future_status::ready); - const auto response = future_result.get(); - ASSERT_TRUE(response->road_geometry.id.id.empty()); + ASSERT_TRUE(future_status == std::future_status::ready); + const auto response = future_result.get(); + ASSERT_TRUE(response->junction.id.id.empty()); + } } // Makes sure the node can transtion to the ACTIVE state. @@ -302,7 +323,7 @@ TEST_F(MaliputQueryNodeAfterConfigurationTest, TransitionToActiveIsSuccessful) { } // Test class used to hold the configuration of the RoadGeometryMock and validate the result of the service call. -class TestRoadGeometryServiceCall : public MaliputQueryNodeAfterConfigurationTest { +class RoadGeometryServiceCallTest : public MaliputQueryNodeAfterConfigurationTest { public: static constexpr int kSizeJunctions{0}; static constexpr int kSizeBranchPoints{0}; @@ -320,7 +341,7 @@ class TestRoadGeometryServiceCall : public MaliputQueryNodeAfterConfigurationTes } }; -TEST_F(TestRoadGeometryServiceCall, RoadGeometryRequestInActiveIsSuccessful) { +TEST_F(RoadGeometryServiceCallTest, RoadGeometryRequestInActiveIsSuccessful) { // Note: there is no point in populating Junctions and BranchPoints, it is already covered // in maliput_ros_translation package. EXPECT_CALL(*(road_network_ptrs_.road_geometry), do_id()).WillRepeatedly(Return(kRoadGeometryId)); @@ -351,6 +372,86 @@ TEST_F(TestRoadGeometryServiceCall, RoadGeometryRequestInActiveIsSuccessful) { ASSERT_EQ(response->road_geometry.branch_point_ids.size(), static_cast(kSizeJunctions)); } +// Test class to wrap the tests of /junction service call. +class JunctionByIdServiceCallTest : public MaliputQueryNodeAfterConfigurationTest { + public: + void SetUp() override { + MaliputQueryNodeAfterConfigurationTest::SetUp(); + AddNodeToExecutorAndSpin(dut_); + TransitionToConfigureFromUnconfigured(); + TransitionToActiveFromConfigured(); + } +}; + +TEST_F(JunctionByIdServiceCallTest, ValidResquestAndResponse) { + static constexpr int kSize{2}; + const maliput::api::SegmentId kSgmentIdA{"segment_id_a"}; + const maliput::api::SegmentId kSgmentIdB{"segment_id_b"}; + const maliput::api::RoadGeometryId kRoadGeometryId{"road_geometry_id"}; + const maliput::api::JunctionId kJunctionId{"junction_id"}; + SegmentMock segment_a; + EXPECT_CALL(segment_a, do_id()).WillRepeatedly(Return(kSgmentIdA)); + SegmentMock segment_b; + EXPECT_CALL(segment_b, do_id()).WillRepeatedly(Return(kSgmentIdB)); + JunctionMock junction; + EXPECT_CALL(junction, do_id()).WillRepeatedly(Return(kJunctionId)); + EXPECT_CALL(junction, do_road_geometry()).WillRepeatedly(Return(road_network_ptrs_.road_geometry)); + EXPECT_CALL(junction, do_num_segments()).WillRepeatedly(Return(kSize)); + EXPECT_CALL(junction, do_segment(0)).WillRepeatedly(Return(&segment_a)); + EXPECT_CALL(junction, do_segment(1)).WillRepeatedly(Return(&segment_b)); + IdIndexMock id_index; + EXPECT_CALL(id_index, DoGetJunction(kJunctionId)).WillRepeatedly(Return(&junction)); + EXPECT_CALL(*(road_network_ptrs_.road_geometry), DoById()).WillRepeatedly(ReturnRef(id_index)); + EXPECT_CALL(*(road_network_ptrs_.road_geometry), do_id()).WillRepeatedly(Return(kRoadGeometryId)); + + auto junction_service = dut_->create_client(kJunctionServiceName); + ASSERT_TRUE(junction_service->wait_for_service(kTimeout)); + auto request = std::make_shared(); + request->id = maliput_ros_translation::ToRosMessage(kJunctionId); + auto future_result = junction_service->async_send_request(request); + auto future_status = future_result.wait_for(kTimeoutServiceCall); + ASSERT_TRUE(future_status == std::future_status::ready); + const auto response = future_result.get(); + + ASSERT_EQ(response->junction.id.id, kJunctionId.string()); + ASSERT_EQ(response->junction.road_geometry_id.id, kRoadGeometryId.string()); + ASSERT_EQ(response->junction.segment_ids.size(), static_cast(kSize)); + ASSERT_EQ(response->junction.segment_ids[0].id, kSgmentIdA.string()); + ASSERT_EQ(response->junction.segment_ids[1].id, kSgmentIdB.string()); +} + +TEST_F(JunctionByIdServiceCallTest, InvalidIdReturnsEmptyResponse) { + const maliput::api::RoadGeometryId kRoadGeometryId{"road_geometry_id"}; + const maliput::api::JunctionId kJunctionId{"invalid_id"}; + IdIndexMock id_index; + EXPECT_CALL(id_index, DoGetJunction(kJunctionId)).WillRepeatedly(Return(nullptr)); + EXPECT_CALL(*(road_network_ptrs_.road_geometry), DoById()).WillRepeatedly(ReturnRef(id_index)); + + auto junction_service = dut_->create_client(kJunctionServiceName); + ASSERT_TRUE(junction_service->wait_for_service(kTimeout)); + auto request = std::make_shared(); + request->id = maliput_ros_translation::ToRosMessage(kJunctionId); + auto future_result = junction_service->async_send_request(request); + auto future_status = future_result.wait_for(kTimeoutServiceCall); + ASSERT_TRUE(future_status == std::future_status::ready); + const auto response = future_result.get(); + + ASSERT_TRUE(response->junction.id.id.empty()); +} + +TEST_F(JunctionByIdServiceCallTest, EmptyIdReturnsEmptyResponse) { + auto junction_service = dut_->create_client(kJunctionServiceName); + ASSERT_TRUE(junction_service->wait_for_service(kTimeout)); + auto request = std::make_shared(); + request->id.id = ""; + auto future_result = junction_service->async_send_request(request); + auto future_status = future_result.wait_for(kTimeoutServiceCall); + ASSERT_TRUE(future_status == std::future_status::ready); + const auto response = future_result.get(); + + ASSERT_TRUE(response->junction.id.id.empty()); +} + } // namespace } // namespace test } // namespace ros diff --git a/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc b/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc index 243ad54..a9420f0 100644 --- a/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc +++ b/maliput_ros/test/maliput_ros/ros/maliput_query_test.cc @@ -43,6 +43,9 @@ namespace ros { namespace test { namespace { +using ::testing::Return; +using ::testing::ReturnRef; + // Test class for MaliputQuery. class MaliputQueryTest : public ::testing::Test { public: @@ -85,6 +88,17 @@ TEST_F(MaliputQueryTest, RoadGeometry) { ASSERT_EQ(dut_->road_geometry(), static_cast(road_geometry_ptr_)); } +// Validates MaliputQuery redirects the query via the IdIndex. +TEST_F(MaliputQueryTest, GetJunctionById) { + const maliput::api::JunctionId kJunctionId{"junction_id"}; + const JunctionMock junction; + IdIndexMock id_index; + EXPECT_CALL(id_index, DoGetJunction(kJunctionId)).WillRepeatedly(Return(&junction)); + EXPECT_CALL(*road_geometry_ptr_, DoById()).WillRepeatedly(ReturnRef(id_index)); + + ASSERT_EQ(&junction, dut_->GetJunctionBy(kJunctionId)); +} + } // namespace } // namespace test } // namespace ros diff --git a/maliput_ros_translation/test/maliput_ros_translation/convert_test.cc b/maliput_ros_translation/test/maliput_ros_translation/convert_test.cc index 594d52c..405c70f 100644 --- a/maliput_ros_translation/test/maliput_ros_translation/convert_test.cc +++ b/maliput_ros_translation/test/maliput_ros_translation/convert_test.cc @@ -45,64 +45,86 @@ namespace { using ::testing::Return; using ::testing::ReturnRef; -class RoadGeometryMock final : public maliput::api::test::MockRoadGeometry { +/// @brief Google mock maliput::api::RoadGeometry. +class RoadGeometryMock final : public maliput::api::RoadGeometry { public: - explicit RoadGeometryMock(const maliput::api::RoadGeometryId& id) : maliput::api::test::MockRoadGeometry(id) {} - MOCK_CONST_METHOD0(do_id, maliput::api::RoadGeometryId()); - MOCK_CONST_METHOD0(do_num_junctions, int()); - MOCK_CONST_METHOD1(do_junction, const maliput::api::Junction*(int)); - MOCK_CONST_METHOD0(do_num_branch_points, int()); - MOCK_CONST_METHOD1(do_branch_point, const maliput::api::BranchPoint*(int)); - MOCK_CONST_METHOD0(do_linear_tolerance, double()); - MOCK_CONST_METHOD0(do_angular_tolerance, double()); - MOCK_CONST_METHOD0(do_scale_length, double()); - MOCK_CONST_METHOD0(do_inertial_to_backend_frame_translation, maliput::math::Vector3()); + MOCK_METHOD(maliput::api::RoadGeometryId, do_id, (), (const)); + MOCK_METHOD(int, do_num_junctions, (), (const)); + MOCK_METHOD(const maliput::api::Junction*, do_junction, (int), (const)); + MOCK_METHOD(int, do_num_branch_points, (), (const)); + MOCK_METHOD(const maliput::api::BranchPoint*, do_branch_point, (int), (const)); + MOCK_METHOD(const maliput::api::RoadGeometry::IdIndex&, DoById, (), (const)); + MOCK_METHOD(maliput::api::RoadPositionResult, DoToRoadPosition, + (const maliput::api::InertialPosition&, const std::optional&), (const)); + MOCK_METHOD(std::vector, DoFindRoadPositions, + (const maliput::api::InertialPosition&, double), (const)); + MOCK_METHOD(double, do_linear_tolerance, (), (const)); + MOCK_METHOD(double, do_angular_tolerance, (), (const)); + MOCK_METHOD(double, do_scale_length, (), (const)); + MOCK_METHOD(std::vector, DoSampleAheadWaypoints, + (const maliput::api::LaneSRoute&, double), (const)); + MOCK_METHOD(maliput::math::Vector3, do_inertial_to_backend_frame_translation, (), (const)); }; +/// @brief Google mock maliput::api::BranchPoint. class BranchPointMock final : public maliput::api::BranchPoint { public: - MOCK_CONST_METHOD0(do_id, maliput::api::BranchPointId()); - MOCK_CONST_METHOD0(do_road_geometry, const maliput::api::RoadGeometry*()); - MOCK_CONST_METHOD1(DoGetConfluentBranches, const maliput::api::LaneEndSet*(const maliput::api::LaneEnd& end)); - MOCK_CONST_METHOD1(DoGetOngoingBranches, const maliput::api::LaneEndSet*(const maliput::api::LaneEnd& end)); - MOCK_CONST_METHOD1(DoGetDefaultBranch, std::optional(const maliput::api::LaneEnd& end)); - MOCK_CONST_METHOD0(DoGetASide, const maliput::api::LaneEndSet*()); - MOCK_CONST_METHOD0(DoGetBSide, const maliput::api::LaneEndSet*()); + MOCK_METHOD(maliput::api::BranchPointId, do_id, (), (const)); + MOCK_METHOD(const maliput::api::RoadGeometry*, do_road_geometry, (), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetConfluentBranches, (const maliput::api::LaneEnd& end), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetOngoingBranches, (const maliput::api::LaneEnd& end), (const)); + MOCK_METHOD(std::optional, DoGetDefaultBranch, (const maliput::api::LaneEnd& end), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetASide, (), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetBSide, (), (const)); }; -class LaneMock final : public maliput::geometry_base::test::MockLane { +/// @brief Google mock maliput::api::Lane. +class LaneMock final : public maliput::api::Lane { public: - explicit LaneMock(const maliput::api::LaneId& id) : MockLane(id) {} - MOCK_CONST_METHOD0(do_id, maliput::api::LaneId()); - MOCK_CONST_METHOD0(do_index, int()); - MOCK_CONST_METHOD0(do_segment, const maliput::api::Segment*()); - MOCK_CONST_METHOD0(do_to_left, const maliput::api::Lane*()); - MOCK_CONST_METHOD0(do_to_right, const maliput::api::Lane*()); - MOCK_CONST_METHOD0(do_length, double()); - MOCK_CONST_METHOD1(DoGetBranchPoint, const maliput::api::BranchPoint*(const maliput::api::LaneEnd::Which)); - MOCK_CONST_METHOD1(DoGetDefaultBranch, std::optional(const maliput::api::LaneEnd::Which)); + MOCK_METHOD(maliput::api::LaneId, do_id, (), (const)); + MOCK_METHOD(int, do_index, (), (const)); + MOCK_METHOD(const maliput::api::Segment*, do_segment, (), (const)); + MOCK_METHOD(const maliput::api::Lane*, do_to_left, (), (const)); + MOCK_METHOD(const maliput::api::Lane*, do_to_right, (), (const)); + MOCK_METHOD(double, do_length, (), (const)); + MOCK_METHOD(const maliput::api::BranchPoint*, DoGetBranchPoint, (const maliput::api::LaneEnd::Which), (const)); + MOCK_METHOD(std::optional, DoGetDefaultBranch, (const maliput::api::LaneEnd::Which), (const)); + MOCK_METHOD(maliput::api::RBounds, do_lane_bounds, (double), (const)); + MOCK_METHOD(maliput::api::RBounds, do_segment_bounds, (double), (const)); + MOCK_METHOD(maliput::api::HBounds, do_elevation_bounds, (double, double), (const)); + MOCK_METHOD(maliput::api::InertialPosition, DoToInertialPosition, (const maliput::api::LanePosition&), (const)); + MOCK_METHOD(maliput::api::Rotation, DoGetOrientation, (const maliput::api::LanePosition&), (const)); + MOCK_METHOD(maliput::api::LanePosition, DoEvalMotionDerivatives, + (const maliput::api::LanePosition&, const maliput::api::IsoLaneVelocity&), (const)); + MOCK_METHOD(maliput::api::LanePositionResult, DoToLanePosition, (const maliput::api::InertialPosition&), (const)); + MOCK_METHOD(maliput::api::LanePositionResult, DoToSegmentPosition, (const maliput::api::InertialPosition&), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetConfluentBranches, (const maliput::api::LaneEnd::Which), (const)); + MOCK_METHOD(const maliput::api::LaneEndSet*, DoGetOngoingBranches, (const maliput::api::LaneEnd::Which), (const)); }; +/// @brief Google mock maliput::api::LaneEndSet. class LaneEndSetMock final : public maliput::api::LaneEndSet { public: - MOCK_CONST_METHOD0(do_size, int()); - MOCK_CONST_METHOD1(do_get, const maliput::api::LaneEnd&(int)); + MOCK_METHOD(int, do_size, (), (const)); + MOCK_METHOD(const maliput::api::LaneEnd&, do_get, (int), (const)); }; +/// @brief Google mock maliput::api::SegmentMock. class SegmentMock final : public maliput::api::Segment { public: - MOCK_CONST_METHOD0(do_id, maliput::api::SegmentId()); - MOCK_CONST_METHOD0(do_junction, const maliput::api::Junction*()); - MOCK_CONST_METHOD0(do_num_lanes, int()); - MOCK_CONST_METHOD1(do_lane, const maliput::api::Lane*(int)); + MOCK_METHOD(maliput::api::SegmentId, do_id, (), (const)); + MOCK_METHOD(const maliput::api::Junction*, do_junction, (), (const)); + MOCK_METHOD(int, do_num_lanes, (), (const)); + MOCK_METHOD(const maliput::api::Lane*, do_lane, (int), (const)); }; +/// @brief Google mock maliput::api::JunctionMock. class JunctionMock final : public maliput::api::Junction { public: - MOCK_CONST_METHOD0(do_id, maliput::api::JunctionId()); - MOCK_CONST_METHOD0(do_road_geometry, const maliput::api::RoadGeometry*()); - MOCK_CONST_METHOD0(do_num_segments, int()); - MOCK_CONST_METHOD1(do_segment, const maliput::api::Segment*(int)); + MOCK_METHOD(maliput::api::JunctionId, do_id, (), (const)); + MOCK_METHOD(const maliput::api::RoadGeometry*, do_road_geometry, (), (const)); + MOCK_METHOD(int, do_num_segments, (), (const)); + MOCK_METHOD(const maliput::api::Segment*, do_segment, (int), (const)); }; class IdsToRosMessageTest : public ::testing::Test { @@ -146,7 +168,7 @@ class LaneEndToRosMessageTest : public ::testing::Test { }; TEST_F(LaneEndToRosMessageTest, LaneEndAtStart) { - LaneMock lane(kLaneId); + LaneMock lane; EXPECT_CALL(lane, do_id()).WillRepeatedly(Return(kLaneId)); const maliput::api::LaneEnd lane_end(&lane, maliput::api::LaneEnd::Which::kStart); @@ -157,7 +179,7 @@ TEST_F(LaneEndToRosMessageTest, LaneEndAtStart) { } TEST_F(LaneEndToRosMessageTest, LaneEndAtFinish) { - LaneMock lane(kLaneId); + LaneMock lane; EXPECT_CALL(lane, do_id()).WillRepeatedly(Return(kLaneId)); const maliput::api::LaneEnd lane_end(&lane, maliput::api::LaneEnd::Which::kFinish); @@ -179,9 +201,9 @@ GTEST_TEST(LaneEndSetToRosMessageTest, ValidLaneEndSet) { const maliput::api::LaneId kLaneIdB{"lane_id_b"}; const maliput::api::LaneEnd::Which kWhichA{maliput::api::LaneEnd::Which::kStart}; const maliput::api::LaneEnd::Which kWhichB{maliput::api::LaneEnd::Which::kFinish}; - LaneMock lane_a(kLaneIdA); + LaneMock lane_a; EXPECT_CALL(lane_a, do_id()).WillRepeatedly(Return(kLaneIdA)); - LaneMock lane_b(kLaneIdB); + LaneMock lane_b; EXPECT_CALL(lane_b, do_id()).WillRepeatedly(Return(kLaneIdB)); const maliput::api::LaneEnd lane_end_a(&lane_a, kWhichA); const maliput::api::LaneEnd lane_end_b(&lane_b, kWhichB); @@ -218,9 +240,9 @@ class BranchPointToRosMessageTest : public ::testing::Test { }; TEST_F(BranchPointToRosMessageTest, ValidBranchPoint) { - LaneMock lane_a(kLaneIdA); + LaneMock lane_a; EXPECT_CALL(lane_a, do_id()).WillRepeatedly(Return(kLaneIdA)); - LaneMock lane_b(kLaneIdB); + LaneMock lane_b; EXPECT_CALL(lane_b, do_id()).WillRepeatedly(Return(kLaneIdB)); const maliput::api::LaneEnd lane_end_a(&lane_a, kWhichA); const maliput::api::LaneEnd lane_end_b(&lane_b, kWhichB); @@ -230,7 +252,7 @@ TEST_F(BranchPointToRosMessageTest, ValidBranchPoint) { LaneEndSetMock lane_end_set_b; EXPECT_CALL(lane_end_set_b, do_size()).WillRepeatedly(Return(1)); EXPECT_CALL(lane_end_set_b, do_get(0)).WillRepeatedly(ReturnRef(lane_end_b)); - RoadGeometryMock road_geometry(kRoadGeometryId); + RoadGeometryMock road_geometry; EXPECT_CALL(road_geometry, do_id()).WillRepeatedly(Return(kRoadGeometryId)); BranchPointMock branch_point; EXPECT_CALL(branch_point, do_id()).WillRepeatedly(Return(kBranchPointId)); @@ -251,13 +273,13 @@ TEST_F(BranchPointToRosMessageTest, ValidBranchPoint) { } TEST_F(BranchPointToRosMessageTest, BranchPointWithEmptyBSide) { - LaneMock lane_a(kLaneIdA); + LaneMock lane_a; EXPECT_CALL(lane_a, do_id()).WillRepeatedly(Return(kLaneIdA)); const maliput::api::LaneEnd lane_end_a(&lane_a, kWhichA); LaneEndSetMock lane_end_set_a; EXPECT_CALL(lane_end_set_a, do_size()).WillRepeatedly(Return(1)); EXPECT_CALL(lane_end_set_a, do_get(0)).WillRepeatedly(ReturnRef(lane_end_a)); - RoadGeometryMock road_geometry(kRoadGeometryId); + RoadGeometryMock road_geometry; EXPECT_CALL(road_geometry, do_id()).WillRepeatedly(Return(kRoadGeometryId)); BranchPointMock branch_point; EXPECT_CALL(branch_point, do_id()).WillRepeatedly(Return(kBranchPointId)); @@ -288,9 +310,9 @@ GTEST_TEST(SegmentToRosMessageTest, ValidSegment) { const maliput::api::LaneId kLaneIdB{"lane_id_b"}; const maliput::api::JunctionId kJunctionId{"junction_id"}; const maliput::api::SegmentId kSegmentId{"segment_id"}; - LaneMock lane_a(kLaneIdA); + LaneMock lane_a; EXPECT_CALL(lane_a, do_id()).WillRepeatedly(Return(kLaneIdA)); - LaneMock lane_b(kLaneIdB); + LaneMock lane_b; EXPECT_CALL(lane_b, do_id()).WillRepeatedly(Return(kLaneIdB)); JunctionMock junction; EXPECT_CALL(junction, do_id()).WillRepeatedly(Return(kJunctionId)); @@ -328,7 +350,7 @@ GTEST_TEST(JunctionToRosMessageTest, ValidJunction) { EXPECT_CALL(segment_a, do_id()).WillRepeatedly(Return(kSgmentIdA)); SegmentMock segment_b; EXPECT_CALL(segment_b, do_id()).WillRepeatedly(Return(kSgmentIdB)); - RoadGeometryMock road_geometry(kRoadGeometryId); + RoadGeometryMock road_geometry; EXPECT_CALL(road_geometry, do_id()).WillRepeatedly(Return(kRoadGeometryId)); JunctionMock junction; EXPECT_CALL(junction, do_id()).WillRepeatedly(Return(kJunctionId)); @@ -374,7 +396,7 @@ GTEST_TEST(RoadGeometryToRosMessageTest, ValidRoadGeometry) { EXPECT_CALL(branch_point_a, do_id()).WillRepeatedly(Return(kBranchPointIdA)); BranchPointMock branch_point_b; EXPECT_CALL(branch_point_b, do_id()).WillRepeatedly(Return(kBranchPointIdB)); - RoadGeometryMock road_geometry(kRoadGeometryId); + RoadGeometryMock road_geometry; EXPECT_CALL(road_geometry, do_id()).WillRepeatedly(Return(kRoadGeometryId)); EXPECT_CALL(road_geometry, do_linear_tolerance()).WillRepeatedly(Return(kLinearTolerance)); EXPECT_CALL(road_geometry, do_angular_tolerance()).WillRepeatedly(Return(kAngularTolerance)); @@ -451,13 +473,13 @@ class LaneToRosMessageTest : public ::testing::Test { } SegmentMock segment; - LaneMock left_lane{kLeftLaneId}; - LaneMock right_lane{kRightLaneId}; - LaneMock default_start_lane{kDefaultStartLaneId}; - LaneMock default_finish_lane{kDefaultFinishLaneId}; + LaneMock left_lane; + LaneMock right_lane; + LaneMock default_start_lane; + LaneMock default_finish_lane; BranchPointMock start_branch_point; BranchPointMock finish_branch_point; - LaneMock lane{kLaneId}; + LaneMock lane; }; TEST_F(LaneToRosMessageTest, FullLane) {