robot_description_builder/link/
helper_functions.rs

1use crate::link::{
2	builder::{CollisionBuilder, LinkBuilder, VisualBuilder},
3	geometry::{BoxGeometry, CylinderGeometry, GeometryInterface, SphereGeometry},
4};
5
6/// TODO: Finalize, this is temp
7/// TODO: ADD NAMED CHOICE for Vis & Col
8/// TODO: Add material Specifierer
9/// TODO: Add Inertial data options
10/// TODO: ADD TEST?
11pub fn new_quick_link(link_name: impl Into<String>, visual: VisualBuilder) -> LinkBuilder {
12	let link_name = link_name.into();
13	let mut link = LinkBuilder::new(&link_name);
14
15	let mut collision_name = link_name.clone();
16	collision_name.push_str("_collision");
17	link = link.add_collider(visual.to_collision().named(collision_name));
18
19	let mut visual_name = link_name.clone();
20	visual_name.push_str("_visual");
21	link = link.add_visual(visual.named(visual_name));
22
23	link
24}
25
26/// TODO: Finalize, this is temp
27/// TODO: ADD NAMED CHOICE for Vis & Col
28/// TODO: Add material Specifierer
29/// TODO: Add Inertial data options
30/// TODO: ADD TEST?
31/// TODO: Deprecate?
32fn new_quick_link_old(
33	link_name: impl Into<String>,
34	geometry: Box<dyn GeometryInterface + Sync + Send>,
35) -> LinkBuilder {
36	let link_name = link_name.into();
37	let mut link = LinkBuilder::new(link_name.clone());
38
39	let mut visual_name = link_name.clone();
40	visual_name.push_str("_visual");
41	link = link.add_visual(VisualBuilder::new_full(
42		visual_name.into(),
43		None, // TODO: NOT HOW I WANT IT
44		geometry.boxed_clone(),
45		None, // TODO: TEMP
46	));
47
48	let mut collision_name = link_name;
49	collision_name.push_str("_collision");
50	link = link.add_collider(CollisionBuilder::new_full(
51		collision_name.into(),
52		None,
53		geometry,
54	));
55
56	link
57}
58
59/// TODO: Finalize, this is temp
60/// TODO: ADD NAMED CHOICE for Vis & Col
61/// TODO: Add material Specifierer
62/// TODO: Add Inertial data options
63/// TODO: ADD TEST?
64pub fn new_box_link(
65	link_name: impl Into<String>,
66	side1: f32,
67	side2: f32,
68	side3: f32,
69) -> LinkBuilder {
70	let geometry = BoxGeometry::new(side1, side2, side3);
71
72	new_quick_link_old(link_name, geometry.into())
73}
74
75/// TODO: Finalize, this is temp
76/// TODO: ADD NAMED CHOICE for Vis & Col
77/// TODO: Add material Specifierer
78/// TODO: Add Inertial data options
79/// TODO: ADD TEST?
80/// TODO: Orientation??
81pub fn new_cylinder_link(link_name: impl Into<String>, radius: f32, length: f32) -> LinkBuilder {
82	let geometry = CylinderGeometry::new(radius, length);
83
84	new_quick_link_old(link_name, geometry.into())
85}
86
87/// TODO: Finalize, this is temp
88/// TODO: ADD NAMED CHOICE for Vis & Col
89/// TODO: Add material Specifierer
90/// TODO: Add Inertial data options
91/// TODO: ADD TEST?
92pub fn new_sphere_link(link_name: impl Into<String>, radius: f32) -> LinkBuilder {
93	let geometry = SphereGeometry::new(radius);
94
95	new_quick_link_old(link_name, geometry.into())
96}
97
98#[cfg(test)]
99mod tests {
100	use crate::{link::helper_functions::*, KinematicInterface};
101
102	#[test]
103	fn test_new_box_link() {
104		let tree = new_box_link("Zelda", 2f32, 3f32, 5f32).build_tree();
105
106		assert_eq!(tree.get_links().try_read().unwrap().len(), 1);
107		assert_eq!(tree.get_newest_link().try_read().unwrap().name(), "Zelda");
108		assert_eq!(tree.get_newest_link().try_read().unwrap().visuals.len(), 1);
109		assert_eq!(
110			tree.get_newest_link().try_read().unwrap().visuals[0].name,
111			Some("Zelda_visual".into())
112		);
113		assert_eq!(
114			tree.get_newest_link().try_read().unwrap().visuals[0]
115				.geometry
116				.volume(),
117			30f32
118		);
119		assert_eq!(
120			tree.get_newest_link().try_read().unwrap().visuals[0]
121				.geometry
122				.surface_area(),
123			62f32
124		);
125
126		assert_eq!(
127			tree.get_newest_link().try_read().unwrap().colliders.len(),
128			1
129		);
130		assert_eq!(
131			tree.get_newest_link().try_read().unwrap().colliders[0].name,
132			Some("Zelda_collision".into())
133		);
134		assert_eq!(
135			tree.get_newest_link().try_read().unwrap().colliders[0]
136				.geometry
137				.volume(),
138			30f32
139		);
140		assert_eq!(
141			tree.get_newest_link().try_read().unwrap().colliders[0]
142				.geometry
143				.surface_area(),
144			62f32
145		);
146		// TODO: UPDATE WHEN FUNCTION IS FINALIZED
147		// TODO: TEST INERTIAL DATA
148	}
149}
robot_description_builder/link/helper_functions.rs

robot_description_builder/link/
helper_functions.rs
