gear.world.template

<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="ariac">
    <!-- Set the initial camera pose to be looking at the workspace. -->
    <gui>
      <camera name="user_camera">
        <pose>-4.33 1.88 6.11 0 0.9 -0.27</pose>
      </camera>
    </gui>

@[if options['unthrottled_physics_update']]@
    <physics type="ode">
      <real_time_update_rate>0</real_time_update_rate>
    </physics>
@[end if]@

@[if not options['disable_shadows']]@
    <!-- Disable shadows. -->
    <scene>
      <shadows>false</shadows>
    </scene>
@[end if]@

    <!-- A global light source -->
    <include>
      <uri>model://sun</uri>
    </include>

    <!-- A directed light source -->
    <light name="camera_spot_light" type='spot'>
      <pose>-10 10 10 0 -1 -0.8</pose>
      <diffuse>0.5 0.5 0.5 1</diffuse>
      <specular>0.1 0.1 0.1 1</specular>
      <direction>0 0 -1</direction>
      <attenuation>
        <range>20</range>
        <constant>0.5</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <cast_shadows>0</cast_shadows>
      <spot>
        <inner_angle>0.6</inner_angle>
        <outer_angle>1</outer_angle>
        <falloff>1</falloff>
      </spot>
    </light>

    <!-- the workcell -->
    <include>
      <uri>model://workcell</uri>
      <pose>0 0 0 0 0 1.57</pose>
    </include>

@[for bin_name, bin_pose in bins.items()]@
    <include>
      <name>@(bin_name)</name>
      <uri>model://workcell_bin</uri>
      <pose>@(' '.join(bin_pose.xyz)) @(' '.join(bin_pose.rpy))</pose>
    </include>
@[end for]@

    <!-- <model name="kinect">
      <pose>1.2 2.0 1.6 1.574 1.574 3.1416</pose>
      
      <link name="link">
        <gravity>false</gravity>
        <inertial>
          <mass>0.1</mass>
          <inertia>
            <ixx>0.000166667</ixx>
            <iyy>0.000166667</iyy>
            <izz>0.000166667</izz>
          </inertia>
        </inertial>

        <collision name="collision">
          <pose>0 0.02 0 0 0 -1.5708</pose>
          <geometry>
            <mesh>
              <uri>model://logical_camera/meshes/camera.dae</uri>
            </mesh>
          </geometry>
        </collision>

        <visual name="visual">
          <pose>0 0.02 0 0 0 -1.5708</pose>
          <geometry>
            <mesh>
              <uri>model://logical_camera/meshes/camera.dae</uri>
            </mesh>
          </geometry>
        </visual>

        <sensor name="kinect_camera" type="depth">
          <camera>
              <vertival_fov>1.0</vertival_fov>
              <horizontal_fov>1.0</horizontal_fov>  
              <image>
                  <width>800</width>
                  <height>800</height>
                  <format>R8G8B8</format>
              </image>
              <depth_camera>

              </depth_camera>
              <clip>
                  <near>0.1</near>
                  <far>100</far>
              </clip>
          </camera>
          <plugin name="camera_controller" filename="libgazebo_ros_openni_kinect.so">
             <alwaysOn>true</alwaysOn>
                <updateRate>10.0</updateRate>
                <cameraName>camera</cameraName>
                <frameName>laser_scanner_1_frame</frameName>                   
            <imageTopicName>rgb/image_raw</imageTopicName>
            <depthImageTopicName>depth/image_raw</depthImageTopicName>
            <pointCloudTopicName>depth/points</pointCloudTopicName>
            <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>              
            <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>            
            <pointCloudCutoff>0.4</pointCloudCutoff>                
                <hackBaseline>0.0</hackBaseline>
                <distortionK1>0.0</distortionK1>
                <distortionK2>0.0</distortionK2>
                <distortionK3>0.0</distortionK3>
                <distortionT1>0.0</distortionT1>
                <distortionT2>0.0</distortionT2>
            <CxPrime>0.0</CxPrime>
            <Cx>0.0</Cx>
            <Cy>0.0</Cy>
            <focalLength>0.0</focalLength>
          </plugin>
          <visualize>true</visualize>
          <always_on>true</always_on>
        </sensor>
      </link>
      <static>true</static>
    </model> -->
