openrr_client/clients/
local_move.rs

Help
1use arci::{BaseVelocity, MoveBase, Navigation};
2use k::nalgebra as na;
3use na::Isometry2;
4use serde::{Deserialize, Serialize};
5
6use crate::Error;
7
8#[derive(Debug, Serialize, Deserialize, Clone)]
9#[serde(deny_unknown_fields)]
10pub struct LocalMoveConfig {
11    pub reach_distance_threshold: f64,
12    pub reach_angle_threshold: f64,
13    pub control_frequency: f64,
14    pub linear_gain: f64,
15    pub angular_gain: f64,
16    pub max_linear_vel: f64,
17    pub max_angular_vel: f64,
18}
19
20impl LocalMoveConfig {
21    pub fn new<P: AsRef<std::path::Path>>(path: P) -> Result<Self, Error> {
22        let config: LocalMoveConfig = toml::from_str(
23            &std::fs::read_to_string(&path)
24                .map_err(|e| Error::NoFile(path.as_ref().to_owned(), e))?,
25        )
26        .map_err(|e| Error::TomlParseFailure(path.as_ref().to_owned(), e))?;
27        Ok(config)
28    }
29}
30
31#[derive(Debug)]
32pub struct LocalMove<N, M>
33where
34    N: Navigation,
35    M: MoveBase,
36{
37    pub nav_client: N,
38    pub vel_client: M,
39    pub config: LocalMoveConfig,
40}
41
42impl<N, M> LocalMove<N, M>
43where
44    N: Navigation,
45    M: MoveBase,
46{
47    pub fn new(nav_client: N, vel_client: M, config: LocalMoveConfig) -> Self {
48        Self {
49            nav_client,
50            vel_client,
51            config,
52        }
53    }
54
55    pub fn is_reached(&self, pose_error: Isometry2<f64>) -> bool {
56        pose_error.translation.vector.norm() < self.config.reach_distance_threshold
57            && pose_error.rotation.angle().abs() < self.config.reach_angle_threshold
58    }
59
60    pub fn send_zero_velocity(&self) -> Result<(), arci::Error> {
61        self.vel_client
62            .send_velocity(&BaseVelocity::new(0.0, 0.0, 0.0))
63    }
64
65    pub fn send_control_velocity_from_pose_error(
66        &self,
67        pose_error: Isometry2<f64>,
68    ) -> Result<(), arci::Error> {
69        // Calculate control velocity
70        // NOTE: pose_error = pose_reference - pose_current
71        let vel_x = na::clamp(
72            self.config.linear_gain * pose_error.translation.vector[0],
73            -self.config.max_linear_vel,
74            self.config.max_linear_vel,
75        );
76        let vel_y = na::clamp(
77            self.config.linear_gain * pose_error.translation.vector[1],
78            -self.config.max_linear_vel,
79            self.config.max_linear_vel,
80        );
81        let vel_theta = na::clamp(
82            self.config.angular_gain * pose_error.rotation.angle(),
83            -self.config.max_angular_vel,
84            self.config.max_angular_vel,
85        );
86
87        self.vel_client
88            .send_velocity(&BaseVelocity::new(vel_x, vel_y, vel_theta))
89    }
90}
91
92#[cfg(test)]
93mod tests {
94    use arci::{DummyMoveBase, DummyNavigation};
95    use assert_approx_eq::assert_approx_eq;
96    use na::Vector2;
97
98    use super::*;
99
100    #[test]
101    fn test_config() {
102        let path = std::path::Path::new("tests/local_move_sample.toml");
103        let config = LocalMoveConfig::new(path).unwrap();
104        assert_approx_eq!(config.reach_distance_threshold, 0.1);
105        assert_approx_eq!(config.control_frequency, 10.0);
106        assert_approx_eq!(config.linear_gain, 1.0);
107        assert_approx_eq!(config.max_linear_vel, 1.0);
108    }
109
110    #[test]
111    fn test() {
112        let path = std::path::Path::new("tests/local_move_sample.toml");
113        let config = LocalMoveConfig::new(path).unwrap();
114        assert_approx_eq!(config.linear_gain, 1.0);
115        let move_base = DummyMoveBase::new();
116        let nav = DummyNavigation::new();
117        let local_move = LocalMove::new(Box::new(nav), Box::new(move_base), config);
118
119        // Check convergence evaluations
120        assert!(local_move.is_reached(Isometry2::new(Vector2::new(0.0, 0.0), 0.0)));
121        assert!(local_move.is_reached(Isometry2::new(Vector2::new(0.05, 0.05), 0.05)));
122        assert!(!local_move.is_reached(Isometry2::new(Vector2::new(0.11, 0.0), 0.0))); // reach_distance_threshold = 0.1
123        assert!(!local_move.is_reached(Isometry2::new(Vector2::new(0.0, 0.0), 0.11))); // reach_angle_threshold = 0.1
124
125        // Set control velocity
126        local_move
127            .send_control_velocity_from_pose_error(Isometry2::new(Vector2::new(1.0, -1.0), 1.0))
128            .unwrap();
129        let vel0 = local_move.vel_client.current_velocity().unwrap();
130        assert_approx_eq!(vel0.x, 1.0); // linear_gain = 1.0
131        assert_approx_eq!(vel0.y, -1.0); // linear_gain = 1.0
132        assert_approx_eq!(vel0.theta, 1.0); // angular_gain = 1.0
133
134        local_move
135            .send_control_velocity_from_pose_error(Isometry2::new(Vector2::new(2.0, -2.0), 2.0))
136            .unwrap(); // saturated case
137        let vel1 = local_move.vel_client.current_velocity().unwrap();
138        assert_approx_eq!(vel1.x, 1.0); // max_linear_vel = 1.0
139        assert_approx_eq!(vel1.y, -1.0); // max_linear_vel = 1.0
140        assert_approx_eq!(vel1.theta, 1.0); // max_angular_vel = 1.0
141
142        // Set zero velocity
143        local_move.send_zero_velocity().unwrap();
144        let vel2 = local_move.vel_client.current_velocity().unwrap();
145        assert_approx_eq!(vel2.x, 0.0);
146        assert_approx_eq!(vel2.y, 0.0);
147        assert_approx_eq!(vel2.theta, 0.0);
148    }
149}
openrr_client/clients/local_move.rs

openrr_client/clients/
local_move.rs
