refactor(world): configuration using xacro with procedural features

This commit is contained in:
Ilya Uraev 2025-03-21 18:37:34 +03:00
parent 51f47ed776
commit 31535e4b10
10 changed files with 332 additions and 225 deletions

View file

@ -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>

View file

@ -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>

View file

@ -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>

View file

@ -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>

View file

@ -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>

View file

@ -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>

View file

@ -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>

View file

@ -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>

View file

@ -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>

View file

@ -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>