@[for name, sensor in sensors.items()]@
@[if sensor.type == "break_beam"]@
    <!-- a break beam sensor called @(name) -->
    <model name="@(name)">
      <pose>@(' '.join(sensor.pose.xyz)) @(' '.join(sensor.pose.rpy))</pose>
      <link name="link">
        <sensor name="@(name)" type="ray">
          <pose>0.025 0 0 0 0 0</pose>
          <plugin name="proximity_ray_plugin" filename="libROSProximityRayPlugin.so" >
            <robotNamespace>ariac</robotNamespace>
            <time_delay>0.05</time_delay><!-- this modifies the update rate of the ray sensor -->
          </plugin>
          <ray>
            <range>
              <min>0.0</min>
              <max>1.0</max> <!-- this represents the distance at which the reflector's placed. max 12m -->
              <resolution>0.001</resolution>
            </range>

            <!-- nothing below this line really needs to be modified upon change of sensor model -->

            <scan>
              <horizontal>
                <samples>1</samples>
                <resolution>1</resolution>
                <min_angle>0</min_angle>
                <max_angle>0</max_angle>
              </horizontal>
            </scan>
            <noise>
              <type>gaussian</type>
              <mean>0.0</mean>
              <stddev>0.001</stddev>
            </noise>
          </ray>

          <always_on>1</always_on>
          <visualize>true</visualize>
        </sensor>
        <gravity>false</gravity>
        <inertial>
          <mass>0.1</mass>
        </inertial>
        <visual name="visual">
          <geometry>
            <mesh>
              <uri>model://hokuyo/meshes/hokuyo.dae</uri>
            </mesh>
          </geometry>
        </visual>
        <collision name="collision">
          <geometry>
            <mesh>
              <uri>model://hokuyo/meshes/hokuyo.dae</uri>
            </mesh>
          </geometry>
        </collision>
      </link>
      <static>true</static>
    </model>

@[end if]@
@[if sensor.type == "proximity_sensor"]@
    <!-- a proximity sensor called @(name) -->
    <model name="@(name)">
      <pose>@(' '.join(sensor.pose.xyz)) @(' '.join(sensor.pose.rpy))</pose>
      <static>true</static>
      <link name="body">
        <visual name="visual">
          <pose>0 0 0 0 1.5708 0</pose>
          <geometry>
            <cylinder>
              <radius>0.006</radius> <!-- diameter of sensing head -->
              <length>0.046</length> <!-- length of sensor body -->
            </cylinder>
          </geometry>
        </visual>
        <collision name="collision">
          <pose>0 0 0 0 1.5708 0</pose>
          <geometry>
            <cylinder>
              <radius>0.006</radius> <!-- diameter of sensing head -->
              <length>0.046</length> <!-- length of sensor body -->
            </cylinder>
          </geometry>
        </collision>
        <sensor name="@(name)" type="ray">
          <pose>0.023 0 0 0 0 0</pose>
          <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
            <gaussianNoise>0.001</gaussianNoise>
            <alwaysOn>true</alwaysOn>
            <updateRate>20</updateRate>
            <topicName>@(name)</topicName>
            <frameName>@(name)_frame</frameName>
            <robotNamespace>ariac</robotNamespace>
            <fov>0.125</fov>
            <radiation>ultrasound</radiation>
          </plugin>

          <ray>
            <range>
              <min>0.01</min>
              <max>0.15</max>
              <resolution>0.01</resolution>
            </range>
            <scan>
              <horizontal>
                  <samples>5</samples>
                  <resolution>1.0</resolution>
                  <min_angle>-0.0625</min_angle>
                  <max_angle>0.0625</max_angle>
                </horizontal>
                <vertical>
                  <samples>5</samples>
                  <resolution>1</resolution>
                  <min_angle>-0.0625</min_angle>
                  <max_angle>0.0625</max_angle>
                </vertical>
            </scan>
          </ray>

          <always_on>1</always_on>
          <visualize>true</visualize>
        </sensor>
        <gravity>false</gravity>
        <inertial>
          <mass>0.1</mass>
        </inertial>
      </link>
      <link name="cord">
        <gravity>false</gravity>
        <visual name="visual">
          <pose>-0.025 0 0 0 1.5708 0</pose>
          <geometry>
            <cylinder>
              <radius>0.001</radius> <!-- diameter of sensing head -->
              <length>0.02</length> <!-- length of sensor body -->
            </cylinder>
          </geometry>
        </visual>
        <collision name="collision">
          <pose>-0.025 0 0 0 1.5708 0</pose>
          <geometry>
            <cylinder>
              <radius>0.001</radius> <!-- diameter of sensing head -->
              <length>0.02</length> <!-- length of sensor body -->
            </cylinder>
          </geometry>
        </collision>
      </link>
    </model>

