feat(grid-agent): add embodied behavior controller (#125)

crates/metacrate-grid-agent/src/backend.rs

@@ -107,6 +107,14 @@ pub struct LibremetaverseWorldSnapshotSource {
     agent: Arc<libremetaverse::AgentManager>,
 }
 
+#[cfg(feature = "live-grid")]
+#[derive(Clone)]
+pub struct LibremetaverseEmbodimentSink {
+    client: libremetaverse::GridClient,
+    agent: Arc<libremetaverse::AgentManager>,
+    generation: Arc<std::sync::RwLock<Option<u64>>>,
+}
+
 #[cfg(feature = "live-grid")]
 impl fmt::Debug for LibremetaverseClientOwner {
     fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
@@ -130,6 +138,7 @@ pub struct LibremetaverseSessionBackend {
     password: crate::config::SecretString,
     interaction: Option<crate::interaction::InteractionIngress>,
     perception: Option<crate::perception::PerceptionIngress>,
+    behavior: Option<crate::behavior::BehaviorIngress>,
     delivery_generation: Arc<std::sync::RwLock<Option<u64>>>,
 }
 
@@ -187,7 +196,7 @@ impl LibremetaverseClientOwner {
         &self,
         connection: crate::config::GridConnection,
     ) -> Result<LibremetaverseSessionBackend, BackendError> {
-        self.session_backend_inner(connection, None, None)
+        self.session_backend_inner(connection, None, None, None)
     }
 
     /// Creates a supervised live session whose generation owns exactly one
@@ -201,7 +210,7 @@ impl LibremetaverseClientOwner {
         connection: crate::config::GridConnection,
         ingress: crate::interaction::InteractionIngress,
     ) -> Result<LibremetaverseSessionBackend, BackendError> {
-        self.session_backend_inner(connection, Some(ingress), None)
+        self.session_backend_inner(connection, Some(ingress), None, None)
     }
 
     /// Creates a session that generation-fences interaction and perception.
@@ -215,7 +224,27 @@ impl LibremetaverseClientOwner {
         interaction: crate::interaction::InteractionIngress,
         perception: crate::perception::PerceptionIngress,
     ) -> Result<LibremetaverseSessionBackend, BackendError> {
-        self.session_backend_inner(connection, Some(interaction), Some(perception))
+        self.session_backend_inner(connection, Some(interaction), Some(perception), None)
+    }
+
+    /// Creates a session that generation-fences interaction, perception, and embodiment.
+    ///
+    /// # Errors
+    ///
+    /// Rejects a malformed avatar identity before any subscription or login.
+    pub fn session_backend_with_agent_services(
+        &self,
+        connection: crate::config::GridConnection,
+        interaction: crate::interaction::InteractionIngress,
+        perception: crate::perception::PerceptionIngress,
+        behavior: crate::behavior::BehaviorIngress,
+    ) -> Result<LibremetaverseSessionBackend, BackendError> {
+        self.session_backend_inner(
+            connection,
+            Some(interaction),
+            Some(perception),
+            Some(behavior),
+        )
     }
 
     fn session_backend_inner(
@@ -223,6 +252,7 @@ impl LibremetaverseClientOwner {
         connection: crate::config::GridConnection,
         interaction: Option<crate::interaction::InteractionIngress>,
         perception: Option<crate::perception::PerceptionIngress>,
+        behavior: Option<crate::behavior::BehaviorIngress>,
     ) -> Result<LibremetaverseSessionBackend, BackendError> {
         let avatar = connection.avatar_name.trim();
         let (first_name, last_name) = avatar
@@ -242,6 +272,7 @@ impl LibremetaverseClientOwner {
             password: connection.password,
             interaction,
             perception,
+            behavior,
             delivery_generation: Arc::clone(&self.delivery_generation),
         })
     }
@@ -261,6 +292,15 @@ impl LibremetaverseClientOwner {
             agent: Arc::clone(&self.agent),
         }
     }
+
+    #[must_use]
+    pub fn embodiment_sink(&self) -> LibremetaverseEmbodimentSink {
+        LibremetaverseEmbodimentSink {
+            client: self.client.clone(),
+            agent: Arc::clone(&self.agent),
+            generation: Arc::clone(&self.delivery_generation),
+        }
+    }
 }
 
 #[cfg(feature = "live-grid")]
