TrafficLights: Bindings for getting the pose.

Signed-off-by: Franco Cipollone <[email protected]>

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>;
     }
 }

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

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 }
+    }
 }

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);
 }