@[end if]@
@[if sensor.type == "logical_camera"]@
    <!-- a logical camera called @(name) -->
    <include>
      <uri>model://logical_camera</uri>
      <name>@(name)</name>
      <pose>@(' '.join(sensor.pose.xyz)) @(' '.join(sensor.pose.rpy))</pose>
      <static>true</static>
    </include>

@[end if]@
@[if sensor.type == "laser_profiler"]@
    <!-- a laser profiler called @(name) -->
    <model name="@(name)">
      <pose>@(' '.join(sensor.pose.xyz)) @(' '.join(sensor.pose.rpy))</pose>
      <static>true</static>
      <link name="link">
        <visual name="body">
          <geometry>
            <box>
              <size>0.093 0.05 0.167</size>
            </box>
          </geometry>
        </visual>
        <collision name='collision'>
          <geometry>
            <box>
              <size>0.093 0.05 0.167</size>
            </box>
          </geometry>
        </collision>
        <sensor name="laser_profiler" type="ray">
          <update_rate>100</update_rate>
          <pose>0 0 0.06 0 0 0</pose>
          <plugin name="proximity_ray_plugin" filename="libgazebo_ros_laser.so" >
            <!-- If the namespace is set to ariac, the frame will also be namespaced -->
            <!-- So, we manually namespace the topic only -->
            <robotNamespace>/</robotNamespace>
            <frameName>@(name)_frame</frameName>
            <topicName>ariac/@(name)</topicName>
          </plugin>
          <ray>
            <range>
              <min>0.181</min>
              <max>0.725</max>
              <resolution>0.001</resolution>
            </range>
            <scan>
              <horizontal>
                <samples>400</samples>
                <resolution>1</resolution>
                <min_angle>-0.251</min_angle>
                <max_angle>0.251</max_angle>
              </horizontal>
            </scan>

            <!-- nothing below this line really needs to be modified upon change of sensor model -->

            <noise>
              <type>gaussian</type>
              <mean>0.0</mean>
              <stddev>0.001</stddev>
            </noise>
          </ray>

          <always_on>1</always_on>
          <visualize>true</visualize>
        </sensor>
        <gravity>false</gravity>
        <inertial>
          <mass>0.1</mass>
        </inertial>
      </link>
    </model>

@[end if]@
@[end for]@

    <!-- a wall to delete objects at the end of the belt -->
    <include>
      <uri>model://deletion_wall</uri>
      <pose>1.2 -4.1 1.41 0 0 1.5708</pose>
    </include>



@{belt_parts_loop = True}@
@{belt_population_cycles = options['belt_population_cycles'] if belt_parts_loop else 1}

