arci_ros/ros_control/
ros_control_action_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, RosControlActionClient,
9    RosControlClientBuilder,
10};
11
12#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
13#[serde(deny_unknown_fields)]
14pub struct RosControlActionClientConfig {
15    pub name: String,
16    pub joint_names: Vec<String>,
17
18    pub controller_name: String,
19    pub state_topic_name: 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 RosControlActionClientConfig {
38    fn build_joint_state_provider(
39        &self,
40        joint_state_topic_name: impl Into<String>,
41    ) -> Arc<LazyJointStateProvider> {
42        RosControlActionClient::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 RosControlActionClientConfig {
53                joint_names,
54                controller_name,
55                send_partial_joints_goal,
56                complete_allowable_errors,
57                complete_timeout_sec,
58                ..
59            } = config;
60
61            rosrust::ros_debug!(
62                "create_joint_trajectory_clients_inner: creating RosControlActionClient"
63            );
64            let mut client = RosControlActionClient::new_with_joint_state_provider(
65                joint_names,
66                &controller_name,
67                send_partial_joints_goal,
68                joint_state_provider,
69            );
70            client.set_complete_condition(Box::new(EachJointDiffCondition::new(
71                complete_allowable_errors,
72                complete_timeout_sec,
73            )));
74            Ok(client)
75        };
76        Ok(if lazy {
77            Arc::new(arci::Lazy::with_joint_names(
78                create_client,
79                self.joint_names.clone(),
80            ))
81        } else {
82            Arc::new(create_client().unwrap())
83        })
84    }
85
86    fn state_topic(&self) -> String {
87        self.state_topic_name.clone()
88    }
89
90    fn wrapper_config(&self) -> &JointTrajectoryClientWrapperConfig {
91        &self.wrapper_config
92    }
93
94    fn name(&self) -> &str {
95        &self.name
96    }
97}