ros_nalgebra/
lib.rs

Help
1/*
2  Copyright 2020 Smile Robotics, Inc
3
4  Licensed under the Apache License, Version 2.0 (the "License");
5  you may not use this file except in compliance with the License.
6  You may obtain a copy of the License at
7
8      http://www.apache.org/licenses/LICENSE-2.0
9
10  Unless required by applicable law or agreed to in writing, software
11  distributed under the License is distributed on an "AS IS" BASIS,
12  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  See the License for the specific language governing permissions and
14  limitations under the License.
15*/
16
17#![doc = include_str!("../README.md")]
18#![warn(missing_debug_implementations, rust_2018_idioms)]
19
20/// Use this macro instead of rosrust::rosmsg_include!() to generate `impl From`.
21///
22/// # Example
23///
24/// ```
25/// mod msg {
26///     ros_nalgebra::rosmsg_include!(nav_msgs/Odometry);
27/// }
28/// ```
29#[macro_export]
30macro_rules! rosmsg_include {
31    ( $($ns:ident / $msg:ident),* $(,)?) => {
32        ::rosrust::rosmsg_include!(
33            geometry_msgs/Point,
34            geometry_msgs/Pose,
35            geometry_msgs/Quaternion,
36            geometry_msgs/Transform,
37            geometry_msgs/Vector3,
38            $($ns/$msg),*
39        );
40        $crate::ros_nalgebra!(self);
41    }
42}
43
44/// Generate `impl From<<ROS_MESSAGE>` and `impl From<NALGEBRA_STRUCT>`
45/// # Example
46///
47/// ```
48/// mod msg {
49///     rosrust::rosmsg_include!(geometry_msgs/Pose);
50/// }
51/// ros_nalgebra::ros_nalgebra_msg!(msg, Quaternion);
52/// ros_nalgebra::ros_nalgebra_msg!(msg, Point);
53/// ros_nalgebra::ros_nalgebra_msg!(msg, Pose);
54/// let pose: nalgebra::Isometry3<f64> = msg::geometry_msgs::Pose::default().into();
55/// ```
56#[macro_export]
57macro_rules! ros_nalgebra_msg {
58    ($($ns:ident)::+, Vector3) => {
59        // Vector3
60        impl From<$($ns::)+geometry_msgs::Vector3> for ::nalgebra::Vector3<f64> {
61            fn from(vec_msg: $($ns::)+geometry_msgs::Vector3) -> Self {
62                ::nalgebra::Vector3::new(vec_msg.x, vec_msg.y, vec_msg.z)
63            }
64        }
65
66        impl From<::nalgebra::Vector3<f64>> for $($ns::)+geometry_msgs::Vector3 {
67            fn from(vec: ::nalgebra::Vector3<f64>) -> Self {
68                $($ns::)+geometry_msgs::Vector3 {
69                    x: vec.x,
70                    y: vec.y,
71                    z: vec.z,
72                }
73            }
74        }
75
76        // Vector3 <-> Translation3
77        impl From<$($ns::)+geometry_msgs::Vector3> for ::nalgebra::Translation3<f64> {
78            fn from(vec_msg: $($ns::)+geometry_msgs::Vector3) -> Self {
79                ::nalgebra::Translation3::new(vec_msg.x, vec_msg.y, vec_msg.z)
80            }
81        }
82
83        impl From<::nalgebra::Translation3<f64>> for $($ns::)+geometry_msgs::Vector3 {
84            fn from(p: ::nalgebra::Translation3<f64>) -> Self {
85                $($ns::)+geometry_msgs::Vector3 {
86                    x: p.vector.x,
87                    y: p.vector.y,
88                    z: p.vector.z,
89                }
90            }
91        }
92    };
93    ($($ns:ident)::+, Point) => {
94        // Point <-> Translation3
95        impl From<$($ns::)+geometry_msgs::Point> for ::nalgebra::Translation3<f64> {
96            fn from(vec_msg: $($ns::)+geometry_msgs::Point) -> Self {
97                ::nalgebra::Translation3::new(vec_msg.x, vec_msg.y, vec_msg.z)
98            }
99        }
100
101        impl From<::nalgebra::Translation3<f64>> for $($ns::)+geometry_msgs::Point {
102            fn from(p: ::nalgebra::Translation3<f64>) -> Self {
103                $($ns::)+geometry_msgs::Point {
104                    x: p.vector.x,
105                    y: p.vector.y,
106                    z: p.vector.z,
107                }
108            }
109        }
110
111        // Point <-> Point3
112        impl From<$($ns::)+geometry_msgs::Point> for ::nalgebra::Point3<f64> {
113            fn from(vec_msg: $($ns::)+geometry_msgs::Point) -> Self {
114                ::nalgebra::Point3::new(vec_msg.x, vec_msg.y, vec_msg.z)
115            }
116        }
117
118        impl From<::nalgebra::Point3<f64>> for $($ns::)+geometry_msgs::Point {
119            fn from(p: ::nalgebra::Point3<f64>) -> Self {
120                $($ns::)+geometry_msgs::Point {
121                    x: p.coords.x,
122                    y: p.coords.y,
123                    z: p.coords.z,
124                }
125            }
126        }
127    };
128    ($($ns:ident)::+, Quaternion) => {
129        // Quaternion <-> UnitQuaternion
130        impl From<$($ns::)+geometry_msgs::Quaternion> for ::nalgebra::UnitQuaternion<f64> {
131            fn from(q_msg: $($ns::)+geometry_msgs::Quaternion) -> Self {
132                ::nalgebra::UnitQuaternion::from_quaternion(::nalgebra::Quaternion::new(
133                    q_msg.w, q_msg.x, q_msg.y, q_msg.z,
134                ))
135            }
136        }
137
138        impl From<::nalgebra::UnitQuaternion<f64>> for $($ns::)+geometry_msgs::Quaternion {
139            fn from(q: ::nalgebra::UnitQuaternion<f64>) -> Self {
140                $($ns::)+geometry_msgs::Quaternion {
141                    x: q.coords.x,
142                    y: q.coords.y,
143                    z: q.coords.z,
144                    w: q.coords.w,
145                }
146            }
147        }
148
149        // Quaternion
150        impl From<$($ns::)+geometry_msgs::Quaternion> for ::nalgebra::Quaternion<f64> {
151            fn from(q_msg: $($ns::)+geometry_msgs::Quaternion) -> Self {
152                ::nalgebra::Quaternion::new(q_msg.w, q_msg.x, q_msg.y, q_msg.z)
153            }
154        }
155
156        impl From<::nalgebra::Quaternion<f64>> for $($ns::)+geometry_msgs::Quaternion {
157            fn from(q: ::nalgebra::Quaternion<f64>) -> Self {
158                $($ns::)+geometry_msgs::Quaternion {
159                    x: q.coords.x,
160                    y: q.coords.y,
161                    z: q.coords.z,
162                    w: q.coords.w,
163                }
164            }
165        }
166    };
167    ($($ns:ident)::+, Pose) => {
168        // Pose <-> Isometry3
169        impl From<$($ns::)+geometry_msgs::Pose> for ::nalgebra::Isometry3<f64> {
170            fn from(pose_msg: $($ns::)+geometry_msgs::Pose) -> Self {
171                ::nalgebra::Isometry3::from_parts(
172                    pose_msg.position.into(),
173                    pose_msg.orientation.into(),
174                )
175            }
176        }
177
178        impl From<::nalgebra::Isometry3<f64>> for $($ns::)+geometry_msgs::Pose {
179            fn from(pose: ::nalgebra::Isometry3<f64>) -> Self {
180                $($ns::)+geometry_msgs::Pose {
181                    position: pose.translation.into(),
182                    orientation: pose.rotation.into(),
183                }
184            }
185        }
186    };
187    ($($ns:ident)::+, Transform) => {
188        // Transform <-> Isometry3
189        impl From<$($ns::)+geometry_msgs::Transform> for ::nalgebra::Isometry3<f64> {
190            fn from(pose_msg: $($ns::)+geometry_msgs::Transform) -> Self {
191                ::nalgebra::Isometry3::from_parts(
192                    pose_msg.translation.into(),
193                    pose_msg.rotation.into(),
194                )
195            }
196        }
197
198        impl From<::nalgebra::Isometry3<f64>> for $($ns::)+geometry_msgs::Transform {
199            fn from(pose: ::nalgebra::Isometry3<f64>) -> Self {
200                $($ns::)+geometry_msgs::Transform {
201                    translation: pose.translation.into(),
202                    rotation: pose.rotation.into(),
203                }
204            }
205        }
206    };
207}
208
209/// Generate Point/Vector3/Quaternion/Pose/Transform converter in `mod $ns`.
210/// `$ns` is the namespace for the rust messages.
211/// For example `msg`.
212///
213/// # Example
214///
215/// ```
216/// mod msg {
217///     rosrust::rosmsg_include!(
218///         geometry_msgs/Point,
219///         geometry_msgs/Pose,
220///         geometry_msgs/Quaternion,
221///         geometry_msgs/Transform,
222///         geometry_msgs/Vector3,
223///     );
224/// }
225/// ros_nalgebra::ros_nalgebra!(msg);
226/// ```
227#[macro_export]
228macro_rules! ros_nalgebra {
229    ($($ns:ident)::+) => {
230        $crate::ros_nalgebra_msg!($($ns)::+, Point);
231        $crate::ros_nalgebra_msg!($($ns)::+, Vector3);
232        $crate::ros_nalgebra_msg!($($ns)::+, Quaternion);
233        $crate::ros_nalgebra_msg!($($ns)::+, Pose);
234        $crate::ros_nalgebra_msg!($($ns)::+, Transform);
235    };
236}
ros_nalgebra/lib.rs

ros_nalgebra/
lib.rs