@[for obj_type, spawn_times in belt_parts.items()]@
    <!-- Pool of @(obj_type) objects -->
    <population name="@(obj_type)_population">
      <model name="populate_conveyor|@(obj_type)">
        <include>
          <uri>model://@(obj_type)_ariac</uri>
        </include>
      </model>
      <pose>-16.5 -3 2.1 0 0 0</pose>
      <distribution>
        <type>grid</type>
        <rows>@(belt_population_cycles * len(spawn_times))</rows>
        <cols>1</cols>
        <step>0.2 0.2 0</step>
      </distribution>
    </population>

@[end for]@

@[if options['insert_agvs']]@
@[for agv_id in [1,2]]@
    <!-- AGV@(str(agv_id)) -->
    <model name="agv@(str(agv_id))">
      <pose>0.3 @(3.3 if agv_id == 1 else -3.3) 0 0 0 @(3.14159 if agv_id == 1 else 0)</pose>
      <include>
        <name>agv@(str(agv_id))</name>
        <uri>model://warehouse_robot_ariac</uri>
      </include>
      <plugin name="agv_plugin" filename="libROSAGVPlugin.so">
        <submit_tray_service_name>/ariac/submit_tray</submit_tray_service_name>
        <clear_tray_service_name>/ariac/kit_tray_@(str(agv_id))/clear_tray</clear_tray_service_name>
        <lock_tray_service_name>/ariac/kit_tray_@(str(agv_id))/lock_models</lock_tray_service_name>
        <index>@(str(agv_id))</index>
      </plugin>

      <!-- a tray for building kits -->
      <!-- (this has to be in a model tag so logical cameras can detect it as a nested model) -->
      <model name="kit_tray_@(str(agv_id))">
        <pose>0.0 0.15 0.75 0 0 0</pose>
        <include>
          <name>kit_tray_@(str(agv_id))</name>
          <uri>model://kit_tray_ariac</uri>
        </include>
        <plugin name="kit_tray_plugin" filename="libROSAriacKitTrayPlugin.so">
          <contact_sensor_name>kit_tray_contact</contact_sensor_name>
          <clear_tray_service_name>/ariac/kit_tray_@(str(agv_id))/clear_tray</clear_tray_service_name>
          <lock_models_service_name>/ariac/kit_tray_@(str(agv_id))/lock_models</lock_models_service_name>
          <update_rate>20</update_rate>
          <faulty_parts>
@[for part_name, part_info in faulty_parts.items()]@
            <name>@(part_name)</name>
@[end for]@
          </faulty_parts>
        </plugin>
      </model>

      <!-- join the tray and agv -->
      <joint name="agv_tray" type="fixed">
          <parent>agv@(str(agv_id))::link</parent>
          <child>kit_tray_@(str(agv_id))::kit_tray_@(str(agv_id))::tray</child>
      </joint>
    </model>

    <model name="quality_control_sensor_@(str(agv_id))">
      <pose>0.3 @(3.5 if agv_id == 1 else -3.5) 1.5 0 1.574 @(-1.574 if agv_id == 1 else 1.574)</pose>
      <plugin name="ros_logical_camera" filename="libROSLogicalCameraPlugin.so">
        <robotNamespace>ariac</robotNamespace>
        <position_noise>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.001</stddev>
          </noise>
        </position_noise>
        <orientation_noise>
          <noise>
            <type>gaussian</type>
            <mean>0.0</mean>
            <stddev>0.01</stddev>
          </noise>
        </orientation_noise>
        <known_model_names>
@[for part_name, part_info in faulty_parts.items()]@
          <name>@(part_name)</name>
