From 31535e4b1029d6c1d5f6404bcbf23ac859779e4b Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Fri, 21 Mar 2025 18:37:34 +0300
Subject: [PATCH] refactor(world): configuration using xacro with procedural
 features

---
 rbs_mill_assist/world/default.sdf             | 225 ------------------
 rbs_mill_assist/world/inc/camera.xacro        |  58 +++++
 rbs_mill_assist/world/inc/from_places.xacro   |  30 +++
 .../world/inc/generate_shildiks.xacro         |  44 ++++
 rbs_mill_assist/world/inc/ground.xacro        |  64 +++++
 rbs_mill_assist/world/inc/light_macro.xacro   |  18 ++
 rbs_mill_assist/world/inc/model_macro.xacro   |  16 ++
 rbs_mill_assist/world/inc/plugins_macro.xacro |  28 +++
 rbs_mill_assist/world/world.xacro             |   6 +
 rbs_mill_assist/world/world_macro.xacro       |  68 ++++++
 10 files changed, 332 insertions(+), 225 deletions(-)
 delete mode 100644 rbs_mill_assist/world/default.sdf
 create mode 100644 rbs_mill_assist/world/inc/camera.xacro
 create mode 100644 rbs_mill_assist/world/inc/from_places.xacro
 create mode 100644 rbs_mill_assist/world/inc/generate_shildiks.xacro
 create mode 100644 rbs_mill_assist/world/inc/ground.xacro
 create mode 100644 rbs_mill_assist/world/inc/light_macro.xacro
 create mode 100644 rbs_mill_assist/world/inc/model_macro.xacro
 create mode 100644 rbs_mill_assist/world/inc/plugins_macro.xacro
 create mode 100644 rbs_mill_assist/world/world.xacro
 create mode 100644 rbs_mill_assist/world/world_macro.xacro

