arci_ros/ros_control/
joint_trajectory_client_wrapper_config.rs

Help
1use std::sync::Arc;
2
3use anyhow::format_err;
4use arci::{
5    Error, JointPositionDifferenceLimiter, JointPositionLimit, JointPositionLimiter,
6    JointPositionLimiterStrategy, JointTrajectoryClient, JointVelocityLimiter,
7};
8use schemars::JsonSchema;
9use serde::{Deserialize, Serialize};
10
11fn new_joint_position_difference_limiter<C>(
12    client: C,
13    position_difference_limits: Option<Vec<f64>>,
14) -> Result<JointPositionDifferenceLimiter<C>, Error>
15where
16    C: JointTrajectoryClient,
17{
18    match position_difference_limits {
19        Some(position_difference_limits) => Ok(JointPositionDifferenceLimiter::new(
20            client,
21            position_difference_limits,
22        )?),
23        None => Err(Error::Other(anyhow::format_err!(
24            "No position_difference_limits is specified"
25        ))),
26    }
27}
28
29fn new_joint_position_limiter<C>(
30    client: C,
31    position_limits: Option<Vec<JointPositionLimit>>,
32    strategy: JointPositionLimiterStrategy,
33    urdf_robot: Option<&urdf_rs::Robot>,
34) -> Result<JointPositionLimiter<C>, Error>
35where
36    C: JointTrajectoryClient,
37{
38    match position_limits {
39        Some(position_limits) => Ok(JointPositionLimiter::new_with_strategy(
40            client,
41            position_limits,
42            strategy,
43        )),
44        None => JointPositionLimiter::from_urdf(client, &urdf_robot.unwrap().joints),
45    }
46}
47
48fn new_joint_velocity_limiter<C>(
49    client: C,
50    velocity_limits: Option<Vec<f64>>,
51    urdf_robot: Option<&urdf_rs::Robot>,
52) -> Result<JointVelocityLimiter<C>, Error>
53where
54    C: JointTrajectoryClient,
55{
56    match velocity_limits {
57        Some(velocity_limits) => Ok(JointVelocityLimiter::new(client, velocity_limits)),
58        None => JointVelocityLimiter::from_urdf(client, &urdf_robot.unwrap().joints),
59    }
60}
61
62#[derive(Debug, Serialize, Deserialize, Clone, JsonSchema)]
63#[serde(deny_unknown_fields)]
64pub struct JointTrajectoryClientWrapperConfig {
65    #[serde(default)]
66    pub wrap_with_joint_position_limiter: bool,
67    #[serde(default)]
68    pub joint_position_limiter_strategy: JointPositionLimiterStrategy,
69
70    #[serde(default)]
71    pub wrap_with_joint_velocity_limiter: bool,
72    pub joint_velocity_limits: Option<Vec<f64>>,
73
74    #[serde(default)]
75    pub wrap_with_joint_position_difference_limiter: bool,
76    pub joint_position_difference_limits: Option<Vec<f64>>,
77
78    // TOML format has a restriction that if a table itself contains tables,
79    // all keys with non-table values must be emitted first.
80    // Therefore, these fields must be located at the end of the struct.
81    pub joint_position_limits: Option<Vec<JointPositionLimit>>,
82}
83
84impl JointTrajectoryClientWrapperConfig {
85    pub(crate) fn check_urdf_is_not_necessary(&self) -> Result<(), arci::Error> {
86        if self.wrap_with_joint_position_limiter && self.joint_position_limits.is_none() {
87            return Err(format_err!(
88                "`wrap_with_joint_position_limiter=true` requires urdf or joint_position_limits \
89                 is specified",
90            )
91            .into());
92        }
93        if self.wrap_with_joint_velocity_limiter && self.joint_velocity_limits.is_none() {
94            return Err(format_err!(
95                "`wrap_with_joint_velocity_limiter=true` requires urdf or joint_velocity_limits \
96                 is specified",
97            )
98            .into());
99        }
100        Ok(())
101    }
102}
103
104pub fn wrap_joint_trajectory_client(
105    config: JointTrajectoryClientWrapperConfig,
106    client: Arc<dyn JointTrajectoryClient>,
107    urdf_robot: Option<&urdf_rs::Robot>,
108) -> Result<Arc<dyn JointTrajectoryClient>, arci::Error> {
109    Ok(if config.wrap_with_joint_velocity_limiter {
110        if config.wrap_with_joint_position_limiter {
111            if config.wrap_with_joint_position_difference_limiter {
112                let client = new_joint_position_difference_limiter(
113                    client,
114                    config.joint_position_difference_limits,
115                )?;
116                let client =
117                    new_joint_velocity_limiter(client, config.joint_velocity_limits, urdf_robot)?;
118                let client = new_joint_position_limiter(
119                    client,
120                    config.joint_position_limits,
121                    config.joint_position_limiter_strategy,
122                    urdf_robot,
123                )?;
124                Arc::new(client)
125            } else {
126                let client =
127                    new_joint_velocity_limiter(client, config.joint_velocity_limits, urdf_robot)?;
128                let client = new_joint_position_limiter(
129                    client,
130                    config.joint_position_limits,
131                    config.joint_position_limiter_strategy,
132                    urdf_robot,
133                )?;
134                Arc::new(client)
135            }
136        } else if config.wrap_with_joint_position_difference_limiter {
137            Arc::new(new_joint_velocity_limiter(
138                new_joint_position_difference_limiter(
139                    client,
140                    config.joint_position_difference_limits,
141                )?,
142                config.joint_velocity_limits,
143                urdf_robot,
144            )?)
145        } else {
146            Arc::new(new_joint_velocity_limiter(
147                client,
148                config.joint_velocity_limits,
149                urdf_robot,
150            )?)
151        }
152    } else if config.wrap_with_joint_position_limiter {
153        if config.wrap_with_joint_position_difference_limiter {
154            Arc::new(new_joint_position_limiter(
155                new_joint_position_difference_limiter(
156                    client,
157                    config.joint_position_difference_limits,
158                )?,
159                config.joint_position_limits,
160                config.joint_position_limiter_strategy,
161                urdf_robot,
162            )?)
163        } else {
164            Arc::new(new_joint_position_limiter(
165                client,
166                config.joint_position_limits,
167                config.joint_position_limiter_strategy,
168                urdf_robot,
169            )?)
170        }
171    } else if config.wrap_with_joint_position_difference_limiter {
172        Arc::new(new_joint_position_difference_limiter(
173            client,
174            config.joint_position_difference_limits,
175        )?)
176    } else {
177        Arc::new(client)
178    })
179}
180
181#[cfg(test)]
182mod tests {
183    use super::*;
184
185    #[test]
186    fn test_new_joint_position_limiter() {
187        use arci::{DummyJointTrajectoryClient, JointPositionLimit, JointPositionLimiter};
188
189        let client = DummyJointTrajectoryClient::new(vec!["a".to_owned(), "b".to_owned()]);
190        let limits = vec![
191            JointPositionLimit::new(-5.0, 5.0),
192            JointPositionLimit::new(-5.0, 5.0),
193        ];
194
195        let result = new_joint_position_limiter(
196            client,
197            Some(limits.clone()),
198            JointPositionLimiterStrategy::Clamp,
199            None,
200        );
201        assert!(result.is_ok());
202        let _result = result.unwrap();
203
204        let client = DummyJointTrajectoryClient::new(vec!["a".to_owned(), "b".to_owned()]);
205        let _correct = JointPositionLimiter::new(client, limits);
206    }
207
208    #[test]
209    #[should_panic]
210    fn test_new_joint_position_limiter_error() {
211        use arci::DummyJointTrajectoryClient;
212
213        let client = DummyJointTrajectoryClient::new(vec!["a".to_owned(), "b".to_owned()]);
214
215        let _ = new_joint_position_limiter(client, None, JointPositionLimiterStrategy::Clamp, None);
216    }
217}
arci_ros/ros_control/joint_trajectory_client_wrapper_config.rs

arci_ros/ros_control/
joint_trajectory_client_wrapper_config.rs
