Adds branch point service

maliput_ros/include/maliput_ros/ros/maliput_query.h

@@ -32,6 +32,7 @@
 #include <memory>
 #include <utility>
 
+#include <maliput/api/branch_point.h>
 #include <maliput/api/junction.h>
 #include <maliput/api/lane.h>
 #include <maliput/api/road_geometry.h>
@@ -72,11 +73,18 @@ class MaliputQuery final {
 
   /// Finds a maliput::api::Lane by its ID.
   /// @param[in] id The maliput::api::LaneId.
-  /// @return A maliput::api::Lane when @p id refers to a valid maliput::api::Lane. Otherwise, nullptr..
+  /// @return A maliput::api::Lane when @p id refers to a valid maliput::api::Lane. Otherwise, nullptr.
   inline const maliput::api::Lane* GetLaneBy(const maliput::api::LaneId& id) const {
     return road_network_->road_geometry()->ById().GetLane(id);
   }
 
+  /// Finds a maliput::api::BranchPoint by its ID.
+  /// @param[in] id The maliput::api::BranchPointId.
+  /// @return A maliput::api::BranchPoint when @p id refers to a valid maliput::api::BranchPoint. Otherwise, nullptr.
+  inline const maliput::api::BranchPoint* GetBranchPointBy(const maliput::api::BranchPointId& id) const {
+    return road_network_->road_geometry()->ById().GetBranchPoint(id);
+  }
+
  private:
   std::unique_ptr<maliput::api::RoadNetwork> road_network_{};
 };

maliput_ros/include/maliput_ros/ros/maliput_query_node.h

@@ -34,6 +34,7 @@
 #include <string>
 #include <utility>
 
+#include <maliput_ros_interfaces/srv/branch_point.hpp>
 #include <maliput_ros_interfaces/srv/junction.hpp>
 #include <maliput_ros_interfaces/srv/lane.hpp>
 #include <maliput_ros_interfaces/srv/road_geometry.hpp>
