arci_ros/ros_control/
joint_state_provider_from_joint_state.rs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
use crate::{error::Error, msg::sensor_msgs::JointState, JointStateProvider, SubscriberHandler};

pub(crate) struct JointStateProviderFromJointState(SubscriberHandler<JointState>);

impl JointStateProviderFromJointState {
    pub(crate) fn new(subscriber_handler: SubscriberHandler<JointState>) -> Self {
        subscriber_handler.wait_message(100);
        Self(subscriber_handler)
    }
}

impl JointStateProvider for JointStateProviderFromJointState {
    fn get_joint_state(&self) -> Result<(Vec<String>, Vec<f64>), arci::Error> {
        let state = self
            .0
            .get()?
            .ok_or_else(|| arci::Error::Other(Error::NoJointStateAvailable.into()))?;
        Ok((state.name, state.position))
    }
}