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

use std::sync::Arc;

use arci::{EachJointDiffCondition, JointTrajectoryClient, SetCompleteCondition};
use schemars::JsonSchema;
use serde::{Deserialize, Serialize};

use crate::{
    JointTrajectoryClientWrapperConfig, LazyJointStateProvider, RosControlClient,
    RosControlClientBuilder,
};

#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
#[serde(deny_unknown_fields)]
pub struct RosControlClientConfig {
    pub name: String,
    pub joint_names: Vec<String>,

    pub controller_name: String,
    pub state_topic_name: Option<String>,
    #[serde(default)]
    pub send_partial_joints_goal: bool,
    pub complete_allowable_errors: Vec<f64>,
    #[serde(default = "default_complete_timeout_sec")]
    pub complete_timeout_sec: f64,

    // TOML format has a restriction that if a table itself contains tables,
    // all keys with non-table values must be emitted first.
    // Therefore, these fields must be located at the end of the struct.
    #[serde(flatten)]
    pub wrapper_config: JointTrajectoryClientWrapperConfig,
}

const fn default_complete_timeout_sec() -> f64 {
    10.0
}

impl RosControlClientBuilder for RosControlClientConfig {
    fn build_joint_state_provider(
        &self,
        joint_state_topic_name: impl Into<String>,
    ) -> Arc<LazyJointStateProvider> {
        RosControlClient::create_joint_state_provider(joint_state_topic_name)
    }

    fn build_joint_trajectory_client(
        &self,
        lazy: bool,
        joint_state_provider: Arc<LazyJointStateProvider>,
    ) -> Result<Arc<dyn JointTrajectoryClient>, arci::Error> {
        let config = self.clone();
        let create_client = move || {
            let RosControlClientConfig {
                joint_names,
                controller_name,
                send_partial_joints_goal,
                complete_allowable_errors,
                complete_timeout_sec,
                ..
            } = config;
            rosrust::ros_debug!("create_joint_trajectory_clients_inner: creating RosControlClient");
            let mut client = RosControlClient::new_with_joint_state_provider(
                joint_names,
                &controller_name,
                send_partial_joints_goal,
                joint_state_provider,
            );
            client.set_complete_condition(Box::new(EachJointDiffCondition::new(
                complete_allowable_errors,
                complete_timeout_sec,
            )));
            Ok(client)
        };
        Ok(if lazy {
            Arc::new(arci::Lazy::with_joint_names(
                create_client,
                self.joint_names.clone(),
            ))
        } else {
            Arc::new(create_client().unwrap())
        })
    }

    fn state_topic(&self) -> String {
        if let Some(s) = &self.state_topic_name {
            s.clone()
        } else {
            RosControlClient::state_topic_name(&self.controller_name)
        }
    }

    fn wrapper_config(&self) -> &JointTrajectoryClientWrapperConfig {
        &self.wrapper_config
    }

    fn name(&self) -> &str {
        &self.name
    }
}