1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149

use arci::{BaseVelocity, MoveBase, Navigation};
use k::nalgebra as na;
use na::Isometry2;
use serde::{Deserialize, Serialize};

use crate::Error;

#[derive(Debug, Serialize, Deserialize, Clone)]
#[serde(deny_unknown_fields)]
pub struct LocalMoveConfig {
    pub reach_distance_threshold: f64,
    pub reach_angle_threshold: f64,
    pub control_frequency: f64,
    pub linear_gain: f64,
    pub angular_gain: f64,
    pub max_linear_vel: f64,
    pub max_angular_vel: f64,
}

impl LocalMoveConfig {
    pub fn new<P: AsRef<std::path::Path>>(path: P) -> Result<Self, Error> {
        let config: LocalMoveConfig = toml::from_str(
            &std::fs::read_to_string(&path)
                .map_err(|e| Error::NoFile(path.as_ref().to_owned(), e))?,
        )
        .map_err(|e| Error::TomlParseFailure(path.as_ref().to_owned(), e))?;
        Ok(config)
    }
}

#[derive(Debug)]
pub struct LocalMove<N, M>
where
    N: Navigation,
    M: MoveBase,
{
    pub nav_client: N,
    pub vel_client: M,
    pub config: LocalMoveConfig,
}

impl<N, M> LocalMove<N, M>
where
    N: Navigation,
    M: MoveBase,
{
    pub fn new(nav_client: N, vel_client: M, config: LocalMoveConfig) -> Self {
        Self {
            nav_client,
            vel_client,
            config,
        }
    }

    pub fn is_reached(&self, pose_error: Isometry2<f64>) -> bool {
        pose_error.translation.vector.norm() < self.config.reach_distance_threshold
            && pose_error.rotation.angle().abs() < self.config.reach_angle_threshold
    }

    pub fn send_zero_velocity(&self) -> Result<(), arci::Error> {
        self.vel_client
            .send_velocity(&BaseVelocity::new(0.0, 0.0, 0.0))
    }

    pub fn send_control_velocity_from_pose_error(
        &self,
        pose_error: Isometry2<f64>,
    ) -> Result<(), arci::Error> {
        // Calculate control velocity
        // NOTE: pose_error = pose_reference - pose_current
        let vel_x = na::clamp(
            self.config.linear_gain * pose_error.translation.vector[0],
            -self.config.max_linear_vel,
            self.config.max_linear_vel,
        );
        let vel_y = na::clamp(
            self.config.linear_gain * pose_error.translation.vector[1],
            -self.config.max_linear_vel,
            self.config.max_linear_vel,
        );
        let vel_theta = na::clamp(
            self.config.angular_gain * pose_error.rotation.angle(),
            -self.config.max_angular_vel,
            self.config.max_angular_vel,
        );

        self.vel_client
            .send_velocity(&BaseVelocity::new(vel_x, vel_y, vel_theta))
    }
}

#[cfg(test)]
mod tests {
    use arci::{DummyMoveBase, DummyNavigation};
    use assert_approx_eq::assert_approx_eq;
    use na::Vector2;

    use super::*;

    #[test]
    fn test_config() {
        let path = std::path::Path::new("tests/local_move_sample.toml");
        let config = LocalMoveConfig::new(path).unwrap();
        assert_approx_eq!(config.reach_distance_threshold, 0.1);
        assert_approx_eq!(config.control_frequency, 10.0);
        assert_approx_eq!(config.linear_gain, 1.0);
        assert_approx_eq!(config.max_linear_vel, 1.0);
    }

    #[test]
    fn test() {
        let path = std::path::Path::new("tests/local_move_sample.toml");
        let config = LocalMoveConfig::new(path).unwrap();
        assert_approx_eq!(config.linear_gain, 1.0);
        let move_base = DummyMoveBase::new();
        let nav = DummyNavigation::new();
        let local_move = LocalMove::new(Box::new(nav), Box::new(move_base), config);

        // Check convergence evaluations
        assert!(local_move.is_reached(Isometry2::new(Vector2::new(0.0, 0.0), 0.0)));
        assert!(local_move.is_reached(Isometry2::new(Vector2::new(0.05, 0.05), 0.05)));
        assert!(!local_move.is_reached(Isometry2::new(Vector2::new(0.11, 0.0), 0.0))); // reach_distance_threshold = 0.1
        assert!(!local_move.is_reached(Isometry2::new(Vector2::new(0.0, 0.0), 0.11))); // reach_angle_threshold = 0.1

        // Set control velocity
        local_move
            .send_control_velocity_from_pose_error(Isometry2::new(Vector2::new(1.0, -1.0), 1.0))
            .unwrap();
        let vel0 = local_move.vel_client.current_velocity().unwrap();
        assert_approx_eq!(vel0.x, 1.0); // linear_gain = 1.0
        assert_approx_eq!(vel0.y, -1.0); // linear_gain = 1.0
        assert_approx_eq!(vel0.theta, 1.0); // angular_gain = 1.0

        local_move
            .send_control_velocity_from_pose_error(Isometry2::new(Vector2::new(2.0, -2.0), 2.0))
            .unwrap(); // saturated case
        let vel1 = local_move.vel_client.current_velocity().unwrap();
        assert_approx_eq!(vel1.x, 1.0); // max_linear_vel = 1.0
        assert_approx_eq!(vel1.y, -1.0); // max_linear_vel = 1.0
        assert_approx_eq!(vel1.theta, 1.0); // max_angular_vel = 1.0

        // Set zero velocity
        local_move.send_zero_velocity().unwrap();
        let vel2 = local_move.vel_client.current_velocity().unwrap();
        assert_approx_eq!(vel2.x, 0.0);
        assert_approx_eq!(vel2.y, 0.0);
        assert_approx_eq!(vel2.theta, 0.0);
    }
}