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

use crate::{
    ControlNode, IkNode, IkNodeConfig, JointsPoseSender, JointsPoseSenderConfig,
    JoyJointTeleopNode, JoyJointTeleopNodeConfig, MoveBaseNode,
};
use arci::{JointTrajectoryClient, MoveBase, Speaker};
use openrr_client::{IkSolverWithChain, JointsPose};
use serde::{Deserialize, Serialize};
use std::{collections::HashMap, sync::Arc};

#[derive(Debug, Serialize, Deserialize, Clone)]
pub struct JoyJointTeleopConfig {
    pub config: JoyJointTeleopNodeConfig,
    pub client_name: String,
}

#[derive(Debug, Serialize, Deserialize, Clone)]
pub struct IkNodeTeleopConfig {
    pub config: IkNodeConfig,
    pub solver_name: String,
    pub joint_trajectory_client_name: String,
}

#[derive(Debug, Serialize, Deserialize, Clone)]
pub struct ControlNodesConfig {
    pub joy_joint_teleop_configs: Vec<JoyJointTeleopConfig>,
    pub move_base_mode: Option<String>,
    pub ik_node_teleop_configs: Vec<IkNodeTeleopConfig>,
    pub joints_pose_sender_config: Option<JointsPoseSenderConfig>,
}

impl ControlNodesConfig {
    pub fn create_control_nodes(
        &self,
        speaker: Arc<dyn Speaker>,
        joint_trajectory_client_map: &HashMap<String, Arc<dyn JointTrajectoryClient>>,
        ik_solver_with_chain_map: &HashMap<String, Arc<IkSolverWithChain>>,
        move_base: Option<Arc<dyn MoveBase>>,
        joints_poses: Vec<JointsPose>,
    ) -> Vec<Box<dyn ControlNode>> {
        let mut nodes: Vec<Box<dyn ControlNode>> = vec![];

        for joy_joint_teleop_config in &self.joy_joint_teleop_configs {
            nodes.push(Box::new(JoyJointTeleopNode::new_from_config(
                joy_joint_teleop_config.config.clone(),
                joint_trajectory_client_map[&joy_joint_teleop_config.client_name].clone(),
                speaker.clone(),
            )));
        }

        if let Some(mode) = &self.move_base_mode {
            if let Some(m) = move_base {
                nodes.push(Box::new(MoveBaseNode::new(mode.clone(), m.clone())));
            }
        }

        for ik_node_teleop_config in &self.ik_node_teleop_configs {
            let ik_node = IkNode::new_from_config(
                ik_node_teleop_config.config.clone(),
                joint_trajectory_client_map[&ik_node_teleop_config.joint_trajectory_client_name]
                    .clone(),
                speaker.clone(),
                ik_solver_with_chain_map[&ik_node_teleop_config.solver_name].clone(),
            );
            nodes.push(Box::new(ik_node));
        }

        if !joints_poses.is_empty() {
            if let Some(sender_config) = &self.joints_pose_sender_config {
                let mut joint_trajectory_clients = HashMap::new();
                for joints_pose in &joints_poses {
                    joint_trajectory_clients.insert(
                        joints_pose.client_name.to_owned(),
                        joint_trajectory_client_map[&joints_pose.client_name].clone(),
                    );
                }
                nodes.push(Box::new(JointsPoseSender::new_from_config(
                    sender_config.clone(),
                    joints_poses,
                    joint_trajectory_clients,
                    speaker.clone(),
                )));
            }
        }

        nodes
    }
}