Skip to content

Commit

Permalink
Routing service (#20)
Browse files Browse the repository at this point in the history 
Signed-off-by: Agustin Alba Chicar <[email protected]>
  • Loading branch information
@agalbachicar
agalbachicar authored Jan 23, 2023
1 parent c5760d0 commit ce8c291
Show file tree
Hide file tree
Showing 24 changed files with 1,137 additions and 2 deletions.
63 changes: 63 additions & 0 deletions maliput_ros/include/maliput_ros/ros/maliput_query.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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_{};
};
Expand Down
56 changes: 56 additions & 0 deletions maliput_ros/include/maliput_ros/ros/maliput_query_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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;
Expand All @@ -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 =
Expand All @@ -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.
Expand All @@ -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.
Expand All @@ -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();
Expand Down Expand Up @@ -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_;
};
Expand Down
Loading

0 comments on commit ce8c291

Please sign in to comment.