Add get junction service

maliput_ros/include/maliput_ros/ros/maliput_query.h

@@ -32,6 +32,7 @@
 #include <memory>
 #include <utility>
 
+#include <maliput/api/junction.h>
 #include <maliput/api/road_geometry.h>
 #include <maliput/api/road_network.h>
 #include <maliput/common/maliput_throw.h>
@@ -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<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/junction.hpp>
 #include <maliput_ros_interfaces/srv/road_geometry.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
@@ -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<maliput_ros_interfaces::srv::Junction::Request> request,
+                        std::shared_ptr<maliput_ros_interfaces::srv::Junction::Response> 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<bool> is_active_;
+  // /junction service.
+  rclcpp::Service<maliput_ros_interfaces::srv::Junction>::SharedPtr junction_srv_;
   // /road_geometry service.
   rclcpp::Service<maliput_ros_interfaces::srv::RoadGeometry>::SharedPtr road_geometry_srv_;
   // Proxy to a maliput::api::RoadNetwork queries.

maliput_ros/src/maliput_ros/ros/maliput_query_node.cc

@@ -32,6 +32,7 @@
 #include <functional>
 #include <stdexcept>
 
+#include <maliput/api/junction.h>
 #include <maliput/api/road_network.h>
 #include <maliput/common/maliput_throw.h>
 #include <maliput/plugin/create_road_network.h>
@@ -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<maliput_ros_interfaces::srv::Junction::Request> request,
+    std::shared_ptr<maliput_ros_interfaces::srv::Junction::Response> 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<std::string>();
 }
@@ -93,6 +110,10 @@ void MaliputQueryNode::TearDownMaliputQuery() {
 
 bool MaliputQueryNode::InitializeAllServices() {
   RCLCPP_INFO(get_logger(), "InitializeAllServices");
+
+  junction_srv_ = this->create_service<maliput_ros_interfaces::srv::Junction>(
+      kJunctionServiceName,
+      std::bind(&MaliputQueryNode::JunctionCallback, this, std::placeholders::_1, std::placeholders::_2));
   road_geometry_srv_ = this->create_service<maliput_ros_interfaces::srv::RoadGeometry>(
       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&) {

maliput_ros/test/maliput_ros/ros/maliput_mock.h

@@ -29,10 +29,14 @@
 #pragma once
 
 #include <optional>
+#include <unordered_map>
 #include <vector>
 
 #include <gmock/gmock.h>
+#include <maliput/api/branch_point.h>
 #include <maliput/api/intersection_book.h>
+#include <maliput/api/junction.h>
+#include <maliput/api/lane.h>
 #include <maliput/api/lane_data.h>
 #include <maliput/api/regions.h>
 #include <maliput/api/road_geometry.h>
@@ -45,6 +49,7 @@
 #include <maliput/api/rules/road_rulebook.h>
 #include <maliput/api/rules/rule_registry.h>
 #include <maliput/api/rules/traffic_light_book.h>
+#include <maliput/api/segment.h>
 
 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<maliput::api::LaneEnd>, 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<maliput::api::LaneEnd>, 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<maliput::api::LaneId, const maliput::api::Lane*>&), 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