mecha10-behavior-runtime 0.9.0

//! Action node that turns a clicked point on the dashboard's video feed into a
//! `go-to-location` navigation goal, expressed in the robot's own frame.
//!
//! This is the MVP ("no SLAM") leg of click-to-navigate: there is no map or localization, so a
//! click can't be projected onto a world frame. Instead the clicked point is projected onto the
//! ground in front of the robot using a simple pinhole/ground-plane approximation (configurable
//! field of view and max range) and published as a `NavigationGoal` whose `target` is a
//! `base_link`-relative `Pose` - i.e. "drive `forward_m` forward and `bearing_rad` to the side of
//! wherever you are right now". `GoToLocationNode` (the companion planning node) is responsible
//! for turning that relative goal into motion via odometry-based dead reckoning.
//!
//! Mirrors `set_instance_track_target.rs`'s request/relay shape, but relays a normalized click
//! point instead of a bounding box, and publishes a `NavigationGoal`/`navigation::CANCEL`
//! instead of a node-specific control command.

use crate::{BehaviorNode, Context, NodeStatus};
use async_trait::async_trait;
use mecha10_core::messages::{NavigationGoal, Pose};
use mecha10_core::topics::navigation;
use mecha10_core::Receiver;
use serde::{Deserialize, Serialize};
use serde_json::Value;

/// A click on the video feed, normalized to the video frame: `(0, 0)` is the top-left corner,
/// `(1, 1)` is the bottom-right corner. `y = 1` (bottom of frame) is treated as directly in
/// front of the robot; `y = 0` (top of frame) is treated as `max_distance_m` away.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub(crate) struct NormalizedPoint {
    x: f32,
    y: f32,
}

#[derive(Debug, Clone, Serialize, Deserialize)]
pub(crate) struct GoToLocationTargetRequest {
    #[serde(default)]
    pub(crate) point: Option<NormalizedPoint>,
}

impl mecha10_core::messages::Message for GoToLocationTargetRequest {}

fn default_request_topic() -> String {
    "/go-to-location/target/request".to_string()
}

fn default_max_distance_m() -> f32 {
    3.0
}

fn default_horizontal_fov_rad() -> f32 {
    // ~60 degrees, a reasonable default camera horizontal field of view.
    std::f32::consts::FRAC_PI_3
}

/// Projects a normalized click into a `(forward_m, bearing_rad)` polar offset in the robot's
/// own frame, using a simple ground-plane approximation: `y` maps linearly onto forward
/// distance (bottom of frame = 0m, top of frame = `max_distance_m`), and `x` maps linearly onto
/// bearing across `horizontal_fov_rad` (left edge = `+fov/2`, right edge = `-fov/2`; positive
/// bearing is a left turn, matching the right-handed `yaw` convention `Pose::new_2d` uses).
pub(crate) fn project_click(point: NormalizedPoint, max_distance_m: f32, horizontal_fov_rad: f32) -> (f32, f32) {
    let y = point.y.clamp(0.0, 1.0);
    let x = point.x.clamp(0.0, 1.0);

    let forward_m = max_distance_m * (1.0 - y);
    let bearing_rad = (0.5 - x) * horizontal_fov_rad;

    (forward_m, bearing_rad)
}

/// Builds the `base_link`-relative goal `Pose` for a projected `(forward_m, bearing_rad)`
/// offset: the target point itself, facing along the bearing to it.
pub(crate) fn relative_goal_pose(forward_m: f32, bearing_rad: f32) -> Pose {
    let mut pose = Pose::new_2d(
        forward_m * bearing_rad.cos(),
        forward_m * bearing_rad.sin(),
        bearing_rad,
    );
    pose.frame_id = Some("base_link".to_string());
    pose
}

#[derive(Debug, Serialize, Deserialize)]
struct SetGoToLocationTargetConfig {
    #[serde(default = "default_request_topic")]
    request_topic: String,
    #[serde(default = "default_max_distance_m")]
    max_distance_m: f32,
    #[serde(default = "default_horizontal_fov_rad")]
    horizontal_fov_rad: f32,
}

