1use std::time::Duration;
42
43mod error;
44pub use error::IncompleteTransmissionError;
45pub use error::InvalidMessageError;
46pub use error::ReceiveError;
47pub use error::SendError;
48
49mod generated;
50
51pub mod msg {
53 pub use super::generated::*;
54}
55
56pub mod sync_peer;
58
59#[cfg(feature = "tokio")]
61pub mod tokio_peer;
62
63#[cfg(feature = "nalgebra")]
65mod nalgebra;
66
67impl msg::EgmHeader {
68 pub fn new(seqno: u32, timestamp_ms: u32, kind: msg::egm_header::MessageType) -> Self {
69 Self {
70 seqno: Some(seqno),
71 tm: Some(timestamp_ms),
72 mtype: Some(kind as i32),
73 }
74 }
75
76 pub fn command(seqno: u32, timestamp_ms: u32) -> Self {
78 Self::new(seqno, timestamp_ms, msg::egm_header::MessageType::MsgtypeCommand)
79 }
80
81 pub fn data(seqno: u32, timestamp_ms: u32) -> Self {
83 Self::new(seqno, timestamp_ms, msg::egm_header::MessageType::MsgtypeData)
84 }
85
86 pub fn correction(seqno: u32, timestamp_ms: u32) -> Self {
88 Self::new(seqno, timestamp_ms, msg::egm_header::MessageType::MsgtypeCorrection)
89 }
90
91 pub fn path_correction(seqno: u32, timestamp_ms: u32) -> Self {
93 Self::new(seqno, timestamp_ms, msg::egm_header::MessageType::MsgtypePathCorrection)
94 }
95}
96
97impl msg::EgmCartesian {
98 pub fn from_mm(x: f64, y: f64, z: f64) -> Self {
100 Self { x, y, z }
101 }
102
103 pub fn as_mm(&self) -> [f64; 3] {
105 [self.x, self.y, self.z]
106 }
107
108 pub fn has_nan(&self) -> bool {
110 self.x.is_nan() || self.y.is_nan() || self.z.is_nan()
111 }
112}
113
114impl From<[f64; 3]> for msg::EgmCartesian {
115 fn from(other: [f64; 3]) -> Self {
117 let [x, y, z] = other;
118 Self::from_mm(x, y, z)
119 }
120}
121
122impl From<&[f64; 3]> for msg::EgmCartesian {
123 fn from(other: &[f64; 3]) -> Self {
125 let &[x, y, z] = other;
126 Self::from_mm(x, y, z)
127 }
128}
129
130impl From<(f64, f64, f64)> for msg::EgmCartesian {
131 fn from(other: (f64, f64, f64)) -> Self {
133 let (x, y, z) = other;
134 Self::from_mm(x, y, z)
135 }
136}
137
138impl msg::EgmQuaternion {
139 pub fn from_wxyz(w: f64, x: f64, y: f64, z: f64) -> Self {
141 Self {
142 u0: w,
143 u1: x,
144 u2: y,
145 u3: z,
146 }
147 }
148
149 pub fn as_wxyz(&self) -> [f64; 4] {
151 [self.u0, self.u1, self.u2, self.u3]
152 }
153
154 pub fn has_nan(&self) -> bool {
156 self.u0.is_nan() || self.u1.is_nan() || self.u2.is_nan() || self.u3.is_nan()
157 }
158}
159
160impl msg::EgmEuler {
161 pub fn from_xyz_degrees(x: f64, y: f64, z: f64) -> Self {
163 Self { x, y, z }
164 }
165
166 pub fn as_xyz_degrees(&self) -> [f64; 3] {
168 [self.x, self.y, self.z]
169 }
170
171 pub fn has_nan(&self) -> bool {
173 self.x.is_nan() || self.y.is_nan() || self.z.is_nan()
174 }
175}
176
177impl msg::EgmClock {
178 pub fn new(sec: u64, usec: u64) -> Self {
180 Self { sec, usec }
181 }
182
183 pub fn elapsed_since_epoch(&self) -> Duration {
187 let secs = self.sec + self.usec / 1_000_000;
188 let nanos = (self.usec % 1_000_000) as u32 * 1_000;
189 Duration::new(secs, nanos)
190 }
191
192 pub fn as_timestamp_ms(&self) -> u32 {
194 self.sec.wrapping_mul(1_000).wrapping_add(self.usec / 1_000) as u32
195 }
196}
197
198#[cfg(test)]
199#[test]
200fn test_clock_to_duration() {
201 use assert2::assert;
202 use msg::EgmClock;
203
204 assert!(EgmClock::new(0, 0).elapsed_since_epoch() == Duration::new(0, 0));
205 assert!(EgmClock::new(1, 0).elapsed_since_epoch() == Duration::new(1, 0));
206 assert!(EgmClock::new(2, 123).elapsed_since_epoch() == Duration::new(2, 000_123_000));
207 assert!(EgmClock::new(3, 987_654).elapsed_since_epoch() == Duration::new(3, 987_654_000));
208 assert!(EgmClock::new(4, 2_345_000).elapsed_since_epoch() == Duration::new(6, 345_000_000));
209}
210
211#[cfg(test)]
212#[test]
213fn test_clock_to_timestampc() {
214 use assert2::assert;
215 use msg::EgmClock;
216
217 assert!(EgmClock::new(0, 0).as_timestamp_ms() == 0);
218 assert!(EgmClock::new(1, 0).as_timestamp_ms() == 1_000);
219 assert!(EgmClock::new(2, 123).as_timestamp_ms() == 2_000);
220 assert!(EgmClock::new(3, 987_654).as_timestamp_ms() == 3_987);
221 assert!(EgmClock::new(4, 2_345_000).as_timestamp_ms() == 6_345);
222}
223
224impl Copy for msg::EgmClock {}
225
226impl std::ops::Add<Duration> for msg::EgmClock {
227 type Output = Self;
228
229 #[allow(clippy::suspicious_arithmetic_impl)]
230 fn add(self, right: Duration) -> Self::Output {
231 let usec = self.usec + u64::from(right.subsec_micros());
232 msg::EgmClock {
233 sec: self.sec + right.as_secs() + usec / 1_000_000,
234 usec: usec % 1_000_000,
235 }
236 }
237}
238
239impl std::ops::Add<msg::EgmClock> for Duration {
240 type Output = msg::EgmClock;
241
242 fn add(self, right: msg::EgmClock) -> Self::Output {
243 right + self
244 }
245}
246
247impl std::ops::Add<&Duration> for &msg::EgmClock {
248 type Output = msg::EgmClock;
249
250 fn add(self, right: &Duration) -> Self::Output {
251 *self + *right
252 }
253}
254
255impl std::ops::Add<&msg::EgmClock> for &Duration {
256 type Output = msg::EgmClock;
257
258 fn add(self, right: &msg::EgmClock) -> Self::Output {
259 *self + *right
260 }
261}
262
263impl std::ops::AddAssign<&Duration> for msg::EgmClock {
264 fn add_assign(&mut self, right: &Duration) {
265 *self = &*self + right
266 }
267}
268
269impl std::ops::AddAssign<Duration> for msg::EgmClock {
270 fn add_assign(&mut self, right: Duration) {
271 *self += &right
272 }
273}
274
275#[cfg(test)]
276#[test]
277fn test_add_duration() {
278 use assert2::assert;
279 use msg::EgmClock;
280 assert!(EgmClock::new(1, 500_000) + Duration::from_secs(1) == EgmClock::new(2, 500_000));
281 assert!(EgmClock::new(1, 500_000) + Duration::from_millis(600) == EgmClock::new(2, 100_000));
282 assert!(&EgmClock::new(1, 500_000) + &Duration::from_secs(1) == EgmClock::new(2, 500_000));
283 assert!(&EgmClock::new(1, 500_000) + &Duration::from_millis(600) == EgmClock::new(2, 100_000));
284 assert!(Duration::from_secs(1) + EgmClock::new(1, 500_000) == EgmClock::new(2, 500_000));
285 assert!(Duration::from_millis(600) + EgmClock::new(1, 500_000) == EgmClock::new(2, 100_000));
286 assert!(&Duration::from_secs(1) + &EgmClock::new(1, 500_000) == EgmClock::new(2, 500_000));
287 assert!(&Duration::from_millis(600) + &EgmClock::new(1, 500_000) == EgmClock::new(2, 100_000));
288
289 let mut clock = EgmClock::new(10, 999_999);
290 clock += Duration::from_micros(1);
291 assert!(clock == EgmClock::new(11, 0));
292 clock += Duration::from_micros(999_999);
293 assert!(clock == EgmClock::new(11, 999_999));
294 clock += Duration::from_micros(2);
295 assert!(clock == EgmClock::new(12, 1));
296}
297
298impl msg::EgmPose {
299 pub fn new(position: impl Into<msg::EgmCartesian>, orientation: impl Into<msg::EgmQuaternion>) -> Self {
301 Self {
302 pos: Some(position.into()),
303 orient: Some(orientation.into()),
304 euler: None,
305 }
306 }
307
308 pub fn has_nan(&self) -> bool {
310 let has_nan = false;
311 let has_nan = has_nan || self.pos.as_ref().map(|x| x.has_nan()).unwrap_or(false);
312 let has_nan = has_nan || self.orient.as_ref().map(|x| x.has_nan()).unwrap_or(false);
313 let has_nan = has_nan || self.euler.as_ref().map(|x| x.has_nan()).unwrap_or(false);
314 has_nan
315 }
316}
317
318impl msg::EgmCartesianSpeed {
319 pub fn from_xyz_mm(x: f64, y: f64, z: f64) -> Self {
321 Self { value: vec![x, y, z] }
322 }
323
324 pub fn has_nan(&self) -> bool {
326 self.value.iter().any(|x| x.is_nan())
327 }
328}
329
330impl From<[f64; 3]> for msg::EgmCartesianSpeed {
331 fn from(other: [f64; 3]) -> Self {
332 let [x, y, z] = other;
333 Self::from_xyz_mm(x, y, z)
334 }
335}
336
337impl From<&[f64; 3]> for msg::EgmCartesianSpeed {
338 fn from(other: &[f64; 3]) -> Self {
339 let &[x, y, z] = other;
340 Self::from_xyz_mm(x, y, z)
341 }
342}
343
344impl From<(f64, f64, f64)> for msg::EgmCartesianSpeed {
345 fn from(other: (f64, f64, f64)) -> Self {
346 let (x, y, z) = other;
347 Self::from_xyz_mm(x, y, z)
348 }
349}
350
351impl msg::EgmJoints {
352 pub fn from_degrees(joints: impl Into<Vec<f64>>) -> Self {
354 Self { joints: joints.into() }
355 }
356
357 pub fn has_nan(&self) -> bool {
359 self.joints.iter().any(|x| x.is_nan())
360 }
361}
362
363impl From<Vec<f64>> for msg::EgmJoints {
364 fn from(other: Vec<f64>) -> Self {
366 Self::from_degrees(other)
367 }
368}
369
370impl From<&[f64]> for msg::EgmJoints {
371 fn from(other: &[f64]) -> Self {
373 Self::from_degrees(other)
374 }
375}
376
377impl From<[f64; 6]> for msg::EgmJoints {
378 fn from(other: [f64; 6]) -> Self {
380 Self::from_degrees(&other[..])
381 }
382}
383
384impl From<&[f64; 6]> for msg::EgmJoints {
385 fn from(other: &[f64; 6]) -> Self {
387 Self::from_degrees(&other[..])
388 }
389}
390
391impl msg::EgmExternalJoints {
392 pub fn from_degrees(joints: impl Into<Vec<f64>>) -> Self {
394 Self { joints: joints.into() }
395 }
396
397 pub fn has_nan(&self) -> bool {
399 self.joints.iter().any(|x| x.is_nan())
400 }
401}
402
403impl From<Vec<f64>> for msg::EgmExternalJoints {
404 fn from(other: Vec<f64>) -> Self {
406 Self::from_degrees(other)
407 }
408}
409
410impl From<&[f64]> for msg::EgmExternalJoints {
411 fn from(other: &[f64]) -> Self {
413 Self::from_degrees(other)
414 }
415}
416
417impl msg::EgmPlanned {
418 pub fn joints(joints: impl Into<msg::EgmJoints>, time: impl Into<msg::EgmClock>) -> Self {
420 Self {
421 time: Some(time.into()),
422 joints: Some(joints.into()),
423 cartesian: None,
424 external_joints: None,
425 }
426 }
427
428 pub fn pose(pose: impl Into<msg::EgmPose>, time: impl Into<msg::EgmClock>) -> Self {
430 Self {
431 time: Some(time.into()),
432 cartesian: Some(pose.into()),
433 joints: None,
434 external_joints: None,
435 }
436 }
437
438 pub fn has_nan(&self) -> bool {
440 let has_nan = false;
441 let has_nan = has_nan || self.joints.as_ref().map(|x| x.has_nan()).unwrap_or(false);
442 let has_nan = has_nan || self.cartesian.as_ref().map(|x| x.has_nan()).unwrap_or(false);
443 let has_nan = has_nan || self.external_joints.as_ref().map(|x| x.has_nan()).unwrap_or(false);
444 has_nan
445 }
446}
447
448impl msg::EgmSpeedRef {
449 pub fn joints(joints: impl Into<msg::EgmJoints>) -> Self {
450 Self {
451 joints: Some(joints.into()),
452 external_joints: None,
453 cartesians: None,
454 }
455 }
456
457 pub fn cartesian(cartesian: impl Into<msg::EgmCartesianSpeed>) -> Self {
458 Self {
459 cartesians: Some(cartesian.into()),
460 joints: None,
461 external_joints: None,
462 }
463 }
464
465 pub fn has_nan(&self) -> bool {
467 let has_nan = false;
468 let has_nan = has_nan || self.joints.as_ref().map(|x| x.has_nan()).unwrap_or(false);
469 let has_nan = has_nan || self.cartesians.as_ref().map(|x| x.has_nan()).unwrap_or(false);
470 let has_nan = has_nan || self.external_joints.as_ref().map(|x| x.has_nan()).unwrap_or(false);
471 has_nan
472 }
473}
474
475impl msg::EgmPathCorr {
476 pub fn new(position: impl Into<msg::EgmCartesian>, age_ms: u32) -> Self {
478 Self {
479 pos: position.into(),
480 age: age_ms,
481 }
482 }
483
484 pub fn has_nan(&self) -> bool {
486 self.pos.has_nan()
487 }
488}
489
490impl msg::EgmSensor {
491 pub fn joint_target(sequence_number: u32, joints: impl Into<msg::EgmJoints>, time: impl Into<msg::EgmClock>) -> Self {
495 let time = time.into();
496 Self {
497 header: Some(msg::EgmHeader::correction(sequence_number, time.as_timestamp_ms())),
498 planned: Some(msg::EgmPlanned::joints(joints, time)),
499 speed_ref: None,
500 }
501 }
502
503 pub fn joint_target_with_speed(sequence_number: u32, joints: impl Into<msg::EgmJoints>, speed: impl Into<msg::EgmJoints>, time: impl Into<msg::EgmClock>) -> Self {
507 let time = time.into();
508 Self {
509 header: Some(msg::EgmHeader::correction(sequence_number, time.as_timestamp_ms())),
510 planned: Some(msg::EgmPlanned::joints(joints, time)),
511 speed_ref: Some(msg::EgmSpeedRef::joints(speed)),
512 }
513 }
514
515 pub fn pose_target(sequence_number: u32, pose: impl Into<msg::EgmPose>, time: impl Into<msg::EgmClock>) -> Self {
519 let time = time.into();
520 Self {
521 header: Some(msg::EgmHeader::correction(sequence_number, time.as_timestamp_ms())),
522 planned: Some(msg::EgmPlanned::pose(pose, time)),
523 speed_ref: None,
524 }
525 }
526
527 pub fn pose_target_with_speed(sequence_number: u32, pose: impl Into<msg::EgmPose>, speed: impl Into<msg::EgmCartesianSpeed>, time: impl Into<msg::EgmClock>) -> Self {
531 let time = time.into();
532 Self {
533 header: Some(msg::EgmHeader::correction(sequence_number, time.as_timestamp_ms())),
534 planned: Some(msg::EgmPlanned::pose(pose, time)),
535 speed_ref: Some(msg::EgmSpeedRef::cartesian(speed)),
536 }
537 }
538
539 pub fn has_nan(&self) -> bool {
541 let has_nan = false;
542 let has_nan = has_nan || self.planned.as_ref().map(|x| x.has_nan()).unwrap_or(false);
543 let has_nan = has_nan || self.speed_ref.as_ref().map(|x| x.has_nan()).unwrap_or(false);
544 has_nan
545 }
546}
547
548impl msg::EgmSensorPathCorr {
549 pub fn new(sequence_number: u32, timestamp_ms: u32, correction: impl Into<msg::EgmCartesian>, age_ms: u32) -> Self {
551 Self {
552 header: Some(msg::EgmHeader::path_correction(sequence_number, timestamp_ms)),
553 path_corr: Some(msg::EgmPathCorr::new(correction, age_ms)),
554 }
555 }
556
557 pub fn has_nan(&self) -> bool {
559 self.path_corr.as_ref().map(|x| x.has_nan()).unwrap_or(false)
560 }
561}
562
563impl msg::EgmFeedBack {
564 pub fn has_nan(&self) -> bool {
566 let has_nan = false;
567 let has_nan = has_nan || self.joints.as_ref().map(|x| x.has_nan()).unwrap_or(false);
568 let has_nan = has_nan || self.cartesian.as_ref().map(|x| x.has_nan()).unwrap_or(false);
569 let has_nan = has_nan || self.external_joints.as_ref().map(|x| x.has_nan()).unwrap_or(false);
570 has_nan
571 }
572}
573
574impl msg::EgmMeasuredForce {
575 pub fn has_nan(&self) -> bool {
577 self.force.iter().any(|x| x.is_nan())
578 }
579}
580
581impl msg::EgmRobot {
582 pub fn sequence_number(&self) -> Option<u32> {
583 self.header.as_ref()?.seqno
584 }
585
586 pub fn timestamp_ms(&self) -> Option<u32> {
587 self.header.as_ref()?.tm
588 }
589
590 pub fn feedback_joints(&self) -> Option<&Vec<f64>> {
591 Some(&self.feed_back.as_ref()?.joints.as_ref()?.joints)
592 }
593
594 pub fn feedback_pose(&self) -> Option<&msg::EgmPose> {
595 self.feed_back.as_ref()?.cartesian.as_ref()
596 }
597
598 pub fn feedback_extenal_joints(&self) -> Option<&Vec<f64>> {
599 Some(&self.feed_back.as_ref()?.external_joints.as_ref()?.joints)
600 }
601
602 pub fn feedback_time(&self) -> Option<msg::EgmClock> {
603 self.feed_back.as_ref()?.time
604 }
605
606 pub fn planned_joints(&self) -> Option<&Vec<f64>> {
607 Some(&self.planned.as_ref()?.joints.as_ref()?.joints)
608 }
609
610 pub fn planned_pose(&self) -> Option<&msg::EgmPose> {
611 self.planned.as_ref()?.cartesian.as_ref()
612 }
613
614 pub fn planned_extenal_joints(&self) -> Option<&Vec<f64>> {
615 Some(&self.planned.as_ref()?.external_joints.as_ref()?.joints)
616 }
617
618 pub fn planned_time(&self) -> Option<msg::EgmClock> {
619 self.planned.as_ref()?.time
620 }
621
622 pub fn motors_enabled(&self) -> Option<bool> {
623 use msg::egm_motor_state::MotorStateType;
624 match self.motor_state.as_ref()?.state() {
625 MotorStateType::MotorsUndefined => None,
626 MotorStateType::MotorsOn => Some(true),
627 MotorStateType::MotorsOff => Some(false),
628 }
629 }
630
631 pub fn rapid_running(&self) -> Option<bool> {
632 use msg::egm_rapid_ctrl_exec_state::RapidCtrlExecStateType;
633 match self.rapid_exec_state.as_ref()?.state() {
634 RapidCtrlExecStateType::RapidUndefined => None,
635 RapidCtrlExecStateType::RapidRunning => Some(true),
636 RapidCtrlExecStateType::RapidStopped => Some(false),
637 }
638 }
639
640 pub fn test_signals(&self) -> Option<&Vec<f64>> {
641 Some(&self.test_signals.as_ref()?.signals)
642 }
643
644 pub fn measured_force(&self) -> Option<&Vec<f64>> {
645 Some(&self.measured_force.as_ref()?.force)
646 }
647
648 pub fn has_nan(&self) -> bool {
650 let has_nan = false;
651 let has_nan = has_nan || self.feed_back.as_ref().map(|x| x.has_nan()).unwrap_or(false);
652 let has_nan = has_nan || self.planned.as_ref().map(|x| x.has_nan()).unwrap_or(false);
653 let has_nan = has_nan || self.measured_force.as_ref().map(|x| x.has_nan()).unwrap_or(false);
654 let has_nan = has_nan || self.utilization_rate.as_ref().map(|x| x.is_nan()).unwrap_or(false);
655 has_nan
656 }
657}
658
659fn encode_to_vec(msg: &impl prost::Message) -> Result<Vec<u8>, prost::EncodeError> {
661 let encoded_len = msg.encoded_len();
662 let mut buffer = Vec::<u8>::with_capacity(encoded_len);
663 msg.encode(&mut buffer)?;
664 Ok(buffer)
665}
666
667#[cfg(test)]
668#[test]
669fn test_encode_to_vec() {
670 use assert2::assert;
671 use prost::Message;
672
673 assert!(encode_to_vec(&true).unwrap().len() == true.encoded_len());
674 assert!(encode_to_vec(&10).unwrap().len() == 10.encoded_len());
675 assert!(encode_to_vec(&String::from("aap noot mies")).unwrap().len() == String::from("aap noot mies").encoded_len());
676}