1582 lines
54 KiB
Rust
1582 lines
54 KiB
Rust
//! Generation-fenced embodied attention and bounded presence behavior.
|
|
|
|
#![allow(clippy::missing_errors_doc)]
|
|
|
|
use crate::backend::{AuthorizedToolBackend, BackendError, BackendFuture};
|
|
use crate::config::BehaviorSettings;
|
|
use crate::interaction::{PacerFuture, ResponsePacer};
|
|
use crate::llm::{ToolDefinition, ToolSchema};
|
|
use crate::perception::WorldPosition;
|
|
use crate::policy::{
|
|
AllowedOrigins, ApprovalRule, AuthorizedAction, Capability, FixedCost, Idempotency,
|
|
OriginClass, PolicyError, PolicyReasonCode, PolicyTool, ResourceCost, ResourceEstimator, Risk,
|
|
};
|
|
use crate::types::{
|
|
BoundedText, MAX_BODY_BYTES, MAX_IDENTIFIER_BYTES, MAX_OBSERVABLE_DETAIL_BYTES, ToolCallOutcome,
|
|
};
|
|
use libremetaverse_types::UUID;
|
|
use libremetaverse_types::compat::CancellationToken;
|
|
use serde::Serialize;
|
|
use serde_json::{Map, Value, json};
|
|
use std::collections::{BTreeMap, BTreeSet};
|
|
use std::error::Error;
|
|
use std::fmt;
|
|
use std::sync::atomic::{AtomicU64, Ordering};
|
|
use std::sync::{Arc, Mutex, MutexGuard};
|
|
use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH};
|
|
use tokio::sync::{mpsc, oneshot, watch};
|
|
use tokio::task::JoinHandle;
|
|
|
|
pub const FACE_AVATAR_TOOL: &str = "behavior_face_avatar";
|
|
pub const FACE_POINT_TOOL: &str = "behavior_face_point";
|
|
pub const LOOK_AROUND_TOOL: &str = "behavior_look_around";
|
|
pub const WALK_SHORT_TOOL: &str = "behavior_walk_short";
|
|
pub const STOP_TOOL: &str = "behavior_stop";
|
|
pub const SIT_TOOL: &str = "behavior_sit";
|
|
pub const STAND_TOOL: &str = "behavior_stand";
|
|
pub const CURRENT_POSE_TOOL: &str = "behavior_current_pose";
|
|
|
|
const WALK_POLL: Duration = Duration::from_millis(250);
|
|
const TURN_INTERVAL: Duration = Duration::from_millis(40);
|
|
const MAX_ACTION_ID_BYTES: usize = MAX_IDENTIFIER_BYTES;
|
|
const MAX_TURN_STEP_DEGREES: f64 = 30.0;
|
|
const ARRIVAL_METERS: f64 = 0.5;
|
|
const STUCK_PROGRESS_METERS: f64 = 0.05;
|
|
const MAX_TOOL_RESULT_BYTES: usize = 8 * 1024;
|
|
|
|
pub type EmbodimentFuture<'a, T> = BackendFuture<'a, Result<T, BehaviorError>>;
|
|
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq, Serialize)]
|
|
#[serde(rename_all = "snake_case")]
|
|
pub enum BehaviorMode {
|
|
Offline,
|
|
Settling,
|
|
Available,
|
|
Engaged,
|
|
Executing,
|
|
Roaming,
|
|
Paused,
|
|
Recovering,
|
|
}
|
|
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq, Serialize)]
|
|
#[serde(rename_all = "snake_case")]
|
|
pub enum BehaviorOutcome {
|
|
Completed,
|
|
Cancelled,
|
|
TimedOut,
|
|
Stuck,
|
|
Rejected,
|
|
Preempted,
|
|
}
|
|
|
|
#[derive(Clone, Debug, PartialEq)]
|
|
pub struct EmbodiedPose {
|
|
pub generation: u64,
|
|
pub region_id: UUID,
|
|
pub position: WorldPosition,
|
|
pub heading_degrees: f64,
|
|
pub sitting: bool,
|
|
}
|
|
|
|
impl EmbodiedPose {
|
|
fn valid(&self, generation: u64, region_id: UUID) -> bool {
|
|
self.generation == generation
|
|
&& self.region_id == region_id
|
|
&& self.position.x.is_finite()
|
|
&& self.position.y.is_finite()
|
|
&& self.position.z.is_finite()
|
|
&& self.heading_degrees.is_finite()
|
|
}
|
|
}
|
|
|
|
#[derive(Clone, Debug, Eq, PartialEq)]
|
|
pub enum BehaviorError {
|
|
NotReady,
|
|
Paused,
|
|
EmergencyStopped,
|
|
InvalidArguments,
|
|
TargetUnavailable,
|
|
RegionBoundary,
|
|
RateLimited,
|
|
Cancelled,
|
|
TimedOut,
|
|
Stuck,
|
|
NativeOperation,
|
|
QueueClosed,
|
|
}
|
|
|
|
impl fmt::Display for BehaviorError {
|
|
fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
|
|
formatter.write_str(match self {
|
|
Self::NotReady => "embodied behavior is unavailable before session readiness",
|
|
Self::Paused => "embodied behavior is paused by the operator",
|
|
Self::EmergencyStopped => "embodied behavior emergency stop is active",
|
|
Self::InvalidArguments => "behavior tool arguments are invalid or outside bounds",
|
|
Self::TargetUnavailable => "the requested attention target is unavailable",
|
|
Self::RegionBoundary => "the requested action would leave the current region",
|
|
Self::RateLimited => "embodied action rate limit is active",
|
|
Self::Cancelled => "embodied action was cancelled",
|
|
Self::TimedOut => "embodied action timed out",
|
|
Self::Stuck => "bounded walk stopped after making no progress",
|
|
Self::NativeOperation => "native movement operation failed",
|
|
Self::QueueClosed => "embodied behavior controller is unavailable",
|
|
})
|
|
}
|
|
}
|
|
|
|
impl Error for BehaviorError {}
|
|
|
|
/// High-level native boundary. No control flags, raw packets, teleport, flight,
|
|
/// animation, touch, following, or unrestricted autopilot are exposed here.
|
|
pub trait EmbodimentSink: Send + Sync + 'static {
|
|
fn current_pose(
|
|
&self,
|
|
generation: u64,
|
|
cancellation: CancellationToken,
|
|
) -> EmbodimentFuture<'_, EmbodiedPose>;
|
|
fn resolve_avatar(
|
|
&self,
|
|
generation: u64,
|
|
avatar_id: UUID,
|
|
cancellation: CancellationToken,
|
|
) -> EmbodimentFuture<'_, WorldPosition>;
|
|
fn face_point(
|
|
&self,
|
|
generation: u64,
|
|
point: WorldPosition,
|
|
cancellation: CancellationToken,
|
|
) -> EmbodimentFuture<'_, ()>;
|
|
fn begin_walk(
|
|
&self,
|
|
generation: u64,
|
|
point: WorldPosition,
|
|
cancellation: CancellationToken,
|
|
) -> EmbodimentFuture<'_, ()>;
|
|
fn validate_walk_target(
|
|
&self,
|
|
generation: u64,
|
|
point: WorldPosition,
|
|
cancellation: CancellationToken,
|
|
) -> EmbodimentFuture<'_, ()>;
|
|
fn stop(&self, generation: u64, cancellation: CancellationToken) -> EmbodimentFuture<'_, ()>;
|
|
fn sit(&self, generation: u64, cancellation: CancellationToken) -> EmbodimentFuture<'_, ()>;
|
|
fn stand(&self, generation: u64, cancellation: CancellationToken) -> EmbodimentFuture<'_, ()>;
|
|
}
|
|
|
|
#[derive(Clone, Debug, Eq, PartialEq)]
|
|
pub enum BehaviorTrigger {
|
|
SessionReady,
|
|
SessionDisconnected,
|
|
RegionChanged,
|
|
PublicResponse {
|
|
delivery_id: String,
|
|
avatar_id: UUID,
|
|
},
|
|
AuthorizedTool {
|
|
authorization_id: u64,
|
|
tool: String,
|
|
},
|
|
IdleTimer,
|
|
OperatorPause,
|
|
OperatorResume,
|
|
EmergencyStop,
|
|
}
|
|
|
|
#[derive(Clone, Debug, Eq, PartialEq)]
|
|
pub enum BehaviorPolicyResult {
|
|
BuiltInAttention,
|
|
Authorized(u64),
|
|
InternalIdle,
|
|
OperatorOverride,
|
|
}
|
|
|
|
#[derive(Clone, Debug, PartialEq)]
|
|
pub enum BehaviorObservation {
|
|
Transition {
|
|
from: BehaviorMode,
|
|
to: BehaviorMode,
|
|
trigger: BehaviorTrigger,
|
|
},
|
|
Action {
|
|
action_id: String,
|
|
generation: Option<u64>,
|
|
region_id: Option<UUID>,
|
|
action: String,
|
|
trigger: BehaviorTrigger,
|
|
policy: BehaviorPolicyResult,
|
|
duration_millis: u64,
|
|
outcome: BehaviorOutcome,
|
|
},
|
|
}
|
|
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
struct ReadyState {
|
|
generation: u64,
|
|
region_id: UUID,
|
|
}
|
|
|
|
#[derive(Clone, Debug)]
|
|
enum BehaviorAction {
|
|
FaceAvatar(UUID),
|
|
FacePoint(WorldPosition),
|
|
LookAround,
|
|
WalkShort {
|
|
heading_degrees: f64,
|
|
distance_meters: f64,
|
|
},
|
|
Stop,
|
|
Sit,
|
|
Stand,
|
|
CurrentPose,
|
|
}
|
|
|
|
impl BehaviorAction {
|
|
fn name(&self) -> &'static str {
|
|
match self {
|
|
Self::FaceAvatar(_) => FACE_AVATAR_TOOL,
|
|
Self::FacePoint(_) => FACE_POINT_TOOL,
|
|
Self::LookAround => LOOK_AROUND_TOOL,
|
|
Self::WalkShort { .. } => WALK_SHORT_TOOL,
|
|
Self::Stop => STOP_TOOL,
|
|
Self::Sit => SIT_TOOL,
|
|
Self::Stand => STAND_TOOL,
|
|
Self::CurrentPose => CURRENT_POSE_TOOL,
|
|
}
|
|
}
|
|
}
|
|
|
|
struct ActionRequest {
|
|
action_id: String,
|
|
action: BehaviorAction,
|
|
trigger: BehaviorTrigger,
|
|
policy: BehaviorPolicyResult,
|
|
reply: oneshot::Sender<Result<Value, BehaviorError>>,
|
|
}
|
|
|
|
#[derive(Default)]
|
|
struct ActionRegistry {
|
|
active: BTreeSet<String>,
|
|
cancelled: BTreeSet<String>,
|
|
}
|
|
|
|
enum Command {
|
|
Action(ActionRequest),
|
|
Attention {
|
|
action_id: String,
|
|
delivery_id: String,
|
|
avatar_id: UUID,
|
|
reply: oneshot::Sender<Result<(), BehaviorError>>,
|
|
},
|
|
Roaming(bool),
|
|
Shutdown(oneshot::Sender<()>),
|
|
}
|
|
|
|
#[derive(Clone)]
|
|
pub struct BehaviorIngress {
|
|
commands: mpsc::Sender<Command>,
|
|
ready: watch::Sender<Option<ReadyState>>,
|
|
paused: watch::Sender<bool>,
|
|
emergency: watch::Sender<bool>,
|
|
action_cancel: watch::Sender<u64>,
|
|
actions: Arc<Mutex<ActionRegistry>>,
|
|
next_action: Arc<AtomicU64>,
|
|
action_capacity: usize,
|
|
}
|
|
|
|
impl fmt::Debug for BehaviorIngress {
|
|
fn fmt(&self, formatter: &mut fmt::Formatter<'_>) -> fmt::Result {
|
|
formatter
|
|
.debug_struct("BehaviorIngress")
|
|
.finish_non_exhaustive()
|
|
}
|
|
}
|
|
|
|
impl BehaviorIngress {
|
|
pub fn connected(&self, generation: u64, region_id: UUID) -> Result<(), BehaviorError> {
|
|
if generation == 0 || region_id == UUID::zero() {
|
|
return Err(BehaviorError::InvalidArguments);
|
|
}
|
|
self.ready
|
|
.send(Some(ReadyState {
|
|
generation,
|
|
region_id,
|
|
}))
|
|
.map_err(|_| BehaviorError::QueueClosed)
|
|
}
|
|
|
|
pub fn region_changed(&self, generation: u64, region_id: UUID) -> Result<(), BehaviorError> {
|
|
self.connected(generation, region_id)
|
|
}
|
|
|
|
pub fn disconnected(&self) {
|
|
let _ = self.ready.send(None);
|
|
}
|
|
|
|
pub fn pause(&self) {
|
|
let _ = self.paused.send(true);
|
|
}
|
|
|
|
pub fn resume(&self) {
|
|
let _ = self.paused.send(false);
|
|
}
|
|
|
|
pub fn emergency_stop(&self) {
|
|
let _ = self.emergency.send(true);
|
|
}
|
|
|
|
pub fn clear_emergency_stop(&self) {
|
|
let _ = self.emergency.send(false);
|
|
}
|
|
|
|
/// Cancels one exact queued or executing behavior action ID.
|
|
pub fn cancel_action(&self, action_id: &str) -> Result<bool, BehaviorError> {
|
|
if !valid_action_id(action_id) {
|
|
return Err(BehaviorError::InvalidArguments);
|
|
}
|
|
let cancelled = {
|
|
let mut actions = lock_actions(&self.actions);
|
|
if actions.active.contains(action_id) {
|
|
actions.cancelled.insert(action_id.to_owned());
|
|
true
|
|
} else {
|
|
false
|
|
}
|
|
};
|
|
if cancelled {
|
|
self.action_cancel.send_modify(|generation| {
|
|
*generation = generation.saturating_add(1);
|
|
});
|
|
}
|
|
Ok(cancelled)
|
|
}
|
|
|
|
/// Marks a separately authorized scheduler task as roaming. The controller
|
|
/// itself never invents a roaming route or exposes follow/wander behavior.
|
|
pub fn set_roaming(&self, roaming: bool) -> Result<(), BehaviorError> {
|
|
self.commands
|
|
.try_send(Command::Roaming(roaming))
|
|
.map_err(|_| BehaviorError::QueueClosed)
|
|
}
|
|
|
|
async fn request(
|
|
&self,
|
|
action_id: String,
|
|
action: BehaviorAction,
|
|
trigger: BehaviorTrigger,
|
|
policy: BehaviorPolicyResult,
|
|
) -> Result<Value, BehaviorError> {
|
|
if !self.register_action(&action_id) {
|
|
return Err(BehaviorError::InvalidArguments);
|
|
}
|
|
let (reply, receive) = oneshot::channel();
|
|
if self
|
|
.commands
|
|
.send(Command::Action(ActionRequest {
|
|
action_id: action_id.clone(),
|
|
action,
|
|
trigger,
|
|
policy,
|
|
reply,
|
|
}))
|
|
.await
|
|
.is_err()
|
|
{
|
|
self.unregister_action(&action_id);
|
|
return Err(BehaviorError::QueueClosed);
|
|
}
|
|
receive.await.map_err(|_| BehaviorError::QueueClosed)?
|
|
}
|
|
|
|
pub async fn attention(
|
|
&self,
|
|
delivery_id: String,
|
|
avatar_id: UUID,
|
|
) -> Result<(), BehaviorError> {
|
|
let action_id = format!(
|
|
"attention-{}",
|
|
self.next_action.fetch_add(1, Ordering::Relaxed)
|
|
);
|
|
if !self.register_action(&action_id) {
|
|
return Err(BehaviorError::InvalidArguments);
|
|
}
|
|
let (reply, receive) = oneshot::channel();
|
|
if self
|
|
.commands
|
|
.send(Command::Attention {
|
|
action_id: action_id.clone(),
|
|
delivery_id,
|
|
avatar_id,
|
|
reply,
|
|
})
|
|
.await
|
|
.is_err()
|
|
{
|
|
self.unregister_action(&action_id);
|
|
return Err(BehaviorError::QueueClosed);
|
|
}
|
|
receive.await.map_err(|_| BehaviorError::QueueClosed)?
|
|
}
|
|
|
|
fn register_action(&self, action_id: &str) -> bool {
|
|
if !valid_action_id(action_id) {
|
|
return false;
|
|
}
|
|
let mut actions = lock_actions(&self.actions);
|
|
actions.active.len() < self.action_capacity && actions.active.insert(action_id.to_owned())
|
|
}
|
|
|
|
fn unregister_action(&self, action_id: &str) {
|
|
let mut actions = lock_actions(&self.actions);
|
|
actions.active.remove(action_id);
|
|
actions.cancelled.remove(action_id);
|
|
}
|
|
}
|
|
|
|
impl ResponsePacer for BehaviorIngress {
|
|
fn prepare_public_response(
|
|
&self,
|
|
delivery_id: String,
|
|
avatar_id: UUID,
|
|
cancellation: CancellationToken,
|
|
) -> PacerFuture<'_> {
|
|
Box::pin(async move {
|
|
tokio::select! {
|
|
() = cancellation.cancelled() => Err(crate::interaction::InteractionPacingError::Cancelled),
|
|
result = self.attention(delivery_id, avatar_id) => result.map_err(|_| crate::interaction::InteractionPacingError::Unavailable),
|
|
}
|
|
})
|
|
}
|
|
}
|
|
|
|
pub struct BehaviorHandle {
|
|
ingress: BehaviorIngress,
|
|
observations: mpsc::Receiver<BehaviorObservation>,
|
|
task: Option<JoinHandle<()>>,
|
|
shutdown_timeout: Duration,
|
|
}
|
|
|
|
impl BehaviorHandle {
|
|
#[must_use]
|
|
pub fn ingress(&self) -> BehaviorIngress {
|
|
self.ingress.clone()
|
|
}
|
|
|
|
pub async fn next_observation(&mut self) -> Option<BehaviorObservation> {
|
|
self.observations.recv().await
|
|
}
|
|
|
|
pub async fn shutdown(mut self) -> Result<(), BehaviorError> {
|
|
let (reply, mut receive) = oneshot::channel();
|
|
self.ingress
|
|
.commands
|
|
.send(Command::Shutdown(reply))
|
|
.await
|
|
.map_err(|_| BehaviorError::QueueClosed)?;
|
|
tokio::time::timeout(self.shutdown_timeout, async {
|
|
loop {
|
|
tokio::select! {
|
|
result = &mut receive => return result.map_err(|_| BehaviorError::QueueClosed),
|
|
observation = self.observations.recv() => {
|
|
if observation.is_none() {
|
|
return receive.await.map_err(|_| BehaviorError::QueueClosed);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
})
|
|
.await
|
|
.map_err(|_| BehaviorError::TimedOut)??;
|
|
if let Some(task) = self.task.take() {
|
|
tokio::time::timeout(self.shutdown_timeout, task)
|
|
.await
|
|
.map_err(|_| BehaviorError::TimedOut)?
|
|
.map_err(|_| BehaviorError::QueueClosed)?;
|
|
}
|
|
Ok(())
|
|
}
|
|
}
|
|
|
|
pub struct BehaviorController {
|
|
settings: BehaviorSettings,
|
|
sink: Arc<dyn EmbodimentSink>,
|
|
queue_capacity: usize,
|
|
observation_capacity: usize,
|
|
shutdown_timeout: Duration,
|
|
random: Arc<dyn BehaviorRandom>,
|
|
}
|
|
|
|
impl BehaviorController {
|
|
pub fn new(
|
|
settings: BehaviorSettings,
|
|
sink: Arc<dyn EmbodimentSink>,
|
|
queue_capacity: usize,
|
|
observation_capacity: usize,
|
|
shutdown_timeout: Duration,
|
|
) -> Result<Self, BehaviorError> {
|
|
Self::with_random(
|
|
settings,
|
|
sink,
|
|
queue_capacity,
|
|
observation_capacity,
|
|
shutdown_timeout,
|
|
Arc::new(SystemBehaviorRandom::new()),
|
|
)
|
|
}
|
|
|
|
pub fn with_random(
|
|
settings: BehaviorSettings,
|
|
sink: Arc<dyn EmbodimentSink>,
|
|
queue_capacity: usize,
|
|
observation_capacity: usize,
|
|
shutdown_timeout: Duration,
|
|
random: Arc<dyn BehaviorRandom>,
|
|
) -> Result<Self, BehaviorError> {
|
|
if !settings.is_valid() {
|
|
return Err(BehaviorError::InvalidArguments);
|
|
}
|
|
if queue_capacity == 0
|
|
|| queue_capacity > 8_192
|
|
|| observation_capacity == 0
|
|
|| observation_capacity > 8_192
|
|
|| shutdown_timeout.is_zero()
|
|
|| shutdown_timeout > Duration::from_mins(1)
|
|
{
|
|
return Err(BehaviorError::InvalidArguments);
|
|
}
|
|
Ok(Self {
|
|
settings,
|
|
sink,
|
|
queue_capacity,
|
|
observation_capacity,
|
|
shutdown_timeout,
|
|
random,
|
|
})
|
|
}
|
|
|
|
#[must_use]
|
|
pub fn start(self) -> BehaviorHandle {
|
|
let (commands, receiver) = mpsc::channel(self.queue_capacity);
|
|
let (ready, ready_rx) = watch::channel(None);
|
|
let (paused, paused_rx) = watch::channel(false);
|
|
let (emergency, emergency_rx) = watch::channel(false);
|
|
let (action_cancel, action_cancel_rx) = watch::channel(0_u64);
|
|
let actions = Arc::new(Mutex::new(ActionRegistry::default()));
|
|
let (observations, observation_rx) = mpsc::channel(self.observation_capacity);
|
|
let ingress = BehaviorIngress {
|
|
commands,
|
|
ready,
|
|
paused,
|
|
emergency,
|
|
action_cancel,
|
|
actions: actions.clone(),
|
|
next_action: Arc::new(AtomicU64::new(1)),
|
|
action_capacity: self.queue_capacity,
|
|
};
|
|
let shutdown_timeout = self.shutdown_timeout;
|
|
let task = tokio::spawn(run_actor(
|
|
self,
|
|
receiver,
|
|
ready_rx,
|
|
paused_rx,
|
|
emergency_rx,
|
|
action_cancel_rx,
|
|
actions,
|
|
observations,
|
|
));
|
|
BehaviorHandle {
|
|
ingress,
|
|
observations: observation_rx,
|
|
task: Some(task),
|
|
shutdown_timeout,
|
|
}
|
|
}
|
|
}
|
|
|
|
pub trait BehaviorRandom: Send + Sync + 'static {
|
|
fn next_u64(&self) -> u64;
|
|
}
|
|
|
|
struct SystemBehaviorRandom(AtomicU64);
|
|
|
|
impl SystemBehaviorRandom {
|
|
fn new() -> Self {
|
|
let seed = SystemTime::now()
|
|
.duration_since(UNIX_EPOCH)
|
|
.map_or(1, |value| {
|
|
u64::try_from(value.as_nanos()).unwrap_or(u64::MAX)
|
|
})
|
|
| 1;
|
|
Self(AtomicU64::new(seed))
|
|
}
|
|
}
|
|
|
|
impl BehaviorRandom for SystemBehaviorRandom {
|
|
fn next_u64(&self) -> u64 {
|
|
let mut current = self.0.load(Ordering::Relaxed);
|
|
loop {
|
|
let mut next = current;
|
|
next ^= next << 13;
|
|
next ^= next >> 7;
|
|
next ^= next << 17;
|
|
match self
|
|
.0
|
|
.compare_exchange_weak(current, next, Ordering::Relaxed, Ordering::Relaxed)
|
|
{
|
|
Ok(_) => return next,
|
|
Err(actual) => current = actual,
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#[allow(clippy::too_many_arguments, clippy::too_many_lines)] // One ordered lifecycle owner.
|
|
async fn run_actor(
|
|
controller: BehaviorController,
|
|
mut commands: mpsc::Receiver<Command>,
|
|
mut ready: watch::Receiver<Option<ReadyState>>,
|
|
mut paused: watch::Receiver<bool>,
|
|
mut emergency: watch::Receiver<bool>,
|
|
action_cancel: watch::Receiver<u64>,
|
|
actions: Arc<Mutex<ActionRegistry>>,
|
|
observations: mpsc::Sender<BehaviorObservation>,
|
|
) {
|
|
let mut mode = BehaviorMode::Offline;
|
|
let mut active_ready: Option<ReadyState> = None;
|
|
let mut last_action: Option<Instant> = None;
|
|
let idle = tokio::time::sleep(controller.settings.idle_interval);
|
|
tokio::pin!(idle);
|
|
loop {
|
|
tokio::select! {
|
|
changed = ready.changed() => {
|
|
if changed.is_err() { break; }
|
|
let next_ready = *ready.borrow();
|
|
let trigger = match (active_ready, next_ready) {
|
|
(Some(previous), Some(next)) if previous.region_id != next.region_id => BehaviorTrigger::RegionChanged,
|
|
(_, Some(_)) => BehaviorTrigger::SessionReady,
|
|
_ => BehaviorTrigger::SessionDisconnected,
|
|
};
|
|
if let Some(previous) = active_ready.filter(|previous| Some(*previous) != next_ready) {
|
|
let _ = controller.sink.stop(previous.generation, CancellationToken::default()).await;
|
|
}
|
|
active_ready = next_ready;
|
|
let next = if *paused.borrow() { BehaviorMode::Paused } else if next_ready.is_some() { BehaviorMode::Settling } else { BehaviorMode::Offline };
|
|
transition(&observations, &mut mode, next, trigger).await;
|
|
if let Some(state) = next_ready {
|
|
tokio::time::sleep(controller.settings.settle_delay).await;
|
|
if ready.borrow().as_ref() == Some(&state) && !*paused.borrow() && !*emergency.borrow() {
|
|
transition(&observations, &mut mode, BehaviorMode::Available, BehaviorTrigger::SessionReady).await;
|
|
}
|
|
}
|
|
idle.as_mut().reset(tokio::time::Instant::now() + controller.settings.idle_interval);
|
|
}
|
|
changed = paused.changed() => {
|
|
if changed.is_err() { break; }
|
|
if *paused.borrow() {
|
|
let current_ready = *ready.borrow();
|
|
if let Some(state) = current_ready { let _ = controller.sink.stop(state.generation, CancellationToken::default()).await; }
|
|
transition(&observations, &mut mode, BehaviorMode::Paused, BehaviorTrigger::OperatorPause).await;
|
|
} else {
|
|
let next = if ready.borrow().is_some() { BehaviorMode::Available } else { BehaviorMode::Offline };
|
|
transition(&observations, &mut mode, next, BehaviorTrigger::OperatorResume).await;
|
|
}
|
|
}
|
|
changed = emergency.changed() => {
|
|
if changed.is_err() { break; }
|
|
if *emergency.borrow() {
|
|
let current_ready = *ready.borrow();
|
|
if let Some(state) = current_ready { let _ = controller.sink.stop(state.generation, CancellationToken::default()).await; }
|
|
transition(&observations, &mut mode, BehaviorMode::Paused, BehaviorTrigger::EmergencyStop).await;
|
|
} else {
|
|
let next = if *paused.borrow() {
|
|
BehaviorMode::Paused
|
|
} else if ready.borrow().is_some() {
|
|
BehaviorMode::Available
|
|
} else {
|
|
BehaviorMode::Offline
|
|
};
|
|
transition(&observations, &mut mode, next, BehaviorTrigger::OperatorResume).await;
|
|
}
|
|
}
|
|
() = &mut idle, if controller.settings.idle_look_enabled => {
|
|
if mode == BehaviorMode::Available {
|
|
let (reply, _) = oneshot::channel();
|
|
let action_id = format!("idle-{}", unix_millis_now());
|
|
let registered = {
|
|
let mut registry = lock_actions(&actions);
|
|
registry.active.len() < controller.queue_capacity
|
|
&& registry.active.insert(action_id.clone())
|
|
};
|
|
if registered {
|
|
let request = ActionRequest { action_id, action: BehaviorAction::LookAround, trigger: BehaviorTrigger::IdleTimer, policy: BehaviorPolicyResult::InternalIdle, reply };
|
|
execute_request(&controller, request, &mut mode, &mut last_action, &ready, &paused, &emergency, &action_cancel, &actions, &observations).await;
|
|
}
|
|
}
|
|
idle.as_mut().reset(tokio::time::Instant::now() + controller.settings.idle_interval);
|
|
}
|
|
command = commands.recv() => {
|
|
let Some(command) = command else { break; };
|
|
match command {
|
|
Command::Shutdown(reply) => {
|
|
let current_ready = *ready.borrow();
|
|
if let Some(state) = current_ready { let _ = controller.sink.stop(state.generation, CancellationToken::default()).await; }
|
|
let _ = reply.send(());
|
|
break;
|
|
}
|
|
Command::Roaming(roaming) => {
|
|
let next = if *paused.borrow() {
|
|
BehaviorMode::Paused
|
|
} else if ready.borrow().is_none() {
|
|
BehaviorMode::Offline
|
|
} else if roaming {
|
|
BehaviorMode::Roaming
|
|
} else {
|
|
BehaviorMode::Available
|
|
};
|
|
transition(&observations, &mut mode, next, BehaviorTrigger::IdleTimer).await;
|
|
}
|
|
Command::Action(request) => execute_request(&controller, request, &mut mode, &mut last_action, &ready, &paused, &emergency, &action_cancel, &actions, &observations).await,
|
|
Command::Attention { action_id, delivery_id, avatar_id, reply } => {
|
|
let request = ActionRequest {
|
|
action_id,
|
|
action: BehaviorAction::FaceAvatar(avatar_id),
|
|
trigger: BehaviorTrigger::PublicResponse { delivery_id, avatar_id },
|
|
policy: BehaviorPolicyResult::BuiltInAttention,
|
|
reply: response_reply(reply),
|
|
};
|
|
let delay = random_duration(&controller, controller.settings.response_delay_min, controller.settings.response_delay_max);
|
|
tokio::time::sleep(delay).await;
|
|
execute_request(&controller, request, &mut mode, &mut last_action, &ready, &paused, &emergency, &action_cancel, &actions, &observations).await;
|
|
if mode == BehaviorMode::Engaged {
|
|
tokio::time::sleep(controller.settings.attention_dwell).await;
|
|
if !*paused.borrow() && ready.borrow().is_some() { transition(&observations, &mut mode, BehaviorMode::Available, BehaviorTrigger::IdleTimer).await; }
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
fn response_reply(
|
|
reply: oneshot::Sender<Result<(), BehaviorError>>,
|
|
) -> oneshot::Sender<Result<Value, BehaviorError>> {
|
|
let (tx, rx) = oneshot::channel();
|
|
tokio::spawn(async move {
|
|
let result = rx
|
|
.await
|
|
.unwrap_or(Err(BehaviorError::QueueClosed))
|
|
.map(|_| ());
|
|
let _ = reply.send(result);
|
|
});
|
|
tx
|
|
}
|
|
|
|
async fn transition(
|
|
observations: &mpsc::Sender<BehaviorObservation>,
|
|
mode: &mut BehaviorMode,
|
|
to: BehaviorMode,
|
|
trigger: BehaviorTrigger,
|
|
) {
|
|
if *mode != to {
|
|
let from = *mode;
|
|
*mode = to;
|
|
let _ = observations
|
|
.send(BehaviorObservation::Transition { from, to, trigger })
|
|
.await;
|
|
}
|
|
}
|
|
|
|
#[allow(clippy::too_many_arguments, clippy::too_many_lines)]
|
|
async fn execute_request(
|
|
controller: &BehaviorController,
|
|
request: ActionRequest,
|
|
mode: &mut BehaviorMode,
|
|
last_action: &mut Option<Instant>,
|
|
ready: &watch::Receiver<Option<ReadyState>>,
|
|
paused: &watch::Receiver<bool>,
|
|
emergency: &watch::Receiver<bool>,
|
|
action_cancel: &watch::Receiver<u64>,
|
|
actions: &Arc<Mutex<ActionRegistry>>,
|
|
observations: &mpsc::Sender<BehaviorObservation>,
|
|
) {
|
|
let started = Instant::now();
|
|
let action_id = request.action_id.clone();
|
|
let action_name = request.action.name().to_owned();
|
|
let state = *ready.borrow();
|
|
let mut result = if take_action_cancellation(actions, &action_id) {
|
|
Err(BehaviorError::Cancelled)
|
|
} else if *emergency.borrow() {
|
|
Err(BehaviorError::EmergencyStopped)
|
|
} else if *paused.borrow() {
|
|
Err(BehaviorError::Paused)
|
|
} else if state.is_none() {
|
|
Err(BehaviorError::NotReady)
|
|
} else if last_action
|
|
.is_some_and(|last| last.elapsed() < controller.settings.min_action_interval)
|
|
&& !matches!(
|
|
request.action,
|
|
BehaviorAction::Stop | BehaviorAction::CurrentPose
|
|
)
|
|
{
|
|
Err(BehaviorError::RateLimited)
|
|
} else {
|
|
Ok(Value::Null)
|
|
};
|
|
if result.is_ok() {
|
|
let state = state.expect("checked ready state");
|
|
let target_mode = if matches!(request.policy, BehaviorPolicyResult::BuiltInAttention) {
|
|
BehaviorMode::Engaged
|
|
} else if matches!(request.policy, BehaviorPolicyResult::InternalIdle) {
|
|
BehaviorMode::Available
|
|
} else {
|
|
BehaviorMode::Executing
|
|
};
|
|
transition(observations, mode, target_mode, request.trigger.clone()).await;
|
|
let mut action_ready = ready.clone();
|
|
let mut action_paused = paused.clone();
|
|
let mut action_emergency = emergency.clone();
|
|
let action_cancel = action_cancel.clone();
|
|
let action_registry = actions.clone();
|
|
let cancelled_id = action_id.clone();
|
|
result = tokio::select! {
|
|
value = tokio::time::timeout(controller.settings.action_timeout, perform_action(controller, state, &request.action)) => value.unwrap_or(Err(BehaviorError::TimedOut)),
|
|
_ = action_ready.changed() => Err(BehaviorError::Cancelled),
|
|
_ = action_paused.changed() => Err(BehaviorError::Paused),
|
|
_ = action_emergency.changed() => Err(BehaviorError::EmergencyStopped),
|
|
() = wait_for_action_cancellation(action_cancel, action_registry, cancelled_id) => Err(BehaviorError::Cancelled),
|
|
};
|
|
if result.is_err() && matches!(request.action, BehaviorAction::WalkShort { .. }) {
|
|
let _ = controller
|
|
.sink
|
|
.stop(state.generation, CancellationToken::default())
|
|
.await;
|
|
}
|
|
*last_action = Some(Instant::now());
|
|
if matches!(
|
|
result,
|
|
Err(BehaviorError::TimedOut | BehaviorError::Stuck | BehaviorError::NativeOperation)
|
|
) {
|
|
transition(
|
|
observations,
|
|
mode,
|
|
BehaviorMode::Recovering,
|
|
request.trigger.clone(),
|
|
)
|
|
.await;
|
|
}
|
|
if matches!(*mode, BehaviorMode::Executing | BehaviorMode::Recovering) {
|
|
transition(
|
|
observations,
|
|
mode,
|
|
BehaviorMode::Available,
|
|
request.trigger.clone(),
|
|
)
|
|
.await;
|
|
}
|
|
if matches!(
|
|
result,
|
|
Err(BehaviorError::Paused | BehaviorError::EmergencyStopped)
|
|
) {
|
|
transition(
|
|
observations,
|
|
mode,
|
|
BehaviorMode::Paused,
|
|
request.trigger.clone(),
|
|
)
|
|
.await;
|
|
} else if matches!(result, Err(BehaviorError::Cancelled)) {
|
|
let lifecycle_mode = if ready.borrow().is_some() {
|
|
BehaviorMode::Settling
|
|
} else {
|
|
BehaviorMode::Offline
|
|
};
|
|
transition(observations, mode, lifecycle_mode, request.trigger.clone()).await;
|
|
} else if result.is_err()
|
|
&& matches!(request.policy, BehaviorPolicyResult::BuiltInAttention)
|
|
{
|
|
transition(
|
|
observations,
|
|
mode,
|
|
BehaviorMode::Available,
|
|
request.trigger.clone(),
|
|
)
|
|
.await;
|
|
}
|
|
}
|
|
let outcome = match &result {
|
|
Ok(_) => BehaviorOutcome::Completed,
|
|
Err(BehaviorError::TimedOut) => BehaviorOutcome::TimedOut,
|
|
Err(BehaviorError::Stuck) => BehaviorOutcome::Stuck,
|
|
Err(BehaviorError::Cancelled) => BehaviorOutcome::Cancelled,
|
|
Err(BehaviorError::Paused | BehaviorError::EmergencyStopped) => BehaviorOutcome::Preempted,
|
|
Err(_) => BehaviorOutcome::Rejected,
|
|
};
|
|
let _ = observations
|
|
.send(BehaviorObservation::Action {
|
|
action_id: action_id.clone(),
|
|
generation: state.map(|value| value.generation),
|
|
region_id: state.map(|value| value.region_id),
|
|
action: action_name,
|
|
trigger: request.trigger,
|
|
policy: request.policy,
|
|
duration_millis: u64::try_from(started.elapsed().as_millis()).unwrap_or(u64::MAX),
|
|
outcome,
|
|
})
|
|
.await;
|
|
let _ = request.reply.send(result);
|
|
unregister_action(actions, &action_id);
|
|
}
|
|
|
|
async fn wait_for_action_cancellation(
|
|
mut signal: watch::Receiver<u64>,
|
|
actions: Arc<Mutex<ActionRegistry>>,
|
|
action_id: String,
|
|
) {
|
|
loop {
|
|
if take_action_cancellation(&actions, &action_id) {
|
|
return;
|
|
}
|
|
if signal.changed().await.is_err() {
|
|
std::future::pending::<()>().await;
|
|
}
|
|
}
|
|
}
|
|
|
|
async fn perform_action(
|
|
controller: &BehaviorController,
|
|
state: ReadyState,
|
|
action: &BehaviorAction,
|
|
) -> Result<Value, BehaviorError> {
|
|
let cancellation = CancellationToken::default();
|
|
match action {
|
|
BehaviorAction::CurrentPose => pose_value(
|
|
&controller
|
|
.sink
|
|
.current_pose(state.generation, cancellation)
|
|
.await?,
|
|
state,
|
|
),
|
|
BehaviorAction::FaceAvatar(avatar_id) => {
|
|
let point = controller
|
|
.sink
|
|
.resolve_avatar(state.generation, *avatar_id, cancellation.clone())
|
|
.await?;
|
|
let pose = controller
|
|
.sink
|
|
.current_pose(state.generation, cancellation.clone())
|
|
.await?;
|
|
validate_pose(&pose, state)?;
|
|
validate_point(
|
|
pose.position,
|
|
point,
|
|
controller.settings.max_attention_distance_meters,
|
|
)?;
|
|
smooth_face(controller, state, &pose, point, cancellation).await?;
|
|
Ok(json!({"status":"completed","target_avatar_id":avatar_id.to_string()}))
|
|
}
|
|
BehaviorAction::FacePoint(point) => {
|
|
let pose = controller
|
|
.sink
|
|
.current_pose(state.generation, cancellation.clone())
|
|
.await?;
|
|
validate_pose(&pose, state)?;
|
|
validate_point(
|
|
pose.position,
|
|
*point,
|
|
controller.settings.max_attention_distance_meters,
|
|
)?;
|
|
smooth_face(controller, state, &pose, *point, cancellation).await?;
|
|
Ok(json!({"status":"completed"}))
|
|
}
|
|
BehaviorAction::LookAround => {
|
|
let pose = controller
|
|
.sink
|
|
.current_pose(state.generation, cancellation.clone())
|
|
.await?;
|
|
validate_pose(&pose, state)?;
|
|
let offset = f64::from(
|
|
u32::try_from(controller.random.next_u64() % 121).expect("bounded random offset"),
|
|
) - 60.0;
|
|
let heading = (pose.heading_degrees + offset).to_radians();
|
|
let point = WorldPosition {
|
|
x: pose.position.x + heading.cos() * 4.0,
|
|
y: pose.position.y + heading.sin() * 4.0,
|
|
z: pose.position.z,
|
|
};
|
|
smooth_face(controller, state, &pose, point, cancellation).await?;
|
|
Ok(json!({"status":"completed"}))
|
|
}
|
|
BehaviorAction::WalkShort {
|
|
heading_degrees,
|
|
distance_meters,
|
|
} => {
|
|
walk(
|
|
controller,
|
|
state,
|
|
*heading_degrees,
|
|
*distance_meters,
|
|
cancellation,
|
|
)
|
|
.await
|
|
}
|
|
BehaviorAction::Stop => {
|
|
controller.sink.stop(state.generation, cancellation).await?;
|
|
Ok(json!({"status":"completed"}))
|
|
}
|
|
BehaviorAction::Sit => {
|
|
controller.sink.sit(state.generation, cancellation).await?;
|
|
Ok(json!({"status":"completed"}))
|
|
}
|
|
BehaviorAction::Stand => {
|
|
controller
|
|
.sink
|
|
.stand(state.generation, cancellation)
|
|
.await?;
|
|
Ok(json!({"status":"completed"}))
|
|
}
|
|
}
|
|
}
|
|
|
|
async fn walk(
|
|
controller: &BehaviorController,
|
|
state: ReadyState,
|
|
heading_degrees: f64,
|
|
distance_meters: f64,
|
|
cancellation: CancellationToken,
|
|
) -> Result<Value, BehaviorError> {
|
|
if !heading_degrees.is_finite()
|
|
|| !distance_meters.is_finite()
|
|
|| !(0.25..=f64::from(controller.settings.max_walk_distance_meters))
|
|
.contains(&distance_meters)
|
|
{
|
|
return Err(BehaviorError::InvalidArguments);
|
|
}
|
|
let start = controller
|
|
.sink
|
|
.current_pose(state.generation, cancellation.clone())
|
|
.await?;
|
|
validate_pose(&start, state)?;
|
|
let heading = heading_degrees.to_radians();
|
|
let target = WorldPosition {
|
|
x: start.position.x + heading.cos() * distance_meters,
|
|
y: start.position.y + heading.sin() * distance_meters,
|
|
z: start.position.z,
|
|
};
|
|
if target.x < 0.5 || target.y < 0.5 {
|
|
return Err(BehaviorError::RegionBoundary);
|
|
}
|
|
controller
|
|
.sink
|
|
.validate_walk_target(state.generation, target, cancellation.clone())
|
|
.await?;
|
|
smooth_face(controller, state, &start, target, cancellation.clone()).await?;
|
|
controller
|
|
.sink
|
|
.begin_walk(state.generation, target, cancellation.clone())
|
|
.await?;
|
|
let deadline = tokio::time::Instant::now() + controller.settings.max_walk_duration;
|
|
let mut last_progress = tokio::time::Instant::now();
|
|
let mut prior = start.position;
|
|
let outcome = loop {
|
|
tokio::time::sleep(WALK_POLL).await;
|
|
if tokio::time::Instant::now() >= deadline {
|
|
break Err(BehaviorError::TimedOut);
|
|
}
|
|
let pose = match controller
|
|
.sink
|
|
.current_pose(state.generation, cancellation.clone())
|
|
.await
|
|
{
|
|
Ok(pose) => pose,
|
|
Err(error) => break Err(error),
|
|
};
|
|
if validate_pose(&pose, state).is_err() {
|
|
break Err(BehaviorError::RegionBoundary);
|
|
}
|
|
if distance(pose.position, target) <= ARRIVAL_METERS {
|
|
break Ok((pose, true));
|
|
}
|
|
if distance(pose.position, prior) >= STUCK_PROGRESS_METERS {
|
|
prior = pose.position;
|
|
last_progress = tokio::time::Instant::now();
|
|
} else if last_progress.elapsed() >= controller.settings.stuck_timeout {
|
|
break Ok((pose, false));
|
|
}
|
|
};
|
|
let stop_result = controller
|
|
.sink
|
|
.stop(state.generation, CancellationToken::default())
|
|
.await;
|
|
match (outcome, stop_result) {
|
|
(Ok((pose, true)), Ok(())) => pose_value(&pose, state),
|
|
(Ok((pose, false)), Ok(())) => Ok(json!({
|
|
"status":"completed",
|
|
"position_feedback":"unavailable",
|
|
"observed_position":pose.position,
|
|
"commanded_target":target
|
|
})),
|
|
(Err(error), _) | (_, Err(error)) => Err(error),
|
|
}
|
|
}
|
|
|
|
async fn smooth_face(
|
|
controller: &BehaviorController,
|
|
state: ReadyState,
|
|
pose: &EmbodiedPose,
|
|
target: WorldPosition,
|
|
cancellation: CancellationToken,
|
|
) -> Result<(), BehaviorError> {
|
|
let desired = (target.y - pose.position.y)
|
|
.atan2(target.x - pose.position.x)
|
|
.to_degrees()
|
|
.rem_euclid(360.0);
|
|
let delta = (desired - pose.heading_degrees + 540.0).rem_euclid(360.0) - 180.0;
|
|
let steps = (1..=6)
|
|
.find(|step| delta.abs() <= MAX_TURN_STEP_DEGREES * f64::from(*step))
|
|
.unwrap_or(6);
|
|
let horizontal_distance = ((target.x - pose.position.x).powi(2)
|
|
+ (target.y - pose.position.y).powi(2))
|
|
.sqrt()
|
|
.clamp(1.0, 4.0);
|
|
for step in 1..=steps {
|
|
if cancellation.is_cancellation_requested() {
|
|
return Err(BehaviorError::Cancelled);
|
|
}
|
|
let point = if step == steps {
|
|
target
|
|
} else {
|
|
let fraction = f64::from(step) / f64::from(steps);
|
|
let heading = (pose.heading_degrees + delta * fraction).to_radians();
|
|
WorldPosition {
|
|
x: pose.position.x + heading.cos() * horizontal_distance,
|
|
y: pose.position.y + heading.sin() * horizontal_distance,
|
|
z: pose.position.z + (target.z - pose.position.z) * fraction,
|
|
}
|
|
};
|
|
controller
|
|
.sink
|
|
.face_point(state.generation, point, cancellation.clone())
|
|
.await?;
|
|
if step != steps {
|
|
tokio::time::sleep(TURN_INTERVAL).await;
|
|
}
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
fn pose_value(pose: &EmbodiedPose, state: ReadyState) -> Result<Value, BehaviorError> {
|
|
validate_pose(pose, state)?;
|
|
Ok(
|
|
json!({"status":"observed","generation":pose.generation,"region_id":pose.region_id.to_string(),"position":pose.position,"heading_degrees":pose.heading_degrees,"sitting":pose.sitting}),
|
|
)
|
|
}
|
|
|
|
fn validate_pose(pose: &EmbodiedPose, state: ReadyState) -> Result<(), BehaviorError> {
|
|
pose.valid(state.generation, state.region_id)
|
|
.then_some(())
|
|
.ok_or(BehaviorError::RegionBoundary)
|
|
}
|
|
|
|
fn validate_point(
|
|
origin: WorldPosition,
|
|
point: WorldPosition,
|
|
maximum: u32,
|
|
) -> Result<(), BehaviorError> {
|
|
if !point.x.is_finite()
|
|
|| !point.y.is_finite()
|
|
|| !point.z.is_finite()
|
|
|| distance(origin, point) > f64::from(maximum)
|
|
|| point.x < 0.0
|
|
|| point.y < 0.0
|
|
{
|
|
return Err(BehaviorError::InvalidArguments);
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
fn distance(left: WorldPosition, right: WorldPosition) -> f64 {
|
|
((left.x - right.x).powi(2) + (left.y - right.y).powi(2) + (left.z - right.z).powi(2)).sqrt()
|
|
}
|
|
|
|
fn random_duration(
|
|
controller: &BehaviorController,
|
|
minimum: Duration,
|
|
maximum: Duration,
|
|
) -> Duration {
|
|
let range = maximum.saturating_sub(minimum).as_millis();
|
|
if range == 0 {
|
|
return minimum;
|
|
}
|
|
let offset = u128::from(controller.random.next_u64()) % (range + 1);
|
|
minimum + Duration::from_millis(u64::try_from(offset).unwrap_or(u64::MAX))
|
|
}
|
|
|
|
fn valid_action_id(value: &str) -> bool {
|
|
!value.is_empty()
|
|
&& value.len() <= MAX_ACTION_ID_BYTES
|
|
&& value.chars().all(|character| {
|
|
character.is_ascii_alphanumeric() || matches!(character, '-' | '_' | '.' | ':' | '|')
|
|
})
|
|
}
|
|
|
|
fn lock_actions(value: &Mutex<ActionRegistry>) -> MutexGuard<'_, ActionRegistry> {
|
|
value
|
|
.lock()
|
|
.unwrap_or_else(std::sync::PoisonError::into_inner)
|
|
}
|
|
|
|
fn take_action_cancellation(actions: &Mutex<ActionRegistry>, action_id: &str) -> bool {
|
|
lock_actions(actions).cancelled.remove(action_id)
|
|
}
|
|
|
|
fn unregister_action(actions: &Mutex<ActionRegistry>, action_id: &str) {
|
|
let mut actions = lock_actions(actions);
|
|
actions.active.remove(action_id);
|
|
actions.cancelled.remove(action_id);
|
|
}
|
|
|
|
fn unix_millis_now() -> u64 {
|
|
SystemTime::now()
|
|
.duration_since(UNIX_EPOCH)
|
|
.map_or(0, |duration| {
|
|
u64::try_from(duration.as_millis()).unwrap_or(u64::MAX)
|
|
})
|
|
}
|
|
|
|
#[derive(Clone)]
|
|
pub struct BehaviorBackend {
|
|
ingress: BehaviorIngress,
|
|
}
|
|
|
|
struct WalkCost {
|
|
maximum_meters: u32,
|
|
}
|
|
|
|
impl ResourceEstimator for WalkCost {
|
|
#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
|
|
fn estimate(&self, arguments: &Value) -> Result<ResourceCost, PolicyReasonCode> {
|
|
let distance = arguments
|
|
.get("distance_meters")
|
|
.and_then(Value::as_f64)
|
|
.filter(|value| {
|
|
value.is_finite() && (0.25..=f64::from(self.maximum_meters)).contains(value)
|
|
})
|
|
.ok_or(PolicyReasonCode::InvalidArguments)?;
|
|
Ok(ResourceCost {
|
|
tool_calls: 1,
|
|
movement_millimeters: (distance * 1_000.0).ceil() as u64,
|
|
..ResourceCost::default()
|
|
})
|
|
}
|
|
}
|
|
|
|
impl BehaviorBackend {
|
|
#[must_use]
|
|
pub const fn new(ingress: BehaviorIngress) -> Self {
|
|
Self { ingress }
|
|
}
|
|
}
|
|
|
|
impl AuthorizedToolBackend for BehaviorBackend {
|
|
fn apply(
|
|
&self,
|
|
action: AuthorizedAction,
|
|
cancellation: CancellationToken,
|
|
) -> BackendFuture<'_, Result<ToolCallOutcome, BackendError>> {
|
|
Box::pin(async move {
|
|
let call_id = action.call().call_id.clone();
|
|
let authorization_id = action.authorization_id();
|
|
let parsed = parse_action(
|
|
action.call().name.as_str(),
|
|
action.call().arguments_json.as_str(),
|
|
);
|
|
let result = match parsed {
|
|
Ok(behavior_action) => tokio::select! {
|
|
() = cancellation.cancelled() => Err(BehaviorError::Cancelled),
|
|
value = self.ingress.request(call_id.as_str().to_owned(), behavior_action, BehaviorTrigger::AuthorizedTool { authorization_id, tool: action.call().name.as_str().to_owned() }, BehaviorPolicyResult::Authorized(authorization_id)) => value,
|
|
},
|
|
Err(error) => Err(error),
|
|
};
|
|
Ok(match result {
|
|
Ok(value) => {
|
|
let serialized =
|
|
serde_json::to_string(&value).map_err(|_| BackendError::Operation {
|
|
operation: "serialize behavior result",
|
|
})?;
|
|
if serialized.len() > MAX_TOOL_RESULT_BYTES {
|
|
return Err(BackendError::Operation {
|
|
operation: "bounded behavior result",
|
|
});
|
|
}
|
|
ToolCallOutcome::Completed {
|
|
call_id,
|
|
result: BoundedText::<MAX_BODY_BYTES>::new("behavior.result", serialized)
|
|
.map_err(|_| BackendError::Operation {
|
|
operation: "bounded behavior result",
|
|
})?,
|
|
}
|
|
}
|
|
Err(error) => ToolCallOutcome::Rejected {
|
|
call_id,
|
|
reason: BoundedText::<MAX_OBSERVABLE_DETAIL_BYTES>::new(
|
|
"behavior.rejection",
|
|
error.to_string(),
|
|
)
|
|
.map_err(|_| BackendError::Operation {
|
|
operation: "bounded behavior rejection",
|
|
})?,
|
|
},
|
|
})
|
|
})
|
|
}
|
|
}
|
|
|
|
fn parse_action(name: &str, json_arguments: &str) -> Result<BehaviorAction, BehaviorError> {
|
|
let value: Value =
|
|
serde_json::from_str(json_arguments).map_err(|_| BehaviorError::InvalidArguments)?;
|
|
let args = value.as_object().ok_or(BehaviorError::InvalidArguments)?;
|
|
match name {
|
|
FACE_AVATAR_TOOL => Ok(BehaviorAction::FaceAvatar(parse_uuid(args, "avatar_id")?)),
|
|
FACE_POINT_TOOL => Ok(BehaviorAction::FacePoint(parse_point(args)?)),
|
|
LOOK_AROUND_TOOL => {
|
|
require_empty(args)?;
|
|
Ok(BehaviorAction::LookAround)
|
|
}
|
|
WALK_SHORT_TOOL => Ok(BehaviorAction::WalkShort {
|
|
heading_degrees: number(args, "heading_degrees")?,
|
|
distance_meters: number(args, "distance_meters")?,
|
|
}),
|
|
STOP_TOOL => {
|
|
require_empty(args)?;
|
|
Ok(BehaviorAction::Stop)
|
|
}
|
|
SIT_TOOL => {
|
|
require_empty(args)?;
|
|
Ok(BehaviorAction::Sit)
|
|
}
|
|
STAND_TOOL => {
|
|
require_empty(args)?;
|
|
Ok(BehaviorAction::Stand)
|
|
}
|
|
CURRENT_POSE_TOOL => {
|
|
require_empty(args)?;
|
|
Ok(BehaviorAction::CurrentPose)
|
|
}
|
|
_ => Err(BehaviorError::InvalidArguments),
|
|
}
|
|
}
|
|
|
|
fn parse_uuid(args: &Map<String, Value>, name: &str) -> Result<UUID, BehaviorError> {
|
|
let text = args
|
|
.get(name)
|
|
.and_then(Value::as_str)
|
|
.ok_or(BehaviorError::InvalidArguments)?;
|
|
let id = UUID::parse(text.to_owned()).map_err(|_| BehaviorError::InvalidArguments)?;
|
|
(id != UUID::zero())
|
|
.then_some(id)
|
|
.ok_or(BehaviorError::InvalidArguments)
|
|
}
|
|
|
|
fn parse_point(args: &Map<String, Value>) -> Result<WorldPosition, BehaviorError> {
|
|
if args.len() != 3 {
|
|
return Err(BehaviorError::InvalidArguments);
|
|
}
|
|
Ok(WorldPosition {
|
|
x: number(args, "x")?,
|
|
y: number(args, "y")?,
|
|
z: number(args, "z")?,
|
|
})
|
|
}
|
|
|
|
fn number(args: &Map<String, Value>, name: &str) -> Result<f64, BehaviorError> {
|
|
args.get(name)
|
|
.and_then(Value::as_f64)
|
|
.filter(|value| value.is_finite())
|
|
.ok_or(BehaviorError::InvalidArguments)
|
|
}
|
|
|
|
fn require_empty(args: &Map<String, Value>) -> Result<(), BehaviorError> {
|
|
args.is_empty()
|
|
.then_some(())
|
|
.ok_or(BehaviorError::InvalidArguments)
|
|
}
|
|
|
|
#[allow(clippy::too_many_lines)] // One registry table keeps the eight schemas auditable together.
|
|
pub fn behavior_policy_tools(settings: &BehaviorSettings) -> Result<Vec<PolicyTool>, PolicyError> {
|
|
let origins = || {
|
|
AllowedOrigins::new([
|
|
OriginClass::AuthorizedIm,
|
|
OriginClass::LocalOperator,
|
|
OriginClass::InternalScheduler,
|
|
])
|
|
};
|
|
let empty = ToolSchema::Object {
|
|
properties: BTreeMap::new(),
|
|
required: BTreeSet::new(),
|
|
additional_properties: false,
|
|
};
|
|
let object = |properties: &[(&str, ToolSchema)], required: &[&str]| ToolSchema::Object {
|
|
properties: properties
|
|
.iter()
|
|
.map(|(name, schema)| ((*name).to_owned(), schema.clone()))
|
|
.collect(),
|
|
required: required.iter().map(|name| (*name).to_owned()).collect(),
|
|
additional_properties: false,
|
|
};
|
|
let specs = [
|
|
(
|
|
FACE_AVATAR_TOOL,
|
|
"Turn attention toward a currently visible avatar.",
|
|
object(&[("avatar_id", ToolSchema::String)], &["avatar_id"]),
|
|
true,
|
|
0,
|
|
),
|
|
(
|
|
FACE_POINT_TOOL,
|
|
"Turn attention toward a nearby point in the current region.",
|
|
object(
|
|
&[
|
|
("x", ToolSchema::Number),
|
|
("y", ToolSchema::Number),
|
|
("z", ToolSchema::Number),
|
|
],
|
|
&["x", "y", "z"],
|
|
),
|
|
true,
|
|
0,
|
|
),
|
|
(
|
|
LOOK_AROUND_TOOL,
|
|
"Make one small bounded attention shift.",
|
|
empty.clone(),
|
|
true,
|
|
0,
|
|
),
|
|
(
|
|
WALK_SHORT_TOOL,
|
|
"Walk a short bounded distance on a heading, then stop.",
|
|
object(
|
|
&[
|
|
("heading_degrees", ToolSchema::Number),
|
|
("distance_meters", ToolSchema::Number),
|
|
],
|
|
&["heading_degrees", "distance_meters"],
|
|
),
|
|
true,
|
|
u64::from(settings.max_walk_distance_meters) * 1_000,
|
|
),
|
|
(
|
|
STOP_TOOL,
|
|
"Immediately stop current bounded movement.",
|
|
empty.clone(),
|
|
true,
|
|
0,
|
|
),
|
|
(
|
|
SIT_TOOL,
|
|
"Sit on the ground when supported.",
|
|
empty.clone(),
|
|
true,
|
|
0,
|
|
),
|
|
(
|
|
STAND_TOOL,
|
|
"Stand from the current seated pose.",
|
|
empty.clone(),
|
|
true,
|
|
0,
|
|
),
|
|
(
|
|
CURRENT_POSE_TOOL,
|
|
"Read current embodied pose and session provenance.",
|
|
empty,
|
|
false,
|
|
0,
|
|
),
|
|
];
|
|
specs
|
|
.into_iter()
|
|
.map(|(name, description, schema, mutating, movement_mm)| {
|
|
let cost = ResourceCost {
|
|
tool_calls: 1,
|
|
movement_millimeters: movement_mm,
|
|
..ResourceCost::default()
|
|
};
|
|
let estimator: Arc<dyn ResourceEstimator> = if name == WALK_SHORT_TOOL {
|
|
Arc::new(WalkCost {
|
|
maximum_meters: settings.max_walk_distance_meters,
|
|
})
|
|
} else {
|
|
Arc::new(FixedCost(cost))
|
|
};
|
|
PolicyTool::new(
|
|
ToolDefinition {
|
|
name: BoundedText::new("behavior.tool.name", name)?,
|
|
description: BoundedText::new("behavior.tool.description", description)?,
|
|
schema,
|
|
mutating,
|
|
},
|
|
if mutating {
|
|
Capability::Movement
|
|
} else {
|
|
Capability::Informational
|
|
},
|
|
if mutating {
|
|
Risk::Movement
|
|
} else {
|
|
Risk::ReadOnly
|
|
},
|
|
origins()?,
|
|
cost,
|
|
Idempotency::Idempotent,
|
|
ApprovalRule::Never,
|
|
true,
|
|
estimator,
|
|
)
|
|
})
|
|
.collect()
|
|
}
|
|
|
|
/// Exact-name router used when one policy gateway serves perception and behavior.
|
|
pub struct AuthorizedBackendRouter {
|
|
routes: BTreeMap<String, Arc<dyn AuthorizedToolBackend>>,
|
|
}
|
|
|
|
impl AuthorizedBackendRouter {
|
|
pub fn new(
|
|
routes: impl IntoIterator<Item = (String, Arc<dyn AuthorizedToolBackend>)>,
|
|
) -> Result<Self, BackendError> {
|
|
let mut mapped = BTreeMap::new();
|
|
for (name, backend) in routes {
|
|
if name.is_empty() || mapped.insert(name, backend).is_some() {
|
|
return Err(BackendError::Configuration {
|
|
component: "authorized backend routes",
|
|
});
|
|
}
|
|
}
|
|
if mapped.is_empty() {
|
|
return Err(BackendError::Configuration {
|
|
component: "authorized backend routes",
|
|
});
|
|
}
|
|
Ok(Self { routes: mapped })
|
|
}
|
|
}
|
|
|
|
impl AuthorizedToolBackend for AuthorizedBackendRouter {
|
|
fn apply(
|
|
&self,
|
|
action: AuthorizedAction,
|
|
cancellation: CancellationToken,
|
|
) -> BackendFuture<'_, Result<ToolCallOutcome, BackendError>> {
|
|
Box::pin(async move {
|
|
let backend = self
|
|
.routes
|
|
.get(action.call().name.as_str())
|
|
.ok_or(BackendError::RejectedMutation)?;
|
|
backend.apply(action, cancellation).await
|
|
})
|
|
}
|
|
}
|