sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-24 08:42:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added ability to dynamically place objects in gear world

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-18 15:13:42 -04:00
parent f0c955ee02
commit 00c880cf3b
4 changed files with 1167 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
rrrobot_ws/setup.sh Executable file
View File

@@ -0,0 +1,2 @@
sudo cp world/gear.py /opt/ros/melodic/lib/osrf_gear/gear.py
sudo cp world/gear.world.template /opt/ros/melodic/share/osrf_gear/worlds/gear.world.template

2
rrrobot_ws/src/simulation_env/test/test_insert_object.cpp
View File

@@ -9,7 +9,7 @@ using std::endl;
int main(int argc, char **argv)
{
ros::init(argc, argv, "inert_models");
ros::init(argc, argv, "insert_models");
ros::NodeHandle nh;

674
rrrobot_ws/world/gear.py Executable file
View File

@@ -0,0 +1,674 @@
#!/usr/bin/env python
# Software License Agreement (Apache License)
#
# Copyright 2016 Open Source Robotics Foundation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from __future__ import print_function
import argparse
import math
import os
import random
import subprocess
import sys
import em
import rospkg
import yaml
rospack = rospkg.RosPack()
#world_dir = '/home/rrrobot/rrrobot_ws/world'
world_dir = os.path.join(rospack.get_path('osrf_gear'), 'worlds')
launch_dir = os.path.join(rospack.get_path('osrf_gear'), 'launch')
template_files = [
#os.path.join(world_dir, 'gear.world.template'),
'/home/rrrobot/rrrobot_ws/world/gear.world.template',
os.path.join(launch_dir, 'gear.launch.template'),
os.path.join(launch_dir, 'gear.urdf.xacro.template'),
]
arm_template_file = os.path.join(launch_dir, 'arm.urdf.xacro.template')
arm_configs = {
'arm1': {
'arm_type': 'ur10',
'pose': {
'xyz': [0.3, 0.92, 0.9],
'rpy': [0.0, 0.0, 0.0]
},
'default_initial_joint_states': {
'elbow_joint': 2.14,
'linear_arm_actuator_joint': 0,
'shoulder_lift_joint': -2.0,
'shoulder_pan_joint': 3.14,
'wrist_1_joint': 3.27,
'wrist_2_joint': -1.51,
'wrist_3_joint': 0,
}
},
'arm2': {
'arm_type': 'ur10',
'pose': {
'xyz': [0.3, -0.92, 0.9],
'rpy': [0.0, 0.0, 0.0]
},
'default_initial_joint_states': {
'elbow_joint': 2.14,
'linear_arm_actuator_joint': 0,
'shoulder_lift_joint': -2.0,
'shoulder_pan_joint': 3.14,
'wrist_1_joint': 3.27,
'wrist_2_joint': -1.51,
'wrist_3_joint': 0,
}
},
}
possible_products = [
'disk_part',
'gasket_part',
'gear_part',
'piston_rod_part',
'pulley_part',
]
sensor_configs = {
'break_beam': None,
'camera': None,
'proximity_sensor': None,
'logical_camera': None,
'laser_profiler': None,
'depth_camera': None,
'quality_control': None,
}
default_sensors = {
'quality_control_sensor_1': {
'type': 'quality_control',
'pose': {
'xyz': [0.3, 3.5, 1.5],
'rpy': [0, 1.574, -1.574]
}
},
'quality_control_sensor_2': {
'type': 'quality_control',
'pose': {
'xyz': [0.3, -3.5, 1.5],
'rpy': [0, 1.574, 1.574]
}
},
}
default_belt_models = {
}
bin_width = 0.6
bin_depth = 0.6
bin_height = 0.72
bin_angle = 0.0
default_bin_origins = {
'bin1': [-0.3, -1.916, 0],
'bin2': [-0.3, -1.15, 0],
'bin3': [-0.3, -0.383, 0],
'bin4': [-0.3, 0.383, 0],
'bin5': [-0.3, 1.15, 0],
'bin6': [-0.3, 1.916, 0],
}
configurable_options = {
'insert_models_over_bins': False,
'disable_shadows': False,
'belt_population_cycles': 5,
'gazebo_state_logging': False,
'spawn_extra_models': False,
'unthrottled_physics_update': False,
'model_type_aliases': {
'belt_model_type1': 'part1',
'belt_model_type2': 'part2',
},
'visualize_sensor_views': False,
'visualize_drop_regions': False,
}
default_time_limit = 500 # seconds
max_count_per_model = 30 # limit on the number of instances of each model type
def initialize_model_id_mappings(random_seed=None):
global global_model_count, model_id_mappings
global_model_count = {} # global count of how many times a model type has been created
randomize = False
if random_seed is not None:
randomize = True
random.seed(random_seed)
# Initialize the mapping between model index and ID that will exist for each model type
model_id_mappings = {}
# Initialize the list of random IDs that will be used in the mappings
# The IDs will be unique across different model types
max_model_id = max_count_per_model * len(possible_products) # can be larger for more spread
random_ids = random.sample(range(0, max_model_id), max_model_id)
for model_type in possible_products:
if not randomize:
# Just use ordinary mapping
model_id_mappings[model_type] = list(range(1, max_count_per_model + 1))
else:
# Use random IDs for the mapping
model_id_mappings[model_type] = random_ids[:max_count_per_model]
del random_ids[:max_count_per_model]
# Helper for converting strings to booleans; copied from https://stackoverflow.com/a/43357954
def str2bool(v):
if v.lower() in ('yes', 'true', 't', 'y', '1'):
return True
elif v.lower() in ('no', 'false', 'f', 'n', '0'):
return False
else:
raise argparse.ArgumentTypeError('Boolean value expected.')
def prepare_arguments(parser):
add = parser.add_argument
add('-n', '--dry-run', action='store_true', default=False,
help='print generated files to stdout, but do not write them to disk')
add('-v', '--verbose', action='store_true', default=False,
help='output additional logging to console')
add('-o', '--output', default='/tmp/ariac/',
help='directory in which to output the generated files')
add('--development-mode', '-d', action='store_true', default=False,
help='if true the competition mode environment variable will not be set (default false)')
add('--no-gui', action='store_true', default=False,
help="don't run the gazebo client gui")
add('-l', '--state-logging', action='store', type=str2bool, nargs='?',
help='generate gazebo state logs (will override config file option)')
add('--log-to-file', action='store_true', default=False,
help='direct the output of the gazebo ros node to log file instead of the console')
add('--visualize-sensor-views', action='store_true', default=False,
help='visualize the views of sensors in gazebo')
mex_group = parser.add_mutually_exclusive_group(required=False)
add = mex_group.add_argument
add('config', nargs='?', metavar='CONFIG',
help='yaml string that is the configuration')
add('-f', '--file', nargs='+', help='list of paths to yaml files that contain the '
'configuration (contents will be concatenated)')
eval_local_vars = {n: getattr(math, n) for n in dir(math) if not n.startswith('__')}
def expand_to_float(val):
if isinstance(val, str):
return float(eval(val, {}, eval_local_vars))
return float(val)
def expand_yaml_substitutions(yaml_dict):
for k, v in yaml_dict.items():
if isinstance(v, dict):
yaml_dict[k] = expand_yaml_substitutions(v)
if k in ['xyz', 'rpy']:
yaml_dict[k] = [expand_to_float(x) for x in v]
if k in ['initial_joint_states']:
yaml_dict[k] = {kp: expand_to_float(vp) for kp, vp in v.items()}
return yaml_dict
class ArmInfo:
def __init__(self, name, arm_type, initial_joint_states, pose):
self.name = name
self.type = arm_type
self.initial_joint_states = initial_joint_states
self.pose = pose
class ModelInfo:
def __init__(self, model_type, pose, reference_frame):
self.type = model_type
self.pose = pose
self.reference_frame = reference_frame
class SensorInfo:
def __init__(self, name, sensor_type, pose):
self.name = name
self.type = sensor_type
self.pose = pose
class PoseInfo:
def __init__(self, xyz, rpy):
self.xyz = [str(f) for f in xyz]
self.rpy = [str(f) for f in rpy]
class DropRegionInfo:
def __init__(self, name, drop_region_min, drop_region_max, destination, frame, model_type):
self.name = name
self.min = [str(f) for f in drop_region_min]
self.max = [str(f) for f in drop_region_max]
self.destination = destination
self.frame = frame
self.type = model_type
def get_field_with_default(data_dict, entry, default_value):
if entry in data_dict:
return data_dict[entry]
else:
return default_value
def get_required_field(entry_name, data_dict, required_entry):
if required_entry not in data_dict:
print("Error: '{0}' entry does not contain a required '{1}' entry"
.format(entry_name, required_entry),
file=sys.stderr)
sys.exit(1)
return data_dict[required_entry]
def replace_type_aliases(model_type):
if model_type in configurable_options['model_type_aliases']:
model_type = configurable_options['model_type_aliases'][model_type]
return model_type
def model_count_post_increment(model_type):
global global_model_count
try:
count = global_model_count[model_type]
except KeyError:
count = 0
global_model_count[model_type] = count + 1
return count
def get_next_model_id(model_type):
return model_id_mappings[model_type][model_count_post_increment(model_type)]
def create_pose_info(pose_dict, offset=None):
xyz = get_field_with_default(pose_dict, 'xyz', [0, 0, 0])
rpy = get_field_with_default(pose_dict, 'rpy', [0, 0, 0])
for key in pose_dict:
if key not in ['xyz', 'rpy']:
print("Warning: ignoring unknown entry in 'pose': " + key, file=sys.stderr)
if offset is not None:
xyz = [sum(i) for i in zip(xyz, offset)]
return PoseInfo(xyz, rpy)
def create_arm_info(name, arm_dict):
arm_type = arm_dict['arm_type']
initial_joint_states = arm_dict['default_initial_joint_states']
pose = create_pose_info(arm_dict['pose'])
return ArmInfo(name, arm_type, initial_joint_states, pose)
def create_sensor_info(name, sensor_data, allow_protected_sensors=False, offset=None):
sensor_type = get_required_field(name, sensor_data, 'type')
pose_dict = get_required_field(name, sensor_data, 'pose')
for key in sensor_data:
if key not in ['type', 'pose']:
print("Warning: ignoring unknown entry in '{0}': {1}"
.format(name, key), file=sys.stderr)
if sensor_type not in sensor_configs:
if not allow_protected_sensors:
print("Error: given sensor type '{0}' is not one of the known sensor types: {1}"
.format(sensor_type, sensor_configs.keys()), file=sys.stderr)
sys.exit(1)
pose_info = create_pose_info(pose_dict, offset=offset)
return SensorInfo(name, sensor_type, pose_info)
def create_sensor_infos(sensors_dict, allow_protected_sensors=False, offset=None):
sensor_infos = {}
for name, sensor_data in sensors_dict.items():
sensor_infos[name] = create_sensor_info(
name, sensor_data,
allow_protected_sensors=allow_protected_sensors, offset=offset)
return sensor_infos
def create_model_info(model_name, model_data):
model_type = get_required_field(model_name, model_data, 'type')
model_type = replace_type_aliases(model_type)
pose_dict = get_required_field(model_name, model_data, 'pose')
reference_frame = get_field_with_default(model_data, 'reference_frame', '')
for key in model_data:
if key not in ['type', 'pose', 'reference_frame']:
print("Warning: ignoring unknown entry in '{0}': {1}"
.format(model_name, key), file=sys.stderr)
pose_info = create_pose_info(pose_dict)
return ModelInfo(model_type, pose_info, reference_frame)
def create_models_to_spawn_infos(models_to_spawn_dict):
models_to_spawn_infos = {}
for reference_frame, reference_frame_data in models_to_spawn_dict.items():
models = get_required_field(reference_frame, reference_frame_data, 'models')
for model_name, model_to_spawn_data in models.items():
model_to_spawn_data['reference_frame'] = reference_frame
model_info = create_model_info(model_name, model_to_spawn_data)
# assign each model a unique name because gazebo can't do this
# if the models all spawn at the same time
scoped_model_name = reference_frame.replace('::', '|') + '|' + \
model_info.type + '_' + str(get_next_model_id(model_info.type))
models_to_spawn_infos[scoped_model_name] = model_info
return models_to_spawn_infos
def create_models_over_bins_infos(models_over_bins_dict):
models_to_spawn_infos = {}
for bin_name, bin_dict in models_over_bins_dict.items():
if bin_name in default_bin_origins:
offset_xyz = [
default_bin_origins[bin_name][0] - bin_depth / 2,
default_bin_origins[bin_name][1] - bin_width / 2,
bin_height + 0.08]
# Allow the origin of the bin to be over-written
if 'xyz' in bin_dict:
offset_xyz = bin_dict['xyz']
else:
offset_xyz = get_required_field(bin_name, bin_dict, 'xyz')
models = get_required_field(bin_name, bin_dict, 'models') or {}
for model_type, model_to_spawn_dict in models.items():
model_to_spawn_data = {}
model_to_spawn_data['type'] = model_type
model_to_spawn_data['reference_frame'] = 'world'
xyz_start = get_required_field(
model_type, model_to_spawn_dict, 'xyz_start')
xyz_end = get_required_field(
model_type, model_to_spawn_dict, 'xyz_end')
rpy = get_required_field(model_type, model_to_spawn_dict, 'rpy')
rpy[1] = -bin_angle
num_models_x = get_required_field(
model_type, model_to_spawn_dict, 'num_models_x')
num_models_y = get_required_field(
model_type, model_to_spawn_dict, 'num_models_y')
step_size = [
(xyz_end[0] - xyz_start[0]) / max(1, num_models_x - 1),
(xyz_end[1] - xyz_start[1]) / max(1, num_models_y - 1)]
# Create a grid of models
for idx_x in range(num_models_x):
for idx_y in range(num_models_y):
model_x_offset = xyz_start[0] + idx_x * step_size[0]
xyz = [
offset_xyz[0] + model_x_offset,
offset_xyz[1] + xyz_start[1] + idx_y * step_size[1],
offset_xyz[2] + xyz_start[2] + model_x_offset * math.tan(bin_angle)]
model_to_spawn_data['pose'] = {'xyz': xyz, 'rpy': rpy}
model_info = create_model_info(model_type, model_to_spawn_data)
# assign each model a unique name because gazebo can't do this
# if the models all spawn at the same time
scoped_model_name = bin_name + '|' + \
model_info.type + '_' + str(get_next_model_id(model_type))
model_info.bin = bin_name
models_to_spawn_infos[scoped_model_name] = model_info
return models_to_spawn_infos
def create_belt_model_infos(belt_models_dict):
belt_model_infos = {}
for obj_type, spawn_times in belt_models_dict.items():
for spawn_time, belt_model_dict in spawn_times.items():
obj_type = replace_type_aliases(obj_type)
if obj_type not in belt_model_infos:
belt_model_infos[obj_type] = {}
belt_model_dict['type'] = obj_type
belt_model_infos[obj_type][spawn_time] = create_model_info('belt_model', belt_model_dict)
return belt_model_infos
def create_drops_info(drops_dict):
drops_info = {}
drop_region_infos = []
drop_regions_dict = get_required_field('drops', drops_dict, 'drop_regions')
for drop_name, drop_region_dict in drop_regions_dict.items():
frame = get_field_with_default(drop_region_dict, 'frame', 'world')
drop_region_min = get_required_field('drop_region', drop_region_dict, 'min')
drop_region_min_xyz = get_required_field('min', drop_region_min, 'xyz')
drop_region_max = get_required_field('drop_region', drop_region_dict, 'max')
drop_region_max_xyz = get_required_field('max', drop_region_max, 'xyz')
destination_info = get_required_field('drop_region', drop_region_dict, 'destination')
destination = create_pose_info(destination_info)
product_type = get_required_field('drop_region', drop_region_dict, 'product_type_to_drop')
product_type = replace_type_aliases(product_type)
drop_region_infos.append(
DropRegionInfo(
drop_name, drop_region_min_xyz, drop_region_max_xyz,
destination, frame, product_type))
drops_info['drop_regions'] = drop_region_infos
return drops_info
def create_order_info(name, order_dict):
shipment_count = get_field_with_default(order_dict, 'shipment_count', 1)
destinations = get_field_with_default(order_dict, 'destinations', ["any"] * shipment_count)
announcement_condition = get_required_field(name, order_dict, 'announcement_condition')
announcement_condition_value = get_required_field(
name, order_dict, 'announcement_condition_value')
products_dict = get_required_field(name, order_dict, 'products')
products = []
for product_name, product_dict in products_dict.items():
products.append(create_model_info(product_name, product_dict))
return {
'announcement_condition': announcement_condition,
'announcement_condition_value': announcement_condition_value,
'products': products,
'shipment_count': shipment_count,
'destinations': destinations
}
def create_order_infos(orders_dict):
order_infos = {}
for order_name, order_dict in orders_dict.items():
order_infos[order_name] = create_order_info(order_name, order_dict)
return order_infos
def create_faulty_products_info(faulty_products_dict):
faulty_product_infos = {}
for product_name in faulty_products_dict:
faulty_product_infos[product_name] = product_name # no other info for now
return faulty_product_infos
def create_bin_infos():
bin_infos = {}
for bin_name, xyz in default_bin_origins.items():
bin_infos[bin_name] = PoseInfo(xyz, [0, bin_angle, 3.14159])
return bin_infos
def create_material_location_info(belt_models, models_over_bins):
material_locations = {}
# Specify that belt products can be found on the conveyor belt
for _, spawn_times in belt_models.items():
for spawn_time, product in spawn_times.items():
if product.type in material_locations:
material_locations[product.type].update(['belt'])
else:
material_locations[product.type] = {'belt'}
# Specify in which bin the different bin products can be found
for product_name, product in models_over_bins.items():
if product.type in material_locations:
material_locations[product.type].update([product.bin])
else:
material_locations[product.type] = {product.bin}
return material_locations
def create_options_info(options_dict):
options = configurable_options
for option, val in options_dict.items():
options[option] = val
return options
def prepare_template_data(config_dict, args):
template_data = {
'arms': [create_arm_info(name, conf) for name, conf in arm_configs.items()],
'sensors': create_sensor_infos(default_sensors, allow_protected_sensors=True),
'models_to_insert': {},
'models_to_spawn': {},
'belt_models': create_belt_model_infos(default_belt_models),
'faulty_products': {},
'drops': {},
'orders': {},
'options': {'insert_agvs': True},
'time_limit': default_time_limit,
'bin_height': bin_height,
'world_dir': world_dir,
'joint_limited_ur10': config_dict.pop('joint_limited_ur10', False),
'sensor_blackout': {},
}
# Process the options first as they may affect the processing of the rest
options_dict = get_field_with_default(config_dict, 'options', {})
template_data['options'].update(create_options_info(options_dict))
if args.state_logging is not None:
template_data['options']['gazebo_state_logging'] = args.state_logging
if args.visualize_sensor_views:
template_data['options']['visualize_sensor_views'] = True
models_over_bins = {}
for key, value in config_dict.items():
if key == 'sensors':
template_data['sensors'].update(
create_sensor_infos(value))
elif key == 'models_over_bins':
models_over_bins = create_models_over_bins_infos(value)
template_data['models_to_insert'].update(models_over_bins)
elif key == 'belt_models':
template_data['belt_models'].update(create_belt_model_infos(value))
elif key == 'drops':
template_data['drops'].update(create_drops_info(value))
elif key == 'faulty_products':
template_data['faulty_products'].update(create_faulty_products_info(value))
elif key == 'orders':
template_data['orders'].update(create_order_infos(value))
elif key == 'sensor_blackout':
template_data['sensor_blackout'].update(value)
elif key == 'options':
pass
elif key == 'models_to_spawn':
template_data['models_to_spawn'].update(
create_models_to_spawn_infos(value))
elif key == 'time_limit':
template_data['time_limit'] = value
else:
print("Error: unknown top level entry '{0}'".format(key), file=sys.stderr)
sys.exit(1)
template_data['bins'] = create_bin_infos()
template_data['material_locations'] = create_material_location_info(
template_data['belt_models'] or {},
models_over_bins,
)
template_data['possible_products'] = possible_products
return template_data
def generate_files(template_data):
files = {}
for template_file in template_files:
with open(template_file, 'r') as f:
data = f.read()
files[template_file] = em.expand(data, template_data)
# Generate files for each arm
for arm_info in template_data['arms']:
template_data['arm'] = arm_info
with open(arm_template_file, 'r') as f:
data = f.read()
files[arm_info.name + '.urdf.xacro'] = em.expand(data, template_data)
return files
def main(sysargv=None):
parser = argparse.ArgumentParser(
description='Prepares and then executes a gazebo simulation based on configurations.')
prepare_arguments(parser)
args = parser.parse_args(sysargv)
config_data = args.config or ''
if args.file is not None:
for file in args.file:
with open(file, 'r') as f:
comp_config_data = f.read()
config_data += comp_config_data
dict_config = yaml.load(config_data) or {}
expanded_dict_config = expand_yaml_substitutions(dict_config)
if args.verbose:
print(yaml.dump({'Using configuration': expanded_dict_config}))
random_seed = expanded_dict_config.pop('random_seed', None)
initialize_model_id_mappings(random_seed)
template_data = prepare_template_data(expanded_dict_config, args)
files = generate_files(template_data)
if not args.dry_run and not os.path.isdir(args.output):
if os.path.exists(args.output) and not os.path.isdir(args.output):
print('Error, given output directory exists but is not a directory.', file=sys.stderr)
sys.exit(1)
print('creating directory: ' + args.output)
os.makedirs(args.output)
for name, content in files.items():
if name.endswith('.template'):
name = name[:-len('.template')]
name = os.path.basename(name)
if args.dry_run:
print('# file: ' + name)
print(content)
else:
file_path = os.path.join(args.output, name)
print('writing file ' + file_path)
with open(file_path, 'w+') as f:
f.write(content)
cmd = [
'roslaunch',
os.path.join(args.output, 'gear.launch'),
'world_path:=' + os.path.join(args.output, 'gear.world'),
'gear_urdf_xacro:=' + os.path.join(args.output, 'gear.urdf.xacro'),
'arm_urdf_dir:=' + args.output,
]
if args.log_to_file:
cmd.append('gazebo_ros_output:=log')
if args.verbose:
cmd += ['verbose:=true']
if args.no_gui:
cmd += ['gui:=false']
if not args.development_mode:
os.environ['ARIAC_COMPETITION'] = '1'
print('Running command: ' + ' '.join(cmd))
if not args.dry_run:
try:
p = subprocess.Popen(cmd)
p.wait()
except KeyboardInterrupt:
pass
finally:
p.wait()
return p.returncode
if __name__ == '__main__':
# Filter out any special ROS remapping arguments.
# This is necessary if the script is being run from a ROS launch file.
import rospy
filtered_argv = rospy.myargv(sys.argv)
sys.exit(main(filtered_argv[1:]))

490
rrrobot_ws/world/gear.world.template Normal file
View File

@@ -0,0 +1,490 @@
@{
import em, StringIO, os
def expand_snippet(filename, data=locals()):
output = StringIO.StringIO()
interpreter = em.Interpreter(output=output)
interpreter.include(os.path.join(world_dir, 'snippet', filename), data)
print output.getvalue()
}@
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<!-- Set the initial camera pose to be looking at the workspace. -->
<gui fullscreen='0'>
<camera name='user_camera'>
<!-- pretty overview shot -->
<pose frame=''>-7.56272 4.06148 6.60308 0 0.636 -0.467992</pose>
<!-- Top down part placement
<pose frame=''>-0.475239 0 4.863623 0 1.570796 0</pose> -->
<!-- Shipment placement AGV 1
<pose frame=''>0.301627 3.242337 2.584521 0 1.570796</pose> -->
<!-- Shipment placement AGV 2
<pose frame=''>0.301627 -3.242337 2.584521 0 1.570796</pose> -->
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</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]@
@[if drops and options['visualize_drop_regions']]@
@[ for drop_region in drops['drop_regions']]@
@[ if drop_region.frame == "world"]@
<model name="@(drop_region.name)">
<pose>@(' '.join([str((float(drop_region.min[i]) + float(drop_region.max[i])) * 0.5) for i in range(3)])) 0 0 0</pose>
<static>true</static>
<link name="link">
<visual name="visual">
<geometry>
<box>
<size>@(' '.join([str(float(drop_region.max[i]) - float(drop_region.min[i])) for i in range(3)]))</size>
</box>
</geometry>
</visual>
</link>
</model>
@[ end if]@
@[ end for]@
@[end if]@
<!-- A global light source -->
<light type="directional" name="sun">
<cast_shadows>true</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>
<!-- A directed light source -->
<light name="camera_spot_light" type='spot'>
<pose>14 -3.0 3.0 -1.55 0.0 -1.62</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>50</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]@
@[for name, sensor in sensors.items()]@
@[if sensor.type == "quality_control"]@
<!-- a quality control sensor called @(name) -->
@{
expand_snippet('quality_control_sensor.sdf.template')
}@
@[end if]@
@[if sensor.type == "break_beam"]@
<!-- a break beam sensor called @(name) -->
@{
expand_snippet('break_beam.sdf.template')
}@
@[end if]@
@[if sensor.type == "proximity_sensor"]@
<!-- a proximity sensor called @(name) -->
@{
expand_snippet('proximity_sensor.sdf.template')
}@
@[end if]@
@[if sensor.type == "logical_camera"]@
<!-- a logical camera called @(name) -->
@{
expand_snippet('logical_camera.sdf.template')
}@
@[end if]@
@[if sensor.type == "depth_camera"]@
<!-- a laser profiler called @(name) -->
@{
expand_snippet('depth_camera.sdf.template')
}@
@[end if]@
@[if sensor.type == "laser_profiler"]@
<!-- a laser profiler called @(name) -->
@{
expand_snippet('laser_profiler.sdf.template')
}@
@[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_models_loop = True}@
@{belt_population_cycles = options['belt_population_cycles'] if belt_models_loop else 1}
@{obj_type_index = 0}@
@[for obj_type, spawn_times in belt_models.items()]@
<!-- Pool of @(obj_type) objects -->
@[ for index in range(belt_population_cycles * len(spawn_times))]@
<include>
<uri>model://@(obj_type)_ariac</uri>
<name>@(obj_type)_@(index)</name>
<pose>@(7.5 + 0.25 * obj_type_index) @(-9.8 + 0.25 * index) -5.0 0 0 0</pose>
</include>
@[ end for]@
@{obj_type_index += 1}@
@[end for]@
<!-- an invisible conveyor belt -->
<include>
<name>conveyor_belt</name>
<pose>0.07 0.8 0.464 0 0 0</pose>
<uri>model://conveyor_belt_tall_ariac</uri>
</include>
@[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_models]@
<!-- 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>
<start_index>0</start_index>
<prefix_object_names>false</prefix_object_names>
<loop_forever>@("true" if belt_models_loop else "false")</loop_forever>
<frame>conveyor_belt::conveyor_belt_fixed</frame>
<object_sequence>
@[for product_name, spawn_times in belt_models.items()]@
@[for spawn_time, product in spawn_times.items()]@
<object>
<time>@(spawn_time)</time>
<type>@(product.type)</type>
<pose>@(' '.join(product.pose.xyz)) @(' '.join(product.pose.rpy))</pose>
</object>
@[end for]@
@[end for]@
</object_sequence>
<update_rate>10</update_rate>
</plugin>
@[end if]@
@[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">
<agv_control_service_name>/ariac/agv@(str(agv_id))/animate</agv_control_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>
<tf_frame_name>kit_tray_@(str(agv_id))</tf_frame_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>
<get_content_service_name>/ariac/kit_tray_@(str(agv_id))/get_content</get_content_service_name>
<update_rate>20</update_rate>
<faulty_parts>
@[for part_name, part_info in faulty_products.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_products.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]@
<!-- 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_service>/ariac/conveyor/control</conveyor_control_service>
<submit_tray_service_name>/ariac/submit_shipment</submit_tray_service_name>
<material_locations_service_name>/ariac/material_locations</material_locations_service_name>
<shipment_content_topic_name>/ariac/trays</shipment_content_topic_name>
<orders_topic>/ariac/orders</orders_topic>
@[for agv_id in [1,2]]@
<agv index="@(agv_id)">
<agv_control_service_name>/ariac/agv@(agv_id)</agv_control_service_name>
<agv_animate_service_name>/ariac/agv@(agv_id)/animate</agv_animate_service_name>
<get_content_service_name>/ariac/kit_tray_@(agv_id)/get_content</get_content_service_name>
</agv>
@[end for]@
@[for order_name, order in orders.items()]@
<order>
<name>@(order_name)</name>
@[if order['announcement_condition'] == 'time']@
<start_time>@(order['announcement_condition_value'])</start_time>
@[end if]@
@[if order['announcement_condition'] == 'wanted_products']@
<interrupt_on_wanted_products>@(order['announcement_condition_value'])</interrupt_on_wanted_products>
@[end if]@
@[if order['announcement_condition'] == 'unwanted_products']@
<interrupt_on_unwanted_products>@(order['announcement_condition_value'])</interrupt_on_unwanted_products>
@[end if]@
@[for shipment_index in range(order['shipment_count'])]
@{
if '_update' in order_name:
order_name = order_name.split('_update')[0]
destination = order['destinations'][shipment_index]
}@
<shipment>
<shipment_type>@(order_name)_shipment_@(shipment_index)</shipment_type>
<destination>@(destination)</destination>
@[for product in order['products']]@
<product>
<type>@(product.type)</type>
<pose>@(' '.join(product.pose.xyz)) @(' '.join(product.pose.rpy))</pose>
</product>
@[end for]@
</shipment>
@[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>
@[if sensor_blackout]@
<sensor_blackout>
<duration>@(sensor_blackout['duration'])</duration>
<product_count>@(sensor_blackout['product_count'])</product_count>
<topic>/ariac/sensor_enable</topic>
</sensor_blackout>
@[end if]@
<update_rate>10</update_rate>
</plugin>
<!-- Plane under the workcell on which 'removed' models can be placed -->
<model name="under_workcell_plane">
<pose>0 0 -5.1 0 0 0</pose>
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>25 25</size>
</plane>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
</friction>
</surface>
</collision>
</link>
</model>
<!-- A ground plane to hide the under_workcell_plane -->
<model name="ground_plane">
<static>true</static>
<pose>-50 50 -0.01 0 0 0</pose>
<link name="link">
<visual name="visual">
<cast_shadows>false</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>150 150</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
</link>
</model>
<!-- A visual and collision of the linear rail the robots ride on -->
<model name="linear_rail">
<pose>0.3 0 0.9 0 0 0</pose>
<static>true</static>
<link name="rail_link">
<visual name="rail_link_visual">
<geometry>
<box>
<size>0.2 4.6 0.1</size>
</box>
</geometry>
<material>
<script>Gazebo/Grey</script>
</material>
</visual>
<collision name="rail_link_collision">
<geometry>
<box>
<size>0.2 4.6 0.1</size>
</box>
</geometry>
</collision>
</link>
</model>
<plugin name='insert_models' filename='libinsert_model.so'/>
</world>
</sdf>