arci_ros/ros_control/
joint_velocity_controller.rs
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}