Routing service

maliput_ros/include/maliput_ros/ros/maliput_query.h

@@ -32,22 +32,31 @@
 #include <memory>
 #include <optional>
 #include <utility>
+#include <vector>
 
 #include <maliput/api/branch_point.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>
 #include <maliput/api/road_network.h>
 #include <maliput/api/segment.h>
 #include <maliput/common/maliput_throw.h>
+#include <maliput/routing/derive_lane_s_routes.h>
 
 namespace maliput_ros {
 namespace ros {
 
 /// @brief Proxy to a maliput::api::RoadNetwork to make queries to it.
 class MaliputQuery final {
  public:
+  struct LaneBoundaries {
+    maliput::api::RBounds lane_boundaries{};
+    maliput::api::RBounds segment_boundaries{};
+    maliput::api::HBounds elevation_boundaries{};
+  };
+
   /// @brief Constructs a MaliputQuery.
   /// @param[in] road_network The maliput::api::RoadNetwork to hold. It must not be nullptr.
   /// @throws maliput::common::assertion_error When @p road_network is nullptr.
@@ -105,6 +114,60 @@ class MaliputQuery final {
     return road_network_->road_geometry()->FindRoadPositions(inertial_position, radius);
   }
 
+  /// Computes the INERTIAL Frame position and orientation of a given maliput::api::RoadPosition @p road_position.
+  /// @param[in] road_position The maliput::api::RoadPosition to map into the INERTIAL Frame.
+  /// @return An optional of a pair holding the maliput::api::InertialPosition and the maliput::api::Rotation at @p
+  /// road_position. When the @p road_position.lane is nullptr, it returns std::nullopt.
+  inline std::optional<std::pair<maliput::api::InertialPosition, maliput::api::Rotation>> ToInertialPose(
+      const maliput::api::RoadPosition& road_position) const {
+    return road_position.lane == nullptr
+               ? std::optional<std::pair<maliput::api::InertialPosition, maliput::api::Rotation>>{}
+               : std::make_pair(road_position.lane->ToInertialPosition(road_position.pos),
+                                road_position.lane->GetOrientation(road_position.pos));
+  }
+
+  /// Computes the motion derivatives at @p road_position scaled by @p velocity.
+  /// @param[in] road_position The maliput::api::RoadPosition where to compute the motion derivatives.
+  /// @param[in] velocity The maliput::api::IsoLaneVelocity to scale the derivatives.
+  /// @return An optional with the motion derivatives packed into a maliput::api::LanePosition.
+  /// When the @p road_position.lane is nullptr, it returns std::nullopt.
+  inline maliput::api::LanePosition EvalMotionDerivatives(const maliput::api::RoadPosition& road_position,
+                                                          const maliput::api::IsoLaneVelocity& velocity) const {
+    return road_position.lane == nullptr ? maliput::api::LanePosition{}
+                                         : road_position.lane->EvalMotionDerivatives(road_position.pos, velocity);
+  }
+
+  /// Computes the lateral and elevation boundaries a given maliput::api::Lane* at @p road_position.
+  /// @param[in] road_position The maliput::api::RoadPosition where to compute the boundaries.
+  /// @return An optional with a LaneBoundaries struct holding the lane, segment and elevation boundaries.
+  /// When the @p road_position.lane is nullptr, it returns std::nullopt.
+  inline std::optional<LaneBoundaries> EvalLaneBoundaries(const maliput::api::RoadPosition& road_position) const {
+    if (road_position.lane == nullptr) {
+      return {};
+    }
+    const maliput::api::Lane* lane = road_position.lane;
+    return {LaneBoundaries{lane->lane_bounds(road_position.pos.s()), lane->segment_bounds(road_position.pos.s()),
+                           lane->elevation_bounds(road_position.pos.s(), road_position.pos.r())}};
+  }
+
+  /// Computes the different maliput::api::LaneSRoutes that joins @p start to @p end.
+  /// @details See maliput::routing::DeriveLaneSRoutes for further details.
+  /// @param[in] start The maliput::api::RoadPosition where to start routing.
+  /// @param[in] end The maliput::api::RoadPosition where to end routing.
+  /// @param[in] max_length_m It is the maximum length of the intermediate lanes
+  /// between @p start and @p end.
+  /// @return A vector of maliput::api::LaneSRoute. If @p start or @p end have nullptr
+  /// as maliput::api::Lanes, or @p max_length_m is negative, or no route has been found
+  /// the vector will be empty.
+  inline std::vector<maliput::api::LaneSRoute> DeriveLaneSRoutes(const maliput::api::RoadPosition& start,
+                                                                 const maliput::api::RoadPosition& end,
+                                                                 double max_length_m) const {
+    if (start.lane == nullptr || end.lane == nullptr || max_length_m < 0.) {
+      return {};
+    }
+    return maliput::routing::DeriveLaneSRoutes(start, end, max_length_m);
+  }
+
  private:
   std::unique_ptr<maliput::api::RoadNetwork> road_network_{};
 };

maliput_ros/include/maliput_ros/ros/maliput_query_node.h

@@ -35,11 +35,15 @@
 #include <utility>
 
 #include <maliput_ros_interfaces/srv/branch_point.hpp>
+#include <maliput_ros_interfaces/srv/derive_lane_s_routes.hpp>
+#include <maliput_ros_interfaces/srv/eval_motion_derivatives.hpp>
 #include <maliput_ros_interfaces/srv/find_road_positions.hpp>
 #include <maliput_ros_interfaces/srv/junction.hpp>
 #include <maliput_ros_interfaces/srv/lane.hpp>
+#include <maliput_ros_interfaces/srv/lane_boundaries.hpp>
 #include <maliput_ros_interfaces/srv/road_geometry.hpp>
 #include <maliput_ros_interfaces/srv/segment.hpp>
+#include <maliput_ros_interfaces/srv/to_inertial_pose.hpp>
 #include <maliput_ros_interfaces/srv/to_road_position.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
@@ -74,13 +78,20 @@ namespace ros {
 ///
 /// This query server offers:
 /// - /branch_point: looks for a maliput::api::BranchPoint by its ID.
+/// - /derive_lane_s_routes: derives all paths from a maliput::api::RoadPosition to another, filtering Lanes whose
+/// length is bigger than a maximum threshold.
+/// - /eval_motion_derivatives: evaluates the motion derivatives at maliput::api::RoadPosition and scales it by a
+/// certain maliput::api::IsoLaneVelocity.
 /// - /find_road_positions: finds all maliput::api::RoadPositionResult in radius distance from a
 /// maliput::api::InertialPosition.
 /// - /junction: looks for a maliput::api::Junction by its ID.
 /// - /lane: looks for a maliput::api::Lane by its ID.
+/// - /lane_boundaries: computes the maliput::api::Lane boundaries at a given maliput::api::RoadPosition.
 /// - /road_geometry: responds the maliput::api::RoadGeometry configuration.
 /// - /segment: looks for a maliput::api::Segment by its ID.
 /// - /to_road_position: maps a maliput::api::InertialPosition into a maliput::api::RoadPosition.
+/// - /to_inertial_pose: maps a maliput::api::RoadPosition into a maliput::api::InertialPosition and the
+/// maliput::api::Rotation there.
 class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode {
  public:
   using LifecyleNodeCallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
@@ -95,11 +106,15 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode {
 
  private:
   static constexpr const char* kBranchPointServiceName = "branch_point";
+  static constexpr const char* kDeriveLaneSRoutes = "derive_lane_s_routes";
+  static constexpr const char* kEvalMotionDerivativesServiceName = "eval_motion_derivatives";
   static constexpr const char* kFindRoadPositionsServiceName = "find_road_positions";
   static constexpr const char* kJunctionServiceName = "junction";
   static constexpr const char* kLaneServiceName = "lane";
+  static constexpr const char* kLaneBoundariesServiceName = "lane_boundaries";
   static constexpr const char* kRoadGeometryServiceName = "road_geometry";
   static constexpr const char* kSegmentServiceName = "segment";
+  static constexpr const char* kToInertialPoseServiceName = "to_inertial_pose";
   static constexpr const char* kToRoadPositionServiceName = "to_road_position";
   static constexpr const char* kYamlConfigurationPath = "yaml_configuration_path";
   static constexpr const char* kYamlConfigurationPathDescription =
@@ -115,6 +130,24 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode {
   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 Derives all paths from a maliput::api::RoadPosition to another.
+  // @details See maliput::routing::DeriveLaneSRoutes for further details.
+  // @pre The node must be in the ACTIVE state.
+  // @param[in] request Holds the maliput::api::RoadPositions and the threshold to query.
+  // @param[out] response Holds the vector maliput::api::LaneSRoutes.
+  void DeriveLaneSRoutesCallback(
+      const std::shared_ptr<maliput_ros_interfaces::srv::DeriveLaneSRoutes::Request> request,
+      std::shared_ptr<maliput_ros_interfaces::srv::DeriveLaneSRoutes::Response> response) const;
+
+  // @brief Evaluates the motion derivatives at a given maliput::api::RoadPosition.
+  // @pre The node must be in the ACTIVE state.
+  // @param[in] request Holds the maliput::api::RoadPosition and the maliput::api::IsoLaneVelocity compute the motion
+  // derivatives.
+  // @param[out] response Packs into a maliput::api::LanePosition the motion derivatives.
+  void EvalMotionDerivativesCallback(
+      const std::shared_ptr<maliput_ros_interfaces::srv::EvalMotionDerivatives::Request> request,
+      std::shared_ptr<maliput_ros_interfaces::srv::EvalMotionDerivatives::Response> response) const;
+
   // @brief Responds all the maliput::api::RoadPositionResults within a given radius distance from a
   // maliput::api::InertialPosition.
   // @pre The node must be in the ACTIVE state.
@@ -138,6 +171,13 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode {
   void LaneCallback(const std::shared_ptr<maliput_ros_interfaces::srv::Lane::Request> request,
                     std::shared_ptr<maliput_ros_interfaces::srv::Lane::Response> response) const;
 
+  // @brief Responds the maliput::api::Lane boundaries at a given maliput::api::RoadPosition.
+  // @pre The node must be in the ACTIVE state.
+  // @param[in] request Holds the maliput::api::RoadPosition where to evaluate the lane boundaries.
+  // @param[out] response Loads the maliput::api::Lane boundaries.
+  void LaneBoundariesCallback(const std::shared_ptr<maliput_ros_interfaces::srv::LaneBoundaries::Request> request,
+                              std::shared_ptr<maliput_ros_interfaces::srv::LaneBoundaries::Response> response) const;
+
   // @brief Responds the maliput::api::RoadGeometry configuration.
   // @pre The node must be in the ACTIVE state.
   // @param[in] request Unused.
@@ -159,6 +199,14 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode {
   void ToRoadPositionCallback(const std::shared_ptr<maliput_ros_interfaces::srv::ToRoadPosition::Request> request,
                               std::shared_ptr<maliput_ros_interfaces::srv::ToRoadPosition::Response> response) const;
 
+  // @brief Computes the INERTIAL Frame transformation of a maliput::api::RoadPosition.
+  // @pre The node must be in the ACTIVE state.
+  // @param[in] request Holds the maliput::api::RoadPosition to transform into a maliput::api::InertialPosition and a
+  // maliput::api::Rotation.
+  // @param[out] response Holds the maliput::api::InertialPosition and the maliput::api::Rotation.
+  void ToInertialPoseCallback(const std::shared_ptr<maliput_ros_interfaces::srv::ToInertialPose::Request> request,
+                              std::shared_ptr<maliput_ros_interfaces::srv::ToInertialPose::Response> response) const;
+
   // @brief Loads the maliput::api::RoadNetwork from the yaml_configuration_path contents.
   // @return true When the load procedure is successful.
   bool LoadMaliputQuery();
@@ -207,18 +255,26 @@ class MaliputQueryNode final : public rclcpp_lifecycle::LifecycleNode {
   std::atomic<bool> is_active_;
   // /branch_point service.
   rclcpp::Service<maliput_ros_interfaces::srv::BranchPoint>::SharedPtr branch_point_srv_;
+  // /derive_lane_s_route service.
+  rclcpp::Service<maliput_ros_interfaces::srv::DeriveLaneSRoutes>::SharedPtr derive_lane_s_routes_srv_;
+  // /eval_motion_derivatives service.
+  rclcpp::Service<maliput_ros_interfaces::srv::EvalMotionDerivatives>::SharedPtr eval_motion_derivatives_srv_;
   // /find_road_positions service.
   rclcpp::Service<maliput_ros_interfaces::srv::FindRoadPositions>::SharedPtr find_road_positions_srv_;
   // /junction service.
   rclcpp::Service<maliput_ros_interfaces::srv::Junction>::SharedPtr junction_srv_;
   // /lane service.
   rclcpp::Service<maliput_ros_interfaces::srv::Lane>::SharedPtr lane_srv_;
+  // /lane_boundaries service.
+  rclcpp::Service<maliput_ros_interfaces::srv::LaneBoundaries>::SharedPtr lane_boundaries_srv_;
   // /road_geometry service.
   rclcpp::Service<maliput_ros_interfaces::srv::RoadGeometry>::SharedPtr road_geometry_srv_;
   // /segment service.
   rclcpp::Service<maliput_ros_interfaces::srv::Segment>::SharedPtr segment_srv_;
   // /to_road_position service.
   rclcpp::Service<maliput_ros_interfaces::srv::ToRoadPosition>::SharedPtr to_road_position_srv_;
+  // /to_inertial_pose service.
+  rclcpp::Service<maliput_ros_interfaces::srv::ToInertialPose>::SharedPtr to_inertial_pose_srv_;
   // Proxy to a maliput::api::RoadNetwork queries.
   std::unique_ptr<maliput_ros::ros::MaliputQuery> maliput_query_;
 };