openrr_tracing/
lib.rs

Help
1#![doc = include_str!("../README.md")]
2
3pub mod de;
4
5use std::time::{Duration, SystemTime};
6
7use tracing::trace;
8
9#[derive(Debug)]
10pub struct Tracing<T>(T);
11
12impl<T> Tracing<T> {
13    pub fn new(v: T) -> Self {
14        Self(v)
15    }
16
17    pub fn get_ref(&self) -> &T {
18        &self.0
19    }
20
21    pub fn get_mut(&mut self) -> &mut T {
22        &mut self.0
23    }
24
25    pub fn into_inner(self) -> T {
26        self.0
27    }
28}
29
30impl<T> From<T> for Tracing<T> {
31    fn from(value: T) -> Self {
32        Self::new(value)
33    }
34}
35
36// TODO: generate impls by codegen
37
38impl<T: arci::JointTrajectoryClient> arci::JointTrajectoryClient for Tracing<T> {
39    // TODO: deserialize/test
40    fn joint_names(&self) -> Vec<String> {
41        let names = self.0.joint_names();
42        trace!(method = "arci::JointTrajectoryClient::joint_names", ?names);
43        names
44    }
45
46    // TODO: deserialize/test
47    fn current_joint_positions(&self) -> Result<Vec<f64>, arci::Error> {
48        let positions = self.0.current_joint_positions()?;
49        trace!(
50            method = "arci::JointTrajectoryClient::current_joint_positions",
51            ?positions
52        );
53        Ok(positions)
54    }
55
56    // TODO: deserialize/test
57    fn send_joint_positions(
58        &self,
59        positions: Vec<f64>,
60        duration: Duration,
61    ) -> Result<arci::WaitFuture, arci::Error> {
62        trace!(
63            method = "arci::JointTrajectoryClient::send_joint_positions",
64            ?positions,
65            ?duration
66        );
67        self.0.send_joint_positions(positions, duration)
68    }
69
70    // TODO: deserialize/test
71    fn send_joint_trajectory(
72        &self,
73        trajectory: Vec<arci::TrajectoryPoint>,
74    ) -> Result<arci::WaitFuture, arci::Error> {
75        trace!(
76            method = "arci::JointTrajectoryClient::send_joint_trajectory",
77            ?trajectory
78        );
79        self.0.send_joint_trajectory(trajectory)
80    }
81}
82
83#[arci::async_trait]
84impl<T: arci::Gamepad> arci::Gamepad for Tracing<T> {
85    // TODO: deserialize/test
86    async fn next_event(&self) -> arci::gamepad::GamepadEvent {
87        let event = self.0.next_event().await;
88        trace!(method = "arci::Gamepad::next_event", ?event);
89        event
90    }
91
92    // TODO: deserialize/test
93    fn stop(&self) {
94        trace!(method = "arci::Gamepad::stop");
95        self.0.stop()
96    }
97}
98
99impl<T: arci::Localization> arci::Localization for Tracing<T> {
100    fn current_pose(&self, frame_id: &str) -> Result<arci::Isometry2<f64>, arci::Error> {
101        let pose = self.0.current_pose(frame_id)?;
102        trace!(
103            method = "arci::Localization::current_pose",
104            frame_id = frame_id,
105            pose_rotation_re = pose.rotation.re,
106            pose_rotation_im = pose.rotation.im,
107            pose_translation_x = pose.translation.x,
108            pose_translation_y = pose.translation.y,
109        );
110        Ok(pose)
111    }
112}
113
114// TODO: test set_motor_effort and get_motor_effort
115impl<T: arci::MotorDriveEffort> arci::MotorDriveEffort for Tracing<T> {
116    fn set_motor_effort(&self, effort: f64) -> Result<(), arci::Error> {
117        trace!(method = "arci::MotorDriveEffort::set_motor_effort", ?effort);
118        self.0.set_motor_effort(effort)
119    }
120
121    fn get_motor_effort(&self) -> Result<f64, arci::Error> {
122        let effort = self.0.get_motor_effort()?;
123        trace!(method = "arci::MotorDriveEffort::get_motor_effort", ?effort);
124        Ok(effort)
125    }
126}
127
128// TODO: test set_motor_position and get_motor_position
129impl<T: arci::MotorDrivePosition> arci::MotorDrivePosition for Tracing<T> {
130    fn set_motor_position(&self, position: f64) -> Result<(), arci::Error> {
131        trace!(
132            method = "arci::MotorDrivePosition::set_motor_position",
133            ?position
134        );
135        self.0.set_motor_position(position)
136    }
137
138    fn get_motor_position(&self) -> Result<f64, arci::Error> {
139        let position = self.0.get_motor_position()?;
140        trace!(
141            method = "arci::MotorDrivePosition::get_motor_position",
142            ?position
143        );
144        Ok(position)
145    }
146}
147
148// TODO: test set_motor_velocity and get_motor_velocity
149impl<T: arci::MotorDriveVelocity> arci::MotorDriveVelocity for Tracing<T> {
150    fn set_motor_velocity(&self, velocity: f64) -> Result<(), arci::Error> {
151        trace!(
152            method = "arci::MotorDriveVelocity::set_motor_velocity",
153            ?velocity
154        );
155        self.0.set_motor_velocity(velocity)
156    }
157
158    fn get_motor_velocity(&self) -> Result<f64, arci::Error> {
159        let velocity = self.0.get_motor_velocity()?;
160        trace!(
161            method = "arci::MotorDriveVelocity::get_motor_velocity",
162            ?velocity
163        );
164        Ok(velocity)
165    }
166}
167
168// TODO: test current_velocity
169impl<T: arci::MoveBase> arci::MoveBase for Tracing<T> {
170    fn send_velocity(&self, velocity: &arci::BaseVelocity) -> Result<(), arci::Error> {
171        trace!(
172            method = "arci::MoveBase::send_velocity",
173            velocity_x = velocity.x,
174            velocity_y = velocity.y,
175            velocity_theta = velocity.theta
176        );
177        self.0.send_velocity(velocity)
178    }
179
180    fn current_velocity(&self) -> Result<arci::BaseVelocity, arci::Error> {
181        let velocity = self.0.current_velocity()?;
182        trace!(
183            method = "arci::MoveBase::current_velocity",
184            velocity_x = velocity.x,
185            velocity_y = velocity.y,
186            velocity_theta = velocity.theta
187        );
188        Ok(velocity)
189    }
190}
191
192// TODO: test send_goal_pose and cancel
193impl<T: arci::Navigation> arci::Navigation for Tracing<T> {
194    fn send_goal_pose(
195        &self,
196        goal: arci::Isometry2<f64>,
197        frame_id: &str,
198        timeout: Duration,
199    ) -> Result<arci::WaitFuture, arci::Error> {
200        trace!(
201            method = "arci::Navigation::send_goal_pose",
202            goal_rotation_re = goal.rotation.re,
203            goal_rotation_im = goal.rotation.im,
204            goal_translation_x = goal.translation.x,
205            goal_translation_y = goal.translation.y,
206            frame_id,
207            timeout_secs = timeout.as_secs(),
208            timeout_nanos = timeout.subsec_nanos(),
209        );
210        self.0.send_goal_pose(goal, frame_id, timeout)
211    }
212
213    fn cancel(&self) -> Result<(), arci::Error> {
214        trace!(method = "arci::Navigation::cancel");
215        self.0.cancel()
216    }
217}
218
219// TODO: test speak
220impl<T: arci::Speaker> arci::Speaker for Tracing<T> {
221    fn speak(&self, message: &str) -> Result<arci::WaitFuture, arci::Error> {
222        trace!(method = "arci::Speaker::speak", ?message);
223        self.0.speak(message)
224    }
225}
226
227// TODO: test resolve_transformation
228impl<T: arci::TransformResolver> arci::TransformResolver for Tracing<T> {
229    fn resolve_transformation(
230        &self,
231        from: &str,
232        to: &str,
233        time: SystemTime,
234    ) -> Result<arci::Isometry3<f64>, arci::Error> {
235        let d = time.duration_since(SystemTime::UNIX_EPOCH).unwrap();
236        trace!(
237            method = "arci::TransformResolver::resolve_transformation",
238            ?from,
239            ?to,
240            time_secs = d.as_secs(),
241            time_nanos = d.subsec_nanos(),
242        );
243        self.0.resolve_transformation(from, to, time)
244    }
245}
openrr_tracing/lib.rs

openrr_tracing/
lib.rs
