arci_ros/ros_control/
joint_state_provider_from_joint_state.rs

1use crate::{error::Error, msg::sensor_msgs::JointState, JointStateProvider, SubscriberHandler};
2
3pub(crate) struct JointStateProviderFromJointState(SubscriberHandler<JointState>);
4
5impl JointStateProviderFromJointState {
6    pub(crate) fn new(subscriber_handler: SubscriberHandler<JointState>) -> Self {
7        subscriber_handler.wait_message(100);
8        Self(subscriber_handler)
9    }
10}
11
12impl JointStateProvider for JointStateProviderFromJointState {
13    fn get_joint_state(&self) -> Result<(Vec<String>, Vec<f64>), arci::Error> {
14        let state = self
15            .0
16            .get()?
17            .ok_or_else(|| arci::Error::Other(Error::NoJointStateAvailable.into()))?;
18        Ok((state.name, state.position))
19    }
20}