arci_urdf_viz/
utils.rs
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}