arci_ros/ros_control/
joint_velocity_controller.rs

Help
1use std::sync::{Arc, Mutex};
2
3use arci::MotorDriveVelocity;
4use msg::{sensor_msgs::JointState, std_msgs::Float64};
5
6use crate::msg;
7
8#[derive(Clone)]
9pub struct JointVelocityController(Arc<JointVelocityControllerInner>);
10
11#[derive(Clone)]
12struct JointVelocityControllerInner {
13    publisher: rosrust::Publisher<Float64>,
14    _subscriber: rosrust::Subscriber,
15    motor_velocity_state: Arc<Mutex<f64>>,
16    is_reverse_direction: bool,
17}
18
19impl JointVelocityController {
20    pub fn new(
21        velocity_topic: &str,
22        joint_state_topic: &str,
23        is_reverse_direction: bool,
24        joint_index: usize,
25    ) -> Self {
26        let motor_velocity_state = Arc::new(Mutex::new(0.));
27        let callback_motor_velocity_state = motor_velocity_state.clone();
28
29        let _subscriber = rosrust::subscribe(joint_state_topic, 1, move |msg: JointState| {
30            let mut vel = callback_motor_velocity_state.lock().unwrap();
31            *vel = msg.velocity[joint_index];
32        })
33        .unwrap();
34
35        JointVelocityController(Arc::new(JointVelocityControllerInner {
36            publisher: rosrust::publish(velocity_topic, 1).unwrap(),
37            _subscriber,
38            motor_velocity_state,
39            is_reverse_direction,
40        }))
41    }
42}
43
44impl MotorDriveVelocity for JointVelocityController {
45    fn set_motor_velocity(&self, velocity: f64) -> Result<(), arci::Error> {
46        let vel = if self.0.is_reverse_direction {
47            -velocity
48        } else {
49            velocity
50        };
51
52        self.0
53            .publisher
54            .send(Float64 { data: vel })
55            .map_err(|e| arci::Error::Connection {
56                message: format!("Error to publish motor velocity. \"{:?}\"", e),
57            })
58    }
59
60    fn get_motor_velocity(&self) -> Result<f64, arci::Error> {
61        let vel = self.0.motor_velocity_state.lock().unwrap();
62        Ok(if self.0.is_reverse_direction {
63            -(*vel)
64        } else {
65            *vel
66        })
67    }
68}
arci_ros/ros_control/joint_velocity_controller.rs

arci_ros/ros_control/
joint_velocity_controller.rs
