From 00c880cf3bba63dd9f763f052f67b92c7f01de04 Mon Sep 17 00:00:00 2001 From: Derek Witcpalek Date: Sat, 18 Apr 2020 15:13:42 -0400 Subject: [PATCH] Added ability to dynamically place objects in gear world --- rrrobot_ws/setup.sh | 2 + .../test/test_insert_object.cpp | 2 +- rrrobot_ws/world/gear.py | 674 ++++++++++++++++++ rrrobot_ws/world/gear.world.template | 490 +++++++++++++ 4 files changed, 1167 insertions(+), 1 deletion(-) create mode 100755 rrrobot_ws/setup.sh create mode 100755 rrrobot_ws/world/gear.py create mode 100644 rrrobot_ws/world/gear.world.template diff --git a/rrrobot_ws/setup.sh b/rrrobot_ws/setup.sh new file mode 100755 index 0000000..431d4ee --- /dev/null +++ b/rrrobot_ws/setup.sh @@ -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 diff --git a/rrrobot_ws/src/simulation_env/test/test_insert_object.cpp b/rrrobot_ws/src/simulation_env/test/test_insert_object.cpp index 1c3d1d5..d30df7a 100644 --- a/rrrobot_ws/src/simulation_env/test/test_insert_object.cpp +++ b/rrrobot_ws/src/simulation_env/test/test_insert_object.cpp @@ -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; diff --git a/rrrobot_ws/world/gear.py b/rrrobot_ws/world/gear.py new file mode 100755 index 0000000..f882605 --- /dev/null +++ b/rrrobot_ws/world/gear.py @@ -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:])) diff --git a/rrrobot_ws/world/gear.world.template b/rrrobot_ws/world/gear.world.template new file mode 100644 index 0000000..c7bef21 --- /dev/null +++ b/rrrobot_ws/world/gear.world.template @@ -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() +}@ + + + + + + + + -7.56272 4.06148 6.60308 0 0.636 -0.467992 + + + + + + + + orbit + perspective + + + +@[if options['unthrottled_physics_update']]@ + + 0 + +@[end if]@ + +@[if not options['disable_shadows']]@ + + + false + +@[end if]@ + +@[if drops and options['visualize_drop_regions']]@ +@[ for drop_region in drops['drop_regions']]@ +@[ if drop_region.frame == "world"]@ + + @(' '.join([str((float(drop_region.min[i]) + float(drop_region.max[i])) * 0.5) for i in range(3)])) 0 0 0 + true + + + + + @(' '.join([str(float(drop_region.max[i]) - float(drop_region.min[i])) for i in range(3)])) + + + + + +@[ end if]@ +@[ end for]@ +@[end if]@ + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + 14 -3.0 3.0 -1.55 0.0 -1.62 + 0.5 0.5 0.5 1 + 0.1 0.1 0.1 1 + 0 0 -1 + + 50 + 0.5 + 0.01 + 0.001 + + 0 + + 0.6 + 1 + 1 + + + + + + model://workcell + 0 0 0 0 0 1.57 + + +@[for bin_name, bin_pose in bins.items()]@ + + @(bin_name) + model://workcell_bin + @(' '.join(bin_pose.xyz)) @(' '.join(bin_pose.rpy)) + +@[end for]@ + +@[for name, sensor in sensors.items()]@ +@[if sensor.type == "quality_control"]@ + +@{ +expand_snippet('quality_control_sensor.sdf.template') +}@ +@[end if]@ + +@[if sensor.type == "break_beam"]@ + +@{ +expand_snippet('break_beam.sdf.template') +}@ +@[end if]@ + +@[if sensor.type == "proximity_sensor"]@ + +@{ +expand_snippet('proximity_sensor.sdf.template') +}@ + +@[end if]@ +@[if sensor.type == "logical_camera"]@ + +@{ +expand_snippet('logical_camera.sdf.template') +}@ + +@[end if]@ +@[if sensor.type == "depth_camera"]@ + +@{ +expand_snippet('depth_camera.sdf.template') +}@ +@[end if]@ +@[if sensor.type == "laser_profiler"]@ + +@{ +expand_snippet('laser_profiler.sdf.template') +}@ +@[end if]@ +@[end for]@ + + + + model://deletion_wall + 1.2 -4.1 1.41 0 0 1.5708 + + +@{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()]@ + +@[ for index in range(belt_population_cycles * len(spawn_times))]@ + + model://@(obj_type)_ariac + @(obj_type)_@(index) + @(7.5 + 0.25 * obj_type_index) @(-9.8 + 0.25 * index) -5.0 0 0 0 + +@[ end for]@ +@{obj_type_index += 1}@ +@[end for]@ + + + + conveyor_belt + 0.07 0.8 0.464 0 0 0 + model://conveyor_belt_tall_ariac + + +@[if options['insert_models_over_bins']]@ + +@[for model_name, model in models_to_insert.items()]@ + + @(model_name) + model://@(model.type)_ariac + @(' '.join(model.pose.xyz)) @(' '.join(model.pose.rpy)) + +@[end for]@ +@[end if]@ +@[if belt_models]@ + + + + /ariac/populate_belt + /ariac/population/rate_modifier + /ariac/population/control + /ariac/population/state + 0 + false + @("true" if belt_models_loop else "false") + conveyor_belt::conveyor_belt_fixed + +@[for product_name, spawn_times in belt_models.items()]@ + @[for spawn_time, product in spawn_times.items()]@ + + + @(product.type) + @(' '.join(product.pose.xyz)) @(' '.join(product.pose.rpy)) + + @[end for]@ +@[end for]@ + + 10 + +@[end if]@ + +@[if options['insert_agvs']]@ +@[for agv_id in [1,2]]@ + + + 0.3 @(3.3 if agv_id == 1 else -3.3) 0 0 0 @(3.14159 if agv_id == 1 else 0) + + agv@(str(agv_id)) + model://warehouse_robot_ariac + + + /ariac/agv@(str(agv_id))/animate + /ariac/kit_tray_@(str(agv_id))/clear_tray + /ariac/kit_tray_@(str(agv_id))/lock_models + @(str(agv_id)) + + + + + + 0.0 0.15 0.75 0 0 0 + + kit_tray_@(str(agv_id)) + model://kit_tray_ariac + + + kit_tray_contact + kit_tray_@(str(agv_id)) + /ariac/kit_tray_@(str(agv_id))/clear_tray + /ariac/kit_tray_@(str(agv_id))/lock_models + /ariac/kit_tray_@(str(agv_id))/get_content + 20 + +@[for part_name, part_info in faulty_products.items()]@ + @(part_name) +@[end for]@ + + + + + + + agv@(str(agv_id))::link + kit_tray_@(str(agv_id))::kit_tray_@(str(agv_id))::tray + + + + + 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) + + ariac + + + gaussian + 0.0 + 0.001 + + + + + gaussian + 0.0 + 0.01 + + + +@[for part_name, part_info in faulty_products.items()]@ + @(part_name) +@[end for]@ + + + + false + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + 0 0.02 0 0 0 -1.5708 + + + model://logical_camera/meshes/camera.dae + + + + + + 0 0.02 0 0 0 -1.5708 + + + model://logical_camera/meshes/camera.dae + + + + + + + 0.73 + 0.78 + 0.7 + 0.35 + + + false + true + 10 + + + + +@[end for]@ +@[end if]@ + + + + + ariac + @(time_limit) + /ariac/start_competition + /ariac/end_competition + /ariac/populate_belt + /ariac/conveyor/control + /ariac/submit_shipment + /ariac/material_locations + /ariac/trays + /ariac/orders +@[for agv_id in [1,2]]@ + + /ariac/agv@(agv_id) + /ariac/agv@(agv_id)/animate + /ariac/kit_tray_@(agv_id)/get_content + +@[end for]@ +@[for order_name, order in orders.items()]@ + + @(order_name) +@[if order['announcement_condition'] == 'time']@ + @(order['announcement_condition_value']) +@[end if]@ +@[if order['announcement_condition'] == 'wanted_products']@ + @(order['announcement_condition_value']) +@[end if]@ +@[if order['announcement_condition'] == 'unwanted_products']@ + @(order['announcement_condition_value']) +@[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] +}@ + + @(order_name)_shipment_@(shipment_index) + @(destination) +@[for product in order['products']]@ + + @(product.type) + @(' '.join(product.pose.xyz)) @(' '.join(product.pose.rpy)) + +@[end for]@ + +@[end for]@ + +@[end for]@ + +@[for material_name, locations in material_locations.items()]@ + + @(material_name) +@[for location in locations]@ + + @(location) + +@[end for]@ + +@[end for]@ + +@[if sensor_blackout]@ + + @(sensor_blackout['duration']) + @(sensor_blackout['product_count']) + /ariac/sensor_enable + +@[end if]@ + 10 + + + + + + 0 0 -5.1 0 0 0 + true + + + + + 0 0 1 + 25 25 + + + + + + 100 + 50 + + + + + + + + + + true + -50 50 -0.01 0 0 0 + + + false + + + 0 0 1 + 150 150 + + + + + + + + + + + + 0.3 0 0.9 0 0 0 + true + + + + + 0.2 4.6 0.1 + + + + + + + + + + + 0.2 4.6 0.1 + + + + + + + + +