arci_urdf_viz/
utils.rs

Help
1use std::fmt;
2
3use arci::nalgebra as na;
4use serde::{de::DeserializeOwned, Deserialize, Serialize};
5use url::Url;
6
7#[derive(Serialize, Deserialize, Debug, Clone)]
8pub(crate) struct JointState {
9    pub(crate) names: Vec<String>,
10    pub(crate) positions: Vec<f64>,
11}
12
13#[derive(Serialize, Deserialize, Debug, Clone)]
14pub(crate) struct BasePose {
15    pub(crate) position: [f64; 3],
16    pub(crate) quaternion: [f64; 4],
17}
18
19impl From<na::Isometry2<f64>> for BasePose {
20    fn from(nav_pose: na::Isometry2<f64>) -> Self {
21        let mut position = [0.0; 3];
22        position[0] = nav_pose.translation.x;
23        position[1] = nav_pose.translation.y;
24        let quaternion = quaternion_from_euler_angles(0.0, 0.0, nav_pose.rotation.angle());
25        Self {
26            position,
27            quaternion,
28        }
29    }
30}
31
32#[derive(Serialize, Deserialize)]
33pub(crate) struct RpcResult {
34    pub(crate) is_ok: bool,
35    pub(crate) reason: String,
36}
37
38fn map_connection_error<E: fmt::Display>(url: &Url) -> impl FnOnce(E) -> arci::Error + '_ {
39    move |e: E| arci::Error::Connection {
40        message: format!("url:{url}: {e}"),
41    }
42}
43
44fn get(url: Url) -> Result<ureq::Response, arci::Error> {
45    ureq::get(url.as_str())
46        .call()
47        .map_err(map_connection_error(&url))
48}
49
50fn post<T: Serialize, U: DeserializeOwned>(url: Url, msg: T) -> Result<U, arci::Error> {
51    ureq::post(url.as_str())
52        .send_json(serde_json::to_value(msg).unwrap())
53        .map_err(map_connection_error(&url))?
54        .into_json()
55        .map_err(map_connection_error(&url))
56}
57
58pub(crate) fn get_joint_positions(base_url: &Url) -> Result<JointState, arci::Error> {
59    get(base_url.join("get_joint_positions").unwrap())?
60        .into_json()
61        .map_err(map_connection_error(base_url))
62}
63
64pub(crate) fn send_joint_positions(
65    base_url: &Url,
66    joint_state: JointState,
67) -> Result<(), arci::Error> {
68    let res: RpcResult = post(base_url.join("set_joint_positions").unwrap(), joint_state)?;
69    if !res.is_ok {
70        return Err(arci::Error::Connection {
71            message: res.reason,
72        });
73    }
74    Ok(())
75}
76
77pub(crate) fn get_robot_origin(base_url: &Url) -> Result<BasePose, arci::Error> {
78    get(base_url.join("get_robot_origin").unwrap())?
79        .into_json()
80        .map_err(map_connection_error(base_url))
81}
82
83pub(crate) fn send_robot_origin(base_url: &Url, base_pose: BasePose) -> Result<(), arci::Error> {
84    let res: RpcResult = post(base_url.join("set_robot_origin").unwrap(), base_pose)?;
85    if !res.is_ok {
86        return Err(arci::Error::Connection {
87            message: res.reason,
88        });
89    }
90    Ok(())
91}
92
93pub(crate) fn get_urdf(base_url: &Url) -> Result<urdf_rs::Robot, arci::Error> {
94    let s = get(base_url.join("get_urdf_text").unwrap())?
95        .into_string()
96        .map_err(map_connection_error(base_url))?;
97    Ok(urdf_rs::read_from_string(&s)?)
98}
99
100pub(crate) fn euler_angles_from_quaternion(q: &[f64; 4]) -> (f64, f64, f64) {
101    to_nalgebra(q).euler_angles()
102}
103
104pub(crate) fn quaternion_from_euler_angles(r: f64, p: f64, y: f64) -> [f64; 4] {
105    let na_q = na::UnitQuaternion::from_euler_angles(r, p, y);
106    from_nalgebra(&na_q)
107}
108
109fn from_nalgebra(na_q: &na::UnitQuaternion<f64>) -> [f64; 4] {
110    let mut q = [0.0; 4];
111    q[0] = na_q.w;
112    q[1] = na_q.i;
113    q[2] = na_q.j;
114    q[3] = na_q.k;
115    q
116}
117
118fn to_nalgebra(q: &[f64; 4]) -> na::UnitQuaternion<f64> {
119    na::UnitQuaternion::from_quaternion(na::Quaternion::new(q[0], q[1], q[2], q[3]))
120}
121
122#[cfg(test)]
123mod tests {
124    use assert_approx_eq::assert_approx_eq;
125
126    use super::*;
127    #[test]
128    fn test_euler_quaternion() {
129        const R: f64 = 0.5;
130        const P: f64 = -0.2;
131        const Y: f64 = 1.0;
132        let q = quaternion_from_euler_angles(R, P, Y);
133        let angles = euler_angles_from_quaternion(&q);
134        assert_approx_eq!(angles.0, R);
135        assert_approx_eq!(angles.1, P);
136        assert_approx_eq!(angles.2, Y);
137        let q2 = quaternion_from_euler_angles(angles.0, angles.1, angles.2);
138        assert_approx_eq!(q[0], q2[0]);
139        assert_approx_eq!(q[1], q2[1]);
140        assert_approx_eq!(q[2], q2[2]);
141        assert_approx_eq!(q[3], q2[3]);
142    }
143}
arci_urdf_viz/utils.rs

arci_urdf_viz/
utils.rs
