From 0f7ce516e66f08c1937ab3707a857dfd40e7abe1 Mon Sep 17 00:00:00 2001 From: Franco Cipollone <53065142+francocipollone@users.noreply.github.com> Date: Thu, 6 Jun 2024 21:07:30 -0300 Subject: [PATCH] FFI and Rust API for RoadRulebook. (#108) Signed-off-by: Franco Cipollone --- maliput-sys/src/api/mod.rs | 3 ++ maliput-sys/src/api/rules/mod.rs | 4 +++ maliput-sys/src/api/rules/rules.h | 5 +++ maliput/src/api/mod.rs | 7 ++++ maliput/src/api/rules/mod.rs | 54 +++++++++++++++++++++++++++++ maliput/tests/common/mod.rs | 41 ++++++++++++++++++++++ maliput/tests/road_rulebook_test.rs | 46 ++++++++++++++++++++++++ 7 files changed, 160 insertions(+) create mode 100644 maliput/tests/road_rulebook_test.rs diff --git a/maliput-sys/src/api/mod.rs b/maliput-sys/src/api/mod.rs index 155f0ea..a55ac3d 100644 --- a/maliput-sys/src/api/mod.rs +++ b/maliput-sys/src/api/mod.rs @@ -55,6 +55,8 @@ pub mod ffi { #[namespace = "maliput::math"] type RollPitchYaw = crate::math::ffi::RollPitchYaw; #[namespace = "maliput::api::rules"] + type RoadRulebook = crate::api::rules::ffi::RoadRulebook; + #[namespace = "maliput::api::rules"] type TrafficLightBook = crate::api::rules::ffi::TrafficLightBook; #[namespace = "maliput::api"] @@ -63,6 +65,7 @@ pub mod ffi { fn road_geometry(self: &RoadNetwork) -> *const RoadGeometry; fn intersection_book(self: Pin<&mut RoadNetwork>) -> *mut IntersectionBook; fn traffic_light_book(self: &RoadNetwork) -> *const TrafficLightBook; + fn rulebook(self: &RoadNetwork) -> *const RoadRulebook; // RoadGeometry bindings definitions. type RoadGeometry; diff --git a/maliput-sys/src/api/rules/mod.rs b/maliput-sys/src/api/rules/mod.rs index 6583b27..0c093fe 100644 --- a/maliput-sys/src/api/rules/mod.rs +++ b/maliput-sys/src/api/rules/mod.rs @@ -160,6 +160,10 @@ pub mod ffi { fn UniqueBulbGroupId_traffic_light_id(id: &UniqueBulbGroupId) -> String; fn UniqueBulbGroupId_bulb_group_id(id: &UniqueBulbGroupId) -> String; + // RoadRulebook bindings definitions. + type RoadRulebook; + fn RoadRulebook_GetDiscreteValueRule(book: &RoadRulebook, rule_id: &String) -> UniquePtr; + // DiscreteValueRule::DiscreteValue bindings definitions. type DiscreteValueRuleDiscreteValue; fn DiscreteValueRuleDiscreteValue_value(value: &DiscreteValueRuleDiscreteValue) -> String; diff --git a/maliput-sys/src/api/rules/rules.h b/maliput-sys/src/api/rules/rules.h index a76bba0..91e8d3e 100644 --- a/maliput-sys/src/api/rules/rules.h +++ b/maliput-sys/src/api/rules/rules.h @@ -32,6 +32,7 @@ #include #include +#include #include #include #include @@ -193,6 +194,10 @@ rust::i32 DiscreteValueRuleDiscreteValue_severity(const DiscreteValueRuleDiscret return discrete_value.severity; } +std::unique_ptr RoadRulebook_GetDiscreteValueRule(const RoadRulebook& road_rulebook, const rust::String& id) { + return std::make_unique(road_rulebook.GetDiscreteValueRule(Rule::Id{std::string(id)})); +} + std::unique_ptr> DiscreteValueRuleDiscreteValue_related_rules(const DiscreteValueRuleDiscreteValue& discrete_value) { std::vector related_rules; for (const auto& related_rule : discrete_value.related_rules) { diff --git a/maliput/src/api/mod.rs b/maliput/src/api/mod.rs index 22d7de2..c486f01 100644 --- a/maliput/src/api/mod.rs +++ b/maliput/src/api/mod.rs @@ -259,6 +259,13 @@ impl RoadNetwork { }, } } + /// Get the `RoadRulebook` of the `RoadNetwork`. + pub fn rulebook(&self) -> rules::RoadRulebook { + let rulebook_ffi = self.rn.rulebook(); + rules::RoadRulebook { + road_rulebook: unsafe { rulebook_ffi.as_ref().expect("Underlying RoadRulebook is null") }, + } + } } /// A Lane Position. diff --git a/maliput/src/api/rules/mod.rs b/maliput/src/api/rules/mod.rs index ad859c9..f278a1a 100644 --- a/maliput/src/api/rules/mod.rs +++ b/maliput/src/api/rules/mod.rs @@ -480,3 +480,57 @@ impl UniqueBulbGroupId { self.unique_bulb_group_id.string().to_string() } } + +/// Interface for querying "rules of the road". This interface +/// provides access to static information about a road network (i.e., +/// information determined prior to the beginning of a simulation). Some +/// rule types may refer to additional dynamic information which will be +/// provided by other interfaces. +pub struct RoadRulebook<'a> { + pub(super) road_rulebook: &'a maliput_sys::api::rules::ffi::RoadRulebook, +} + +impl<'a> RoadRulebook<'a> { + /// Returns the DiscreteValueRule with the specified `id`. + pub fn get_discrete_value_rule(&self, rule_id: &String) -> DiscreteValueRule { + DiscreteValueRule { + discrete_value_rule: maliput_sys::api::rules::ffi::RoadRulebook_GetDiscreteValueRule( + self.road_rulebook, + rule_id, + ), + } + } +} + +/// ## Rule +/// +/// A Rule may have multiple states that affect agent behavior while it is +/// driving through the rule's zone. The possible states of a Rule must be +/// semantically coherent. The current state of a Rule is given by a +/// [RuleStateProvider]. States can be: +/// +/// - range based ([RangeValueRule]). +/// - discrete ([DiscreteValueRule]). +/// +/// ## DiscreteValueRule +/// +/// [DiscreteValue]s are defined by a string value. +/// Semantics of this rule are based on _all_ possible values that this +/// [DiscreteValueRule::type_id] could have (as specified by RuleRegistry::FindRuleByType()), +/// not only the subset of values that a specific instance of this rule can +/// be in. +pub struct DiscreteValueRule { + discrete_value_rule: cxx::UniquePtr, +} + +impl DiscreteValueRule { + /// Returns the Id of the rule as a string. + pub fn id(&self) -> String { + maliput_sys::api::rules::ffi::DiscreteValueRule_id(&self.discrete_value_rule) + } + /// Returns the type of the rule as a string. + /// Example: "right-of-way-rule-type-id", "direction-usage-rule-type-id" + pub fn type_id(&self) -> String { + maliput_sys::api::rules::ffi::DiscreteValueRule_type_id(&self.discrete_value_rule) + } +} diff --git a/maliput/tests/common/mod.rs b/maliput/tests/common/mod.rs index 679069b..2768a65 100644 --- a/maliput/tests/common/mod.rs +++ b/maliput/tests/common/mod.rs @@ -67,6 +67,47 @@ pub fn create_t_shape_road_network_with_books() -> RoadNetwork { RoadNetwork::new("maliput_malidrive", &road_network_properties) } +#[allow(dead_code)] +pub fn create_loop_road_pedestrian_crosswalk_road_network_with_books() -> RoadNetwork { + let rm = ResourceManager::new(); + let loop_road_pedestrian_crosswalk_xodr_path = rm + .get_resource_path_by_name("maliput_malidrive", "LoopRoadPedestrianCrosswalk.xodr") + .unwrap(); + let loop_road_pedestrian_crosswalk_books_path = rm + .get_resource_path_by_name("maliput_malidrive", "LoopRoadPedestrianCrosswalk.yaml") + .unwrap(); + + let road_network_properties = HashMap::from([ + ("road_geometry_id", "my_rg_from_rust"), + ( + "opendrive_file", + loop_road_pedestrian_crosswalk_xodr_path.to_str().unwrap(), + ), + ( + "road_rule_book", + loop_road_pedestrian_crosswalk_books_path.to_str().unwrap(), + ), + ( + "rule_registry", + loop_road_pedestrian_crosswalk_books_path.to_str().unwrap(), + ), + ( + "traffic_light_book", + loop_road_pedestrian_crosswalk_books_path.to_str().unwrap(), + ), + ( + "phase_ring_book", + loop_road_pedestrian_crosswalk_books_path.to_str().unwrap(), + ), + ( + "intersection_book", + loop_road_pedestrian_crosswalk_books_path.to_str().unwrap(), + ), + ("linear_tolerance", "0.01"), + ]); + RoadNetwork::new("maliput_malidrive", &road_network_properties) +} + #[allow(dead_code)] pub fn assert_inertial_position_equality( left: &maliput::api::InertialPosition, diff --git a/maliput/tests/road_rulebook_test.rs b/maliput/tests/road_rulebook_test.rs new file mode 100644 index 0000000..618dac3 --- /dev/null +++ b/maliput/tests/road_rulebook_test.rs @@ -0,0 +1,46 @@ +// BSD 3-Clause License +// +// Copyright (c) 2024, Woven by Toyota. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +mod common; + +// TShapeRoad maps is being used to test the road rulebook API and its components. +// YAML information about the RoadRulebook can be found at: +// https://github.com/maliput/maliput_malidrive/blob/352601969b1363cc13fe2008c198a3d95843bf5b/resources/LoopRoadPedestrianCrosswalk.yaml#L64 + +#[test] +fn road_rulebook_test_api() { + let road_network = common::create_loop_road_pedestrian_crosswalk_road_network_with_books(); + + let book = road_network.rulebook(); + let expected_rule_id = String::from("Vehicle-Stop-In-Zone-Behavior Rule Type/WestToEastSouth"); + let expected_type_id = String::from("Vehicle-Stop-In-Zone-Behavior Rule Type"); + let rule = book.get_discrete_value_rule(&expected_rule_id); + assert_eq!(rule.id(), expected_rule_id); + assert_eq!(rule.type_id(), expected_type_id); +}