mecha10_sim/sim/
robot_generator.rs

1//! Robot scene (.tscn) generator
2//!
3//! Generates standalone robot .tscn files from mecha10.json configuration.
4//! The robot scene includes:
5//! - CharacterBody3D root (physics body)
6//! - CollisionShape3D (capsule collision)
7//! - MeshInstance3D (visual representation)
8//! - Sensor nodes (Camera3D, RayCast3D for LiDAR, etc.)
9
10use anyhow::{Context, Result};
11use std::fs;
12use std::path::Path;
13
14use super::profile::RobotProfile;
15
16pub struct RobotGenerator {
17    profile: RobotProfile,
18    rl_mode: bool,
19}
20
21impl RobotGenerator {
22    /// Create new robot generator from config file
23    pub fn from_config_file(config_path: &Path) -> Result<Self> {
24        let profile = RobotProfile::from_config_file(config_path)?;
25        Ok(Self { profile, rl_mode: true })
26    }
27
28    /// Create robot generator with explicit RL mode setting
29    pub fn from_config_file_with_mode(config_path: &Path, rl_mode: bool) -> Result<Self> {
30        let profile = RobotProfile::from_config_file(config_path)?;
31        Ok(Self { profile, rl_mode })
32    }
33
34    /// Generate robot .tscn file
35    pub fn generate(&self, output_path: &Path) -> Result<()> {
36        println!("🤖 Generating robot scene...");
37        println!("   Platform: {}", self.profile.platform);
38        println!("   Sensors: {}", self.profile.sensors.join(", "));
39
40        if let Some(parent) = output_path.parent() {
41            fs::create_dir_all(parent)
42                .with_context(|| format!("Failed to create output directory: {}", parent.display()))?;
43        }
44
45        let tscn_content = self.generate_tscn();
46
47        fs::write(output_path, tscn_content)
48            .with_context(|| format!("Failed to write robot scene: {}", output_path.display()))?;
49
50        println!("   ✅ Generated: {}", output_path.display());
51
52        Ok(())
53    }
54
55    /// Generate .tscn file content
56    fn generate_tscn(&self) -> String {
57        let mut lines = Vec::new();
58
59        let is_sentry = self.profile.platform == "sentry";
60        let load_steps = if self.rl_mode { 5 } else { 3 } + if is_sentry { 3 } else { 0 };
61        lines.push(format!("[gd_scene load_steps={} format=3]", load_steps));
62        lines.push("".to_string());
63
64        if self.rl_mode {
65            lines.push(
66                "[ext_resource type=\"Script\" path=\"res://components/robot_templates/rover_base_rl.gd\" id=\"1\"]"
67                    .to_string(),
68            );
69            lines.push(
70                "[ext_resource type=\"Script\" path=\"res://components/controllers/rover_ai_controller.gd\" id=\"2\"]"
71                    .to_string(),
72            );
73            lines.push("".to_string());
74        }
75
76        lines.push("[sub_resource type=\"CapsuleShape3D\" id=\"CapsuleShape3D_1\"]".to_string());
77        lines.push(format!("radius = {}", self.get_robot_radius()));
78        lines.push(format!("height = {}", self.get_robot_height()));
79        lines.push("".to_string());
80
81        lines.push("[sub_resource type=\"CapsuleMesh\" id=\"CapsuleMesh_1\"]".to_string());
82        lines.push(format!("radius = {}", self.get_robot_radius()));
83        lines.push(format!("height = {}", self.get_robot_height()));
84        lines.push("".to_string());
85
86        if is_sentry {
87            lines.push("[sub_resource type=\"BoxMesh\" id=\"PanServoMesh\"]".to_string());
88            lines.push("size = Vector3(0.08, 0.06, 0.06)".to_string());
89            lines.push("".to_string());
90
91            lines.push("[sub_resource type=\"BoxMesh\" id=\"TiltServoMesh\"]".to_string());
92            lines.push("size = Vector3(0.06, 0.06, 0.08)".to_string());
93            lines.push("".to_string());
94
95            lines.push("[sub_resource type=\"BoxMesh\" id=\"CameraHousingMesh\"]".to_string());
96            lines.push("size = Vector3(0.05, 0.05, 0.05)".to_string());
97            lines.push("".to_string());
98        }
99
100        lines.push("[node name=\"Robot\" type=\"CharacterBody3D\"]".to_string());
101        if self.rl_mode {
102            lines.push("script = ExtResource(\"1\")".to_string());
103        }
104        lines.push("".to_string());
105
106        lines.push("[node name=\"CollisionShape3D\" type=\"CollisionShape3D\" parent=\".\"]".to_string());
107        lines.push("shape = SubResource(\"CapsuleShape3D_1\")".to_string());
108        lines.push("".to_string());
109
110        lines.push("[node name=\"MeshInstance3D\" type=\"MeshInstance3D\" parent=\".\"]".to_string());
111        lines.push("mesh = SubResource(\"CapsuleMesh_1\")".to_string());
112        lines.push("".to_string());
113
114        if self.rl_mode {
115            lines.push("[node name=\"AIController3D\" type=\"Node3D\" parent=\".\"]".to_string());
116            lines.push("script = ExtResource(\"2\")".to_string());
117            lines.push("".to_string());
118        }
119
120        self.add_sensor_nodes(&mut lines);
121
122        lines.push("[node name=\"Markers\" type=\"Node3D\" parent=\".\"]".to_string());
123        lines.push("".to_string());
124
125        lines.push("[node name=\"CenterOfMass\" type=\"Marker3D\" parent=\"Markers\"]".to_string());
126        lines.push("".to_string());
127
128        lines.join("\n")
129    }
130
131    /// Add sensor nodes based on robot profile
132    fn add_sensor_nodes(&self, lines: &mut Vec<String>) {
133        let parent = ".";
134
135        let is_sentry = self.profile.platform == "sentry";
136
137        if is_sentry {
138            // Sentry has no flat, fixed-angle camera mount - it mounts its camera on an
139            // articulated pan/tilt joint chain instead. See `add_pan_tilt_rig`.
140            self.add_pan_tilt_rig(lines);
141        } else if self.profile.sensors.contains(&"camera".to_string()) {
142            if self.rl_mode {
143                lines.push(format!(
144                    "[node name=\"CameraRL\" type=\"Node3D\" parent=\"{}\"]",
145                    parent
146                ));
147                lines.push("script = ExtResource(\"res://components/sensors/camera_rl/camera_rl.gd\")".to_string());
148                lines.push(format!(
149                    "transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, {}, {})",
150                    self.get_robot_height() / 2.0 - 0.1,
151                    0.3
152                ));
153                let camera_dims = mecha10_core::config::camera_dimensions();
154                lines.push(format!(
155                    "resolution = Vector2i({}, {})",
156                    camera_dims.width, camera_dims.height
157                ));
158                lines.push("fov_degrees = 90.0".to_string());
159                lines.push("observation_enabled = true".to_string());
160                lines.push("".to_string());
161            } else {
162                lines.push(format!(
163                    "[node name=\"Camera\" type=\"Camera3D\" parent=\"{}\"]",
164                    parent
165                ));
166                lines.push(format!(
167                    "transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, {}, {})",
168                    self.get_robot_height() / 2.0 - 0.1,
169                    0.3
170                ));
171                lines.push("fov = 60.0".to_string());
172                lines.push("".to_string());
173            }
174        }
175
176        if self.profile.sensors.contains(&"lidar".to_string()) {
177            if self.rl_mode {
178                lines.push(format!("[node name=\"LidarRL\" type=\"Node3D\" parent=\"{}\"]", parent));
179                lines.push("script = ExtResource(\"res://components/sensors/lidar_rl/lidar_rl.gd\")".to_string());
180                lines.push(format!(
181                    "transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, {}, 0)",
182                    self.get_robot_height() / 2.0
183                ));
184                lines.push("num_rays = 16".to_string());
185                lines.push("ray_length = 10.0".to_string());
186                lines.push("observation_enabled = true".to_string());
187                lines.push("".to_string());
188            } else {
189                lines.push(format!("[node name=\"LiDAR\" type=\"Node3D\" parent=\"{}\"]", parent));
190                lines.push(format!(
191                    "transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, {}, 0)",
192                    self.get_robot_height() / 2.0
193                ));
194                lines.push("".to_string());
195
196                for i in 0..36 {
197                    let angle_deg = i * 10;
198                    let angle_rad = (angle_deg as f32).to_radians();
199                    let x = angle_rad.cos();
200                    let z = angle_rad.sin();
201
202                    lines.push(format!(
203                        "[node name=\"Ray{}\" type=\"RayCast3D\" parent=\"{}/LiDAR\"]",
204                        i, parent
205                    ));
206                    lines.push(format!("target_position = Vector3({}, 0, {})", x * 10.0, z * 10.0));
207                    lines.push("enabled = true".to_string());
208                    lines.push("".to_string());
209                }
210            }
211        }
212
213        if self.profile.sensors.contains(&"imu".to_string()) {
214            lines.push(format!("[node name=\"IMU\" type=\"Marker3D\" parent=\"{}\"]", parent));
215            lines.push("".to_string());
216        }
217    }
218
219    /// Add the sentry platform's pan (yaw) / tilt (pitch) joint chain, with its camera mounted
220    /// at the end of the chain instead of as a flat, fixed-angle child of the robot root.
221    ///
222    /// Sentry is a stationary base (no locomotion) whose camera is carried by two nested
223    /// pivots: `PanPivot` (parented at the robot root, sitting on top of the base) and
224    /// `TiltPivot` (parented at `PanPivot`, so tilt rotates relative to whatever pan has
225    /// already rotated to). Each pivot gets its own `BoxMesh` servo body so the rig is
226    /// actually visible instead of relying on editor-only gizmos for empty `Node3D`s, and
227    /// the tilt servo sits a bit above the pan servo so it has room to rotate without
228    /// clipping into it. The camera gets a housing mesh as a sibling under `TiltPivot` so
229    /// it visibly reads as mounted on the top (tilt) servo rather than floating detached.
230    /// The camera itself is only emitted when the profile actually lists a `"camera"`
231    /// sensor - the pivots and their servo meshes are emitted unconditionally since they
232    /// represent the robot's real joints regardless of what sensors are configured.
233    fn add_pan_tilt_rig(&self, lines: &mut Vec<String>) {
234        lines.push("[node name=\"PanPivot\" type=\"Node3D\" parent=\".\"]".to_string());
235        lines.push(format!(
236            "transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, {}, 0)",
237            self.get_robot_height() / 2.0
238        ));
239        lines.push("".to_string());
240
241        lines.push("[node name=\"PanServoMesh\" type=\"MeshInstance3D\" parent=\"PanPivot\"]".to_string());
242        lines.push("mesh = SubResource(\"PanServoMesh\")".to_string());
243        lines.push("".to_string());
244
245        lines.push("[node name=\"TiltPivot\" type=\"Node3D\" parent=\"PanPivot\"]".to_string());
246        lines.push("".to_string());
247
248        lines.push("[node name=\"TiltServoMesh\" type=\"MeshInstance3D\" parent=\"PanPivot/TiltPivot\"]".to_string());
249        lines.push("mesh = SubResource(\"TiltServoMesh\")".to_string());
250        lines.push("transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0.05, 0)".to_string());
251        lines.push("".to_string());
252
253        lines.push(
254            "[node name=\"CameraHousingMesh\" type=\"MeshInstance3D\" parent=\"PanPivot/TiltPivot\"]".to_string(),
255        );
256        lines.push("mesh = SubResource(\"CameraHousingMesh\")".to_string());
257        lines.push("transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0.08, 0.05, 0)".to_string());
258        lines.push("".to_string());
259
260        if self.profile.sensors.contains(&"camera".to_string()) {
261            if self.rl_mode {
262                lines.push("[node name=\"CameraRL\" type=\"Node3D\" parent=\"PanPivot/TiltPivot\"]".to_string());
263                lines.push("script = ExtResource(\"res://components/sensors/camera_rl/camera_rl.gd\")".to_string());
264                lines.push("transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0.08, 0.05, 0)".to_string());
265                let camera_dims = mecha10_core::config::camera_dimensions();
266                lines.push(format!(
267                    "resolution = Vector2i({}, {})",
268                    camera_dims.width, camera_dims.height
269                ));
270                lines.push("fov_degrees = 90.0".to_string());
271                lines.push("observation_enabled = true".to_string());
272                lines.push("".to_string());
273            } else {
274                lines.push("[node name=\"Camera\" type=\"Camera3D\" parent=\"PanPivot/TiltPivot\"]".to_string());
275                lines.push("transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0.08, 0.05, 0)".to_string());
276                lines.push("fov = 60.0".to_string());
277                lines.push("".to_string());
278            }
279        }
280    }
281
282    /// Per-platform (radius, height) dimensions, in meters. Shared table behind
283    /// [`Self::get_robot_radius`]/[`Self::get_robot_height`], which only differ in which
284    /// element of the pair they return.
285    fn platform_dimensions(platform: &str) -> (f32, f32) {
286        match platform {
287            "differential-drive" => (0.3, 1.0),
288            "holonomic" => (0.35, 1.2),
289            "legged" => (0.25, 1.5),
290            "arm" => (0.15, 2.0),
291            "sentry" => (0.1, 0.4),
292            _ => (0.3, 1.0),
293        }
294    }
295
296    /// Get robot radius based on platform
297    fn get_robot_radius(&self) -> f32 {
298        Self::platform_dimensions(self.profile.platform.as_str()).0
299    }
300
301    /// Get robot height based on platform
302    fn get_robot_height(&self) -> f32 {
303        Self::platform_dimensions(self.profile.platform.as_str()).1
304    }
305}
306
307#[cfg(test)]
308mod tests {
309    use super::*;
310
311    fn make_profile(platform: &str, sensors: &[&str]) -> RobotProfile {
312        RobotProfile {
313            platform: platform.to_string(),
314            sensors: sensors.iter().map(|s| s.to_string()).collect(),
315            sensor_configs: Vec::new(),
316            task_nodes: Vec::new(),
317            capabilities: Vec::new(),
318            difficulty_preference: "medium".to_string(),
319        }
320    }
321
322    #[test]
323    fn sentry_platform_dimensions_are_compact_not_differential_drive_fallback() {
324        let sentry_dims = RobotGenerator::platform_dimensions("sentry");
325        let fallback_dims = RobotGenerator::platform_dimensions("differential-drive");
326
327        assert_eq!(sentry_dims, (0.1, 0.4));
328        assert_ne!(sentry_dims, fallback_dims);
329    }
330
331    #[test]
332    fn sentry_with_camera_emits_pan_tilt_rig_and_camera() {
333        let profile = make_profile("sentry", &["camera"]);
334        let generator = RobotGenerator {
335            profile,
336            rl_mode: false,
337        };
338
339        let tscn = generator.generate_tscn();
340
341        assert!(tscn.contains("PanPivot"));
342        assert!(tscn.contains("TiltPivot"));
343        assert!(tscn.contains("Camera"));
344        assert!(tscn.contains("parent=\"PanPivot/TiltPivot\""));
345    }
346
347    #[test]
348    fn non_sentry_platform_never_emits_pan_tilt_rig() {
349        let profile = make_profile("differential-drive", &["camera"]);
350        let generator = RobotGenerator {
351            profile,
352            rl_mode: false,
353        };
354
355        let tscn = generator.generate_tscn();
356
357        assert!(!tscn.contains("PanPivot"));
358        assert!(!tscn.contains("TiltPivot"));
359    }
360
361    #[test]
362    fn sentry_without_camera_still_emits_rig_but_no_camera_node() {
363        let profile = make_profile("sentry", &["lidar"]);
364        let generator = RobotGenerator {
365            profile,
366            rl_mode: false,
367        };
368
369        let tscn = generator.generate_tscn();
370
371        assert!(tscn.contains("PanPivot"));
372        assert!(tscn.contains("TiltPivot"));
373        // The pan/tilt rig's `CameraHousingMesh` is emitted unconditionally (it's cosmetic
374        // mount hardware, not a sensor), so it legitimately contains the substring "Camera".
375        // What must be absent is the actual `Camera3D` sensor node, since no "camera" sensor
376        // was configured on this profile.
377        assert!(!tscn.contains("type=\"Camera3D\""));
378    }
379}
mecha10_sim/sim/robot_generator.rs

mecha10_sim/sim/
robot_generator.rs
