def resolve_ros_path (path): filename = resource_retriever.get_filename (path) if filename.startswith ("file://"): return filename[7:] else: print ("Path might not be understood by blender: " + filename) return filename
def loadLayoutYaml(self, layout_param): # Initialize layout of the buttons from yaml file # The yaml file can be specified by rosparam layout_yaml_file = rospy.get_param("~layout_yaml_file", layout_param) resolved_layout_yaml_file = get_filename( layout_yaml_file)[len("file://"):] # check file exists if not os.path.exists(resolved_layout_yaml_file): self.showError("Cannot find %s (%s)" % (layout_yaml_file, resolved_layout_yaml_file)) sys.exit(1) self.setupButtons(resolved_layout_yaml_file) self.show()
def setUpClass(cls): folder = 'package://lkh_solver/tsplib' path = resource_retriever.get_filename(folder, use_protocol=False) tmp = [] for name in os.listdir(path): fullpath = os.path.join(path, name) extension = os.path.splitext(name) if os.path.isfile(fullpath) and extension in [ '.gtsp', '.tsp', '.tour' ]: tmp.append(fullpath) cls.files = tmp print('---') print('Parsing all the files here:', folder)
def loadLayoutYaml(self, layout_param): # Initialize layout of the buttons from yaml file # The yaml file can be specified by rosparam layout_yaml_file = rospy.get_param( layout_param, "package://jsk_rqt_plugins/resource/service_button_layout.yaml") resolved_layout_yaml_file = get_filename(layout_yaml_file)[len("file://"):] # check file exists if not os.path.exists(resolved_layout_yaml_file): self.showError("Cannot find %s (%s)" % ( layout_yaml_file, resolved_layout_yaml_file)) sys.exit(1) self.setupButtons(resolved_layout_yaml_file) self.show()
def __init__(self): rospy.init_node('cv', anonymous=True) self.bridge = CvBridge() self.camera = rospy.get_param('~camera') # Load in model configurations with open(rr.get_filename('package://cv/models/models.yaml', use_protocol=False)) as f: self.models = yaml.load(f) # The topic that the camera publishes its feed to self.camera_feed_topic = '/camera/{}/image_raw'.format(self.camera) # Toggle model service name self.enable_service = 'enable_model_{}'.format(self.camera)
def loadLayoutYaml(self, layout_param): # Initialize layout of the buttons from yaml file # The yaml file can be specified by rosparam # layout_yaml_file = rospy.get_param( # layout_param, # "package://jsk_rqt_plugins/resource/service_button_layout.yaml") layout_yaml_file = "package://jsk_rqt_plugins/resource/command_button_layout.yaml" resolved_layout_yaml_file = get_filename(layout_yaml_file)[len("file://"):] # check file exists if not os.path.exists(resolved_layout_yaml_file): self.showError("Cannot find %s (%s)" % ( layout_yaml_file, resolved_layout_yaml_file)) sys.exit(1) self.setupButtons(resolved_layout_yaml_file) self.show()
def __init__(self): self._pub = rospy.Publisher(self.JOY_DEST_TOPIC, Twist, queue_size=50) self._current_joy_msg = Twist() self._movement_type = Movement.TRANSLATION joystick_type = rospy.get_param("~/joy_pub/joystick_type") with open( rr.get_filename('package://joystick/config/joystick.yaml', use_protocol=False)) as f: data = yaml.load(f) self._button_indices = data[joystick_type] rospy.init_node(self.NODE_NAME) rospy.Subscriber(self.JOYSTICK_RAW_TOPIC, Joy, self._parse_data) rospy.spin()
def test_glkh_solver(self): folder = 'package://glkh_solver/gtsplib' path = resource_retriever.get_filename(folder, use_protocol=False) files = [] for name in os.listdir(path): fullpath = os.path.join(path, name) if os.path.isfile(fullpath) and fullpath.endswith('.gtsp'): files.append(fullpath) # Solve them pkg = 'glkh_solver' rosnode = 'glkh_solver' params = lkh.solver.SolverParameters() params.trace_level = 0 for problem_file in files: tour, info = lkh.solver.lkh_solver(problem_file, params, pkg, rosnode)
def init_model(self, model_name): model = self.models[model_name] # Model already initialized; return from method if model.get('predictor') is not None: return weights_file = rr.get_filename('package://cv/models/{}'.format(model['weights']), use_protocol=False) predictor = Model.load(weights_file, model['classes']) publisher_name = '{}/{}'.format(model['topic'], self.camera) publisher = rospy.Publisher(publisher_name, CVObject, queue_size=10) model['predictor'] = predictor model['publisher'] = publisher
def __init__(self): super(ServiceButtonWidget, self).__init__() self.lock = Lock() # Initialize layout of the buttons from yaml file # The yaml file can be specified by rosparam layout_yaml_file = rospy.get_param( "~layout_file", "package://jsk_rqt_plugins/resource/service_button_layout.yaml") resolved_layout_yaml_file = get_filename(layout_yaml_file)[len("file://"):] # check file exists if not os.path.exists(resolved_layout_yaml_file): self.showError("Cannot find %s (%s)" % ( layout_yaml_file, resolved_layout_yaml_file)) sys.exit(1) self.setupButtons(resolved_layout_yaml_file) self.show()
def setupButtons(self, yaml_file): """ Parse yaml file and setup Buttons. Format of the yaml file should be: - name: 'button name' (required) image: 'path to image' (optional) service: 'service' (required) column: 'column index' (optional, defaults to 0) - name: 'button name' (required) image: 'path to image' (optional) service: 'service' (required) column: 'column index' (optional, defaults to 0) """ self.buttons = [] with open(yaml_file) as f: yaml_data = yaml.load(f) # first lookup column num column_indices = [d['column'] for d in yaml_data] max_column_index = max(*column_indices) self.layout = QtGui.QHBoxLayout() self.layout_boxes = [QtGui.QVBoxLayout() for i in range(max_column_index + 1)] for layout in self.layout_boxes: self.layout.addLayout(layout) for button_data in yaml_data: # check if all the field is available if not button_data.has_key("name"): self.showError("name field is missed in yaml") raise Exception("name field is missed in yaml") if not button_data.has_key("service"): self.showError("service field is missed in yaml") raise Exception("service field is missed in yaml") name = button_data['name'] button = QtGui.QPushButton() button.setSizePolicy(QSizePolicy(QSizePolicy.Preferred, QSizePolicy.Preferred)) if button_data.has_key("image"): image_file = get_filename(button_data["image"])[len("file://"):] if os.path.exists(image_file): icon = QtGui.QIcon(image_file) button.setIcon(icon) button.setIconSize(QSize(100, 100)) else: button.setText(name) button.clicked.connect(self.buttonCallback(button_data['service'])) self.layout_boxes[button_data['column']].addWidget(button) self.buttons.append(button) self.setLayout(self.layout)
def execute(self, goal): self.saleae.set_capture_seconds(goal.capture_duration) package_path = 'package://acoustics/data/' + goal.save_name + '_{1}.csv' save_paths = [] for i in range(goal.capture_count): self.publish_feedback(i + 1, goal.capture_count + 1, "Starting capture {}".format(i)) save_paths.append(package_path.format(i)) save_path = rr.get_filename(package_path.format(i), use_protocol=False) self.saleae.export_data2( save_path, analog_channels=self.HYDROPHONE_SET[goal.hydrophone_set]) self.format_csv(save_path) self.publish_feedback(goal.capture_count + 1, goal.capture_count + 1, "Saleae capture complete") self.publish_result(save_paths)
def run(self): if self.hp is None: return [] dis = [np.linalg.norm(np.array(h) - np.array(self.pinger_loc)) for h in self.hp] # hydrophone channel data pings = [np.zeros(self.samples) for i in range(4)] # ping s = np.arange(0, math.floor(0.004 * self.sf)) # complex conjugate transpose (np.matrix.H) ping = 0.1 * np.sin((s / self.sf * self.pinger_frequency * 2 * math.pi)) # samples until ping occurs buffer = 40000 for i in range(4): start_i = int(math.ceil(dis[i] / self.VS * self.sf)) + buffer end_i = int(math.ceil((dis[i] / self.VS + 0.004) * self.sf)) + buffer pings[i][start_i: end_i] = ping start_i = int(math.ceil(dis[i] / self.VS * self.sf + 2.048 * self.sf)) + buffer end_i = int(math.ceil((dis[i] / self.VS + 0.004) * self.sf + 2.048 * self.sf)) + buffer pings[i][start_i: end_i] = ping file_paths = [] for i in range(1, 5): mean = 0 std = 0.01 hs = [p + np.random.normal(mean, std, self.samples) for p in pings] df = pandas.DataFrame({'Channel 0': hs[0], 'Channel 1': hs[1], 'Channel 2': hs[2], 'Channel 3': hs[3]}) fileop = (self.hydrophone_set, self.pinger_loc[0], self.pinger_loc[1], self.pinger_loc[2]) filepath = f"package://acoustics/data/simulated_{fileop[0]}_{fileop[1]}_{fileop[2]}_{fileop[3]}_({i}).csv" df.to_csv(rr.get_filename(filepath, use_protocol=False)) file_paths.append(filepath) self.publish(curr_stage=i, total_stages=4, msg="File generation successful, moving onto next file") return file_paths
def createSpawnMessage( model_name='', model_path='', xyz=[0, 0, 0], rpy=[0, 0, 0], reference_frame='world' ): """ Create a gazebo_msgs.msg.SpawnModel request message from the parameters. model_name -- Name of the model in the simulation model_path -- Path to the sdf file of the model xyz -- array of length 3 with the xyz coordinates rpy -- array of length 3 with the rpy rotations. These are converted to a quaternion reference_frame -- the reference frame of the coordinates returns -- SpawnModelRequest instance """ request = SpawnModelRequest() request.model_name = model_name file = open(resource_retriever.get_filename(model_path, use_protocol=False)) request.model_xml = file.read() file.close() request.initial_pose.position.x = xyz[0] request.initial_pose.position.y = xyz[1] request.initial_pose.position.z = xyz[2] quaternion = transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2]) request.initial_pose.orientation.x = quaternion[0] request.initial_pose.orientation.y = quaternion[1] request.initial_pose.orientation.z = quaternion[2] request.initial_pose.orientation.w = quaternion[3] request.reference_frame = reference_frame return request
def __init__(self): rospy.init_node('thruster_controls') self.sim = rospy.get_param('~/thruster_controls/sim') if not self.sim: self.pub = rospy.Publisher(self.ROBOT_PUB_TOPIC, ThrusterSpeeds, queue_size=3) else: self.pub = rospy.Publisher(self.SIM_PUB_TOPIC, Float32MultiArray, queue_size=3) self.enable_service = rospy.Service('enable_controls', SetBool, self.handle_enable_controls) self.tm = ThrusterManager(rr.get_filename('package://controls/config/cthulhu.config', use_protocol=False)) self.listener = TransformListener() for d in utils.get_axes(): rospy.Subscriber(utils.get_controls_move_topic(d), Float64, self._on_pid_received, d) rospy.Subscriber(utils.get_power_topic(d), Float64, self._on_power_received, d) self.pid_outputs = np.zeros(6) self.pid_outputs_local = np.zeros(6) self.powers = np.zeros(6) self.t_allocs = np.zeros(8)
def setupButtons(self, yaml_file): """ Parse yaml file and setup Buttons. Format of the yaml file should be: - name: 'button name' (required) image: 'path to image for icon' (optional) image_size: 'width and height of icon' (optional) service: 'service' (required) column: 'column index' (optional, defaults to 0) """ self.buttons = [] with open(yaml_file) as f: yaml_data = yaml.load(f) # lookup colum direction direction = 'vertical' for d in yaml_data: if d.has_key('direction'): if d['direction'] == 'horizontal': direction = 'horizontal' else: # d['direction'] == 'vertical': direction = 'vertical' yaml_data.remove(d) break # lookup column num column_indices = [d['column'] for d in yaml_data] max_column_index = max(*column_indices) if direction == 'vertical': self.layout = QHBoxLayout() self.layout_boxes = [ QVBoxLayout() for i in range(max_column_index + 1) ] else: # direction == 'horizontal' self.layout = QVBoxLayout() self.layout_boxes = [ QHBoxLayout() for i in range(max_column_index + 1) ] self.button_groups = [ QGroupBox() for i in range(max_column_index + 1) ] for button_data in yaml_data: # check if all the field is available if not button_data.has_key("name"): self.showError("name field is missed in yaml") raise Exception("name field is missed in yaml") if not button_data.has_key("service"): self.showError("service field is missed in yaml") raise Exception("service field is missed in yaml") if self.button_type == "push": button = QToolButton() else: # self.button_type == "radio": button = QRadioButton() button.setSizePolicy( QSizePolicy(QSizePolicy.Preferred, QSizePolicy.Preferred)) if button_data.has_key("image"): image_file = get_filename( button_data["image"])[len("file://"):] if os.path.exists(image_file): icon = QtGui.QIcon(image_file) button.setIcon(icon) if button_data.has_key("image_size"): button.setIconSize( QSize(button_data["image_size"][0], button_data["image_size"][1])) else: button.setIconSize(QSize(100, 100)) if button_data.has_key("name"): name = button_data['name'] button.setText(name) if button_data.has_key('service_type'): if button_data['service_type'] == 'Trigger': service_type = Trigger elif button_data['service_type'] == 'Empty': service_type = Empty elif button_data['service_type'] == 'SetBool': service_type = SetBool else: raise Exception("Unsupported service type: {}".format( button_data['service_type'])) else: service_type = Empty if service_type == SetBool: button.setCheckable(True) button.clicked.connect( self.buttonCallback(button_data['service'], service_type, button)) if self.button_type == "push": button.setToolButtonStyle( QtCore.Qt.ToolButtonTextUnderIcon) if ((self.button_type == "radio" or service_type == SetBool) and ("default_value" in button_data and button_data['default_value'])): button.setChecked(True) self.layout_boxes[button_data['column']].addWidget(button) self.buttons.append(button) for i in range(len(self.button_groups)): self.button_groups[i].setLayout(self.layout_boxes[i]) for group in self.button_groups: self.layout.addWidget(group) self.setLayout(self.layout)
def setupButtons(self, yaml_file): """ Parse yaml file and setup Buttons. Format of the yaml file should be: - name: 'button name' (required) image: 'path to image for icon' (optional) image_size: 'width and height of icon' (optional) service: 'service' (required) column: 'column index' (optional, defaults to 0) """ self.buttons = [] with open(yaml_file) as f: yaml_data = yaml.load(f) # lookup colum direction direction = 'vertical' for d in yaml_data: if d.has_key('direction'): if d['direction'] == 'horizontal': direction = 'horizontal' else: # d['direction'] == 'vertical': direction = 'vertical' yaml_data.remove(d) break # lookup column num column_indices = [d['column'] for d in yaml_data] max_column_index = max(*column_indices) if direction == 'vertical': self.layout = QtGui.QHBoxLayout() self.layout_boxes = [QtGui.QVBoxLayout() for i in range(max_column_index + 1)] else: # direction == 'horizontal' self.layout = QtGui.QVBoxLayout() self.layout_boxes = [QtGui.QHBoxLayout() for i in range(max_column_index + 1)] self.button_groups = [QtGui.QGroupBox() for i in range(max_column_index + 1)] for button_data in yaml_data: # check if all the field is available if not button_data.has_key("name"): self.showError("name field is missed in yaml") raise Exception("name field is missed in yaml") if not button_data.has_key("service"): self.showError("service field is missed in yaml") raise Exception("service field is missed in yaml") if self.button_type == "push": button = QtGui.QToolButton() else: # self.button_type == "Radio": button = QtGui.QRadioButton() button.setSizePolicy(QSizePolicy(QSizePolicy.Preferred, QSizePolicy.Preferred)) if button_data.has_key("image"): image_file = get_filename(button_data["image"])[len("file://"):] if os.path.exists(image_file): icon = QtGui.QIcon(image_file) button.setIcon(icon) if button_data.has_key("image_size"): button.setIconSize(QSize(button_data["image_size"][0], button_data["image_size"][1])) else: button.setIconSize(QSize(100, 100)) if button_data.has_key("name"): name = button_data['name'] button.setText(name) button.clicked.connect(self.buttonCallback(button_data['service'])) if self.button_type == "push": button.setToolButtonStyle(QtCore.Qt.ToolButtonTextUnderIcon) else: # self.button_type == "Radio": if button_data.has_key("default_value") and button_data['default_value']: button.setChecked(True) self.layout_boxes[button_data['column']].addWidget(button) self.buttons.append(button) for i in range(len(self.button_groups)): self.button_groups[i].setLayout(self.layout_boxes[i]) for group in self.button_groups: self.layout.addWidget(group) self.setLayout(self.layout)
# Select the targets subsets randomly np.random.seed(1) indices = range(num_targets) num_targets_list = [25, 50, 100, 150, 200, 245] targets_indices_list = [] for num in num_targets_list: subset = np.random.choice(num_targets, size=num, replace=False).tolist() targets_indices_list.append(subset) # Check if the benchmarks HDF5 file exists f = None generate = (args.all or args.tsp_solvers or args.metrics or args.discretization or args.others) if generate: uri = 'package://robotsp/data/benchmarks.hdf5' filename = resource_retriever.get_filename(uri, use_protocol=False) if os.path.isfile(filename): logger.warning( 'Benchmarks file already exists: {}'.format(filename)) answer = raw_input('Overwrite selected benchmarks? (y/[n]): ') if answer.lower() == 'y': f = h5py.File(filename, 'r+') else: f = h5py.File(filename, 'w') if f is not None: try: # Update targets group_name = 'targets' if group_name in f.keys(): del f[group_name] f[group_name] = [
#! /usr/bin/env python import resource_retriever import catkin_pkg.package manifest = resource_retriever.get_filename('package://criutils/package.xml', use_protocol=False) catkin_package = catkin_pkg.package.parse_package(manifest) __version__ = catkin_package.version
def __init__(self, namespace='~'): # get image_trasport before ConnectionBasedTransport subscribes ~input self.transport_hint = rospy.get_param(namespace + 'image_transport', 'raw') rospy.loginfo("Using transport {}".format(self.transport_hint)) super(EdgeTPUHumanPoseEstimator, self).__init__() self.bridge = CvBridge() self.classifier_name = rospy.get_param(namespace + 'classifier_name', rospy.get_name()) model_file = 'package://coral_usb/python/coral_usb/posenet/' + \ 'models/mobilenet/' + \ 'posenet_mobilenet_v1_075_481_641_quant_decoder_edgetpu.tflite' model_file = rospy.get_param(namespace + 'model_file', model_file) if model_file is not None: self.model_file = get_filename(model_file, False) self.duration = rospy.get_param(namespace + 'visualize_duration', 0.1) self.enable_visualization = rospy.get_param( namespace + 'enable_visualization', True) device_id = rospy.get_param(namespace + 'device_id', None) if device_id is None: device_path = None else: device_path = ListEdgeTpuPaths(EDGE_TPU_STATE_NONE)[device_id] assigned_device_paths = ListEdgeTpuPaths(EDGE_TPU_STATE_ASSIGNED) if device_path in assigned_device_paths: rospy.logwarn('device {} is already assigned: {}'.format( device_id, device_path)) self.engine = PoseEngine(self.model_file, mirror=False, device_path=device_path) self.resized_H = self.engine.image_height self.resized_W = self.engine.image_width # only for human self.label_ids = [0] self.label_names = ['human'] # dynamic reconfigure dyn_namespace = namespace if namespace == '~': dyn_namespace = '' self.srv = Server(EdgeTPUHumanPoseEstimatorConfig, self.config_callback, namespace=dyn_namespace) self.pub_pose = self.advertise(namespace + 'output/poses', PeoplePoseArray, queue_size=1) self.pub_rects = self.advertise(namespace + 'output/rects', RectArray, queue_size=1) self.pub_class = self.advertise(namespace + 'output/class', ClassificationResult, queue_size=1) # visualize timer if self.enable_visualization: self.lock = threading.Lock() self.pub_image = self.advertise(namespace + 'output/image', Image, queue_size=1) self.pub_image_compressed = self.advertise( namespace + 'output/image/compressed', CompressedImage, queue_size=1) self.timer = rospy.Timer(rospy.Duration(self.duration), self.visualize_cb) self.img = None self.visibles = None self.points = None
def process_vehicles(vehicles, datapath): ''' Looks for the vehicle file in datapath Resolves corresponding urdf or xacro - convention: uwsim urdf files come from Gazebo urdf files - XXX.urdf or XXX.xacro -> XXX_uwsim.urdf Creates Gazebo spawner with namespace ''' vehicle_nodes = [] for vehicle in vehicles: name = vehicle.findtext('name') uwsim_urdf_file = vehicle.findtext('file') # get to Gazebo xacro/urdf file gazebo_model_file, datadir = uwsim_to_abspath( uwsim_urdf_file.replace('_uwsim.urdf', '.xacro'), datapath) if datadir == '': gazebo_model_file, datadir = uwsim_to_abspath( uwsim_urdf_file.replace('_uwsim', ''), datapath) if datadir == '': rospy.loginfo('Could not find original file for ' + uwsim_urdf_file + ' in ' + scene_file) sys.exit(1) uwsim_urdf_file = gazebo_model_file.replace( '.urdf', '_uwsim.urdf').replace('.xacro', '_uwsim.urdf') # parse Gazebo file (urdf or xacro) if 'xacro' in gazebo_model_file: uwsim_urdf_xml = subprocess.Popen( ['rosrun', 'xacro', 'xacro', '--inorder', gazebo_model_file], stdout=subprocess.PIPE) uwsim_urdf_xml = etree.fromstring(uwsim_urdf_xml.stdout.read()) # uwsim_urdf_xml = xacro.parse(gazebo_model_file) # xacro.eval_self_contained(uwsim_urdf_xml) # uwsim_urdf_xml = etree.fromstring(uwsim_urdf_xml.toxml()) else: uwsim_urdf_xml = etree.parse(gazebo_model_file).getroot() # clean up URDF: keep only joints and links for child in uwsim_urdf_xml.getchildren(): if child.tag not in ('joint', 'link'): uwsim_urdf_xml.remove(child) # and remove collision / inertial / buoyancy from links to_be_removed = ('collision', 'inertial', 'buoyancy') for link in uwsim_urdf_xml.findall('link'): for tag in to_be_removed: for node in link.findall(tag): link.remove(node) # change mesh url-filenames to uwsim relative filenames meshes = uwsim_urdf_xml.findall('link/visual/geometry/mesh') if len(meshes) != 0: # create uwsim urdf only if mesh in gazebo urdf, otherwise trust the user for mesh in meshes: mesh_file = resource_retriever.get_filename( substitution_args.resolve_args(mesh.get('filename')))[7:] # get uwsim relative path for mesh mesh_file, datadir = abspath_to_uwsim(mesh_file, datapath) # if mesh in dae, try to find corresponding osg if '.dae' in mesh_file: mesh_osg, datadir_osg = uwsim_to_abspath( mesh_file.replace('.dae', '.osg'), datapath) if datadir_osg != '': mesh_file, datadir = abspath_to_uwsim( mesh_osg, datapath) if datadir == '': rospy.loginfo('Could not find relative path for ' + mesh_file + ' in ' + gazebo_model_file) sys.exit(1) else: if os.path.lexists(datadir + '/' + os.path.splitext(mesh_file)[0] + '.osg'): mesh_file = mesh_file.split('.')[0] + '.osg' mesh.set('filename', mesh_file) # write uwsim urdf insert_header_and_write(uwsim_urdf_xml, gazebo_model_file, uwsim_urdf_file) # get nodes to write in gazebo spawner vehicle_nodes.append(create_spawner(vehicle, gazebo_model_file)) return vehicle_nodes
def process_vehicles(vehicles, datapath): ''' Looks for the vehicle file in datapath Resolves corresponding urdf or xacro - convention: uwsim urdf files come from Gazebo urdf files - XXX.urdf or XXX.xacro -> XXX_uwsim.urdf Creates Gazebo spawner with namespace ''' vehicle_nodes = [] for vehicle in vehicles: name = vehicle.findtext('name') uwsim_urdf_file = vehicle.findtext('file') # get to Gazebo xacro/urdf file gazebo_model_file, datadir = uwsim_to_abspath(uwsim_urdf_file.replace('_uwsim.urdf','.xacro'), datapath) if datadir == '': gazebo_model_file, datadir = uwsim_to_abspath(uwsim_urdf_file.replace('_uwsim',''), datapath) if datadir == '': rospy.loginfo('Could not find original file for ' + uwsim_urdf_file + ' in ' + scene_file) sys.exit(1) uwsim_urdf_file = gazebo_model_file.replace('.urdf', '_uwsim.urdf').replace('.xacro', '_uwsim.urdf') # parse Gazebo file (urdf or xacro) if 'xacro' in gazebo_model_file: uwsim_urdf_xml = subprocess.Popen(['rosrun', 'xacro', 'xacro', '--inorder', gazebo_model_file], stdout=subprocess.PIPE) uwsim_urdf_xml = etree.fromstring(uwsim_urdf_xml.stdout.read()) # uwsim_urdf_xml = xacro.parse(gazebo_model_file) # xacro.eval_self_contained(uwsim_urdf_xml) # uwsim_urdf_xml = etree.fromstring(uwsim_urdf_xml.toxml()) else: uwsim_urdf_xml = etree.parse(gazebo_model_file).getroot() # clean up URDF: keep only joints and links for child in uwsim_urdf_xml.getchildren(): if child.tag not in ('joint', 'link'): uwsim_urdf_xml.remove(child) # and remove collision / inertial / buoyancy from links to_be_removed = ('collision', 'inertial', 'buoyancy') for link in uwsim_urdf_xml.findall('link'): for tag in to_be_removed: for node in link.findall(tag): link.remove(node) # change mesh url-filenames to uwsim relative filenames meshes = uwsim_urdf_xml.findall('link/visual/geometry/mesh') if len(meshes) != 0: # create uwsim urdf only if mesh in gazebo urdf, otherwise trust the user for mesh in meshes: mesh_file = resource_retriever.get_filename(substitution_args.resolve_args(mesh.get('filename')))[7:] # get uwsim relative path for mesh mesh_file, datadir = abspath_to_uwsim(mesh_file, datapath) # if mesh in dae, try to find corresponding osg if '.dae' in mesh_file: mesh_osg, datadir_osg = uwsim_to_abspath(mesh_file.replace('.dae','.osg'), datapath) if datadir_osg != '': mesh_file, datadir = abspath_to_uwsim(mesh_osg, datapath) if datadir == '': rospy.loginfo('Could not find relative path for ' + mesh_file + ' in ' + gazebo_model_file) sys.exit(1) else: if os.path.lexists(datadir + '/' + os.path.splitext(mesh_file)[0] + '.osg'): mesh_file = mesh_file.split('.')[0] + '.osg' mesh.set('filename', mesh_file) # write uwsim urdf insert_header_and_write(uwsim_urdf_xml, gazebo_model_file, uwsim_urdf_file) # get nodes to write in gazebo spawner vehicle_nodes.append(create_spawner(vehicle, gazebo_model_file)) return vehicle_nodes
def __init__(self, namespace='~'): # get image_trasport before ConnectionBasedTransport subscribes ~input self.transport_hint = rospy.get_param(namespace + 'image_transport', 'raw') rospy.loginfo("Using transport {}".format(self.transport_hint)) super(EdgeTPUSemanticSegmenter, self).__init__() rospack = rospkg.RosPack() pkg_path = rospack.get_path('coral_usb') self.bridge = CvBridge() self.classifier_name = rospy.get_param(namespace + 'classifier_name', rospy.get_name()) model_file = os.path.join( pkg_path, './models/deeplabv3_mnv2_pascal_quant_edgetpu.tflite') model_file = rospy.get_param(namespace + 'model_file', model_file) label_file = rospy.get_param(namespace + 'label_file', None) if model_file is not None: self.model_file = get_filename(model_file, False) if label_file is not None: label_file = get_filename(label_file, False) self.duration = rospy.get_param(namespace + 'visualize_duration', 0.1) self.enable_visualization = rospy.get_param( namespace + 'enable_visualization', True) device_id = rospy.get_param(namespace + 'device_id', None) if device_id is None: device_path = None else: device_path = ListEdgeTpuPaths(EDGE_TPU_STATE_NONE)[device_id] assigned_device_paths = ListEdgeTpuPaths(EDGE_TPU_STATE_ASSIGNED) if device_path in assigned_device_paths: rospy.logwarn('device {} is already assigned: {}'.format( device_id, device_path)) self.engine = BasicEngine(self.model_file, device_path) self.input_shape = self.engine.get_input_tensor_shape()[1:3] if label_file is None: self.label_names = [ 'background', 'aeroplane', 'bicycle', 'bird', 'boat', 'bottle', 'bus', 'car', 'cat', 'chair', 'cow', 'diningtable', 'dog', 'horse', 'motorbike', 'person', 'pottedplant', 'sheep', 'sofa', 'train', 'tvmonitor' ] self.label_ids = list(range(len(self.label_names))) else: self.label_ids, self.label_names = self._load_labels(label_file) self.namespace = namespace self.pub_label = self.advertise(namespace + 'output/label', Image, queue_size=1) # visualize timer if self.enable_visualization: self.lock = threading.Lock() self.pub_image = self.advertise(namespace + 'output/image', Image, queue_size=1) self.pub_image_compressed = self.advertise( namespace + 'output/image/compressed', CompressedImage, queue_size=1) self.timer = rospy.Timer(rospy.Duration(self.duration), self.visualize_cb) self.img = None self.header = None self.label = None
def __init__(self, fixed_transforms_dict=None): self.pubs = {} self.models = {} self.pnp_solvers = {} self.pub_dimension = {} self.draw_colors = {} self.dimensions = {} self.class_ids = {} self.model_transforms = {} self.meshes = {} self.mesh_scales = {} self.cv_bridge = CvBridge() self.mesh_clouds = {} self.input_is_rectified = rospy.get_param('input_is_rectified', True) self.downscale_height = rospy.get_param('downscale_height', 500) self.config_detect = lambda: None self.config_detect.mask_edges = 1 self.config_detect.mask_faces = 1 self.config_detect.vertex = 1 self.config_detect.threshold = 0.5 self.config_detect.softmax = 1000 self.config_detect.thresh_angle = rospy.get_param('thresh_angle', 0.5) self.config_detect.thresh_map = rospy.get_param('thresh_map', 0.01) self.config_detect.sigma = rospy.get_param('sigma', 3) self.config_detect.thresh_points = rospy.get_param("thresh_points", 0.1) self.downsampling_leaf_size = rospy.get_param('~downsampling_leaf_size', 0.02) self.fixed_transforms_dict = fixed_transforms_dict # For each object to detect, load network model, create PNP solver, and start ROS publishers for model, weights_url in rospy.get_param('weights').items(): self.models[model] = \ ModelData( model, resource_retriever.get_filename(weights_url, use_protocol=False) ) self.models[model].load_net_model() try: M = np.array(rospy.get_param('model_transforms')[model], dtype='float64') self.model_transforms[model] = tf.transformations.quaternion_from_matrix(M) except KeyError: self.model_transforms[model] = np.array([0.0, 0.0, 0.0, 1.0], dtype='float64') try: self.meshes[model] = rospy.get_param('meshes')[model] except KeyError: pass try: self.mesh_scales[model] = rospy.get_param('mesh_scales')[model] except KeyError: self.mesh_scales[model] = 1.0 try: cloud = PlyData.read(rospy.get_param('meshes_ply')[model]).elements[0].data cloud = np.transpose(np.vstack((cloud['x'], cloud['y'], cloud['z']))) if self.fixed_transforms_dict is None: fixed_transform = np.eye(4) else: fixed_transform = np.transpose(np.array(self.fixed_transforms_dict[model])) fixed_transform[:3,3] = [i/100 for i in fixed_transform[:3,3]] # fixed_transform = np.linalg.inv(fixed_transform) if model == "coke_bottle" or model == "sprite_bottle": fixed_transform[1,3] = 0.172 print("Fixed transform : {}".format(fixed_transform)) cloud_pose = pcl.PointCloud() cloud_pose.from_array(cloud) sor = cloud_pose.make_voxel_grid_filter() sor.set_leaf_size(self.downsampling_leaf_size, self.downsampling_leaf_size, self.downsampling_leaf_size) cloud_pose = sor.filter() self.mesh_clouds[model] = self.transform_cloud(np.asarray(cloud_pose), mat=fixed_transform) # self.mesh_clouds[model] = np.asarray(cloud_pose) # Points x 3 for dim of below rospy.logwarn("Loaded mesh cloud for : {} with size : {}, scaling : {}".format(model, cloud.shape[0], self.mesh_scales[model])) # scale_transform = tf.transformations.scale_matrix(self.mesh_scales[model]) # cloud = np.hstack((cloud, np.ones((cloud.shape[0], 1)))) # cloud = np.matmul(scale_transform, np.transpose(cloud)) # self.mesh_clouds[model] = np.transpose(cloud)[:, :3] except KeyError: rospy.logwarn("Couldn't load mesh ply") pass self.draw_colors[model] = tuple(rospy.get_param("draw_colors")[model]) self.dimensions[model] = tuple(rospy.get_param("dimensions")[model]) self.class_ids[model] = rospy.get_param("class_ids")[model] self.pnp_solvers[model] = \ CuboidPNPSolver( model, cuboid3d=Cuboid3d(rospy.get_param('dimensions')[model]) ) self.pubs[model] = \ rospy.Publisher( '{}/pose_{}'.format(rospy.get_param('topic_publishing'), model), PoseStamped, queue_size=10 ) self.pub_dimension[model] = \ rospy.Publisher( '{}/dimension_{}'.format(rospy.get_param('topic_publishing'), model), String, queue_size=10 ) # Start ROS publishers self.pub_rgb_dope_points = \ rospy.Publisher( rospy.get_param('topic_publishing') + "/rgb_points", ImageSensor_msg, queue_size=10 ) self.pub_detections = \ rospy.Publisher( 'detected_objects', Detection3DArray, queue_size=10 ) self.pub_markers = \ rospy.Publisher( 'markers', MarkerArray, queue_size=10 ) self.pub_pose_cloud = \ rospy.Publisher( rospy.get_param('topic_publishing') + "/pose_cloud", PointCloud2, queue_size=10 ) camera_ns = rospy.get_param('camera', 'dope/webcam') info_manager = CameraInfoManager(cname='dope_webcam_{}'.format(0), namespace=camera_ns) try: camera_info_url = rospy.get_param('camera_info_url') if not info_manager.setURL(camera_info_url): rospy.logwarn('Camera info URL invalid: %s', camera_info_url) except KeyError: # we don't have a camera_info_url, so we'll keep the # default ('file://${ROS_HOME}/camera_info/${NAME}.yaml') pass info_manager.loadCameraInfo() self.info_manager = info_manager self.camera_info = info_manager.getCameraInfo() # Start ROS subscriber # image_sub = message_filters.Subscriber( # rospy.get_param('~topic_camera'), # ImageSensor_msg # ) # info_sub = message_filters.Subscriber( # rospy.get_param('~topic_camera_info'), # CameraInfo # ) # ts = message_filters.TimeSynchronizer([image_sub, info_sub], 1) # ts.registerCallback(self.image_callback) print("Running DOPE...")
# todo add option exclude links parser.add_argument("-e", "--exclude", nargs="+", default=[], help="names of links to be excluded from collision model simplification") args = parser.parse_args() if args.ros: import resource_retriever urdf_handler = URDFHandler(args.input_urdf, args.exclude) filename_dict = urdf_handler.get_filenames(args.select) i = 1 n = len(filename_dict) for link, collision_models in filename_dict.items(): print(f"{link} ({i}/{n})") for c in collision_models: # resolve filename if, uses package:// prefix if ros is enabled resolved_filename = c.geometry.filename if args.ros: resolved_filename = resource_retriever.get_filename(resolved_filename, use_protocol=False) mesh = trimesh.load(resolved_filename) bb_tf = np.linalg.inv(np.matrix(mesh.apply_obb())) bb_bounds = mesh.bounding_box_oriented.bounds bb_size = [bb_bounds[1][0]*2, bb_bounds[1][1]*2, bb_bounds[1][2]*2] original_rotation = transforms3d.euler.euler2mat(c.origin.rotation[0], c.origin.rotation[1], c.origin.rotation[2], axes="sxyz") original_transformation_affine = transforms3d.affines.compose(T=c.origin.position, R=original_rotation, Z=[1,1,1]) new_affine = np.matmul(original_transformation_affine, bb_tf) T,R,_,_ = transforms3d.affines.decompose44(new_affine) c.origin.position = T c.origin.rotation = list(transforms3d.euler.mat2euler(R, axes="sxyz")) b = Box(bb_size) c.geometry = b
def _add_payload_to_tesseract_msgs(self, payload): if not _use_tesseract: return msg=payload.payload_msg payload.attached_link=msg.header.frame_id attach_obj = AttachableObject() attach_obj.name = payload.payload_msg.name urdf_root=self.urdf.get_root() urdf_chain=self.urdf.get_chain(urdf_root, payload.attached_link, joints=True, links=False) urdf_chain.reverse() touch_links=[] touch_root = None for j_name in urdf_chain: j=self.urdf.joint_map[j_name] if j.type != "fixed": break touch_root=j.parent def _touch_recurse(touch): ret=[touch] if touch in self.urdf.child_map: for c_j,c in self.urdf.child_map[touch]: if self.urdf.joint_map[c_j].type == "fixed": ret.extend(_touch_recurse(c)) return ret if touch_root is not None: touch_links.extend(_touch_recurse(touch_root)) for i in xrange(len(msg.collision_geometry.mesh_resources)): mesh_filename=urlparse.urlparse(resource_retriever.get_filename(msg.collision_geometry.mesh_resources[i])).path mesh_pose = msg.collision_geometry.mesh_poses[i] mesh_scale=msg.collision_geometry.mesh_scales[i] mesh_scale = (mesh_scale.x, mesh_scale.y, mesh_scale.z) mesh_msg = load_mesh_file_to_mesh_msg(mesh_filename, mesh_scale) attach_obj.collision.meshes.extend(mesh_msg) attach_obj.collision.mesh_poses.extend([mesh_pose]*len(mesh_msg)) attach_obj.collision.mesh_collision_object_types.extend([Int32(0)]*len(mesh_msg)) attach_obj.visual.meshes.extend(mesh_msg) attach_obj.visual.mesh_poses.extend([mesh_pose]*len(mesh_msg)) if len(msg.collision_geometry.mesh_colors) > i: mesh_color=msg.collision_geometry.mesh_colors[i] attach_obj.visual.mesh_colors.extend([mesh_color]*len(mesh_msg)) attach_obj.collision.mesh_colors.extend([mesh_color]*len(mesh_msg)) for i in xrange(len(msg.collision_geometry.primitives)): attach_obj.collision.primitives.append(msg.collision_geometry.primitives[i]) primitive_pose = msg.collision_geometry.primitive_poses[i] attach_obj.collision.primitive_poses.append(primitive_pose) attach_obj.collision.primitive_collision_object_types.append(Int32(0)) attach_obj.visual.primitives.append(msg.collision_geometry.primitives[i]) attach_obj.visual.primitive_poses.append(primitive_pose) if len(msg.collision_geometry.mesh_colors) > i: attach_obj.collision.primitive_colors.append(msg.collision_geometry.mesh_colors[i]) attach_obj.visual.primitive_colors.append(msg.collision_geometry.mesh_colors[i]) attach_obj.inertia = msg.inertia body_info = AttachedBodyInfo() body_info.parent_link_name=msg.header.frame_id body_info.object_name=payload.payload_msg.name body_info.transform=msg.pose body_info.touch_links=touch_links #self._pub_aco.publish(aco) return attach_obj, body_info
def __init__(self, model_file=None, label_file=None, namespace='~'): # get image_trasport before ConnectionBasedTransport subscribes ~input self.transport_hint = rospy.get_param(namespace + 'image_transport', 'raw') rospy.loginfo("Using transport {}".format(self.transport_hint)) super(EdgeTPUDetectorBase, self).__init__() self.bridge = CvBridge() self.classifier_name = rospy.get_param(namespace + 'classifier_name', rospy.get_name()) model_file = rospy.get_param(namespace + 'model_file', model_file) label_file = rospy.get_param(namespace + 'label_file', label_file) if model_file is not None: self.model_file = get_filename(model_file, False) if label_file is not None: label_file = get_filename(label_file, False) self.duration = rospy.get_param(namespace + 'visualize_duration', 0.1) self.enable_visualization = rospy.get_param( namespace + 'enable_visualization', True) device_id = rospy.get_param(namespace + 'device_id', None) if device_id is None: device_path = None else: device_path = ListEdgeTpuPaths(EDGE_TPU_STATE_NONE)[device_id] assigned_device_paths = ListEdgeTpuPaths(EDGE_TPU_STATE_ASSIGNED) if device_path in assigned_device_paths: rospy.logwarn('device {} is already assigned: {}'.format( device_id, device_path)) self.engine = DetectionEngine(self.model_file, device_path=device_path) if label_file is None: self.label_ids = None self.label_names = None else: self.label_ids, self.label_names = self._load_labels(label_file) self.pub_rects = self.advertise(namespace + 'output/rects', RectArray, queue_size=1) self.pub_class = self.advertise(namespace + 'output/class', ClassificationResult, queue_size=1) # visualize timer if self.enable_visualization: self.lock = threading.Lock() self.pub_image = self.advertise(namespace + 'output/image', Image, queue_size=1) self.pub_image_compressed = self.advertise( namespace + 'output/image/compressed', CompressedImage, queue_size=1) self.timer = rospy.Timer(rospy.Duration(self.duration), self.visualize_cb) self.img = None self.header = None self.bboxes = None self.labels = None self.scores = None
def __init__(self, cname='camera', url='', namespace=''): self.cname = cname self.file = rr.get_filename(url, use_protocol=False) self.camera_info = loadCalibrationFile(self.file, self.cname) self.svc = rospy.Service(namespace + 'set_camera_info', SetCameraInfo, self.setCameraInfo)