pub struct SetGoToLocationTargetNode {
    request_topic: String,
    max_distance_m: f32,
    horizontal_fov_rad: f32,
    receiver: Option<Receiver<Value>>,
    active: bool,
}

impl std::fmt::Debug for SetGoToLocationTargetNode {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        f.debug_struct("SetGoToLocationTargetNode")
            .field("request_topic", &self.request_topic)
            .field("max_distance_m", &self.max_distance_m)
            .field("horizontal_fov_rad", &self.horizontal_fov_rad)
            .field("subscribed", &self.receiver.is_some())
            .field("active", &self.active)
            .finish()
    }
}

impl SetGoToLocationTargetNode {
    /// Create a new node from JSON configuration. `null` is normalized to `{}` so this node can
    /// be referenced with no config at all, matching `SetInstanceTrackTargetNode::from_json`.
    pub fn from_json(config: Value) -> anyhow::Result<Self> {
        let config = if config.is_null() {
            serde_json::json!({})
        } else {
            config
        };
        let cfg: SetGoToLocationTargetConfig = serde_json::from_value(config)?;
        Ok(Self {
            request_topic: cfg.request_topic,
            max_distance_m: cfg.max_distance_m,
            horizontal_fov_rad: cfg.horizontal_fov_rad,
            receiver: None,
            active: false,
        })
    }

    async fn publish_goal(&self, ctx: &Context, point: NormalizedPoint) -> anyhow::Result<()> {
        let (forward_m, bearing_rad) = project_click(point, self.max_distance_m, self.horizontal_fov_rad);
        let goal_id = format!("go-to-location-{}", mecha10_core::prelude::now_micros());

        let goal = NavigationGoal {
            timestamp: mecha10_core::prelude::now_micros(),
            goal_id: goal_id.clone(),
            target: relative_goal_pose(forward_m, bearing_rad),
            max_linear_velocity: None,
            max_angular_velocity: None,
            position_tolerance: None,
            orientation_tolerance: None,
        };

        ctx.publish_to(navigation::GOAL, &goal).await?;
        tracing::debug!(
            "SetGoToLocationTargetNode: published goal {} (forward={:.2}m, bearing={:.2}rad)",
            goal_id,
            forward_m,
            bearing_rad
        );
        Ok(())
    }

    async fn publish_cancel(&self, ctx: &Context) -> anyhow::Result<()> {
        ctx.publish_to(navigation::CANCEL, &true).await?;
        tracing::debug!("SetGoToLocationTargetNode: published cancel");
        Ok(())
    }

    /// Drains every pending request off `receiver`, returning the last one seen (if any) - only
    /// the most recent click matters. A request with `point: null` (or no `point` key at all)
    /// means "cancel the current goal".
    fn poll_latest_request(receiver: &mut Receiver<Value>) -> Option<Option<NormalizedPoint>> {
        let mut latest = None;
        while let Ok(value) = receiver.try_recv() {
            match serde_json::from_value::<GoToLocationTargetRequest>(value) {
                Ok(request) => latest = Some(request.point),
                Err(e) => tracing::warn!("SetGoToLocationTargetNode: dropping malformed target request: {}", e),
            }
        }
        latest
    }
}

#[async_trait]
impl BehaviorNode for SetGoToLocationTargetNode {
    async fn tick(&mut self, ctx: &Context) -> anyhow::Result<NodeStatus> {
        if self.receiver.is_none() {
            self.receiver = Some(ctx.subscribe_raw::<Value>(&self.request_topic).await?);
        }

        let latest = Self::poll_latest_request(self.receiver.as_mut().expect("just subscribed above"));
        if let Some(point) = latest {
            match point {
                Some(point) => {
                    self.publish_goal(ctx, point).await?;
                    self.active = true;
                }
                None => {
                    self.publish_cancel(ctx).await?;
                    self.active = false;
                }
            }
        }

        Ok(if self.active {
            NodeStatus::Running
        } else {
            NodeStatus::Failure
        })
    }