@@ -437,6 +477,222 @@ impl crate::perception::WorldSnapshotSource for LibremetaverseWorldSnapshotSourc
     }
 }
 
+#[cfg(feature = "live-grid")]
+impl LibremetaverseEmbodimentSink {
+    fn ensure_generation(&self, generation: u64) -> Result<(), crate::behavior::BehaviorError> {
+        let current = *self
+            .generation
+            .read()
+            .unwrap_or_else(std::sync::PoisonError::into_inner);
+        if current == Some(generation) && self.client.network().native_connected() {
+            Ok(())
+        } else {
+            Err(crate::behavior::BehaviorError::NotReady)
+        }
+    }
+
+    fn native_pose(
+        &self,
+        generation: u64,
+    ) -> Result<crate::behavior::EmbodiedPose, crate::behavior::BehaviorError> {
+        self.ensure_generation(generation)?;
+        let simulator = self
+            .client
+            .network()
+            .native_current_sim()
+            .ok_or(crate::behavior::BehaviorError::NotReady)?;
+        let position = self.agent.sim_position();
+        let rotation = self.agent.sim_rotation();
+        let sin_yaw = 2.0 * f64::from(rotation.w * rotation.z + rotation.x * rotation.y);
+        let cos_yaw = 1.0 - 2.0 * f64::from(rotation.y * rotation.y + rotation.z * rotation.z);
+        Ok(crate::behavior::EmbodiedPose {
+            generation,
+            region_id: simulator.region_id,
+            position: native_position(position),
+            heading_degrees: sin_yaw.atan2(cos_yaw).to_degrees().rem_euclid(360.0),
+            sitting: self.agent.sitting_on() != 0,
+        })
+    }
+}
+
+#[cfg(feature = "live-grid")]
+#[allow(clippy::cast_possible_truncation)]
+impl crate::behavior::EmbodimentSink for LibremetaverseEmbodimentSink {
+    fn current_pose(
+        &self,
+        generation: u64,
+        cancellation: CancellationToken,
+    ) -> crate::behavior::EmbodimentFuture<'_, crate::behavior::EmbodiedPose> {
+        Box::pin(async move {
+            if cancellation.is_cancellation_requested() {
+                return Err(crate::behavior::BehaviorError::Cancelled);
+            }
+            self.native_pose(generation)
+        })
+    }
+
+    fn resolve_avatar(
+        &self,
+        generation: u64,
+        avatar_id: UUID,
+        cancellation: CancellationToken,
+    ) -> crate::behavior::EmbodimentFuture<'_, crate::perception::WorldPosition> {
+        Box::pin(async move {
+            if cancellation.is_cancellation_requested() {
+                return Err(crate::behavior::BehaviorError::Cancelled);
+            }
+            self.ensure_generation(generation)?;
+            let simulator = self
+                .client
+                .network()
+                .native_current_sim()
+                .ok_or(crate::behavior::BehaviorError::NotReady)?;
+            simulator
+                .objects_avatars
+                .read()
+                .unwrap_or_else(std::sync::PoisonError::into_inner)
+                .values()
+                .find(|avatar| avatar.id == avatar_id)
+                .map(|avatar| native_position(avatar.position))
+                .ok_or(crate::behavior::BehaviorError::TargetUnavailable)
+        })
+    }
+
+    fn face_point(
+        &self,
+        generation: u64,
+        point: crate::perception::WorldPosition,
+        cancellation: CancellationToken,
+    ) -> crate::behavior::EmbodimentFuture<'_, ()> {
+        Box::pin(async move {
+            if cancellation.is_cancellation_requested() {
+                return Err(crate::behavior::BehaviorError::Cancelled);
+            }
+            self.ensure_generation(generation)?;
+            self.agent
+                .movement
+                .turn_toward(
+                    libremetaverse_types::Vector3 {
+                        x: point.x as f32,
+                        y: point.y as f32,
+                        z: point.z as f32,
+                    },
+                    Some(true),
+                )
+                .map_err(|_| crate::behavior::BehaviorError::NativeOperation)?;
+            Ok(())
+        })
+    }
+
+    fn begin_walk(
+        &self,
+        generation: u64,
+        point: crate::perception::WorldPosition,
+        cancellation: CancellationToken,
+    ) -> crate::behavior::EmbodimentFuture<'_, ()> {
+        Box::pin(async move {
+            if cancellation.is_cancellation_requested() {
+                return Err(crate::behavior::BehaviorError::Cancelled);
+            }
+            self.ensure_generation(generation)?;
+            self.agent
+                .auto_pilot_local(
+                    point.x.round() as i32,
+                    point.y.round() as i32,
+                    point.z as f32,
+                )
+                .map_err(|_| crate::behavior::BehaviorError::NativeOperation)
+        })
+    }
+
+    fn validate_walk_target(
+        &self,
+        generation: u64,
+        point: crate::perception::WorldPosition,
+        cancellation: CancellationToken,
+    ) -> crate::behavior::EmbodimentFuture<'_, ()> {
+        Box::pin(async move {
+            if cancellation.is_cancellation_requested() {
+                return Err(crate::behavior::BehaviorError::Cancelled);
+            }
+            self.ensure_generation(generation)?;
+            let simulator = self
+                .client
+                .network()
+                .native_current_sim()
+                .ok_or(crate::behavior::BehaviorError::NotReady)?;
+            let parcels = self.client.parcels();
+            let current = parcels
+                .get_parcel_local_id(simulator.clone(), self.agent.sim_position())
+                .map_err(|_| crate::behavior::BehaviorError::NativeOperation)?;
+            let target = parcels
+                .get_parcel_local_id(
+                    simulator,
+                    libremetaverse_types::Vector3 {
+                        x: point.x as f32,
+                        y: point.y as f32,
+                        z: point.z as f32,
+                    },
+                )
+                .map_err(|_| crate::behavior::BehaviorError::NativeOperation)?;
+            if current == 0 || current != target {
+                return Err(crate::behavior::BehaviorError::RegionBoundary);
+            }
+            Ok(())
+        })
+    }
+
+    fn stop(
+        &self,
+        generation: u64,
+        cancellation: CancellationToken,
+    ) -> crate::behavior::EmbodimentFuture<'_, ()> {
+        Box::pin(async move {
+            if cancellation.is_cancellation_requested() {
+                return Err(crate::behavior::BehaviorError::Cancelled);
+            }
+            self.ensure_generation(generation)?;
+            self.agent
+                .auto_pilot_cancel()
+                .map(|_| ())
+                .map_err(|_| crate::behavior::BehaviorError::NativeOperation)
+        })
+    }
+
+    fn sit(
+        &self,
+        generation: u64,
+        cancellation: CancellationToken,
+    ) -> crate::behavior::EmbodimentFuture<'_, ()> {
+        Box::pin(async move {
+            if cancellation.is_cancellation_requested() {
+                return Err(crate::behavior::BehaviorError::Cancelled);
+            }
+            self.ensure_generation(generation)?;
+            self.agent
+                .sit()
+                .map_err(|_| crate::behavior::BehaviorError::NativeOperation)
+        })
+    }
+
+    fn stand(
+        &self,
+        generation: u64,
+        cancellation: CancellationToken,
+    ) -> crate::behavior::EmbodimentFuture<'_, ()> {
+        Box::pin(async move {
+            if cancellation.is_cancellation_requested() {
+                return Err(crate::behavior::BehaviorError::Cancelled);
+            }
+            self.ensure_generation(generation)?;
+            self.agent
+                .stand()
+                .map(|_| ())
+                .map_err(|_| crate::behavior::BehaviorError::NativeOperation)
+        })
+    }
+}
+
 #[cfg(feature = "live-grid")]
 fn native_position(value: libremetaverse_types::Vector3) -> crate::perception::WorldPosition {
     crate::perception::WorldPosition {
@@ -580,6 +836,7 @@ struct LibremetaverseSession {
     subscriptions: Vec<libremetaverse_types::compat::Subscription>,
     interaction: Option<crate::interaction::InteractionIngress>,
     perception: Option<crate::perception::PerceptionIngress>,
+    behavior: Option<crate::behavior::BehaviorIngress>,
     delivery_generation: Arc<std::sync::RwLock<Option<u64>>>,
 }
 
@@ -616,16 +873,20 @@ impl crate::session::GridSession for LibremetaverseSession {
         cancellation: CancellationToken,
     ) -> crate::session::SessionFuture<'static, Result<(), crate::session::SessionFailure>> {
         Box::pin(async move {
-            *self
-                .delivery_generation
-                .write()
-                .unwrap_or_else(std::sync::PoisonError::into_inner) = None;
             if let Some(interaction) = &self.interaction {
                 let _ = interaction.try_disconnected();
             }
             if let Some(perception) = &self.perception {
                 perception.disconnected();
             }
+            if let Some(behavior) = &self.behavior {
+                behavior.disconnected();
+                tokio::task::yield_now().await;
+            }
+            *self
+                .delivery_generation
+                .write()
+                .unwrap_or_else(std::sync::PoisonError::into_inner) = None;
             self.subscriptions.clear();
             self.network
                 .native_logout_async(Some(cancellation))
@@ -694,6 +955,7 @@ impl crate::session::GridSessionBackend for LibremetaverseSessionBackend {
             let disconnected_sender = sender.clone();
             let disconnected_interaction = self.interaction.clone();
             let disconnected_perception = self.perception.clone();
+            let disconnected_behavior = self.behavior.clone();
             let disconnected_generation = Arc::clone(&self.delivery_generation);
             let disconnected = self
                 .network
@@ -707,6 +969,9 @@ impl crate::session::GridSessionBackend for LibremetaverseSessionBackend {
                     if let Some(perception) = &disconnected_perception {
                         perception.disconnected();
                     }
+                    if let Some(behavior) = &disconnected_behavior {
+                        behavior.disconnected();
+                    }
                     let kind = match event.reason() {
                         libremetaverse::NetworkManagerDisconnectType::NetworkTimeout
                         | libremetaverse::NetworkManagerDisconnectType::ClientInitiated => {
@@ -727,6 +992,7 @@ impl crate::session::GridSessionBackend for LibremetaverseSessionBackend {
             let ready_sender = sender.clone();
             let ready_interaction = self.interaction.clone();
             let ready_perception = self.perception.clone();
+            let ready_behavior = self.behavior.clone();
             let ready_network = self.network.clone();
             let ready_agent = Arc::clone(&self.agent);
             let ready_generation = Arc::clone(&self.delivery_generation);
@@ -747,18 +1013,27 @@ impl crate::session::GridSessionBackend for LibremetaverseSessionBackend {
                         {
                             let _ = perception.connected(generation, simulator.region_id);
                         }
+                        if let (Some(behavior), Some(simulator)) =
+                            (&ready_behavior, ready_network.native_current_sim())
+                        {
+                            let _ = behavior.connected(generation, simulator.region_id);
+                        }
                         let _ = ready_sender.try_send(crate::session::SessionSignal::Ready);
                     }
                 }));
             let mut subscriptions = vec![disconnected, ready];
             if let Some(perception) = &self.perception {
                 let crossing_perception = perception.clone();
+                let crossing_behavior = self.behavior.clone();
                 let crossing_network = self.network.clone();
                 subscriptions.push(self.network.native_subscribe_sim_changed(Arc::new(
                     move |_| {
                         if let Some(simulator) = crossing_network.native_current_sim() {
                             let _ =
                                 crossing_perception.region_changed(generation, simulator.region_id);
+                            if let Some(behavior) = &crossing_behavior {
+                                let _ = behavior.region_changed(generation, simulator.region_id);
+                            }
                         }
                     },
                 )));
@@ -806,6 +1081,11 @@ impl crate::session::GridSessionBackend for LibremetaverseSessionBackend {
                 {
                     let _ = perception.connected(generation, simulator.region_id);
                 }
+                if let (Some(behavior), Some(simulator)) =
+                    (&self.behavior, self.network.native_current_sim())
+                {
+                    let _ = behavior.connected(generation, simulator.region_id);
+                }
                 let _ = sender.try_send(crate::session::SessionSignal::Ready);
             }
             let session: Box<dyn crate::session::GridSession> = Box::new(LibremetaverseSession {
@@ -815,6 +1095,7 @@ impl crate::session::GridSessionBackend for LibremetaverseSessionBackend {
                 subscriptions,
                 interaction: self.interaction.clone(),
                 perception: self.perception.clone(),
+                behavior: self.behavior.clone(),
                 delivery_generation: Arc::clone(&self.delivery_generation),
             });
             Ok(session)