From e1682fd80463518697b2dc4f4b1c124de871d1e8 Mon Sep 17 00:00:00 2001
From: Arjo Chakravarty <arjoc@intrinsic.ai>
Date: Wed, 5 Jul 2023 09:52:58 +0000
Subject: [PATCH] Adds a feature to serialize `LinearTrajectory2d`

This PR adds a feature to serialize 2D Linear trajectories. This is
useful when deploying directly on robots as the trajectory may need to
be sent over the wire.

TBH I'm not sure if this PR belongs here but I found it a useful hack
for myself.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
---
 mapf/Cargo.toml                       |  3 ++
 mapf/src/motion/se2/mod.rs            | 72 ++++++++++++++++++++++++++-
 mapf/src/motion/se2/timed_position.rs |  1 +
 3 files changed, 75 insertions(+), 1 deletion(-)

diff --git a/mapf/Cargo.toml b/mapf/Cargo.toml
index b14771c..6c145ec 100644
--- a/mapf/Cargo.toml
+++ b/mapf/Cargo.toml
@@ -32,3 +32,6 @@ slotmap = "1.0"
 
 [dev-dependencies]
 approx = "*"
+
+[features]
+serde = ["nalgebra/serde-serialize", "time-point/serde"]
\ No newline at end of file
diff --git a/mapf/src/motion/se2/mod.rs b/mapf/src/motion/se2/mod.rs
index a2cc0ba..a904c34 100644
--- a/mapf/src/motion/se2/mod.rs
+++ b/mapf/src/motion/se2/mod.rs
@@ -37,8 +37,8 @@ impl Velocity {
 }
 
 pub mod timed_position;
-pub use timed_position::*;
 
+pub use timed_position::*;
 pub mod space;
 pub use space::*;
 
@@ -47,8 +47,78 @@ pub use oriented::*;
 
 pub type LinearTrajectorySE2 = super::Trajectory<WaypointSE2>;
 
+#[cfg(feature = "serde")]
+use serde::de::{Deserializer, Error, SeqAccess, Visitor};
+#[cfg(feature = "serde")]
+use serde::ser::{Serialize, SerializeSeq, Serializer};
+
+#[cfg(feature = "serde")]
+impl serde::Serialize for LinearTrajectorySE2 {
+    fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
+    where
+        S: serde::Serializer,
+    {
+        let mut seq = serializer.serialize_seq(Some(self.len()))?;
+        for e in self.iter() {
+            seq.serialize_element(&e)?;
+        }
+        seq.end()
+    }
+}
+
+#[cfg(feature = "serde")]
+struct SE2TrajectoryVisitor;
+
+#[cfg(feature = "serde")]
+impl<'de> Visitor<'de> for SE2TrajectoryVisitor {
+    type Value = LinearTrajectorySE2;
+
+    fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
+        formatter.write_str("SE2 key value sequence.")
+    }
+
+    fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
+    where
+        A: SeqAccess<'de>,
+    {
+        let start = if let Some(wp) = seq.next_element::<WaypointSE2>()? {
+            Some(wp)
+        } else {
+            None
+        };
+
+        let second = if let Some(wp) = seq.next_element::<WaypointSE2>()? {
+            Some(wp)
+        } else {
+            None
+        };
+
+        if start.is_some() && second.is_some() {
+            if let Ok(mut traj) = Trajectory::new(start.unwrap(), second.unwrap()) {
+                while let Some(wp) = seq.next_element::<WaypointSE2>()? {
+                    traj.insert(wp);
+                }
+                return Ok(traj);
+            }
+        }
+        return Err(A::Error::custom("Trajectory needs at least 2 points"));
+    }
+}
+
+#[cfg(feature = "serde")]
+impl<'de> serde::Deserialize<'de> for LinearTrajectorySE2 {
+    fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
+    where
+        D: Deserializer<'de>,
+    {
+        deserializer.deserialize_seq(SE2TrajectoryVisitor)
+    }
+}
+
 pub mod quickest_path;
 pub use quickest_path::{QuickestPathHeuristic, QuickestPathPlanner};
 
 pub mod differential_drive_line_follow;
 pub use differential_drive_line_follow::*;
+
+use super::Trajectory;
diff --git a/mapf/src/motion/se2/timed_position.rs b/mapf/src/motion/se2/timed_position.rs
index 9042a3a..73c0781 100644
--- a/mapf/src/motion/se2/timed_position.rs
+++ b/mapf/src/motion/se2/timed_position.rs
@@ -28,6 +28,7 @@ use crate::{
 use arrayvec::ArrayVec;
 
 #[derive(Clone, Copy, PartialEq)]
+#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
 pub struct WaypointSE2 {
     pub time: TimePoint,
     pub position: Position,