From 55d43baf7bf9b769c02b85faa4a3af50e074c613 Mon Sep 17 00:00:00 2001
From: Franco Cipollone <franco.c@ekumenlabs.com>
Date: Wed, 15 May 2024 21:34:56 +0000
Subject: [PATCH] TrafficLights: Bindings for getting the pose.

Signed-off-by: Franco Cipollone <franco.c@ekumenlabs.com>
---
 maliput-sys/src/api/rules/mod.rs     |  8 ++++++++
 maliput-sys/src/api/rules/rules.h    |  8 ++++++++
 maliput/src/api/rules/mod.rs         | 16 ++++++++++++++++
 maliput/tests/traffic_light_tests.rs | 15 +++++++++++++--
 4 files changed, 45 insertions(+), 2 deletions(-)

diff --git a/maliput-sys/src/api/rules/mod.rs b/maliput-sys/src/api/rules/mod.rs
index 63a4dfc..3c69f8c 100644
--- a/maliput-sys/src/api/rules/mod.rs
+++ b/maliput-sys/src/api/rules/mod.rs
@@ -38,6 +38,12 @@ pub mod ffi {
     unsafe extern "C++" {
         include!("api/rules/rules.h");
 
+        // Forward declarations
+        #[namespace = "maliput::api"]
+        type InertialPosition = crate::api::ffi::InertialPosition;
+        #[namespace = "maliput::api"]
+        type Rotation = crate::api::ffi::Rotation;
+
         // TrafficLightBook bindings definitions.
         type TrafficLightBook;
         fn TrafficLightBook_TrafficLights(book: &TrafficLightBook) -> UniquePtr<CxxVector<ConstTrafficLightPtr>>;
@@ -46,5 +52,7 @@ pub mod ffi {
         // TrafficLight bindings definitions.
         type TrafficLight;
         fn TrafficLight_id(traffic_light: &TrafficLight) -> String;
+        fn TrafficLight_position_road_network(traffic_light: &TrafficLight) -> UniquePtr<InertialPosition>;
+        fn TrafficLight_orientation_road_network(traffic_light: &TrafficLight) -> UniquePtr<Rotation>;
     }
 }
diff --git a/maliput-sys/src/api/rules/rules.h b/maliput-sys/src/api/rules/rules.h
index b09dc9f..b571ce7 100644
--- a/maliput-sys/src/api/rules/rules.h
+++ b/maliput-sys/src/api/rules/rules.h
@@ -61,6 +61,14 @@ rust::String TrafficLight_id(const TrafficLight& traffic_light) {
   return traffic_light.id().string();
 }
 
+std::unique_ptr<maliput::api::InertialPosition> TrafficLight_position_road_network(const TrafficLight& traffic_light) {
+  return std::make_unique<maliput::api::InertialPosition>(traffic_light.position_road_network());
+}
+
+std::unique_ptr<maliput::api::Rotation> TrafficLight_orientation_road_network(const TrafficLight& traffic_light) {
+  return std::make_unique<maliput::api::Rotation>(traffic_light.orientation_road_network());
+}
+
 }  // namespace rules
 }  // namespace api
 }  // namespace maliput
diff --git a/maliput/src/api/rules/mod.rs b/maliput/src/api/rules/mod.rs
index d52c3af..8326e3e 100644
--- a/maliput/src/api/rules/mod.rs
+++ b/maliput/src/api/rules/mod.rs
@@ -92,4 +92,20 @@ impl<'a> TrafficLight<'a> {
     pub fn id(&self) -> String {
         maliput_sys::api::rules::ffi::TrafficLight_id(self.traffic_light)
     }
+
+    /// Get the position of the [TrafficLight] in the road network.
+    /// ## Return
+    /// An [crate::api::InertialPosition] representing the position of the [TrafficLight] in the road network.
+    pub fn position_road_network(&self) -> crate::api::InertialPosition {
+        let inertial_position = maliput_sys::api::rules::ffi::TrafficLight_position_road_network(self.traffic_light);
+        crate::api::InertialPosition { ip: inertial_position }
+    }
+
+    /// Get the orientation of the [TrafficLight] in the road network.
+    /// ## Return
+    /// An [crate::api::Rotation] representing the orientation of the [TrafficLight] in the road network.
+    pub fn orientation_road_network(&self) -> crate::api::Rotation {
+        let rotation = maliput_sys::api::rules::ffi::TrafficLight_orientation_road_network(self.traffic_light);
+        crate::api::Rotation { r: rotation }
+    }
 }
diff --git a/maliput/tests/traffic_light_tests.rs b/maliput/tests/traffic_light_tests.rs
index e112313..40c5a33 100644
--- a/maliput/tests/traffic_light_tests.rs
+++ b/maliput/tests/traffic_light_tests.rs
@@ -43,11 +43,22 @@ fn traffic_light_test_api() {
     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(&String::from("wrong_traffic_light_id"));
+    assert!(traffic_light.is_none());
+
     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());
+    let position = traffic_light.position_road_network();
+    assert_eq!(position.x(), 46.0);
+    assert_eq!(position.y(), -5.0);
+    assert_eq!(position.z(), 2.0);
+
+    let orientation = traffic_light.orientation_road_network();
+    use std::f64::consts::PI;
+    assert_eq!(orientation.roll(), -PI);
+    assert_eq!(orientation.pitch(), 0.0);
+    assert_eq!(orientation.yaw(), PI);
 }