    fn name(&self) -> &str {
        "SetGoToLocationTargetNode"
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::test_helpers::create_test_context;
    use std::f32::consts::PI;

    #[test]
    fn project_click_maps_bottom_center_to_directly_ahead() {
        let (forward_m, bearing_rad) = project_click(NormalizedPoint { x: 0.5, y: 1.0 }, 3.0, PI / 3.0);
        assert_eq!(forward_m, 0.0);
        assert_eq!(bearing_rad, 0.0);
    }

    #[test]
    fn project_click_maps_top_center_to_max_distance_ahead() {
        let (forward_m, bearing_rad) = project_click(NormalizedPoint { x: 0.5, y: 0.0 }, 3.0, PI / 3.0);
        assert_eq!(forward_m, 3.0);
        assert_eq!(bearing_rad, 0.0);
    }

    #[test]
    fn project_click_maps_left_edge_to_a_positive_left_bearing() {
        let (_, bearing_rad) = project_click(NormalizedPoint { x: 0.0, y: 0.5 }, 3.0, PI / 3.0);
        assert!((bearing_rad - PI / 6.0).abs() < 1e-6);
    }

    #[test]
    fn project_click_maps_right_edge_to_a_negative_right_bearing() {
        let (_, bearing_rad) = project_click(NormalizedPoint { x: 1.0, y: 0.5 }, 3.0, PI / 3.0);
        assert!((bearing_rad + PI / 6.0).abs() < 1e-6);
    }

    #[test]
    fn project_click_clamps_out_of_range_coordinates() {
        let (forward_m, _) = project_click(NormalizedPoint { x: 0.5, y: -1.0 }, 3.0, PI / 3.0);
        assert_eq!(forward_m, 3.0);
        let (forward_m, _) = project_click(NormalizedPoint { x: 0.5, y: 2.0 }, 3.0, PI / 3.0);
        assert_eq!(forward_m, 0.0);
    }

    #[test]
    fn relative_goal_pose_is_base_link_relative_and_faces_the_bearing() {
        let pose = relative_goal_pose(2.0, PI / 2.0);
        assert_eq!(pose.frame_id, Some("base_link".to_string()));
        assert!((pose.position[0] - 0.0).abs() < 1e-5);
        assert!((pose.position[1] - 2.0).abs() < 1e-5);
    }

    #[test]
    fn from_json_defaults_topic_distance_and_fov() {
        let node = SetGoToLocationTargetNode::from_json(serde_json::json!({})).unwrap();
        assert_eq!(node.request_topic, "/go-to-location/target/request");
        assert_eq!(node.max_distance_m, 3.0);
        assert_eq!(node.horizontal_fov_rad, std::f32::consts::FRAC_PI_3);
        assert!(!node.active);
    }

    #[test]
    fn from_json_accepts_null_the_same_as_an_empty_object() {
        let node = SetGoToLocationTargetNode::from_json(Value::Null).unwrap();
        assert_eq!(node.request_topic, "/go-to-location/target/request");
    }

    #[test]
    fn from_json_reads_overridden_fields() {
        let node = SetGoToLocationTargetNode::from_json(serde_json::json!({
            "request_topic": "/custom/request",
            "max_distance_m": 5.0,
            "horizontal_fov_rad": 1.0
        }))
        .unwrap();
        assert_eq!(node.request_topic, "/custom/request");
        assert_eq!(node.max_distance_m, 5.0);
        assert_eq!(node.horizontal_fov_rad, 1.0);
    }

    #[test]
    fn poll_latest_request_returns_none_when_empty() {
        let (_tx, rx) = tokio::sync::mpsc::unbounded_channel::<Value>();
        let mut receiver = Receiver::from(rx);
        assert_eq!(SetGoToLocationTargetNode::poll_latest_request(&mut receiver), None);
    }

    #[test]
    fn poll_latest_request_keeps_only_the_most_recent_click() {
        let (tx, rx) = tokio::sync::mpsc::unbounded_channel::<Value>();
        let mut receiver = Receiver::from(rx);
        tx.send(serde_json::json!({ "point": { "x": 0.1, "y": 0.1 } })).unwrap();
        tx.send(serde_json::json!({ "point": { "x": 0.9, "y": 0.9 } })).unwrap();

        assert_eq!(
            SetGoToLocationTargetNode::poll_latest_request(&mut receiver),
            Some(Some(NormalizedPoint { x: 0.9, y: 0.9 }))
        );
    }

    #[test]
    fn poll_latest_request_accepts_a_null_point_as_a_cancel_request() {
        let (tx, rx) = tokio::sync::mpsc::unbounded_channel::<Value>();
        let mut receiver = Receiver::from(rx);
        tx.send(serde_json::json!({ "point": null })).unwrap();

        assert_eq!(
            SetGoToLocationTargetNode::poll_latest_request(&mut receiver),
            Some(None)
        );
    }

    #[test]
    fn poll_latest_request_drops_malformed_requests_and_keeps_polling() {
        let (tx, rx) = tokio::sync::mpsc::unbounded_channel::<Value>();
        let mut receiver = Receiver::from(rx);
        tx.send(serde_json::json!({ "point": "not-a-point" })).unwrap();
        tx.send(serde_json::json!({ "point": { "x": 0.5, "y": 0.5 } })).unwrap();

        assert_eq!(
            SetGoToLocationTargetNode::poll_latest_request(&mut receiver),
            Some(Some(NormalizedPoint { x: 0.5, y: 0.5 }))
        );
    }

    #[tokio::test]
    #[ignore = "Requires Redis infrastructure"]
    async fn tick_returns_failure_until_a_click_is_received() {
        let ctx = create_test_context("test_set_go_to_location_target_idle").await;

        let mut node = SetGoToLocationTargetNode::from_json(serde_json::json!({})).unwrap();
        assert_eq!(node.tick(&ctx).await.unwrap(), NodeStatus::Failure);
    }

    #[tokio::test]
    #[ignore = "Requires Redis infrastructure"]
    async fn tick_publishes_a_navigation_goal_for_a_relayed_click() {
        let ctx = create_test_context("test_set_go_to_location_target_click").await;

        let mut node = SetGoToLocationTargetNode::from_json(serde_json::json!({
            "request_topic": "/go-to-location/target/request"
        }))
        .unwrap();

        node.tick(&ctx).await.unwrap();

        let mut goal_receiver = ctx.subscribe(navigation::GOAL).await.unwrap();
        ctx.publish_raw(
            "/go-to-location/target/request",
            &serde_json::json!({ "point": { "x": 0.5, "y": 1.0 } }),
        )
        .await
        .unwrap();

        tokio::time::sleep(std::time::Duration::from_millis(50)).await;
        let status = node.tick(&ctx).await.unwrap();
        assert_eq!(status, NodeStatus::Running);

        let goal = tokio::time::timeout(std::time::Duration::from_millis(500), goal_receiver.recv())
            .await
            .expect("timed out waiting for published goal")
            .expect("channel closed without a message");

        assert_eq!(goal.target.frame_id, Some("base_link".to_string()));
    }

    #[tokio::test]
    #[ignore = "Requires Redis infrastructure"]
    async fn tick_publishes_cancel_for_a_cleared_click() {
        let ctx = create_test_context("test_set_go_to_location_target_clear").await;

        let mut node = SetGoToLocationTargetNode::from_json(serde_json::json!({
            "request_topic": "/go-to-location/target/request"
        }))
        .unwrap();

        node.tick(&ctx).await.unwrap();

        let mut cancel_receiver = ctx.subscribe(navigation::CANCEL).await.unwrap();
        ctx.publish_raw(
            "/go-to-location/target/request",
            &serde_json::json!({ "point": { "x": 0.5, "y": 1.0 } }),
        )
        .await
        .unwrap();
        tokio::time::sleep(std::time::Duration::from_millis(50)).await;
        node.tick(&ctx).await.unwrap();

        ctx.publish_raw("/go-to-location/target/request", &serde_json::json!({ "point": null }))
            .await
            .unwrap();
        tokio::time::sleep(std::time::Duration::from_millis(50)).await;
        let status = node.tick(&ctx).await.unwrap();
        assert_eq!(status, NodeStatus::Failure);

        let cancel = tokio::time::timeout(std::time::Duration::from_millis(500), cancel_receiver.recv())
            .await
            .expect("timed out waiting for cancel")
            .expect("channel closed without a message");
        assert!(cancel);
    }
}