arci_ros/ros_control/
utils.rs

Help
1use std::{collections::HashMap, sync::Arc, time::Duration};
2
3use anyhow::format_err;
4use arci::{copy_joint_positions, Error, JointTrajectoryClient, TrajectoryPoint};
5
6use crate::{
7    msg::trajectory_msgs::{JointTrajectory, JointTrajectoryPoint},
8    wrap_joint_trajectory_client, JointStateProvider, LazyJointStateProvider,
9    RosControlClientBuilder,
10};
11
12pub(crate) fn extract_current_joint_positions_from_state(
13    client: &dyn JointTrajectoryClient,
14    state_provider: &dyn JointStateProvider,
15) -> Result<Vec<f64>, Error> {
16    let (state_joint_names, state_joint_positions) = state_provider.get_joint_state()?;
17    // TODO: cache index map and use it
18    let mut result = vec![0.0; client.joint_names().len()];
19    copy_joint_positions(
20        &state_joint_names,
21        &state_joint_positions,
22        &client.joint_names(),
23        &mut result,
24    )?;
25    Ok(result)
26}
27
28pub(crate) fn create_joint_trajectory_message_for_send_joint_positions(
29    client: &dyn JointTrajectoryClient,
30    state_provider: &dyn JointStateProvider,
31    positions: &[f64],
32    duration: Duration,
33    send_partial_joints_goal: bool,
34) -> Result<JointTrajectory, arci::Error> {
35    if send_partial_joints_goal {
36        Ok(JointTrajectory {
37            points: vec![JointTrajectoryPoint {
38                positions: positions.to_owned(),
39                // add zero velocity to use cubic interpolation in trajectory_controller
40                velocities: vec![0.0; client.joint_names().len()],
41                time_from_start: duration.into(),
42                ..Default::default()
43            }],
44            joint_names: client.joint_names(),
45            ..Default::default()
46        })
47    } else {
48        let (state_joint_names, state_joint_positions) = state_provider.get_joint_state()?;
49        let partial_names = client.joint_names();
50        if partial_names.len() != positions.len() {
51            return Err(arci::Error::LengthMismatch {
52                model: partial_names.len(),
53                input: positions.len(),
54            });
55        }
56        // TODO: cache index map and use it
57        let full_names = state_joint_names;
58        let full_dof = full_names.len();
59
60        let mut full_positions = state_joint_positions;
61        copy_joint_positions(&partial_names, positions, &full_names, &mut full_positions)?;
62
63        let point_with_full_positions = JointTrajectoryPoint {
64            positions: full_positions,
65            // add zero velocity to use cubic interpolation in trajectory_controller
66            velocities: vec![0.0; full_dof],
67            time_from_start: duration.into(),
68            ..Default::default()
69        };
70        Ok(JointTrajectory {
71            joint_names: full_names,
72            points: vec![point_with_full_positions],
73            ..Default::default()
74        })
75    }
76}
77
78pub(crate) fn create_joint_trajectory_message_for_send_joint_trajectory(
79    client: &dyn JointTrajectoryClient,
80    state_provider: &dyn JointStateProvider,
81    trajectory: &[TrajectoryPoint],
82    send_partial_joints_goal: bool,
83) -> Result<JointTrajectory, arci::Error> {
84    if send_partial_joints_goal {
85        Ok(JointTrajectory {
86            points: trajectory
87                .iter()
88                .map(|tp| JointTrajectoryPoint {
89                    positions: tp.positions.clone(),
90                    velocities: if let Some(velocities) = &tp.velocities {
91                        velocities.clone()
92                    } else {
93                        vec![]
94                    },
95                    time_from_start: tp.time_from_start.into(),
96                    ..Default::default()
97                })
98                .collect(),
99            joint_names: client.joint_names(),
100            ..Default::default()
101        })
102    } else {
103        let (state_joint_names, state_joint_positions) = state_provider.get_joint_state()?;
104        // TODO: cache index map and use it
105        let current_full_positions = state_joint_positions;
106        let full_names = state_joint_names;
107        let full_dof = current_full_positions.len();
108
109        Ok(JointTrajectory {
110            points: trajectory
111                .iter()
112                .map(|tp| {
113                    let mut full_positions = current_full_positions.clone();
114                    copy_joint_positions(
115                        &client.joint_names(),
116                        &tp.positions,
117                        &full_names,
118                        &mut full_positions,
119                    )?;
120                    Ok(JointTrajectoryPoint {
121                        positions: full_positions,
122                        velocities: if let Some(partial_velocities) = &tp.velocities {
123                            let mut full_velocities = vec![0.0; full_dof];
124                            copy_joint_positions(
125                                &client.joint_names(),
126                                partial_velocities,
127                                &full_names,
128                                &mut full_velocities,
129                            )?;
130                            full_velocities
131                        } else {
132                            vec![]
133                        },
134                        time_from_start: tp.time_from_start.into(),
135                        ..Default::default()
136                    })
137                })
138                .collect::<Result<Vec<_>, arci::Error>>()?,
139            joint_names: full_names,
140            ..Default::default()
141        })
142    }
143}
144
145/// Returns a map of clients for each builder.
146///
147/// The key for the map is [the name of the client](RosControlClientBuilder::name),
148/// and in case of conflict, it becomes an error.
149///
150/// Returns empty map when `builders` are empty.
151pub fn create_joint_trajectory_clients<B>(
152    builders: Vec<B>,
153    urdf_robot: Option<&urdf_rs::Robot>,
154) -> Result<HashMap<String, Arc<dyn JointTrajectoryClient>>, arci::Error>
155where
156    B: RosControlClientBuilder,
157{
158    create_joint_trajectory_clients_inner(builders, urdf_robot, false)
159}
160
161/// Returns a map of clients that will be created lazily for each builder.
162///
163/// See [create_joint_trajectory_clients] for more.
164pub fn create_joint_trajectory_clients_lazy<B>(
165    builders: Vec<B>,
166    urdf_robot: Option<&urdf_rs::Robot>,
167) -> Result<HashMap<String, Arc<dyn JointTrajectoryClient>>, arci::Error>
168where
169    B: RosControlClientBuilder,
170{
171    create_joint_trajectory_clients_inner(builders, urdf_robot, true)
172}
173
174fn create_joint_trajectory_clients_inner<B>(
175    builders: Vec<B>,
176    urdf_robot: Option<&urdf_rs::Robot>,
177    lazy: bool,
178) -> Result<HashMap<String, Arc<dyn JointTrajectoryClient>>, arci::Error>
179where
180    B: RosControlClientBuilder,
181{
182    if builders.is_empty() {
183        return Ok(HashMap::default());
184    }
185
186    let mut clients = HashMap::new();
187    let mut state_topic_name_to_provider: HashMap<String, Arc<LazyJointStateProvider>> =
188        HashMap::new();
189    for builder in builders {
190        if urdf_robot.is_none() {
191            builder.wrapper_config().check_urdf_is_not_necessary()?;
192        }
193        let state_topic_name = builder.state_topic();
194        let joint_state_provider = if let Some(joint_state_provider) =
195            state_topic_name_to_provider.get(&state_topic_name)
196        {
197            joint_state_provider.clone()
198        } else {
199            let joint_state_provider = builder.build_joint_state_provider(&state_topic_name);
200            state_topic_name_to_provider.insert(state_topic_name, joint_state_provider.clone());
201            joint_state_provider
202        };
203
204        let name = builder.name().to_owned();
205        let client = builder.build_joint_trajectory_client(lazy, joint_state_provider)?;
206        let client =
207            wrap_joint_trajectory_client(builder.wrapper_config().clone(), client, urdf_robot)?;
208        if clients.insert(name.clone(), client).is_some() {
209            return Err(format_err!("client named '{name}' has already been specified").into());
210        }
211    }
212    Ok(clients)
213}
arci_ros/ros_control/utils.rs

arci_ros/ros_control/
utils.rs
