robot_description_builder/transmission/
transmission_joint.rs

1use std::sync::{Arc, Weak};
2
3use crate::{
4	cluster_objects::kinematic_data_tree::KinematicDataTree,
5	identifiers::GroupID,
6	joint::Joint,
7	utils::{ArcRW, WeakLock},
8};
9
10#[cfg(feature = "urdf")]
11use crate::to_rdf::to_urdf::ToURDF;
12#[cfg(feature = "xml")]
13use itertools::Itertools;
14#[cfg(feature = "xml")]
15use quick_xml::{events::attributes::Attribute, name::QName};
16
17use super::{BuildTransmissionError, TransmissionHardwareInterface};
18
19#[derive(Debug, PartialEq, Clone, Default)]
20pub struct TransmissionJointBuilder {
21	joint_name: String,
22	hardware_interfaces: Vec<TransmissionHardwareInterface>,
23}
24
25impl TransmissionJointBuilder {
26	/// TODO: DOC
27	///
28	/// The minum hardware interfaces is 1 so I require one at creation
29	pub fn new(
30		joint_name: impl Into<String>,
31		hardware_interface: TransmissionHardwareInterface,
32	) -> Self {
33		Self {
34			joint_name: joint_name.into(),
35			hardware_interfaces: vec![hardware_interface],
36		}
37	}
38
39	pub fn add_hw_interface(&mut self, hardware_interface: TransmissionHardwareInterface) {
40		self.hardware_interfaces.push(hardware_interface)
41	}
42
43	pub fn with_hw_inteface(mut self, hardware_interface: TransmissionHardwareInterface) -> Self {
44		self.hardware_interfaces.push(hardware_interface);
45
46		self
47	}
48
49	pub fn name(&self) -> &String {
50		&self.joint_name
51	}
52
53	pub fn hw_interfaces(&self) -> &Vec<TransmissionHardwareInterface> {
54		&self.hardware_interfaces
55	}
56
57	pub fn hw_interfaces_mut(&mut self) -> &mut Vec<TransmissionHardwareInterface> {
58		&mut self.hardware_interfaces
59	}
60
61	pub(super) fn build(
62		self,
63		tree: &Arc<KinematicDataTree>,
64	) -> Result<TransmissionJoint, BuildTransmissionError> {
65		let joint = match tree.joints.mread()?.get(self.name()).map(Weak::clone) {
66			Some(joint) => joint,
67			None => return Err(BuildTransmissionError::InvalidJoint(self.joint_name)),
68		};
69
70		Ok(TransmissionJoint {
71			joint,
72			hardware_interfaces: self.hardware_interfaces,
73		})
74	}
75}
76
77impl<Name> From<(Name, TransmissionHardwareInterface)> for TransmissionJointBuilder
78where
79	Name: Into<String>,
80{
81	fn from(value: (Name, TransmissionHardwareInterface)) -> Self {
82		let (name, hardware_interface) = value;
83
84		Self::new(name, hardware_interface)
85	}
86}
87
88impl<Name> From<(Name, Vec<TransmissionHardwareInterface>)> for TransmissionJointBuilder
89where
90	Name: Into<String>,
91{
92	fn from(value: (Name, Vec<TransmissionHardwareInterface>)) -> Self {
93		let (name, hardware_interfaces) = value;
94
95		Self {
96			joint_name: name.into(),
97			hardware_interfaces,
98		}
99	}
100}
101
102#[derive(Debug)]
103// pub(super)
104pub struct TransmissionJoint {
105	/// TODO: This is not the way for the builder since it is not transmutable to other groups
106	joint: WeakLock<Joint>,
107	/// TODO:
108	hardware_interfaces: Vec<TransmissionHardwareInterface>,
109}
110
111impl TransmissionJoint {
112	pub fn joint(&self) -> WeakLock<Joint> {
113		Weak::clone(&self.joint)
114	}
115
116	pub fn hardware_interfaces(&self) -> &Vec<TransmissionHardwareInterface> {
117		&self.hardware_interfaces
118	}
119
120	pub fn rebuild(&self) -> TransmissionJointBuilder {
121		TransmissionJointBuilder {
122			joint_name: self
123				.joint
124				.upgrade()
125				.unwrap() // This unwrap is Ok
126				.read()
127				.unwrap() // FIXME: This unwrap is not Ok
128				.name()
129				.clone(),
130			hardware_interfaces: self.hardware_interfaces.clone(),
131		}
132	}
133}
134
135#[cfg(feature = "urdf")]
136impl ToURDF for TransmissionJoint {
137	fn to_urdf(
138		&self,
139		writer: &mut quick_xml::Writer<std::io::Cursor<Vec<u8>>>,
140		urdf_config: &crate::to_rdf::to_urdf::URDFConfig,
141	) -> Result<(), quick_xml::Error> {
142		writer
143			.create_element("joint")
144			.with_attribute(Attribute {
145				key: QName(b"name"),
146				value: self
147					.joint
148					.upgrade()
149					.unwrap() // FIXME: Is UNWRAP OK?
150					.read()
151					.unwrap() // FIXME: Is UNWRAP OK?
152					.name()
153					.display()
154					.as_bytes()
155					.into(),
156			})
157			.write_inner_content(|writer| -> quick_xml::Result<()> {
158				self.hardware_interfaces
159					.iter()
160					.map(|hw_interface| hw_interface.to_urdf(writer, urdf_config))
161					.process_results(|iter| iter.collect::<Vec<_>>())?;
162				Ok(())
163			})?;
164
165		Ok(())
166	}
167}
168
169impl PartialEq for TransmissionJoint {
170	fn eq(&self, other: &Self) -> bool {
171		Weak::ptr_eq(&self.joint, &other.joint)
172			&& self.hardware_interfaces == other.hardware_interfaces
173	}
174}
175
176impl Clone for TransmissionJoint {
177	fn clone(&self) -> Self {
178		Self {
179			joint: Weak::clone(&self.joint),
180			hardware_interfaces: self.hardware_interfaces.clone(),
181		}
182	}
183}
robot_description_builder/transmission/transmission_joint.rs

robot_description_builder/transmission/
transmission_joint.rs
