1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125

use crate::ControlNode;
use arci::{
    gamepad::{Button, GamepadEvent},
    JointTrajectoryClient, Speaker,
};
use async_trait::async_trait;
use openrr_client::JointsPose;
use serde::{Deserialize, Serialize};
use std::{collections::HashMap, sync::Arc, time::Duration};

pub struct JointsPoseSender<S>
where
    S: Speaker,
{
    mode: String,
    joints_poses: Vec<JointsPose>,
    joint_trajectory_clients: HashMap<String, Arc<dyn JointTrajectoryClient>>,
    speaker: S,
    submode: String,
    pose_index: usize,
    is_trigger_holding: bool,
    is_sending: bool,
    duration: Duration,
}

impl<S> JointsPoseSender<S>
where
    S: Speaker,
{
    pub fn new(
        mode: String,
        joints_poses: Vec<JointsPose>,
        joint_trajectory_clients: HashMap<String, Arc<dyn JointTrajectoryClient>>,
        speaker: S,
        duration: Duration,
    ) -> Self {
        Self {
            mode,
            submode: format!(
                " {} {}",
                joints_poses[0].client_name, joints_poses[0].pose_name
            ),
            joints_poses,
            joint_trajectory_clients,
            speaker,
            pose_index: 0,
            is_trigger_holding: false,
            is_sending: false,
            duration,
        }
    }
    pub fn new_from_config(
        config: JointsPoseSenderConfig,
        joints_poses: Vec<JointsPose>,
        joint_trajectory_clients: HashMap<String, Arc<dyn JointTrajectoryClient>>,
        speaker: S,
    ) -> Self {
        Self::new(
            config.mode,
            joints_poses,
            joint_trajectory_clients,
            speaker,
            Duration::from_secs_f64(config.duration_secs),
        )
    }
}

#[async_trait]
impl<S> ControlNode for JointsPoseSender<S>
where
    S: Speaker,
{
    fn set_event(&mut self, event: arci::gamepad::GamepadEvent) {
        match event {
            GamepadEvent::ButtonPressed(Button::East) => {
                self.pose_index = (self.pose_index + 1) % self.joints_poses.len();
                let joints_pose = &self.joints_poses[self.pose_index];
                self.submode = format!(" {} {}", joints_pose.client_name, joints_pose.pose_name);
                self.speaker
                    .speak(&format!("{}{}", self.mode, self.submode()));
            }
            GamepadEvent::ButtonPressed(Button::RightTrigger2) => {
                self.is_trigger_holding = true;
            }
            GamepadEvent::ButtonReleased(Button::RightTrigger2) => {
                self.is_trigger_holding = false;
                self.is_sending = false;
            }
            GamepadEvent::ButtonPressed(Button::West) => {
                self.is_sending = true;
            }
            GamepadEvent::ButtonReleased(Button::West) => {
                self.is_sending = false;
            }
            _ => {}
        }
    }
    async fn proc(&self) {
        let joints_pose = &self.joints_poses[self.pose_index];
        let client = self.joint_trajectory_clients[&joints_pose.client_name].clone();
        if self.is_sending && self.is_trigger_holding {
            client
                .send_joint_positions(joints_pose.positions.to_owned(), self.duration)
                .await
                .unwrap();
        } else {
            client
                .send_joint_positions(client.current_joint_positions().unwrap(), self.duration)
                .await
                .unwrap();
        }
    }
    fn mode(&self) -> &str {
        &self.mode
    }
    fn submode(&self) -> &str {
        &self.submode
    }
}

#[derive(Debug, Serialize, Deserialize, Clone)]
pub struct JointsPoseSenderConfig {
    pub mode: String,
    pub duration_secs: f64,
}