diff --git a/rbs_mill_assist/world/default.sdf b/rbs_mill_assist/world/default.sdf
deleted file mode 100644
index 4914e0c..0000000
--- a/rbs_mill_assist/world/default.sdf
+++ /dev/null
@@ -1,225 +0,0 @@
-<?xml version="1.0"?>
-<sdf version="1.9">
-  <world name="rbs_freazer_world">
-    <scene>
-      <ambient>1.0 1.0 1.0</ambient>
-      <grid>false</grid>
-    </scene>
-
-    <physics name='1ms' type='ignored'>
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1.0</real_time_factor>
-      <dart>
-        <!-- <collision_detector>bullet</collision_detector> -->
-        <!-- <solver> -->
-        <!--   <solver_type>pgs</solver_type> -->
-        <!-- </solver> -->
-      </dart>
-    </physics>
-    <plugin
-      filename="gz-sim-physics-system"
-      name="gz::sim::systems::Physics">
-      <engine>
-        <filename>gz-physics-dartsim-plugin</filename>
-      </engine>
-    </plugin>
-    <plugin
-      filename="gz-sim-sensors-system"
-      name="gz::sim::systems::Sensors">
-      <render_engine>ogre2</render_engine>
-    </plugin>
-    <plugin
-      filename="gz-sim-user-commands-system"
-      name="gz::sim::systems::UserCommands">
-    </plugin>
-    <plugin
-      filename="gz-sim-scene-broadcaster-system"
-      name="gz::sim::systems::SceneBroadcaster">
-    </plugin>
-    <plugin filename="gz-sim-contact-system"
-      name="gz::sim::systems::Contact">
-    </plugin>
-
-    <light type="directional" name="sun">
-      <cast_shadows>false</cast_shadows>
-      <pose>0 0 10 0 0 0</pose>
-      <diffuse>0.8 0.8 0.8 1</diffuse>
-      <specular>0.2 0.2 0.2 1</specular>
-      <attenuation>
-        <range>1000</range>
-        <constant>0.9</constant>
-        <linear>0.01</linear>
-        <quadratic>0.001</quadratic>
-      </attenuation>
-      <direction>-0.5 0.1 -0.9</direction>
-    </light>
-
-    <include>
-      <name>shildik_0</name>
-      <pose>-0.403959 0.250 0.02 0 0 1.57</pose>
-      <uri>model://shildik</uri>
-    </include>
-    <include>
-      <name>shildik_1</name>
-      <pose>-0.447918 0.250 0.02 0 0 1.57</pose>
-      <uri>model://shildik</uri>
-    </include>
-    <include>
-      <name>shildik_2</name>
-      <pose>-0.491997 0.250 0.02 0 0 1.57</pose>
-      <uri>model://shildik</uri>
-    </include>
-    <include>
-      <name>shildik_3</name>
-      <pose>-0.536033 0.250 0.02 0 0 1.57</pose>
-      <uri>model://shildik</uri>
-    </include>
-    <include>
-      <name>shildik_4</name>
-      <pose>-0.491997 0.250 0.03 0 0 1.57</pose>
-      <uri>model://shildik</uri>
-    </include>
-
-    <include>
-      <static>true</static>
-      <pose>0.350 -0.170 0 0 0 3.14159</pose>
-      <uri>model://laser</uri>
-    </include>
-
-    <include>
-      <static>true</static>
-      <pose>-0.050 0.250 0.0 0.0 0.0 1.57</pose>
-      <uri>model://bunker</uri>
-    </include>
-
-    <include>
-      <static>true</static>
-      <pose>-0.470 0.250 0 0 0 1.57</pose>
-      <uri>model://bunker_4_slots</uri>
-    </include>
-
-    <include>
-      <static>true</static>
-      <pose>0.350 -0.170 0.01 0 0 1.57</pose>
-      <uri>model://conductor</uri>
-    </include>
-
-    <model name="rgbd_camera">
-      <pose>0 -0.93 0.42 0 0.4 1.81</pose>
-      <static>1</static>
-      <link name="rgbd_camera">
-
-        <!-- Visual -->
-        <visual name="visual">
-          <pose>0 0 0 0 0 0</pose>
-          <geometry>
-            <box>
-              <size>0.01 0.01 0.01</size>
-            </box>
-          </geometry>
-          <material>
-            <ambient>0 0 0 1</ambient>
-            <diffuse>0 0 0 1</diffuse>
-            <specular>0 0 0 1</specular>
-          </material>
-        </visual>
-
-        <!-- Collision -->
-        <collision name="collision">
-          <pose>0 0 0 0 0 0</pose>
-          <geometry>
-            <box>
-              <size>0.01 0.01 0.01</size>
-            </box>
-          </geometry>
-        </collision>
-
-        <!-- RGBD Sensor -->
-        <sensor name="rgbd_camera" type="rgbd_camera">
-          <camera>
-            <horizontal_fov>1.047</horizontal_fov>
-            <image>
-              <width>1920</width>
-              <height>1080</height>
-              <format>R8G8B8</format>
-            </image>
-            <clip>
-              <near>0.1</near>
-              <far>3</far>
-            </clip>
-            <depth_camera>
-              <output>depth</output>
-            </depth_camera>
-          </camera>
-          <always_on>1</always_on>
-          <update_rate>30</update_rate>
-          <visualize>true</visualize>
-          <topic>/rgbd_camera/image</topic>
-          <enable_metrics>true</enable_metrics>
-        </sensor>
-      </link>
-    </model>
-
-
-    <model name="table">
-      <static>1</static>
-      <link name='table_link'>
-        <pose>0 0 -0.40000000000000002 0 0 0</pose>
-        <inertial>
-          <pose>0 0 0 0 0 0</pose>
-          <mass>10</mass>
-          <inertia>
-            <ixx>4.708333333333333</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>8.6666666666666661</iyy>
-            <iyz>0</iyz>
-            <izz>8.0416666666666661</izz>
-          </inertia>
-        </inertial>
-        <collision name='table_link_collision'>
-          <pose>0 0 0 0 0 0</pose>
-          <geometry>
-            <box>
-              <size>1.2 0.69999999999999996 0.80000000000000004</size>
-            </box>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1.0</mu>
-                <mu2>1.0</mu2>
-              </ode>
-            </friction>
-            <bounce />
-            <contact>
-              <ode>
-                <soft_cfm>0.00001</soft_cfm>
-                <soft_erp>0.8</soft_erp>
-                <kp>1e5</kp>
-                <kd>1e3</kd>
-              </ode>
-            </contact>
-          </surface>
-        </collision>
-        <visual name='table_link_visual'>
-          <pose>0 0 0 0 0 0</pose>
-          <geometry>
-            <box>
-              <size>1.2 0.69999999999999996 0.80000000000000004</size>
-            </box>
-          </geometry>
-        <material>
-          <diffuse>0.6 0.4 0.2 1</diffuse>
-          <ambient>0.6 0.4 0.2 1</ambient>
-          <specular>0.1 0.1 0.1 1</specular>
-          <emissive>0 0 0 1</emissive>
-        </material>
-        </visual>
-        <enable_wind>false</enable_wind>
-      </link>
-    </model>
-
-
-  </world>
-</sdf>
diff --git a/rbs_mill_assist/world/inc/camera.xacro b/rbs_mill_assist/world/inc/camera.xacro
new file mode 100644
index 0000000..74d6747
--- /dev/null
+++ b/rbs_mill_assist/world/inc/camera.xacro
@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<sdf xmlns:xacro="http://www.ros.org/wiki/xacro" version="1.7">
+  <xacro:macro name="rgbd_camera" params="name:=rgbd_camera *pose width:=1920 height:=1080">
+    <model name="${name}">
+      <xacro:insert_block name="pose" />
+      <static>1</static>
+      <link name="${name}_link">
+
+        <visual name="visual">
+          <pose>0 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.01 0.01 0.01</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0 0 0 1</ambient>
+            <diffuse>0 0 0 1</diffuse>
+            <specular>0 0 0 1</specular>
+          </material>
+        </visual>
+
+        <collision name="collision">
+          <pose>0 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.01 0.01 0.01</size>
+            </box>
+          </geometry>
+        </collision>
+
+        <sensor name="${name}_sensor" type="rgbd_camera">
+          <camera>
+            <horizontal_fov>1.047</horizontal_fov>
+            <image>
+              <width>1920</width>
+              <height>1080</height>
+              <format>R8G8B8</format>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>3</far>
+            </clip>
+            <depth_camera>
+              <output>depth</output>
+            </depth_camera>
+          </camera>
+          <always_on>1</always_on>
+          <update_rate>30</update_rate>
+          <visualize>true</visualize>
+          <topic>/${name}/image</topic>
+          <enable_metrics>true</enable_metrics>
+        </sensor>
+      </link>
+    </model>
+
+  </xacro:macro>
+</sdf>
diff --git a/rbs_mill_assist/world/inc/from_places.xacro b/rbs_mill_assist/world/inc/from_places.xacro
new file mode 100644
index 0000000..323a60f
--- /dev/null
+++ b/rbs_mill_assist/world/inc/from_places.xacro
@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<sdf xmlns:xacro="http://www.ros.org/wiki/xacro" version="1.7">
+  <xacro:include filename="$(find rbs_mill_assist)/world/inc/model_macro.xacro" />
+  <xacro:macro name="from_places" params="models:=^">
+    <xacro:if value="${models}">
+      <xacro:property name="model" value="${models.pop(0)}" />
+      <xacro:model model_name="${model['name']}" static="true">
+        <pose>
+          ${model["pose"]["position"]["x"]}
+          ${model["pose"]["position"]["y"]}
+          ${model["pose"]["position"]["z"]}
+          
+          <xacro:if value="${'orientation' in model['pose']}">
+            <xacro:property name="orientation" value="${model['pose']['orientation']}"/>
+            <xacro:if value="${'r' in orientation and 'p' in orientation and 'y' in orientation}">
+              ${orientation['r']}
+              ${orientation['p']}
+              ${orientation['y']}
+            </xacro:if>
+          </xacro:if>
+          
+          <xacro:unless value="${'orientation' in model['pose']}">
+            0 0 0
+          </xacro:unless>
+        </pose>
+      </xacro:model>
+      <xacro:from_places />
+    </xacro:if>
+  </xacro:macro>
+</sdf>
diff --git a/rbs_mill_assist/world/inc/generate_shildiks.xacro b/rbs_mill_assist/world/inc/generate_shildiks.xacro
new file mode 100644
index 0000000..2f271c8
--- /dev/null
+++ b/rbs_mill_assist/world/inc/generate_shildiks.xacro
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<sdf xmlns:xacro="http://www.ros.org/wiki/xacro" version="1.7">
+  <xacro:include filename="$(find rbs_mill_assist)/world/inc/model_macro.xacro" />
+  <xacro:macro name="shildiks_to_bunker" params="count ref_frames index:=0 rpy:='0 0 0' z_offset:=0">
+    <xacro:if value="${count &gt; 0}">
+      <xacro:if value="${index &lt; len(ref_frames)}">
+        <!-- Extract the slot by index -->
+        <xacro:property name="slot" value="${ref_frames[index]}" />
+
+        <xacro:model model_name="shildik" scene_name="shildik${count}">
+          <xacro:if value="${slot['relative_to'] is not None}">
+            <pose relative_to="${slot['relative_to']}">
+              ${slot["pose"]["position"]["x"]} ${slot["pose"]["position"]["y"]} ${slot["pose"]["position"]["z"] + z_offset} ${rpy}
+            </pose>
+          </xacro:if>
+          <xacro:unless value="${slot['relative_to'] is not None}">
+            <pose>${slot["pose"]["position"]["x"]} ${slot["pose"]["position"]["y"]} ${slot["pose"]["position"]["z"] + z_offset} ${rpy}</pose>
+          </xacro:unless>
+        </xacro:model>
+
+        <!-- Recursive call for the next slot -->
+        <xacro:shildiks_to_bunker 
+          count="${count - 1}" 
+          ref_frames="${ref_frames}" 
+          index="${index + 1}" 
+          rpy="${rpy}" 
+          z_offset="${z_offset}"
+        />
+      </xacro:if>
+
+      <!-- If slots are finished, increase z_offset and start over -->
+      <xacro:unless value="${index &lt; len(ref_frames)}">
+        <xacro:property name="new_z_offset" value="${z_offset + 0.003249}" />
+        <xacro:shildiks_to_bunker 
+          count="${count}" 
+          ref_frames="${ref_frames}" 
+          index="0" 
+          rpy="${rpy}" 
+          z_offset="${new_z_offset}"
+        />
+      </xacro:unless>
+    </xacro:if>
+  </xacro:macro>
+</sdf>
diff --git a/rbs_mill_assist/world/inc/ground.xacro b/rbs_mill_assist/world/inc/ground.xacro
new file mode 100644
index 0000000..71fab2a
--- /dev/null
+++ b/rbs_mill_assist/world/inc/ground.xacro
@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<sdf xmlns:xacro="http://www.ros.org/wiki/xacro" version="1.7">
+  <xacro:macro name="table" params="">
+    <model name="table">
+      <static>1</static>
+      <link name='table_link'>
+        <pose>0 0 -0.40000000000000002 0 0 0</pose>
+        <inertial>
+          <pose>0 0 0 0 0 0</pose>
+          <mass>10</mass>
+          <inertia>
+            <ixx>4.708333333333333</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>8.6666666666666661</iyy>
+            <iyz>0</iyz>
+            <izz>8.0416666666666661</izz>
+          </inertia>
+        </inertial>
+        <collision name='table_link_collision'>
+          <density>1000</density>
+          <pose>0 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.2 0.69999999999999996 0.80000000000000004</size>
+            </box>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>1.0</mu>
+                <mu2>1.0</mu2>
+              </ode>
+            </friction>
+            <bounce />
+            <contact>
+              <ode>
+                <soft_cfm>0.00001</soft_cfm>
+                <soft_erp>0.8</soft_erp>
+                <kp>1e5</kp>
+                <kd>1e3</kd>
+              </ode>
+            </contact>
+          </surface>
+        </collision>
+        <visual name='table_link_visual'>
+          <pose>0 0 0 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>1.2 0.69999999999999996 0.80000000000000004</size>
+            </box>
+          </geometry>
+          <material>
+            <diffuse>0.6 0.4 0.2 1</diffuse>
+            <ambient>0.6 0.4 0.2 1</ambient>
+            <specular>0.1 0.1 0.1 1</specular>
+            <emissive>0 0 0 1</emissive>
+          </material>
+        </visual>
+        <enable_wind>false</enable_wind>
+      </link>
+    </model>
+  </xacro:macro>
+</sdf>
diff --git a/rbs_mill_assist/world/inc/light_macro.xacro b/rbs_mill_assist/world/inc/light_macro.xacro
new file mode 100644
index 0000000..64ec7b4
--- /dev/null
+++ b/rbs_mill_assist/world/inc/light_macro.xacro
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<sdf xmlns:xacro="http://www.ros.org/wiki/xacro" version="1.7">
+  <xacro:macro name="light">
+    <light type="directional" name="sun">
+      <cast_shadows>false</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+  </xacro:macro>
+</sdf>
diff --git a/rbs_mill_assist/world/inc/model_macro.xacro b/rbs_mill_assist/world/inc/model_macro.xacro
new file mode 100644
index 0000000..82e64c4
--- /dev/null
+++ b/rbs_mill_assist/world/inc/model_macro.xacro
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<sdf xmlns:xacro="http://www.ros.org/wiki/xacro" version="1.7">
+  <xacro:macro name="model" params="scene_name:='' model_name *pose static:=false">
+    <xacro:if value="${scene_name==''}">
+      <xacro:property name="scene_name" value="${model_name}"/>
+    </xacro:if>
+    <include>
+      <xacro:if value="${scene_name!=''}">
+        <name>${scene_name}</name>
+      </xacro:if>
+      <static>${static}</static>
+      <xacro:insert_block name="pose"/>
+      <uri>model://${model_name}</uri>
+    </include>
+  </xacro:macro>
+</sdf>
diff --git a/rbs_mill_assist/world/inc/plugins_macro.xacro b/rbs_mill_assist/world/inc/plugins_macro.xacro
new file mode 100644
index 0000000..54c228c
--- /dev/null
+++ b/rbs_mill_assist/world/inc/plugins_macro.xacro
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<sdf xmlns:xacro="http://www.ros.org/wiki/xacro" version="1.7">
+  <xacro:macro name="plugins" params="render_engine physics_engine">
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+      <engine>
+        <filename>${physics_engine}</filename>
+      </engine>
+    </plugin>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>${render_engine}</render_engine>
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+    <plugin filename="gz-sim-contact-system"
+      name="gz::sim::systems::Contact">
+    </plugin>
+  </xacro:macro>
+</sdf>
diff --git a/rbs_mill_assist/world/world.xacro b/rbs_mill_assist/world/world.xacro
new file mode 100644
index 0000000..9986d4b
--- /dev/null
+++ b/rbs_mill_assist/world/world.xacro
@@ -0,0 +1,6 @@
+<?xml version="1.0"?>
+<sdf xmlns:xacro="http://www.ros.org/wiki/xacro" version="1.7">
+  <xacro:include filename="$(find rbs_mill_assist)/world/world_macro.xacro"/>
+
+  <xacro:world/>
+</sdf>
diff --git a/rbs_mill_assist/world/world_macro.xacro b/rbs_mill_assist/world/world_macro.xacro
new file mode 100644
index 0000000..d7647d9
--- /dev/null
+++ b/rbs_mill_assist/world/world_macro.xacro
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<sdf xmlns:xacro="http://www.ros.org/wiki/xacro" version="1.7">
+
+  <xacro:include filename="$(find rbs_mill_assist)/world/inc/plugins_macro.xacro" />
+  <xacro:include filename="$(find rbs_mill_assist)/world/inc/light_macro.xacro" />
+  <xacro:include filename="$(find rbs_mill_assist)/world/inc/model_macro.xacro" />
+  <xacro:include filename="$(find rbs_mill_assist)/world/inc/generate_shildiks.xacro" />
+  <xacro:include filename="$(find rbs_mill_assist)/world/inc/camera.xacro" />
+  <xacro:include filename="$(find rbs_mill_assist)/world/inc/ground.xacro" />
+  <xacro:include filename="$(find rbs_mill_assist)/world/inc/from_places.xacro" />
+
+  <xacro:macro name="world" params="">
+    <world name="rbs_freazer_world">
+      <scene>
+        <ambient>1.0 1.0 1.0</ambient>
+        <grid>false</grid>
+      </scene>
+      <physics name='1ms' type='ignored'>
+        <max_step_size>0.001</max_step_size>
+        <real_time_factor>1.0</real_time_factor>
+      </physics>
+
+      <xacro:plugins render_engine="ogre2" physics_engine="gz-physics-dartsim-plugin" />
+      <xacro:light />
+
+
+      <xacro:property name="yaml_file" value="$(find rbs_mill_assist)/world/config/slots.yaml" />
+      <xacro:property name="slots" value="${xacro.load_yaml(yaml_file)}" />
+
+      <xacro:shildiks_to_bunker
+        count="5"
+        ref_frames="${slots['ref_frames']}"
+        rpy="0 0 0"
+        z_offset="0.01"
+      />
+
+      <xacro:property name="places_file" value="$(find rbs_mill_assist)/world/config/places.yaml" />
+      <xacro:property name="places" value="${xacro.load_yaml(places_file)}" />
+
+      <xacro:from_places models="${places['models']}"/>
+
+      
+      <!---->
+      <!-- <xacro:model model_name="laser" static="true"> -->
+      <!--   <pose>0.350 -0.170 0 0 0 3.14159</pose> -->
+      <!-- </xacro:model> -->
+      <!---->
+      <!-- <xacro:model model_name="bunker" static="true"> -->
+      <!--   <pose>-0.310 0.250 0.0 0.0 0.0 1.57</pose> -->
+      <!-- </xacro:model> -->
+      <!---->
+      <!-- <xacro:model model_name="bunker_4_slots" static="true"> -->
+      <!--   <pose>-0.150 0.250 0 0 0 1.57</pose> -->
+      <!-- </xacro:model> -->
+      <!---->
+      <!-- <xacro:model model_name="conductor" static="true"> -->
+      <!--   <pose>0.350 -0.170 0.01 0 0 1.57</pose> -->
+      <!-- </xacro:model> -->
+
+      <xacro:rgbd_camera>
+        <pose>0 -0.93 0.42 0 0.4 1.81</pose>
+      </xacro:rgbd_camera>
+
+      <xacro:table />
+
+    </world>
+  </xacro:macro>
+</sdf>