diff --git a/maliput-sys/src/api/api.h b/maliput-sys/src/api/api.h index 785bb99..8d4a61e 100644 --- a/maliput-sys/src/api/api.h +++ b/maliput-sys/src/api/api.h @@ -151,6 +151,10 @@ std::unique_ptr Lane_GetDefaultBranch(const Lane& lane, bool start) { return default_branch ? std::make_unique(*default_branch) : nullptr; } +std::unique_ptr Lane_EvalMotionDerivatives(const Lane& lane, const LanePosition& lane_position, rust::f64 sigma_v, rust::f64 rho_v, rust::f64 eta_v) { + return std::make_unique(lane.EvalMotionDerivatives(lane_position, IsoLaneVelocity{sigma_v, rho_v, eta_v})); +} + std::unique_ptr RoadPosition_new(const Lane* lane, const LanePosition& pos) { return std::make_unique(lane, pos); } diff --git a/maliput-sys/src/api/mod.rs b/maliput-sys/src/api/mod.rs index 49d2786..2b20a51 100644 --- a/maliput-sys/src/api/mod.rs +++ b/maliput-sys/src/api/mod.rs @@ -122,6 +122,13 @@ pub mod ffi { fn Lane_GetConfluentBranches(lane: &Lane, start: bool) -> *const LaneEndSet; fn Lane_GetOngoingBranches(lane: &Lane, start: bool) -> *const LaneEndSet; fn Lane_GetDefaultBranch(lane: &Lane, start: bool) -> UniquePtr; + fn Lane_EvalMotionDerivatives( + lane: &Lane, + lane_position: &LanePosition, + sigma_v: f64, + rho_v: f64, + eta_v: f64, + ) -> UniquePtr; // Segment bindings definitions type Segment; diff --git a/maliput/src/api/mod.rs b/maliput/src/api/mod.rs index 8e5da7b..36d511a 100644 --- a/maliput/src/api/mod.rs +++ b/maliput/src/api/mod.rs @@ -510,6 +510,28 @@ impl HBounds { } } +/// Isometric velocity vector in a `Lane`-frame. +/// +/// sigma_v, rho_v, and eta_v are the components of velocity in a +/// (sigma, rho, eta) coordinate system. (sigma, rho, eta) have the same +/// orientation as the (s, r, h) at any given point in space, however they +/// form an isometric system with a Cartesian distance metric. Hence, +/// IsoLaneVelocity represents a "real" physical velocity vector (albeit +/// with an orientation relative to the road surface). +#[derive(Default, Copy, Clone, Debug, PartialEq)] +pub struct IsoLaneVelocity { + pub sigma_v: f64, + pub rho_v: f64, + pub eta_v: f64, +} + +impl IsoLaneVelocity { + /// Create a new `IsoLaneVelocity` with the given `sigma_v`, `rho_v`, and `eta_v` components. + pub fn new(sigma_v: f64, rho_v: f64, eta_v: f64) -> IsoLaneVelocity { + IsoLaneVelocity { sigma_v, rho_v, eta_v } + } +} + /// A maliput::api::Lane /// Wrapper around C++ implementation `maliput::api::Lane`. pub struct Lane<'a> { @@ -628,6 +650,22 @@ impl<'a> Lane<'a> { let bounds = maliput_sys::api::ffi::Lane_elevation_bounds(self.lane, s, r); HBounds::new(bounds.min(), bounds.max()) } + /// Computes derivatives of [LanePosition] given a velocity vector `velocity`. + /// `velocity` is a isometric velocity vector oriented in the `Lane`-frame + /// at `position`. + /// + /// Returns `Lane`-frame derivatives packed into a [LanePosition] struct. + pub fn eval_motion_derivatives(&self, lane_position: &LanePosition, velocity: &IsoLaneVelocity) -> LanePosition { + LanePosition { + lp: maliput_sys::api::ffi::Lane_EvalMotionDerivatives( + self.lane, + lane_position.lp.as_ref().expect(""), + velocity.sigma_v, + velocity.rho_v, + velocity.eta_v, + ), + } + } /// Returns the lane's BranchPoint for the specified end. pub fn get_branch_point(&self, end: &LaneEnd) -> BranchPoint { assert! { diff --git a/maliput/tests/lane_test.rs b/maliput/tests/lane_test.rs index 1d21285..650f02a 100644 --- a/maliput/tests/lane_test.rs +++ b/maliput/tests/lane_test.rs @@ -90,6 +90,11 @@ fn lane_api() { default_branch.is_none(), branch_point.get_default_branch(&lane_end).is_none() ); + + let velocity = maliput::api::IsoLaneVelocity::new(1., 0., 0.); + let expected_derivatives = maliput::api::LanePosition::new(1., 0., 0.); + let lane_frame_derivatives = lane.eval_motion_derivatives(&maliput::api::LanePosition::new(0., 0., 0.), &velocity); + assert_eq!(expected_derivatives, lane_frame_derivatives); } #[test]