@@ -70,6 +71,7 @@ namespace ros {
 /// - MaliputQueryNode::on_cleanup(): the maliput::api::RoadNetwork, the MaliputQuery, and the services are torn down.
 ///
 /// This query server offers:
+/// - /branch_point: looks for a maliput::api::BranchPoint by its ID.
 /// - /junction: looks for a maliput::api::Junction by its ID.
 /// - /lane: looks for a maliput::api::Lane by its ID.
 /// - /road_geometry: responds the maliput::api::RoadGeometry configuration.
@@ -87,6 +89,7 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode {
                    const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
 
  private:
+  static constexpr const char* kBranchPointServiceName = "branch_point";
   static constexpr const char* kJunctionServiceName = "junction";
   static constexpr const char* kLaneServiceName = "lane";
   static constexpr const char* kRoadGeometryServiceName = "road_geometry";
@@ -98,6 +101,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::BranchPoint configuration.
+  // @pre The node must be in the ACTIVE state.
+  // @param[in] request Holds the maliput::api::BranchPointId to query.
+  // @param[out] response Loads the maliput::api::BranchPoint description.
+  void BranchPointCallback(const std::shared_ptr<maliput_ros_interfaces::srv::BranchPoint::Request> request,
+                           std::shared_ptr<maliput_ros_interfaces::srv::BranchPoint::Response> response) 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.
@@ -172,6 +182,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<bool> is_active_;
+  // /branch_point service.
+  rclcpp::Service<maliput_ros_interfaces::srv::BranchPoint>::SharedPtr branch_point_srv_;
   // /junction service.
   rclcpp::Service<maliput_ros_interfaces::srv::Junction>::SharedPtr junction_srv_;
   // /lane service.

maliput_ros/src/maliput_ros/ros/maliput_query_node.cc

@@ -68,6 +68,22 @@ void MaliputQueryNode::RoadGeometryCallback(
   response->road_geometry = maliput_ros_translation::ToRosMessage(maliput_query_->road_geometry());
 }
 
+void MaliputQueryNode::BranchPointCallback(
+    const std::shared_ptr<maliput_ros_interfaces::srv::BranchPoint::Request> request,
+    std::shared_ptr<maliput_ros_interfaces::srv::BranchPoint::Response> response) const {
+  RCLCPP_INFO(get_logger(), "BranchPointCallback");
+  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 /branch_point with invalid value for BranchPointId.");
+    return;
+  }
+  response->branch_point = maliput_ros_translation::ToRosMessage(
+      maliput_query_->GetBranchPointBy(maliput_ros_translation::FromRosMessage(request->id)));
+}
+
 void MaliputQueryNode::JunctionCallback(
     const std::shared_ptr<maliput_ros_interfaces::srv::Junction::Request> request,
     std::shared_ptr<maliput_ros_interfaces::srv::Junction::Response> response) const {
@@ -142,6 +158,9 @@ void MaliputQueryNode::TearDownMaliputQuery() {
 bool MaliputQueryNode::InitializeAllServices() {
   RCLCPP_INFO(get_logger(), "InitializeAllServices");
 
+  branch_point_srv_ = this->create_service<maliput_ros_interfaces::srv::BranchPoint>(
+      kBranchPointServiceName,
+      std::bind(&MaliputQueryNode::BranchPointCallback, this, std::placeholders::_1, std::placeholders::_2));
   junction_srv_ = this->create_service<maliput_ros_interfaces::srv::Junction>(
       kJunctionServiceName,
       std::bind(&MaliputQueryNode::JunctionCallback, this, std::placeholders::_1, std::placeholders::_2));
@@ -158,6 +177,7 @@ bool MaliputQueryNode::InitializeAllServices() {
 
 void MaliputQueryNode::TearDownAllServices() {
   RCLCPP_INFO(get_logger(), "TearDownAllServices");
+  branch_point_srv_.reset();
   junction_srv_.reset();
   lane_srv_.reset();
   road_geometry_srv_.reset();

maliput_ros/test/maliput_ros/ros/maliput_query_node_test.cc

@@ -180,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* kBranchPointServiceName = "/my_namespace/branch_point";
+  static constexpr const char* kBranchPointServiceType = "maliput_ros_interfaces/srv/BranchPoint";
   static constexpr const char* kJunctionServiceName = "/my_namespace/junction";
   static constexpr const char* kJunctionServiceType = "maliput_ros_interfaces/srv/Junction";
   static constexpr const char* kLaneServiceName = "/my_namespace/lane";
@@ -287,6 +289,7 @@ 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[kBranchPointServiceName][0].c_str(), kBranchPointServiceType);
   ASSERT_STREQ(service_names_and_types[kJunctionServiceName][0].c_str(), kJunctionServiceType);
   ASSERT_STREQ(service_names_and_types[kLaneServiceName][0].c_str(), kLaneServiceType);
   ASSERT_STREQ(service_names_and_types[kSegmentServiceName][0].c_str(), kSegmentServiceType);
@@ -313,6 +316,18 @@ TEST_F(MaliputQueryNodeAfterConfigurationTest, CallingServiceBeforeActiveYieldsT
     const auto response = future_result.get();
     ASSERT_TRUE(response->road_geometry.id.id.empty());
   }
+  {
+    ASSERT_TRUE(WaitForService(dut_, kBranchPointServiceName, kTimeout, kSleepPeriod));
+
+    auto service = dut_->create_client<maliput_ros_interfaces::srv::BranchPoint>(kBranchPointServiceName);
+    auto request = std::make_shared<maliput_ros_interfaces::srv::BranchPoint::Request>();
+    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->branch_point.id.id.empty());
+  }
   {
     ASSERT_TRUE(WaitForService(dut_, kJunctionServiceName, kTimeout, kSleepPeriod));
 
@@ -479,6 +494,7 @@ TEST_F(JunctionByIdServiceCallTest, EmptyIdReturnsEmptyResponse) {
   ASSERT_TRUE(service->wait_for_service(kTimeout));
   auto request = std::make_shared<maliput_ros_interfaces::srv::Junction::Request>();
   request->id.id = "";
+
   auto future_result = service->async_send_request(request);
   auto future_status = future_result.wait_for(kTimeoutServiceCall);
   ASSERT_TRUE(future_status == std::future_status::ready);
@@ -592,7 +608,6 @@ TEST_F(LaneByIdServiceCallTest, ValidResquestAndResponse) {
   const maliput::api::LaneId kDefaultFinishLaneId{"default_finish_lane_id"};
   const maliput::api::LaneEnd::Which kDefaultFinishWhich{maliput::api::LaneEnd::Which::kFinish};
   const maliput::api::LaneId kLaneId{"lane_id"};
-  const std::string kNullLaneId{""};
   SegmentMock segment;
   EXPECT_CALL(segment, do_id()).WillRepeatedly(Return(kSegmentId));
   LaneMock left_lane;
@@ -684,6 +699,99 @@ TEST_F(LaneByIdServiceCallTest, EmptyIdReturnsEmptyResponse) {
   ASSERT_TRUE(response->lane.id.id.empty());
 }
 
+// Test class to wrap the tests of /branch_point service call.
+class BranchPointByIdServiceCallTest : public MaliputQueryNodeAfterConfigurationTest {
+ public:
+  void SetUp() override {
+    MaliputQueryNodeAfterConfigurationTest::SetUp();
+    AddNodeToExecutorAndSpin(dut_);
+    TransitionToConfigureFromUnconfigured();
+    TransitionToActiveFromConfigured();
+  }
+};
+
+TEST_F(BranchPointByIdServiceCallTest, ValidResquestAndResponse) {
+  const maliput::api::LaneId kLaneIdA{"lane_id_a"};
+  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};
+  const maliput::api::RoadGeometryId kRoadGeometryId{"kRoadGeometryId"};
+  const maliput::api::BranchPointId kBranchPointId{"branch_point_id"};
+  LaneMock lane_a;
+  EXPECT_CALL(lane_a, do_id()).WillRepeatedly(Return(kLaneIdA));
+  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);
+  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));
+  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;
+  EXPECT_CALL(road_geometry, do_id()).WillRepeatedly(Return(kRoadGeometryId));
+  BranchPointMock branch_point;
+  EXPECT_CALL(branch_point, do_id()).WillRepeatedly(Return(kBranchPointId));
+  EXPECT_CALL(branch_point, do_road_geometry()).WillRepeatedly(Return(&road_geometry));
+  EXPECT_CALL(branch_point, DoGetASide()).WillRepeatedly(Return(&lane_end_set_a));
+  EXPECT_CALL(branch_point, DoGetBSide()).WillRepeatedly(Return(&lane_end_set_b));
+
+  IdIndexMock id_index;
+  EXPECT_CALL(id_index, DoGetBranchPoint(kBranchPointId)).WillRepeatedly(Return(&branch_point));
+  EXPECT_CALL(*(road_network_ptrs_.road_geometry), DoById()).WillRepeatedly(ReturnRef(id_index));
+  auto request = std::make_shared<maliput_ros_interfaces::srv::BranchPoint::Request>();
+  request->id = maliput_ros_translation::ToRosMessage(kBranchPointId);
+
+  auto service = dut_->create_client<maliput_ros_interfaces::srv::BranchPoint>(kBranchPointServiceName);
+  ASSERT_TRUE(service->wait_for_service(kTimeout));
+  auto future_result = service->async_send_request(request);
+  auto future_status = future_result.wait_for(kTimeoutServiceCall);
+  ASSERT_TRUE(future_status == std::future_status::ready);
+  auto response = future_result.get();
+
+  ASSERT_EQ(response->branch_point.id.id, kBranchPointId.string());
+  ASSERT_EQ(response->branch_point.road_geometry_id.id, kRoadGeometryId.string());
+  ASSERT_EQ(response->branch_point.a_side.lane_ends.size(), static_cast<size_t>(1));
+  ASSERT_EQ(response->branch_point.a_side.lane_ends[0].lane_id.id, kLaneIdA.string());
+  ASSERT_EQ(response->branch_point.a_side.lane_ends[0].end, maliput_ros_interfaces::msg::LaneEnd::WHICHSTART);
+  ASSERT_EQ(response->branch_point.b_side.lane_ends.size(), static_cast<size_t>(1));
+  ASSERT_EQ(response->branch_point.b_side.lane_ends[0].lane_id.id, kLaneIdB.string());
+  ASSERT_EQ(response->branch_point.b_side.lane_ends[0].end, maliput_ros_interfaces::msg::LaneEnd::WHICHFINISH);
+}
+
+TEST_F(BranchPointByIdServiceCallTest, InvalidIdReturnsEmptyResponse) {
+  const maliput::api::BranchPointId kBranchPointId{"invalid_id"};
+  IdIndexMock id_index;
+  EXPECT_CALL(id_index, DoGetBranchPoint(kBranchPointId)).WillRepeatedly(Return(nullptr));
+  EXPECT_CALL(*(road_network_ptrs_.road_geometry), DoById()).WillRepeatedly(ReturnRef(id_index));
+  auto request = std::make_shared<maliput_ros_interfaces::srv::BranchPoint::Request>();
+  request->id = maliput_ros_translation::ToRosMessage(kBranchPointId);
+
+  auto service = dut_->create_client<maliput_ros_interfaces::srv::BranchPoint>(kBranchPointServiceName);
+  ASSERT_TRUE(service->wait_for_service(kTimeout));
+  auto future_result = service->async_send_request(request);
+  auto future_status = future_result.wait_for(kTimeoutServiceCall);
+  ASSERT_TRUE(future_status == std::future_status::ready);
+  auto response = future_result.get();
+
+  ASSERT_TRUE(response->branch_point.id.id.empty());
+}
+
+TEST_F(BranchPointByIdServiceCallTest, EmptyIdReturnsEmptyResponse) {
+  auto request = std::make_shared<maliput_ros_interfaces::srv::BranchPoint::Request>();
+  request->id.id = "";
+
+  auto service = dut_->create_client<maliput_ros_interfaces::srv::BranchPoint>(kBranchPointServiceName);
+  ASSERT_TRUE(service->wait_for_service(kTimeout));
+  auto future_result = service->async_send_request(request);
+  auto future_status = future_result.wait_for(kTimeoutServiceCall);
+  ASSERT_TRUE(future_status == std::future_status::ready);
+  auto response = future_result.get();
+
+  ASSERT_TRUE(response->branch_point.id.id.empty());
+}
+
 }  // namespace
 }  // namespace test
 }  // namespace ros

maliput_ros/test/maliput_ros/ros/maliput_query_test.cc

@@ -121,6 +121,17 @@ TEST_F(MaliputQueryTest, GetLaneById) {
   ASSERT_EQ(dut_->GetLaneBy(kLaneId), &lane);
 }
 
+// Validates MaliputQuery redirects the query via the IdIndex.
+TEST_F(MaliputQueryTest, GetBranchPointById) {
+  const maliput::api::BranchPointId kBranchPointId{"branch_point_id"};
+  const BranchPointMock branch_point;
+  IdIndexMock id_index;
+  EXPECT_CALL(id_index, DoGetBranchPoint(kBranchPointId)).WillRepeatedly(Return(&branch_point));
+  EXPECT_CALL(*road_geometry_ptr_, DoById()).WillRepeatedly(ReturnRef(id_index));
+
+  ASSERT_EQ(dut_->GetBranchPointBy(kBranchPointId), &branch_point);
+}
+
 }  // namespace
 }  // namespace test
 }  // namespace ros