diff --git a/.devcontainer/rust-zen/devcontainer.json b/.devcontainer/rust-zen/devcontainer.json index f1a2262..aecc3e5 100644 --- a/.devcontainer/rust-zen/devcontainer.json +++ b/.devcontainer/rust-zen/devcontainer.json @@ -16,7 +16,8 @@ "pbkit.vscode-pbkit", "streetsidesoftware.code-spell-checker", "yzhang.markdown-all-in-one", - "tamasfe.even-better-toml" + "tamasfe.even-better-toml", + "ritwickdey.LiveServer" ] } } diff --git a/maliput-sys/build.rs b/maliput-sys/build.rs index 39c9632..9547c7e 100644 --- a/maliput-sys/build.rs +++ b/maliput-sys/build.rs @@ -36,6 +36,8 @@ fn main() -> Result<(), Box> { println!("cargo:rerun-if-changed=build.rs"); println!("cargo:rerun-if-changed=src/api/api.h"); println!("cargo:rerun-if-changed=src/api/mod.rs"); + println!("cargo:rerun-if-changed=src/api/rules/rules.h"); + println!("cargo:rerun-if-changed=src/api/rules/mod.rs"); println!("cargo:rerun-if-changed=src/lib.rs"); println!("cargo:rerun-if-changed=src/math/math.h"); println!("cargo:rerun-if-changed=src/math/mod.rs"); @@ -58,10 +60,15 @@ fn main() -> Result<(), Box> { println!("cargo:rustc-link-lib=base"); println!("cargo:rustc-link-lib=api"); - cxx_build::bridges(["src/math/mod.rs", "src/api/mod.rs", "src/plugin/mod.rs"]) - .flag_if_supported("-std=c++17") - .include("src") - .compile("maliput-sys"); + cxx_build::bridges([ + "src/math/mod.rs", + "src/api/rules/mod.rs", + "src/api/mod.rs", + "src/plugin/mod.rs", + ]) + .flag_if_supported("-std=c++17") + .include("src") + .compile("maliput-sys"); let maliput_malidrive_plugin_path = PathBuf::from( env::var("DEP_MALIPUT_SDK_MALIPUT_MALIDRIVE_PLUGIN_PATH") diff --git a/maliput-sys/src/api/mod.rs b/maliput-sys/src/api/mod.rs index 733dd90..155f0ea 100644 --- a/maliput-sys/src/api/mod.rs +++ b/maliput-sys/src/api/mod.rs @@ -28,6 +28,8 @@ // 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. +pub mod rules; + #[cxx::bridge(namespace = "maliput::api")] pub mod ffi { /// Shared struct for `Lane` pointers. @@ -52,12 +54,15 @@ pub mod ffi { type Matrix3 = crate::math::ffi::Matrix3; #[namespace = "maliput::math"] type RollPitchYaw = crate::math::ffi::RollPitchYaw; + #[namespace = "maliput::api::rules"] + type TrafficLightBook = crate::api::rules::ffi::TrafficLightBook; #[namespace = "maliput::api"] // RoadNetwork bindings definitions. type RoadNetwork; fn road_geometry(self: &RoadNetwork) -> *const RoadGeometry; fn intersection_book(self: Pin<&mut RoadNetwork>) -> *mut IntersectionBook; + fn traffic_light_book(self: &RoadNetwork) -> *const TrafficLightBook; // RoadGeometry bindings definitions. type RoadGeometry; diff --git a/maliput-sys/src/api/rules/mod.rs b/maliput-sys/src/api/rules/mod.rs new file mode 100644 index 0000000..63a4dfc --- /dev/null +++ b/maliput-sys/src/api/rules/mod.rs @@ -0,0 +1,50 @@ +// 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. + +#[cxx::bridge(namespace = "maliput::api::rules")] +pub mod ffi { + /// Shared struct for `TrafficLight` pointers. + /// This is needed because `*const f` can't be used directly in the CxxVector collection. + struct ConstTrafficLightPtr { + pub traffic_light: *const TrafficLight, + } + unsafe extern "C++" { + include!("api/rules/rules.h"); + + // TrafficLightBook bindings definitions. + type TrafficLightBook; + fn TrafficLightBook_TrafficLights(book: &TrafficLightBook) -> UniquePtr>; + fn TrafficLightBook_GetTrafficLight(book: &TrafficLightBook, id: &String) -> *const TrafficLight; + + // TrafficLight bindings definitions. + type TrafficLight; + fn TrafficLight_id(traffic_light: &TrafficLight) -> String; + } +} diff --git a/maliput-sys/src/api/rules/rules.h b/maliput-sys/src/api/rules/rules.h new file mode 100644 index 0000000..b09dc9f --- /dev/null +++ b/maliput-sys/src/api/rules/rules.h @@ -0,0 +1,66 @@ +// 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. +#pragma once + +#include +#include + +#include +#include + +#include + +#include "maliput-sys/src/api/rules/mod.rs.h" + +namespace maliput { +namespace api { +namespace rules { + +std::unique_ptr> TrafficLightBook_TrafficLights(const TrafficLightBook& traffic_light_book) { + const auto traffic_lights_cpp = traffic_light_book.TrafficLights(); + std::vector traffic_lights; + traffic_lights.reserve(traffic_lights_cpp.size()); + for (const auto traffic_light : traffic_lights_cpp) { + traffic_lights.push_back({traffic_light}); + } + return std::make_unique>(std::move(traffic_lights)); +} + +const TrafficLight* TrafficLightBook_GetTrafficLight(const TrafficLightBook& traffic_light_book, const rust::String& id) { + return traffic_light_book.GetTrafficLight(TrafficLight::Id{std::string(id)}); +} + +rust::String TrafficLight_id(const TrafficLight& traffic_light) { + return traffic_light.id().string(); +} + +} // namespace rules +} // namespace api +} // namespace maliput diff --git a/maliput/src/api/mod.rs b/maliput/src/api/mod.rs index a1261d4..22d7de2 100644 --- a/maliput/src/api/mod.rs +++ b/maliput/src/api/mod.rs @@ -33,6 +33,8 @@ use crate::math::Quaternion; use crate::math::RollPitchYaw; use crate::math::Vector3; +pub mod rules; + /// A RoadGeometry. /// Wrapper around C++ implementation `maliput::api::RoadGeometry`. /// See RoadNetwork for an example of how to get a RoadGeometry. @@ -246,6 +248,17 @@ impl RoadNetwork { }, } } + /// Get the `TrafficLightBook` of the `RoadNetwork`. + pub fn traffic_light_book(&self) -> rules::TrafficLightBook { + let traffic_light_book_ffi = self.rn.traffic_light_book(); + rules::TrafficLightBook { + traffic_light_book: unsafe { + traffic_light_book_ffi + .as_ref() + .expect("Underlying TrafficLightBook is null") + }, + } + } } /// A Lane Position. @@ -1353,7 +1366,7 @@ impl<'a> IntersectionBook<'a> { /// * `id` - The id of the Intersection to get. /// /// ## Returns - /// * An Option + /// * An `Option` /// * Some(Intersection) - The Intersection with the specified id. /// * None - If the Intersection with the specified id does not exist. pub fn get_intersection(&mut self, id: &str) -> Option { diff --git a/maliput/src/api/rules/mod.rs b/maliput/src/api/rules/mod.rs new file mode 100644 index 0000000..d52c3af --- /dev/null +++ b/maliput/src/api/rules/mod.rs @@ -0,0 +1,95 @@ +// 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. + +/// Interface for accessing the [TrafficLight] in the [super::RoadNetwork] +pub struct TrafficLightBook<'a> { + pub(super) traffic_light_book: &'a maliput_sys::api::rules::ffi::TrafficLightBook, +} + +impl<'a> TrafficLightBook<'a> { + /// Get all the [TrafficLight]s in the [TrafficLightBook] + /// ## Return + /// A vector of [TrafficLight]s + pub fn traffic_lights(&self) -> Vec { + let traffic_lights_cpp = maliput_sys::api::rules::ffi::TrafficLightBook_TrafficLights(self.traffic_light_book); + traffic_lights_cpp + .into_iter() + .map(|tl| TrafficLight { + traffic_light: unsafe { tl.traffic_light.as_ref().expect("") }, + }) + .collect::>() + } + + /// Get a [TrafficLight] by its id + /// ## Arguments + /// * `id` - The id of the [TrafficLight] + /// ## Return + /// The [TrafficLight] with the given id. + /// If no [TrafficLight] is found with the given id, return None. + pub fn get_traffic_light(&self, id: &String) -> Option { + let traffic_light = maliput_sys::api::rules::ffi::TrafficLightBook_GetTrafficLight(self.traffic_light_book, id); + if traffic_light.is_null() { + return None; + } + Some(TrafficLight { + traffic_light: unsafe { + traffic_light + .as_ref() + .expect("Unable to get underlying traffic light pointer") + }, + }) + } +} + +/// Models a traffic light. A traffic light is a physical signaling device +/// typically located at road intersections. It contains one or more groups of +/// light bulbs with varying colors and shapes. The lighting patterns of the +/// bulbs signify right-of-way rule information to the agents navigating the +/// intersection (e.g., vehicles, bicyclists, pedestrians, etc.). Typically, an +/// intersection will be managed by multiple traffic lights. +/// +/// Note that traffic lights are physical manifestations of underlying +/// right-of-way rules and thus naturally have lower signal-to-noise ratio +/// relative to the underlying rules. Thus, oracular agents should directly use +/// the underlying right-of-way rules instead of traffic lights when navigating +/// intersections. TrafficLight exists for testing autonomous vehicles that do +/// not have access to right-of-way rules. +pub struct TrafficLight<'a> { + pub traffic_light: &'a maliput_sys::api::rules::ffi::TrafficLight, +} + +impl<'a> TrafficLight<'a> { + /// Get the id of the [TrafficLight]. + /// ## Return + /// The id of the [TrafficLight]. + pub fn id(&self) -> String { + maliput_sys::api::rules::ffi::TrafficLight_id(self.traffic_light) + } +} diff --git a/maliput/tests/traffic_light_tests.rs b/maliput/tests/traffic_light_tests.rs new file mode 100644 index 0000000..e112313 --- /dev/null +++ b/maliput/tests/traffic_light_tests.rs @@ -0,0 +1,53 @@ +// 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; + +#[test] +fn traffic_light_test_api() { + let road_network = common::create_t_shape_road_network_with_books(); + + let road_geometry = road_network.road_geometry(); + assert_eq!(road_geometry.id(), "my_rg_from_rust"); + + let book = road_network.traffic_light_book(); + let expected_traffic_light_id = String::from("EastFacing"); + let traffic_lights = book.traffic_lights(); + assert_eq!(traffic_lights.len(), 1); + let only_traffic_light = traffic_lights.first().expect("No traffic lights found"); + assert_eq!(only_traffic_light.id(), expected_traffic_light_id); + + let traffic_light = book.get_traffic_light(&expected_traffic_light_id); + assert!(traffic_light.is_some()); + let traffic_light = traffic_light.unwrap(); + assert_eq!(traffic_light.id(), expected_traffic_light_id); + + let traffic_light = book.get_traffic_light(&String::from("wrong_traffic_light_id")); + assert!(traffic_light.is_none()); +}