@[end for]@
        </known_model_names>
      </plugin>
      <link name="link">
        <gravity>false</gravity>
        <inertial>
          <mass>0.1</mass>
          <inertia>
            <ixx>0.000166667</ixx>
            <iyy>0.000166667</iyy>
            <izz>0.000166667</izz>
          </inertia>
        </inertial>

        <collision name="collision">
          <pose>0 0.02 0 0 0 -1.5708</pose>
          <geometry>
            <mesh>
              <uri>model://logical_camera/meshes/camera.dae</uri>
            </mesh>
          </geometry>
        </collision>

        <visual name="visual">
          <pose>0 0.02 0 0 0 -1.5708</pose>
          <geometry>
            <mesh>
              <uri>model://logical_camera/meshes/camera.dae</uri>
            </mesh>
          </geometry>
        </visual>

        <sensor name="logical_camera" type="logical_camera">
          <logical_camera>
            <near>0.73</near>
            <far>0.78</far>
            <horizontal_fov>0.7</horizontal_fov>
            <aspect_ratio>0.35</aspect_ratio>
          </logical_camera>

          <visualize>false</visualize>
          <always_on>true</always_on>
          <update_rate>10</update_rate>
        </sensor>
      </link>
    </model>

@[end for]@
@[end if]@

    <!-- an invisible conveyor belt -->
    <model name="conveyor_belt">

      <model name="conveyor_belt_fixed">
        <static>true</static>
        <pose>1.21 5 0.8126 0 0 -1.57079</pose>
        <link name="link">
          <pose>0 0 0 0 0 0</pose>
        </link>
      </model>

      <model name="conveyor_belt_moving">
        <static>false</static>
        <pose>1.21 -2 0.8126 0 0 -1.57079</pose>
        <link name="belt">
          <pose>-5 0 0 0 0 0</pose>
          <inertial>
            <inertia>
              <ixx>3.8185</ixx>
              <ixy>0</ixy>
              <ixz>0</ixz>
              <iyy>1781.5</iyy>
              <iyz>0</iyz>
              <izz>1784.72</izz>
            </inertia>
            <mass>100</mass>
          </inertial>
          <!--Uncomment for debugging -->
          <!--
          <visual name="belt_visual">
            <geometry>
              <box>
                <size>14.62206 0.65461 0.18862</size>
              </box>
            </geometry>
          </visual>
          -->
          <collision name="belt_collision">
            <geometry>
              <box>
                <size>14.62206 0.65461 0.18862</size>
              </box>
            </geometry>
            <surface>
              <friction>
                <ode>
                  <mu>1.0</mu>
                  <mu2>1.0</mu2>
                </ode>
                <torsional>
                  <coefficient>1000.0</coefficient>
                  <patch_radius>0.1</patch_radius>
                </torsional>
              </friction>
            </surface>
          </collision>
        </link>
      </model>

      <joint name="belt_joint" type="prismatic">
        <parent>conveyor_belt_fixed::link</parent>
        <child>conveyor_belt_moving::belt</child>
        <axis>
          <xyz>1 0 0</xyz>
          <limit>
            <lower>0</lower>
            <upper>1.0</upper>
          </limit>
        </axis>
      </joint>

      <plugin name="conveyor_belt_plugin" filename="libROSConveyorBeltPlugin.so">
        <robot_namespace>/ariac</robot_namespace>
        <population_rate_modifier_topic>/ariac/population/rate_modifier</population_rate_modifier_topic>
        <link>conveyor_belt::conveyor_belt_moving::belt</link>
        <power>0</power>
      </plugin>
    </model>

@[if options['insert_models_over_bins']]@
    <!-- Populate the bins -->
@[for model_name, model in models_to_insert.items()]@
    <include>
      <name>@(model_name)</name>
      <uri>model://@(model.type)_ariac</uri>
      <pose>@(' '.join(model.pose.xyz)) @(' '.join(model.pose.rpy))</pose>
    </include>
@[end for]@
@[end if]@
@[if belt_parts]@

    <!-- Populate the conveyor belt -->
    <plugin filename="libROSPopulationPlugin.so" name="populate_conveyor">
      <activation_topic>/ariac/populate_belt</activation_topic>
      <rate_modifier_topic>/ariac/population/rate_modifier</rate_modifier_topic>
      <control_topic>/ariac/population/control</control_topic>
      <state_topic>/ariac/population/state</state_topic>
      <loop_forever>@("true" if belt_parts_loop else "false")</loop_forever>
      <frame>conveyor_belt::conveyor_belt_fixed</frame>
      <object_sequence>
