arci_ros/ros_control/
ros_control_client_config.rs

1use std::sync::Arc;
2
3use arci::{EachJointDiffCondition, JointTrajectoryClient, SetCompleteCondition};
4use schemars::JsonSchema;
5use serde::{Deserialize, Serialize};
6
7use crate::{
8    JointTrajectoryClientWrapperConfig, LazyJointStateProvider, RosControlClient,
9    RosControlClientBuilder,
10};
11
12#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
13#[serde(deny_unknown_fields)]
14pub struct RosControlClientConfig {
15    pub name: String,
16    pub joint_names: Vec<String>,
17
18    pub controller_name: String,
19    pub state_topic_name: Option<String>,
20    #[serde(default)]
21    pub send_partial_joints_goal: bool,
22    pub complete_allowable_errors: Vec<f64>,
23    #[serde(default = "default_complete_timeout_sec")]
24    pub complete_timeout_sec: f64,
25
26    // TOML format has a restriction that if a table itself contains tables,
27    // all keys with non-table values must be emitted first.
28    // Therefore, these fields must be located at the end of the struct.
29    #[serde(flatten)]
30    pub wrapper_config: JointTrajectoryClientWrapperConfig,
31}
32
33const fn default_complete_timeout_sec() -> f64 {
34    10.0
35}
36
37impl RosControlClientBuilder for RosControlClientConfig {
38    fn build_joint_state_provider(
39        &self,
40        joint_state_topic_name: impl Into<String>,
41    ) -> Arc<LazyJointStateProvider> {
42        RosControlClient::create_joint_state_provider(joint_state_topic_name)
43    }
44
45    fn build_joint_trajectory_client(
46        &self,
47        lazy: bool,
48        joint_state_provider: Arc<LazyJointStateProvider>,
49    ) -> Result<Arc<dyn JointTrajectoryClient>, arci::Error> {
50        let config = self.clone();
51        let create_client = move || {
52            let RosControlClientConfig {
53                joint_names,
54                controller_name,
55                send_partial_joints_goal,
56                complete_allowable_errors,
57                complete_timeout_sec,
58                ..
59            } = config;
60            rosrust::ros_debug!("create_joint_trajectory_clients_inner: creating RosControlClient");
61            let mut client = RosControlClient::new_with_joint_state_provider(
62                joint_names,
63                &controller_name,
64                send_partial_joints_goal,
65                joint_state_provider,
66            );
67            client.set_complete_condition(Box::new(EachJointDiffCondition::new(
68                complete_allowable_errors,
69                complete_timeout_sec,
70            )));
71            Ok(client)
72        };
73        Ok(if lazy {
74            Arc::new(arci::Lazy::with_joint_names(
75                create_client,
76                self.joint_names.clone(),
77            ))
78        } else {
79            Arc::new(create_client().unwrap())
80        })
81    }
82
83    fn state_topic(&self) -> String {
84        if let Some(s) = &self.state_topic_name {
85            s.clone()
86        } else {
87            RosControlClient::state_topic_name(&self.controller_name)
88        }
89    }
90
91    fn wrapper_config(&self) -> &JointTrajectoryClientWrapperConfig {
92        &self.wrapper_config
93    }
94
95    fn name(&self) -> &str {
96        &self.name
97    }
98}