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)