@[for part_name, spawn_times in belt_parts.items()]@
  @[for spawn_time, part in spawn_times.items()]@
        <object>
          <time>@(spawn_time)</time>
          <type>@(part.type)</type>
          <pose>@(' '.join(part.pose.xyz)) @(' '.join(part.pose.rpy))</pose>
        </object>
  @[end for]@
@[end for]@
      </object_sequence>
      <update_rate>10</update_rate>
    </plugin>
@[end if]@

    <!-- The NIST-ARIAC task manager -->
    <plugin filename="libROSAriacTaskManagerPlugin.so" name="task_manager">
      <robot_namespace>ariac</robot_namespace>
      <competition_time_limit>@(time_limit)</competition_time_limit>
      <start_competition_service_name>/ariac/start_competition</start_competition_service_name>
      <end_competition_service_name>/ariac/end_competition</end_competition_service_name>
      <population_activate_topic>/ariac/populate_belt</population_activate_topic>
      <conveyor_control_topic>/ariac/conveyor/control</conveyor_control_topic>
      <submit_tray_service_name>/ariac/submit_tray</submit_tray_service_name>
      <material_locations_service_name>/ariac/material_locations</material_locations_service_name>
      <orders_topic>/ariac/orders</orders_topic>
@[for order_name, order in orders.items()]@
      <order>
@[if order['announcement_condition'] == 'time']@
        <start_time>@(order['announcement_condition_value'])</start_time>
@[end if]@
@[if order['announcement_condition'] == 'wanted_parts']@
        <interrupt_on_wanted_parts>@(order['announcement_condition_value'])</interrupt_on_wanted_parts>
@[end if]@
@[if order['announcement_condition'] == 'unwanted_parts']@
        <interrupt_on_unwanted_parts>@(order['announcement_condition_value'])</interrupt_on_unwanted_parts>
@[end if]@
@[for kit_index in range(order['kit_count'])]
        <kit>
          <kit_type>@(order_name)_kit_@(kit_index)</kit_type>
@[for part in order['parts']]@
          <object>
            <type>@(part.type)</type>
            <pose>@(' '.join(part.pose.xyz)) @(' '.join(part.pose.rpy))</pose>
          </object>
@[end for]@
        </kit>
@[end for]@
      </order>
@[end for]@
      <material_locations>
@[for material_name, locations in material_locations.items()]@
        <material>
          <type>@(material_name)</type>
@[for location in locations]@
          <location>
            <storage_unit>@(location)</storage_unit>
          </location>
@[end for]@
        </material>
@[end for]@
      </material_locations>

    </plugin>

@[if options['fill_demo_tray'] and orders]@
@{
order_name, order = orders.items()[0]
count = {}
}@
    <!-- Populate kit tray with order parts -->
    <plugin filename="libPopulationPlugin.so" name="populate_kit_tray_1">
      <activation_topic>/ariac/populate_belt</activation_topic>
      <loop_forever>false</loop_forever>
      <frame>agv1::kit_tray_1::kit_tray_1::tray</frame>
      <object_sequence>
@[for part in order['parts']]@
        <object>
          <time>0.0</time>
          <type>@(part.type)</type>
          <pose>@(' '.join(part.pose.xyz[0:-1])) 0.1 @(' '.join(part.pose.rpy))</pose>
        </object>
@[end for]@
      </object_sequence>
    </plugin>

    <!-- Insert models to fill the kit tray -->
@[for part in order['parts']]@
@{
try:
    id = count[part.type]
except:
    id = 0
}@
    <include>
      <name>populate_kit_tray_1|@(part.type)_clone_@(id)</name>
      <uri>model://@(part.type)_ariac</uri>
      <pose>-7.5 -3 2.1 0 0 0</pose>
    </include>
@{
count[part.type] = id + 1
}@
@[end for]@

@[end if]@

  </world>
</sdf>