def get_pkg_dir_from_prefix(path, ros_package_path=None):
if RE_PREFIXED_TAG not in path:
return path
splitpath = path.split(RE_PREFIXED_END_TAG)
after_prefix_path = splitpath[-1]
prefix = splitpath[0].split(RE_PREFIXED_BEGIN_TAG)[-1]
if prefix == DEFAULT_PREFIX_NAME:
if ros_package_path is not None:
rpath = ros_package_path+after_prefix_path
else:
raise ROSPkgException("Unknown prefixed ${prefix} path ! not associated to ros package path !")
elif prefix == ROS_WS_PREFIX_NAME:
rpath = get_ros_workspace_dir()+after_prefix_path
elif prefix == ROS_WS_SRC_PREFIX_NAME:
print get_ros_workspace_src_dir(),after_prefix_path
rpath = get_ros_workspace_src_dir()+after_prefix_path
else:
rpath = get_pkg_dir(prefix)+after_prefix_path
return rpath
def setDirectoryNames(self):
"""
Sets the dictionary database movement dancing reem structure.
"""
base = packages.get_pkg_dir(PACKAGE_WHERE_TO_FIND_DANCING_MOVEMENTS)
dbdir = os.path.join(base, DANCING_DATABASE_DIR)
self.DANGEROUS_DIR = os.path.join(dbdir, 'dangerous')
self.MIDDLE_DIR = os.path.join(dbdir, 'middle')
self.UP_DIR = os.path.join(dbdir, 'up')
self.SIDES_DIR = os.path.join(dbdir, 'sides')
self.INITIAL_DIR = os.path.join(dbdir, 'initial')
self.listOfDirectories = {'dangerous': self.DANGEROUS_DIR,
'middle': self.MIDDLE_DIR,
'up': self.UP_DIR,
'sides': self.SIDES_DIR,
'initial': self.INITIAL_DIR}
self.movements = {'dangerous': filter(lambda x: x.endswith('.xml'), os.listdir(self.DANGEROUS_DIR)),
'middle': filter(lambda x: x.endswith('.xml'), os.listdir(self.MIDDLE_DIR)),
'up': filter(lambda x: x.endswith('.xml'), os.listdir(self.UP_DIR)),
'sides': filter(lambda x: x.endswith('.xml'), os.listdir(self.SIDES_DIR)),
'initial': filter(lambda x: x.endswith('.xml'), os.listdir(self.INITIAL_DIR))}
self.previousMov = ''
def resolve(self, string, arg_dict):
"""Resolves $() substitutions in the roslaunch
"""
original = string
regex = r'\$\(([^)]+)\)'
match = re.search(regex, string)
while match is not None:
full_match = match.group(0)
m = match.group(1)
keyword = m.split(' ')[0]
substitution = full_match.replace('$(', '$_(') #default substitution
if keyword == 'arg':
arg_name = m.split(' ')[1]
if arg_name in arg_dict:
substitution = arg_dict[arg_name]
else:
print 'Argument %s not set' % arg_name
elif keyword == 'env':
env_variable = m.split(' ')[1]
if env_variable in os.environ:
substitution = os.environ[env_variable]
else:
print 'Environment variable %s not set' % env_variable
elif keyword == 'find':
package_name = m.split(' ')[1]
try:
package_path = get_pkg_dir(package_name)
substitution = package_path
except InvalidROSPkgException:
print 'Could not find pacakage %s' % package_name
string = string.replace(full_match, substitution)
match = re.search(regex, string)
return {'resolved':string, 'variable':original}
def __init__(self, pgm_grasp_type=True):
self.pgm_grasp_type = pgm_grasp_type
self.isrr_limit = False #True
self.hand_config_frame_id = None
pkg_path = rp.get_pkg_dir('prob_grasp_planner')
self.pca_model_path = pkg_path + '/train_models_sim/pca/pca.model'
self.train_model_path = pkg_path + '/train_models_sim/classifiers/'
self.prior_path = pkg_path + '/train_models_sim/priors/'
self.non_type_prior = True
self.load_learned_models()
#Use the computer tars or t
self.tars = True #False
if self.tars:
self.grasp_config_log_save_path = '/media/kai/logs/multi_finger_sim_data/grad_des_ls_log/'
else:
self.grasp_config_log_save_path = '/dataspace/data_kai/logs/multi_finger_sim_data/grad_des_ls_log/'
self.iter_total_num = 1000 #500
self.log_inf = True
#The regularization value is related with both the
#data (voxel, config) dimension and the number of GMM components.
self.reg_log_prior = 0.1 #1.
#self.reg_log_prior = 0.01
#self.reg_log_prior = 0.05
#Regularization for log likelihood, this is mainly used to
#test the inference with only prior.
self.reg_log_lkh = 1.
self.q_max = 2.
self.q_min = -2.
def create_ikfast_package(args):
try:
setattr(args, 'ikfast_plugin_pkg_path', get_pkg_dir(args.ikfast_plugin_pkg))
except InvalidROSPkgException:
args.ikfast_plugin_pkg_path = os.path.abspath(args.ikfast_plugin_pkg)
print("Failed to find package: %s. Will create it in %s." %
(args.ikfast_plugin_pkg, args.ikfast_plugin_pkg_path))
# update pkg name to basename of path
args.ikfast_plugin_pkg=os.path.basename(args.ikfast_plugin_pkg_path)
src_path = args.ikfast_plugin_pkg_path + '/src/'
if not os.path.exists(src_path):
os.makedirs(src_path)
include_path = args.ikfast_plugin_pkg_path + '/include/'
if not os.path.exists(include_path):
os.makedirs(include_path)
# Create package.xml
pkg_xml_path = args.ikfast_plugin_pkg_path + '/package.xml'
if not os.path.exists(pkg_xml_path):
root = xmlElement("package", format="2")
root.append(xmlElement("name", text=args.ikfast_plugin_pkg))
root.append(xmlElement("version", text="0.0.0"))
root.append(xmlElement("description", text="IKFast plugin for " + args.robot_name))
root.append(xmlElement("license", text="BSD"))
user_name = getuser()
root.append(xmlElement("maintainer", email="*****@*****.**" % user_name, text=user_name))
root.append(xmlElement("buildtool_depend", text="catkin"))
with open(pkg_xml_path, 'w') as f:
etree.ElementTree(root).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8")
print("Created package.xml at: '%s'" % pkg_xml_path)
def create_ikfast_package(args):
try:
setattr(args, 'ikfast_plugin_pkg_path', get_pkg_dir(args.ikfast_plugin_pkg))
except InvalidROSPkgException:
args.ikfast_plugin_pkg_path = os.path.abspath(args.ikfast_plugin_pkg)
print("Failed to find package: %s. Will create it in %s." %
(args.ikfast_plugin_pkg, args.ikfast_plugin_pkg_path))
# update pkg name to basename of path
args.ikfast_plugin_pkg=os.path.basename(args.ikfast_plugin_pkg_path)
src_path = args.ikfast_plugin_pkg_path + '/src/'
if not os.path.exists(src_path):
os.makedirs(src_path)
include_path = args.ikfast_plugin_pkg_path + '/include/'
if not os.path.exists(include_path):
os.makedirs(include_path)
# Create package.xml
pkg_xml_path = args.ikfast_plugin_pkg_path + '/package.xml'
if not os.path.exists(pkg_xml_path):
root = xmlElement("package", format="2")
root.append(xmlElement("name", text=args.ikfast_plugin_pkg))
root.append(xmlElement("version", text="0.0.0"))
root.append(xmlElement("description", text="IKFast plugin for " + args.robot_name))
root.append(xmlElement("license", text="BSD"))
user_name = getuser()
root.append(xmlElement("maintainer", email="*****@*****.**" % user_name, text=user_name))
root.append(xmlElement("buildtool_depend", text="catkin"))
with open(pkg_xml_path, 'w') as f:
etree.ElementTree(root).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8")
print("Created package.xml at: '%s'" % pkg_xml_path)
def SelectRandomMP3FileFromMp3Lib():
"""
Selects radomly a song in the mp3_lib dir in the dancing_reem package.
It now take mp3 files and wav files. We dont get .ogg because soundstretch doesnt.
"""
base = packages.get_pkg_dir(DANCING_REEM_PKG)
path_to_mp3_lib = os.path.join(base, NAME_MP3_LIBRARY_DIR)
list_mp3_songs_in_mp3lib = filter(lambda x: x.endswith('.mp3'), os.listdir(path_to_mp3_lib))
list_wav_songs_in_mp3lib = filter(lambda x: x.endswith('.wav'), os.listdir(path_to_mp3_lib))
list_songs_in_mp3lib = list_mp3_songs_in_mp3lib + list_wav_songs_in_mp3lib
print "Song LIST" + str(list_songs_in_mp3lib)
if len(list_songs_in_mp3lib) == 0:
rospy.loginfo("###### The MP3 Library is EMPTY ######")
song_path = 'None'
else:
random_song_name = list_songs_in_mp3lib[randint(0, len(list_songs_in_mp3lib) - 1)]
print "SELECTED SONG: " + str(random_song_name)
song_path = os.path.join(path_to_mp3_lib, random_song_name)
return song_path
def __init__(self, node_uri, node_name):
rsc = os.path.join(get_pkg_dir('airbus_plugin_node_manager'),
'resources')
self._icon_node_start = QIcon(rsc + '/start.png')
self._icon_node_stop = QIcon(rsc + '/stop.png')
self.uri = QLabel(node_uri)
self.uri.setContentsMargins(0, 0, 10, 0)
self.uri.setMinimumHeight(40)
self.name = QLabel(node_name)
self.name.setContentsMargins(0, 0, 10, 0)
self.name.setMinimumHeight(40)
self.status = QLabel(self.RUNNING)
self.status.setStyleSheet('qproperty-alignment: AlignCenter;')
self.status.setMinimumSize(QSize(100, 40))
self.ping = QLabel('...')
self.ping.setStyleSheet("qproperty-alignment: AlignCenter;")
self.button_start_stop = QPushButton()
self.button_start_stop.setIcon(self._icon_node_stop)
self.button_start_stop.setIconSize(QSize(30, 30))
self.button_start_stop.setFixedSize(QSize(100, 40))
self.button_start_stop_widget = widget_creator(self.button_start_stop)
if node_name not in self.NODE_EXCEPTION:
self.button_start_stop.clicked.connect(self.start_stop_slot)
self.button_start_stop.setEnabled(False)
self.current_status = self.NONE
def cmd_create_launch(data_central, argv):
'''Creates ROS launch files for all combinations of agents and robots.'''
parser = OptionParser(prog='create-launch',
usage=cmd_create_launch.short_usage)
parser.disable_interspersed_args()
parser.add_option("-a", "--agent", dest='id_agent', help="Agent ID")
parser.add_option("-r", "--robot", dest='id_robot', help="Robot ID")
parser.add_option('-p', '--package', dest='package',
default='bootstrapping_adapter',
help='Which ROS package to put the launch file in [%default].')
# TODO: bag
(options, args) = parser.parse_args(argv)
check_no_spurious(args)
bo_config = data_central.get_bo_config()
if options.id_agent is None:
id_agents = bo_config.agents.keys()
else:
id_agents = [options.id_agent] # TODO: expand
if options.id_robot is None:
id_robots = bo_config.robots.keys()
else:
id_robots = [options.id_robot] # TODO: expand
for id_agent in id_agents:
check_contained(id_agent, bo_config.agents)
for id_robot in id_robots:
check_contained(id_robot, bo_config.robots)
from roslib import packages #@UnresolvedImport
out = packages.get_pkg_dir(options.package)
outdir = os.path.join(out, 'launch', 'boot_olympics')
root = os.path.realpath(data_central.root)
for id_robot, id_agent in itertools.product(id_robots, id_agents):
robot_ros_node = wrap_python_robot(bo_config.robots[id_robot], root)
agent_ros_node = wrap_python_agent(bo_config.agents[id_agent], root)
xml = create_launch_xml(agent_ros_node=agent_ros_node,
robot_ros_node=robot_ros_node,
agent_node_id='my_agent',
robot_node_id='my_robot',
namespace='boot_olympics',
bag=None,
output=None)
basename = '%s-%s' % (id_robot, id_agent)
filename = os.path.join(outdir, '%s.launch' % basename)
make_sure_dir_exists(filename)
with open(filename, 'w') as f:
f.write('<!-- Created by %s on %s -->\n' %
('boot_olympics', isodate()))
f.write(xml)
logger.info('Writing on %r.' % filename)
def __init__(self):
super(WalkingModule, self).__init__(module_name='walking_module')
self.is_initial_walk = False
# Initialize OP3 ROS publishers
self.set_walking_command_pub = rospy.Publisher(
'/robotis/walking/command', String, queue_size=0)
self.set_walking_param_pub = rospy.Publisher(
'/robotis/walking/set_params', WalkingParam, queue_size=0)
self.module_control_pub = rospy.Publisher(
'/robotis/enable_ctrl_module', String, queue_size=0)
# Initialize configuration param variables
self.current_walking_param = WalkingParam()
self.initial_param = None
self.marching_param = None
self.forward_param = None
self.backward_param = None
self.turn_left_param = None
self.turn_right_param = None
# Load configurations
config_file_path = os.path.join(rospkg.get_pkg_dir('obstacle_run'),
'config',
'walking_configurations.json')
self.load_configuration(file_path=config_file_path)
def __init__(self):
rospy.init_node("declination_provider")
wmm_path = path.join(get_pkg_dir('declination'), "wmm", "WMM.COF")
self.gm = GeoMag(wmm_path)
self.fix = None
self.sub = rospy.Subscriber("fix", NavSatFix, self._fix)
self.pub = rospy.Publisher("declination", Float32, latch=True)
def find_template_dir():
for candidate in [os.path.dirname(__file__) + '/../templates']:
if os.path.exists(candidate) and os.path.exists(candidate + '/ikfast.h'):
return os.path.realpath(candidate)
try:
return os.path.join(get_pkg_dir(plugin_gen_pkg), 'ikfast_kinematics_plugin/templates')
except InvalidROSPkgException:
raise Exception("Can't find package %s" % plugin_gen_pkg)
def find_template_dir():
for candidate in [os.path.dirname(__file__) + '/../templates']:
if os.path.exists(candidate) and os.path.exists(candidate + '/ikfast.h'):
return os.path.realpath(candidate)
try:
return os.path.join(get_pkg_dir(plugin_gen_pkg), 'ikfast_kinematics_plugin/templates')
except InvalidROSPkgException:
raise Exception("Can't find package %s" % plugin_gen_pkg)
def execute(self, userdata):
absolute_path_mov_name = os.path.join(packages.get_pkg_dir(NAME_OF_PKG_PLAY_BALL_DEMO), 'scripts/stop_demo.sh')
print str(absolute_path_mov_name)
assert (os.path.exists(absolute_path_mov_name)), "The path to the stop_demo.sh file doesn't exist"
absolute_path_mov_name
rospy.loginfo("STOPPED PLAY_BALL_DEMO.SH ")
return succeeded
def initializeCommandVariables(userdata):
global follow_grammar_name_index
follow_grammar_name_index = 0
try:
global bag
bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag', 'w')
finally:
debugPrint("Odometry bag opened", 3)
return succeeded
def __init__(self):
# Parameters
self.grasp_patch_rows = 400
self.grasp_patch_cols = 400
self.rgbd_channels = 8
self.rate_dec_epochs = 1000 #400
self.training_epochs = 3000 #1200
self.batch_size = 8 #16
self.display_step = 10 #0
self.dropout_prob = 0.75 #0.9
self.config_dim = 14 #11
#self.read_data_ratio = 0.25
#self.read_batch_epochs = 5000
self.read_data_ratio = 0.5
self.read_batch_epochs = 5000
self.classes_num = 1
#rgbd_patches_save_path = '/data_space/data_kai/multi_finger_exp_data'
rgbd_patches_save_path = '/mnt/tars_data/multi_finger_sim_data_complete_v4/'
self.grasp_patches_file_path = rgbd_patches_save_path + 'grasp_patches.h5'
self.grasp_data_file_path = rgbd_patches_save_path + 'grasp_data.h5'
self.logs_path = '/home/qingkai/tf_logs/multi_finger_sim_data'
pkg_path = rp.get_pkg_dir('prob_grasp_planner')
self.cnn_model_path = pkg_path + '/models/grasp_cnn_planner/' + \
'models/cnn_rgbd_config.ckpt'
self.prior_path = pkg_path + '/models/grasp_cnn_planner/priors/'
# tf Graph input
self.holder_grasp_patch = tf.placeholder(tf.float32, [
None, self.grasp_patch_rows, self.grasp_patch_cols,
self.rgbd_channels
],
name='holder_grasp_patch')
self.holder_config = tf.placeholder(tf.float32,
[None, self.config_dim],
name='holder_config')
self.holder_labels = tf.placeholder(tf.float32,
[None, self.classes_num],
name='holder_labels')
self.keep_prob = tf.placeholder(tf.float32, name='holder_keep_prob')
self.learning_rate = tf.placeholder(tf.float32,
name='holder_learn_rate')
self.train_samples_pct = 0.8
self.train_samples_num = -1
self.testing_samples_num = -1
self.training_labels = None
self.training_grasp_samples = None
self.training_samples_fingers = None
self.testing_labels = None
self.testing_grasp_samples = None
self.testing_samples_fingers = None
self.read_data_batch = False
self.alpha_ridge = 0.5
#Need to change this to false for cross validation.
self.use_all_data_train = True
def generateRosMessages(toc):
""" Generates the *.msg files for ROS from the TOC
"""
if not toc:
rospy.logwarn("No TOC available to generate ROS messages from")
return
path = get_pkg_dir('crazyflieROS')+"/msg/"
for g in toc.keys():
makeMsg(g, path, toc[g] )
def _parse(self, launch_file, digraph):
launch_name = launch_file.split('/')[-1].split('.')[0]
root = None
try:
root = ElementTree.parse(launch_file)
except Exception as ex:
html.HTMLException(ex, self)
return
tree = root.getroot()
for elem in tree:
#<include file="$(find pma_startup)/launch/pma1_driver.launch" />
if elem.tag == 'include':
include_split = elem.attrib["file"].split(")")
prefix = include_split[0]
pkg_name = prefix.split(" ")[-1]
child = include_split[-1].split("/")[-1].split('.')[0]
n1 = NODE(launch_name)
n1.setAttrib(NODE.SHAPE, SHAPE.Box)
n1.setAttrib(NODE.STYLE, STYLE.FILLED)
n1.setAttrib(NODE.COLOR, RgbColor.LightGray)
digraph.addNode(n1)
n2 = NODE(child)
n2.setAttrib(NODE.SHAPE, SHAPE.Box)
n2.setAttrib(NODE.STYLE, STYLE.FILLED)
n2.setAttrib(NODE.COLOR, RgbColor.LightGray)
digraph.addNode(n2)
digraph.connect(n1, n2)
sub_launch = get_pkg_dir(pkg_name) + include_split[-1]
self._parse(sub_launch, digraph)
#<node name="map_server_high_res" pkg="map_server" type="map_server" args="$(arg map_file_high_res)" respawn="false">
elif elem.tag == "node":
parent = launch_name
child = Prefix.arg(elem.attrib["name"], tree)
if parent == child:
child += "_node"
nd = NODE(child)
nd.setAttrib(NODE.SHAPE, SHAPE.Ellipse)
nd.setAttrib(NODE.STYLE, STYLE.FILLED)
nd.setAttrib(NODE.COLOR, RgbColor.LightSeaGreen)
digraph.addNode(nd)
digraph.connect(parent, nd)
def create_buttons(self):
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
file_name=rospy.get_param('xml_button_file', None)
file_name_pkg=rospy.get_param('xml_button_file_pkg', None)
if file_name==None:
file_name =unicode(get_pkg_dir("python_gui"))+"/xml/default.xml"
elif file_name_pkg != None:
file_name = unicode(get_pkg_dir(file_name_pkg))+file_name
print "file name:\n"+file_name
file_map = etree.parse(file_name)
list_event = file_map.getroot()
grid = QtGui.QGridLayout()
i=0
for child in list_event:
self.string_map[child.get('name')]=child.get('event')
btn=QtGui.QPushButton(child.get('name'), self)
grid.addWidget(btn,i,0)
btn.clicked.connect(self.buttonClicked)
tooltip_text=child.get('tooltip')
if tooltip_text!=None:
btn.setToolTip(tooltip_text)
i=i+1
#layout: a vbox divided in 3 parts
# the grid, a Stretch to fill in the space and a status bar
vbox = QtGui.QVBoxLayout()
vbox.addLayout(grid)
self.statBar = QtGui.QStatusBar(self)
vbox.addStretch(1)
vbox.addWidget(self.statBar)
self.statBar.showMessage("Event sender by GB")
self.setLayout(vbox)
self.setGeometry(300, 300, 290, 150)
self.move(1500, 150)
self.setWindowTitle('Event sender')
self.setWindowIcon(QtGui.QIcon(unicode(get_pkg_dir("python_gui")) + '/resources/es_ico.png'))
self.show()
def test_params(self):
mock = self._load(os.path.join(self.xml_dir, 'test-params-valid.xml'))
param_d = {}
for p in mock.params:
param_d[p.key] = p.value
self.assertEquals('pass', param_d['/override'])
self.assertEquals('bar2', param_d['/somestring1'])
self.assertEquals('10', param_d['/somestring2'])
self.assertEquals(1, param_d['/someinteger1'])
self.assertEquals(2, param_d['/someinteger2'])
self.assertAlmostEquals(3.14159, param_d['/somefloat1'], 2)
self.assertAlmostEquals(5.0, param_d['/somefloat2'], 1)
self.assertEquals("a child namespace parameter 1",
param_d['/wg/wgchildparam'], p.value)
self.assertEquals("a child namespace parameter 2",
param_d['/wg2/wg2childparam1'], p.value)
self.assertEquals("a child namespace parameter 3",
param_d['/wg2/wg2childparam2'], p.value)
import xmlrpclib
from roslib.packages import get_pkg_dir
f = open(os.path.join(get_pkg_dir('roslaunch'), 'example.launch'))
try:
contents = f.read()
finally:
f.close()
p = [p for p in mock.params if p.key == '/configfile'][0]
self.assertEquals(contents, p.value, 1)
p = [p for p in mock.params if p.key == '/binaryfile'][0]
self.assertEquals(xmlrpclib.Binary(contents), p.value, 1)
f = open(
os.path.join(get_pkg_dir('roslib'), 'scripts',
'python-reserved-words.txt'))
try:
contents = f.read()
finally:
f.close()
p = [p for p in mock.params if p.key == '/commandoutput'][0]
self.assertEquals(contents, p.value, 1)
def get_pkg_dir_from_prefix(path, ros_package_path=None):
if RE_PREFIXED_TAG not in path:
return path
splitpath = path.split(RE_PREFIXED_END_TAG)
after_prefix_path = splitpath[-1]
prefix = splitpath[0].split(RE_PREFIXED_BEGIN_TAG)[-1]
rpath = get_pkg_dir(prefix)+after_prefix_path
return rpath
def create_buttons(self):
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
file_name = rospy.get_param('xml_button_file', None)
file_name_pkg = rospy.get_param('xml_button_file_pkg', None)
if file_name == None:
file_name = unicode(get_pkg_dir("python_gui")) + "/xml/default.xml"
elif file_name_pkg != None:
file_name = unicode(get_pkg_dir(file_name_pkg)) + file_name
print "file name:\n" + file_name
file_map = etree.parse(file_name)
list_event = file_map.getroot()
grid = QtGui.QGridLayout()
i = 0
for child in list_event:
self.string_map[child.get('name')] = child.get('event')
btn = QtGui.QPushButton(child.get('name'), self)
grid.addWidget(btn, i, 0)
btn.clicked.connect(self.buttonClicked)
tooltip_text = child.get('tooltip')
if tooltip_text != None:
btn.setToolTip(tooltip_text)
i = i + 1
#layout: a vbox divided in 3 parts
# the grid, a Stretch to fill in the space and a status bar
vbox = QtGui.QVBoxLayout()
vbox.addLayout(grid)
self.statBar = QtGui.QStatusBar(self)
vbox.addStretch(1)
vbox.addWidget(self.statBar)
self.statBar.showMessage("Event sender by GB")
self.setLayout(vbox)
self.setGeometry(300, 300, 290, 150)
self.move(1500, 150)
self.setWindowTitle('Event sender')
self.show()
def parse_find_keyword(self, string):
match = re.match(r'.*\$\(find (\S+)\).*', string)
if match is not None:
package_name = match.group(1)
try:
package_path = get_pkg_dir(package_name)
except InvalidROSPkgException:
self.add_issue('cannot find package %s' % (package_name))
return None
string = re.sub(r'\$\(find \S+\)', package_path, string)
return string
def getStandAloneInstance(pkg_name, dashboard_class, lng="en"):
dashboard_instance = dashboard_class(Context(QMainWindow()))
dashboard_descriptor = ET.parse("%s/dashboard_descriptor.xml"%get_pkg_dir(pkg_name))
dashboard_params = DashboardProvider.getParameters(dashboard_descriptor.getroot(), None)
dashboard_instance.setup(dashboard_descriptor, dashboard_params)
dashboard_instance.onTranslate(lng)
return dashboard_instance
def find(path):
if Prefix.is_prefixed(path):
try:
key, value, rex = Prefix.get_prefix(path)
if key == Prefix.KEY_FIND:
return get_pkg_dir(value)+rex
else:
raise Exception("Bad prefixed key '%s'"%key)
except Exception as ex:
raise ex
else:
return path
def update_moveit_package(args):
try:
moveit_config_pkg_path = get_pkg_dir(args.moveit_config_pkg)
except InvalidROSPkgException:
raise Exception("Failed to find package: " + args.moveit_config_pkg)
try:
srdf_file_name = moveit_config_pkg_path + "/config/" + args.srdf_filename
srdf = etree.parse(srdf_file_name).getroot()
except IOError:
raise Exception("Failed to find SRDF file: " + srdf_file_name)
except etree.XMLSyntaxError as err:
raise Exception(
"Failed to parse xml in file: %s\n%s" % (srdf_file_name, err.msg)
)
if args.robot_name_in_srdf != srdf.get("name"):
raise Exception(
"Robot name in srdf ('%s') doesn't match expected name ('%s')"
% (srdf.get("name"), args.robot_name_in_srdf)
)
groups = srdf.findall("group")
if len(groups) < 1:
raise Exception("No planning groups are defined in the SRDF")
planning_group = None
for group in groups:
if group.get("name").lower() == args.planning_group_name.lower():
planning_group = group
if planning_group is None:
raise Exception(
"Planning group '%s' not defined in the SRDF. Available groups: \n%s"
% (
args.planning_group_name,
", ".join([group_name.get("name") for group_name in groups]),
)
)
# Modify kinematics.yaml file
kin_yaml_file_name = moveit_config_pkg_path + "/config/kinematics.yaml"
with open(kin_yaml_file_name, "r") as f:
kin_yaml_data = yaml.safe_load(f)
kin_yaml_data[args.planning_group_name]["kinematics_solver"] = args.plugin_name
with open(kin_yaml_file_name, "w") as f:
yaml.dump(kin_yaml_data, f, default_flow_style=False)
print("Modified kinematics.yaml at '%s'" % kin_yaml_file_name)
def MoveNameToPath(movement_name, current_pos):
"""
Given a movement name it returns the absolute path to that file.
In Case it doesn't exist, it will raise an assertion.
"""
base_package_path = packages.get_pkg_dir(PACKAGE_WHERE_TO_FIND_DANCING_MOVEMENTS)
aux_database_dir_path = os.path.join(base_package_path, DANCING_DATABASE_DIR)
aux_area_dir_path = os.path.join(aux_database_dir_path, current_pos)
absolute_path_mov_name = os.path.join(aux_area_dir_path, movement_name)
print str(absolute_path_mov_name)
assert (os.path.exists(absolute_path_mov_name)), "The path to the movement file doesn't exist"
return absolute_path_mov_name
def test_params(self):
mock = self._load(os.path.join(self.xml_dir, 'test-params-valid.xml'))
param_d = {}
for p in mock.params:
param_d[p.key] = p.value
self.assertEquals('pass', param_d['/override'])
self.assertEquals('bar2', param_d['/somestring1'])
self.assertEquals('10', param_d['/somestring2'])
self.assertEquals(1, param_d['/someinteger1'])
self.assertEquals(2, param_d['/someinteger2'])
self.assertAlmostEquals(3.14159, param_d['/somefloat1'], 2)
self.assertAlmostEquals(5.0, param_d['/somefloat2'], 1)
self.assertEquals("a child namespace parameter 1", param_d['/wg/wgchildparam'], p.value)
self.assertEquals("a child namespace parameter 2", param_d['/wg2/wg2childparam1'], p.value)
self.assertEquals("a child namespace parameter 3", param_d['/wg2/wg2childparam2'], p.value)
import xmlrpclib
from roslib.packages import get_pkg_dir
f = open(os.path.join(get_pkg_dir('roslaunch'), 'example.launch'))
try:
contents = f.read()
finally:
f.close()
p = [p for p in mock.params if p.key == '/configfile'][0]
self.assertEquals(contents, p.value, 1)
p = [p for p in mock.params if p.key == '/binaryfile'][0]
self.assertEquals(xmlrpclib.Binary(contents), p.value, 1)
f = open(os.path.join(get_pkg_dir('roslaunch'), 'example.launch'))
try:
contents = f.read()
finally:
f.close()
p = [p for p in mock.params if p.key == '/commandoutput'][0]
self.assertEquals(contents, p.value, 1)
def getStandAloneInstance(pkg_name, plugin_class, lng="en"):
plugin_instance = plugin_class(Context(QMainWindow()))
plugin_descriptor = ET.parse("%s/plugin_descriptor.xml"%get_pkg_dir(pkg_name))
plugin_params = PluginProvider.getParameters(plugin_descriptor.getroot(), None)
plugin_instance.tryToCreate(plugin_params)
plugin_instance.tryToResume()
plugin_instance.onTranslate(lng)
return plugin_instance
def _parse(self, launch, dot):
self._launch_directory = launch
launch_name = launch.split('/')[-1].split('.')[0]
root = None
try:
root = ElementTree.parse(self._launch_directory)
except:
raise Exception("Invalid launcher path from %s !"%self._launch_directory)
tree = root.getroot()
for elem in tree:
#<include file="$(find pma_startup)/launch/pma1_driver.launch" />
if elem.tag == 'include':
include_split = elem.attrib["file"].split(")")
prefix = include_split[0]
pkg_name = prefix.split(" ")[-1]
child = include_split[-1].split("/")[-1].split('.')[0]
dot.write('%s [shape=box, style=filled, color=lightgray];\n'%launch_name)
dot.write('%s [shape=box, style=filled, color=lightgray];\n'%child)
dot.write("%s -> %s\n"%(launch_name, child))
sub_launch = get_pkg_dir(pkg_name)+include_split[-1]
self._parse(sub_launch, dot)
#<node name="map_server_high_res" pkg="map_server" type="map_server" args="$(arg map_file_high_res)" respawn="false">
elif elem.tag == "node":
parent = launch_name
child = Prefix.arg(elem.attrib["name"],tree)
if parent == child:
child += "_node"
dot.write('%s [shape=ellipse, style=filled, color=lightseagreen];\n'%child)
dot.write("%s -> %s\n"%(parent,child))
def update_moveit_package(args):
try:
moveit_config_pkg_path = get_pkg_dir(args.moveit_config_pkg)
except InvalidROSPkgException:
raise Exception("Failed to find package: " + args.moveit_config_pkg)
try:
srdf_file_name = moveit_config_pkg_path + '/config/' + args.srdf_filename
srdf = etree.parse(srdf_file_name).getroot()
except IOError:
raise Exception("Failed to find SRDF file: " + srdf_file_name)
except etree.XMLSyntaxError as err:
raise Exception("Failed to parse xml in file: %s\n%s" % (srdf_file_name, err.msg))
if args.robot_name_in_srdf != srdf.get('name'):
raise Exception("Robot name in srdf ('%s') doesn't match expected name ('%s')" %
(srdf.get('name'), args.robot_name_in_srdf))
groups = srdf.findall('group')
if len(groups) < 1:
raise Exception("No planning groups are defined in the SRDF")
planning_group = None
for group in groups:
if group.get('name').lower() == args.planning_group_name.lower():
planning_group = group
if planning_group is None:
raise Exception("Planning group '%s' not defined in the SRDF. Available groups: \n%s" % (
args.planning_group_name, ", ".join([group_name.get('name') for group_name in groups])))
# Modify kinematics.yaml file
kin_yaml_file_name = moveit_config_pkg_path + "/config/kinematics.yaml"
with open(kin_yaml_file_name, 'r') as f:
kin_yaml_data = yaml.safe_load(f)
kin_yaml_data[args.planning_group_name]["kinematics_solver"] = args.plugin_name
with open(kin_yaml_file_name, 'w') as f:
yaml.dump(kin_yaml_data, f, default_flow_style=False)
print("Modified kinematics.yaml at '%s'" % kin_yaml_file_name)
def __init__(self, launch, machine):
self.launch_name = QLabel(launch)
self.launch_name.setContentsMargins(0, 0, 10, 0)
self.launch_name.setMinimumHeight(40)
self.combo_machines = QComboBox()
self.combo_machines.setMinimumHeight(40)
self.combo_machines.addItem('cobotgui-dev:127.0.0.1')
self.combo_machines.addItem('cobot:192.168.0.1')
rsc = os.path.join(get_pkg_dir('plugin_node_manager'), 'resources')
icon_launch = QIcon(rsc + '/launch.png')
self.button_launch = QPushButton()
self.button_launch.setIcon(icon_launch)
self.button_launch.setIconSize(QSize(30, 30))
self.button_launch.setFixedSize(QSize(100, 40))
self.button_launch.clicked.connect(self._launch_node_slot)
self.button_launch_widget = widget_creator(self.button_launch)
def arms_out_of_self_colision(self):
self._print_title("ARMS OUT OF SELF COLISION")
if self.using_the_robot:
if self.__check_action(ARMS_ACTION_NAME) == aborted:
return aborted
else:
self._print_warning("Not checking. ROS_MASTER_URI nor COMPUTER_NAME contains %s" % ROBOTS_NAME)
return aborted
robot = os.environ.get('PAL_ROBOT')
ros_master_uri = os.environ.get('ROS_MASTER_URI')
remotelly_executing = ros_master_uri.rfind('localhost')
rospy.loginfo('remotelly_executing: %s' % remotelly_executing) # FIXME: Remove this line after test in the robot
MOTION_FOLDER_PATH = ''
plan = False
checkSafety = False
if robot == 'rh2c' or robot == 'rh2m' or robot == 'reemh3c' or robot == 'reemh3m' or robot == 'reemh3' or remotelly_executing == -1:
check_for_door = True # FIXME: This variable is not being used.
MOTION_FOLDER_PATH = "/mnt_flash/stacks/reem_alive/alive_engine/config/database/Stopped/interact_1.xml"
if remotelly_executing == -1:
MOTION_FOLDER_PATH = packages.get_pkg_dir('pal_smach_utils') + '/config/interact_1.xml'
plan = True
checkSafety = False
motion_goal = MotionManagerGoal()
motion_goal.plan = plan
motion_goal.filename = MOTION_FOLDER_PATH
motion_goal.checkSafety = checkSafety
motion_goal.repeat = False
motion_manager_action = SimpleActionState(ARMS_ACTION_NAME, MotionManagerAction, goal=motion_goal)
userdata_hacked = {}
status = motion_manager_action.execute(userdata_hacked)
if status == succeeded:
rospy.loginfo(self.colors.GREEN_BOLD + "Arms out of self colisin: OK " + self.colors.NATIVE_COLOR)
else:
self.ALL_OK = False
rospy.loginfo(self.colors.RED_BOLD + "Arms out of self colisin: Failed " + self.colors.NATIVE_COLOR)
def __init__(self, gui):
self._gui = gui
self._datamodel = QStandardItemModel(0, 3)
self._gui._table_launch.setModel(self._datamodel)
self._gui._table_launch.verticalHeader().setVisible(False)
node_launchers_path = '/'.join(
[get_pkg_dir('node_launchers'), 'launch'])
self._launch_list = []
# self._gui._table_launch.horizontalHeader().setResizeMode(QHeaderView.Stretch)
i = 0
for launch_file in os.listdir(node_launchers_path):
self._add_launcher(launch_file, '/cobotgui-dev', i)
i += 1
self._gui._table_launch.resizeColumnsToContents()
self._gui._table_launch.resizeRowsToContents()
def _manifest_msg_srv_export(ctx, type_):
exist = []
for pkg in ctx.pkgs:
pkg_dir = roslib.packages.get_pkg_dir(pkg)
d = os.path.join(pkg_dir, type_)
if os.path.isdir(d):
files = os.listdir(d)
if filter(lambda x: x.endswith('.'+type_), files):
try:
m_file = roslib.manifest.manifest_file(pkg, True)
except InvalidROSPkgException:
# ignore wet package from further investigation
env = os.environ
pkg_path = get_pkg_dir(pkg, True, ros_root=env['ROS_ROOT'])
if os.path.exists(os.path.join(pkg_path, PACKAGE_FILE)):
continue
raise
m = roslib.manifest.parse_file(m_file)
cflags = m.get_export('cpp', 'cflags')
include = '-I${prefix}/%s/cpp'%type_
if filter(lambda x: include in x, cflags):
exist.append(pkg)
return exist
def __init__(self):
self.query_batch_num = 3
pkg_path = rp.get_pkg_dir('prob_grasp_planner')
self.load_pre_train_pca = True
self.pre_train_pca_path = pkg_path + '/models/grasp_type_planner/' + \
'train_models/pca/pca.model'
self.load_pre_train_clf = False
# TODO: if self.load_pre_train_clf is true, the logistic regression training
# algorithm needs to be changed from the default liblinear to other methods,
# such as sag or lbfgs, because liblinear doesn't support warm_start being true.
self.pre_train_clf_path = pkg_path + '/models/grasp_type_planner' + \
'/train_models/classifiers/'
self.load_pre_train_prior = False
self.pre_train_prior_path = pkg_path + '/models/grasp_type_planner' + \
'/train_models/priors_all/'
self.grasp_data_path = '/mnt/tars_data/grasp_queries/'
self.save_pca_model = False
self.gmm_components_num = 2
self.al_clf_path = pkg_path + '/models/grasp_type_planner' + \
'/al_models/classifiers/'
self.al_prior_path = pkg_path + '/models/grasp_type_planner' + \
'/al_models/priors/'
self.grasp_model_inf = GraspPgmInfer(pgm_grasp_type=True)
def _manifest_msg_srv_export(ctx, type_):
exist = []
for pkg in ctx.pkgs:
pkg_dir = roslib.packages.get_pkg_dir(pkg)
d = os.path.join(pkg_dir, type_)
if os.path.isdir(d):
files = os.listdir(d)
if filter(lambda x: x.endswith('.' + type_), files):
try:
m_file = roslib.manifest.manifest_file(pkg, True)
except InvalidROSPkgException:
# ignore wet package from further investigation
env = os.environ
pkg_path = get_pkg_dir(pkg, True, ros_root=env['ROS_ROOT'])
if os.path.exists(os.path.join(pkg_path, PACKAGE_FILE)):
continue
raise
m = roslib.manifest.parse_file(m_file)
cflags = m.get_export('cpp', 'cflags')
include = '-I${prefix}/%s/cpp' % type_
if filter(lambda x: include in x, cflags):
exist.append(pkg)
return exist
#! /usr/bin/env python
# -*- encoding: utf-8 -*-
import roslib
roslib.load_manifest('metronome_test')
import rospy
import smach
import os
from roslib import packages
from bpm_analysis import BpmAnalysisSM
from pal_smach_utils.utils.global_common import succeeded, preempted, aborted
from pal_smach_utils.utils.play_sound_sm import PlaySoundAtRateGiven
SOUND_TO_REPEAT_PATH = os.path.join(packages.get_pkg_dir('metronome_test'), 'sounds_lib/146718__fins__button.wav')
class MetronomeDummyState(smach.State):
def __init__(self):
smach.State.__init__(self,
outcomes=[succeeded, preempted, aborted],
output_keys=['sound_to_repeat_path'])
def execute(self, userdata):
userdata.sound_to_repeat_path = SOUND_TO_REPEAT_PATH
print "SOUND TO BE REPEATED = " + str(SOUND_TO_REPEAT_PATH)
return succeeded
from pal_smach_utils.speech.did_you_say_yes_or_no_sm import HearingConfirmationSM
LISTEN_COMMAND_TIMEOUT = 1.5
SLEEP_TIME_BETWEEN_LISTEN_AND_RECORD_ODOMETRY_ITERATIONS = 2.0
ELEVATOR_DISTANCE_TO_HUMAN = 0.65
follow_grammar_name_index = 0
follow_grammar_names = ["robocup/elevator_out"] # ['robocup/elevator_in', 'robocup/elevator_out']
follow_grammar_values = ["getout"] # ['getin', 'getout']
follow_tts_texts = ["I get out of the elevator"] # ["Ok. I follow you to the elevator.", "I get out of the elevator."]
follow_confirm_messages = ["that you want me to get out of the elevator"] # ["that you want to get into the elevator", "that you want me to get out of the elevator"]
missunterstood_text = "Sorry, i didn't understand you. Can you repeat please?"
bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag', 'w')
class RecordOdometry(smach.StateMachine):
def __init__(self):
smach.StateMachine.__init__(self, [succeeded, preempted, aborted])
with self:
def read_odometry_cb(userdata, message):
# if the value of the grammar is getout, we already heard a getin command
# and we can record the odometry till we hear the getout command
value = follow_grammar_values[follow_grammar_name_index]
if value == 'getout':
try:
global bag
debugPrint("Writing odometry message to the bag...", 3)
# -*- encoding: utf-8 -*-
import roslib
roslib.load_manifest('play_sound_test')
import rospy
import smach
from roslib import packages
from pal_smach_utils.utils.global_common import succeeded, preempted, aborted
from pal_smach_utils.utils.play_sound_sm import SpeakText, PlaySoundOnceState, StopSoundState
from pal_smach_utils.utils.timeout_container import SleepState
import os
TIMEOUT = 10.0
PATH_TO_SEND = os.path.join(packages.get_pkg_dir('dancing_reem'), 'mp3_lib/Daft_Punk_ft_Pharrell_Get_Lucky.wav')
PATH_TO_SEND_WAV = os.path.join(packages.get_pkg_dir('dancing_reem'), 'mp3_lib/Daft_Punk_ft_Pharrell_Get_Lucky.wav')
TEXT_TO_SAY1 = "We are starting the Playing songs test. Let's go!"
TEXT_TO_SAY2 = "We have finished the test. Let's go and bake a pie"
class SoundDummyState(smach.State):
def __init__(self):
smach.State.__init__(self,
outcomes=[succeeded, preempted, aborted],
output_keys=['path', 'text1', 'text2'])
def execute(self, userdata):
userdata.path = PATH_TO_SEND
#import plot_roc_pr_curve as plot_curve
import matplotlib.pyplot as plt
#import map_classifier as mc
#import os
#os.sys.path.append('./ppca/src/pca')
#import pca
#import ppca as prob_pca
import pickle
from scipy.stats import multivariate_normal
import rospy
from prob_grasp_planner.srv import *
#import proc_grasp_data as pgd
import roslib.packages as rp
pkg_path = rp.get_pkg_dir('prob_grasp_planner')
sys.path.append(pkg_path + '/src/grasp_type_planner')
from grasp_pgm_inference import GraspPgmInfer
class GraspPgmUpdate:
'''
Update grasp pgms using active learning grasp queries.
'''
def __init__(self):
self.query_batch_num = 3
pkg_path = rp.get_pkg_dir('prob_grasp_planner')
self.load_pre_train_pca = True
self.pre_train_pca_path = pkg_path + '/models/grasp_type_planner/' + \
'train_models/pca/pca.model'
self.load_pre_train_clf = False
cv.rectangle(output.frame,
tl_pt,
br_pt,
obstacle.output_colour,
thickness=2)
obstacle.publish_msg(position_text, area)
# Check if the obstacle is selected, show in debug if selected
if selected_object == field_object_list[obstacle.name]:
gui.show_debug_window(obstacle_mat.frame, obstacle.name)
if __name__ == '__main__':
# Load configurations
configuration = Configuration(configuration_directory=os.path.join(
rospkg.get_pkg_dir('vision'), 'config'))
configuration.load()
colour_space_roi = None
# Initialize ROS
rospy.init_node('vision_node')
rate = rospy.Rate(10)
# Initialize obstacles/lines and load configurations
field = Field(configuration=configuration.config['field'])
lines = Lines(configuration=configuration.config['lines'])
obstacle = Obstacle(configuration=configuration.config['obstacle'])
field_object_list = OrderedDict([('field', field), ('lines', lines),
('obstacle', obstacle)])
selected_object = field
#!/usr/bin/python
# ROS specific imports
import roslib; roslib.load_manifest('floor_nav')
import rospy
import igraph as ig
from math import *
from task_manager_lib.TaskClient import *
from roslib.packages import get_pkg_dir
rospy.init_node('task_client')
server_node = rospy.get_param("~server","/task_server")
default_period = rospy.get_param("~period",0.05)
tc = TaskClient(server_node,default_period)
rospy.loginfo("Mission connected to server: " + server_node)
g = ig.Graph.Read_Picklez(get_pkg_dir('floor_graph')+"/graph.picklez")
tc.WaitForAuto()
try:
tc.Wait(duration=1.0)
# Hand-made hamiltonian path
node_seq = [0, 2, 1, 4, 3, 5, 6, 11, 7, 10, 9, 8]
for node in node_seq:
tc.GoTo(goal_x=g.vs[node]["x"],goal_y=g.vs[node]["y"])
tc.Wait(duration=1.0)
except TaskException, e:
rospy.logerr("Exception caught: " + str(e))
if not rospy.core.is_shutdown():
tc.SetManual()
def printOdometryBag(userdata):
'''
bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag')
odometry_marker_pub = rospy.Publisher("/track_operator/odometry_elevator", Marker)
i = 0
for topic, msg, t in bag.read_messages(topics=['/base_odometry/odom']):
#rospy.loginfo(str(topic) + '\n\n\n')
#rospy.loginfo(str(msg) + '\n\n\n')
#rospy.loginfo(str(t) + '\n\n\n')
marker = Marker()
marker.header.frame_id = "/odom"
marker.ns = "odometry_elevator_namespace"
marker.id = i
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.pose = msg.pose.pose
marker.scale.x = 0.5
marker.scale.y = 0.7
marker.scale.z = 0.9
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.lifetime = rospy.Duration(600.0)
marker.header.stamp = rospy.Time.now()
rospy.loginfo("\033[00;32m" + str(marker) + "\033[m")
odometry_marker_pub.publish(marker)
i += 1
bag.close()
'''
bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag')
odometry_marker_pub = rospy.Publisher("/track_operator/baselink_to_odometry_elevator", Marker)
i = 0
stack = []
for topic, msg, t in bag.read_messages(topics=['/base_odometry/odom']):
#rospy.loginfo(str(topic) + '\n\n\n')
#rospy.loginfo(str(msg) + '\n\n\n')
#rospy.loginfo(str(t) + '\n\n\n')
pose_transformed = transform_pose(msg.pose.pose, '/odom', '/base_link')
if (getDebugLevel() >= 3):
marker = Marker()
marker.header.frame_id = "/base_link"
marker.ns = "baselink_to_odometry_elevator_namespace"
marker.id = i
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.pose = pose_transformed
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.lifetime = rospy.Duration(600.0)
marker.header.stamp = rospy.Time.now()
odometry_marker_pub.publish(marker)
debugPrint("\033[01;33m" + str(marker) + "\033[m", 3)
stack.append(pose_transformed)
i += 1
bag.close()
#TODO SHOULD WORK MOST OF THE TIMES, BUT IF THE BAG IS EMPTY OR WHITH LESS THAN 2 POSES THIS CODE WILL CRASH!!!
try:
initial_pose = stack.pop()
final_pose = stack.pop()
last_dist = euclidean_distance(initial_pose, final_pose)
while (len(stack) > 0 and last_dist < 2.0):
new_final_pose = stack.pop()
new_dist = euclidean_distance(new_final_pose, initial_pose)
if (new_dist > last_dist):
last_dist = new_dist
final_pose = new_final_pose
except Exception as ex:
debugPrint(str(ex), 0)
debugPrint("The bag was empty. Setting a final pose of 2 meters backwards...", 0)
initial_pose = Pose()
initial_pose.position.x = 0.0
initial_pose.position.y = 0.0
final_pose = Pose()
final_pose.position.x = -2.0
final_pose.position.y = 0.0
marker = Marker()
marker.header.frame_id = "/base_link"
marker.ns = "baselink_to_odometry_elevator_namespace"
marker.id = i
marker.type = marker.CYLINDER
marker.action = marker.ADD
marker.pose = initial_pose
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 2.0
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.lifetime = rospy.Duration(600.0)
marker.header.stamp = rospy.Time.now()
debugPrint(" Publishing \033[01;32m(GREEN)\033[mmarker of the INITIAL pose outside the elevator...", 3)
debugPrint(rospy.loginfo("\033[01;32m" + str(marker) + "\033[m"), 3)
odometry_marker_pub.publish(marker)
marker = Marker()
marker.header.frame_id = "/base_link"
marker.ns = "baselink_to_odometry_elevator_namespace"
marker.id = i + 1
marker.type = marker.CYLINDER
marker.action = marker.ADD
marker.pose = final_pose
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 1.0
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.lifetime = rospy.Duration(600.0)
marker.header.stamp = rospy.Time.now()
debugPrint(" Publishing \033[01;33m(RED)\033[mmarker of the FINAL pose outside the elevator...", 3)
debugPrint(" \033[01;33m" + str(marker) + "\033[m", 3)
odometry_marker_pub.publish(marker)
userdata.final_pose = final_pose
return succeeded
#! /usr/bin/env python
# -.- coding: utf-8 -.-
from roslib import packages
from rospy import logwarn
from pal_smach_utils.utils.colors import Colors
import yaml
CONFIG_FILE = packages.get_pkg_dir('cocktail_party') + '/config/cocktail_party.yaml'
CONFIG = yaml.load(open(CONFIG_FILE, 'r'))
ROBOT_GRAMMAR_PATH = "/mnt_flash/etc/interaction/sphinx/model/gram/en_US/"
LOCAL_GRAMMAR_PATH = packages.get_pkg_dir('cocktail_party') + "/config/"
colors = Colors()
class CocktailPartyVariables():
"""CocktailPartyVariables class.
Use this class to get all variables required by the CocktailPartyStateMachine.
"""
def __init__(self, print_variables=True):
"""Constructor for CocktailPartyVariables.
@type print_variables: boolean
@param print_variables: If is not False, 0 or None, all the variables of this class and its values will be printed.
"""
self.print_variables = print_variables
self.TOPICS = []
self.SERVICES = []
self.ACTIONS = []
#!/usr/bin/env python
import roslib
roslib.load_manifest('grasp_pipeline')
import rospy
from grasp_pipeline.srv import *
from geometry_msgs.msg import Pose, Quaternion
from sensor_msgs.msg import JointState
from std_msgs.msg import Char
from sensor_msgs.msg import JointState
import moveit_msgs.msg
import numpy as np
import roslib.packages as rp
import sys
sys.path.append(rp.get_pkg_dir('hand_services') + '/scripts')
from hand_client import handClient
class ControlAllegroConfig:
def __init__(self, publish_prefix='/allegro_hand_right'):
rospy.init_node('control_allegro_config_node')
self.hand_client = handClient()
self.dof = 16
def control_allegro(self, control_type='p'):
rospy.loginfo('Control allegro to target joint states.')
target_joint_angles = np.array(
self.allegro_target_joint_state.position)
reached = self.hand_client.send_pos_cmd(target_joint_angles)
def test_resolve_args(self):
from roslib.substitution_args import resolve_args, SubstitutionException
from roslib.packages import get_pkg_dir
roslib_dir = get_pkg_dir('roslib', required=True)
anon_context = {'foo': 'bar'}
arg_context = {'fuga': 'hoge', 'car': 'cdr'}
context = {'anon': anon_context, 'arg': arg_context }
tests = [
('$(find roslib)', roslib_dir),
('hello$(find roslib)', 'hello'+roslib_dir),
('$(find roslib )', roslib_dir),
('$$(find roslib )', '$'+roslib_dir),
('$( find roslib )', roslib_dir),
('$(find roslib )', roslib_dir),
('$(find roslib)$(find roslib)', roslib_dir+roslib_dir),
('$(find roslib)/foo/bar.xml', roslib_dir+os.sep+'foo'+os.sep+'bar.xml'),
(r'$(find roslib)\foo\bar.xml $(find roslib)\bar.xml', roslib_dir+os.sep+'foo'+os.sep+'bar.xml '+roslib_dir+os.sep+'bar.xml'),
('$(find roslib)\\foo\\bar.xml more/stuff\\here', roslib_dir+os.sep+'foo'+os.sep+'bar.xml more/stuff\\here'),
('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
('$(env ROS_ROOT )', os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT)', os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT)$(optenv ROS_ROOT)', os.environ['ROS_ROOT']+os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT alternate text)', os.environ['ROS_ROOT']),
('$(optenv NOT_ROS_ROOT)', ''),
('$(optenv NOT_ROS_ROOT)more stuff', 'more stuff'),
('$(optenv NOT_ROS_ROOT alternate)', 'alternate'),
('$(optenv NOT_ROS_ROOT alternate text)', 'alternate text'),
# #1776
('$(anon foo)', 'bar'),
('$(anon foo)/baz', 'bar/baz'),
('$(anon foo)/baz/$(anon foo)', 'bar/baz/bar'),
# arg
('$(arg fuga)', 'hoge'),
('$(arg fuga)$(arg fuga)', 'hogehoge'),
('$(arg car)$(arg fuga)', 'cdrhoge'),
('$(arg fuga)hoge', 'hogehoge'),
]
for arg, val in tests:
self.assertEquals(val, resolve_args(arg, context=context))
# more #1776
r = resolve_args('$(anon foo)/bar')
self.assert_('/bar' in r)
self.failIf('$(anon foo)' in r)
# test against strings that should not match
noop_tests = [
'$(find roslib', '$find roslib', '', ' ', 'noop', 'find roslib', 'env ROS_ROOT', '$$', ')', '(', '()',
None,
]
for t in noop_tests:
self.assertEquals(t, resolve_args(t))
failures = [
'$((find roslib))', '$(find $roslib)',
'$(find)', '$(find roslib roslib)', '$(export roslib)',
'$(env)', '$(env ROS_ROOT alternate)',
'$(env NOT_SET)',
'$(optenv)',
'$(anon)',
'$(anon foo bar)',
]
for f in failures:
try:
resolve_args(f)
self.fail("resolve_args(%s) should have failed"%f)
except SubstitutionException: pass
import roslib
roslib.load_manifest("execute_movement_xml_test")
import rospy
import smach
import smach_ros
from roslib import packages
from pal_smach_utils.utils.global_common import succeeded, preempted, aborted
from execute_movement import ExecuteMovement
import os
from pal_smach_utils.utils.check_dependences import CheckDependencesState
from pal_smach_utils.utils.time_controlling_states import GetCurrentROSTimeState
from write_old_movement import WriteOldMovement
DUMB_MOVEMENT_XML_NAME = os.path.join(
packages.get_pkg_dir("dancing_reem"),
"dancing_movements_database/testing_movement_files/transition_from_middle_to_sides_without_parameter.xml",
)
DUMB_MOVEMENT_XML_NAME2 = os.path.join(
packages.get_pkg_dir("dancing_reem"),
"dancing_movements_database/testing_movement_files/waiting_to_send_movement.xml",
)
# DUMB_MOVEMENT_XML_NAME = '/mnt_flash/robocup2013/reem_at_iri/dancing_reem/dancing_movements_database/testing_movement_files/transition_from_middle_to_sides_without_parameter.xml'
# DUMB_MOVEMENT_XML_NAME2 = '/mnt_flash/robocup2013/reem_at_iri/dancing_reem/dancing_movements_database/testing_movement_files/transition_from_sides_to_middle_without_parameter.xml'
"""
robot = os.environ.get('PAL_ROBOT')
ros_master_uri = os.environ.get('ROS_MASTER_URI')
remotelly_executing = ros_master_uri.rfind('localhost')
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. # # Please send comments, questions, or patches to [email protected] # import roslib; roslib.load_manifest('applanix_generated_msgs') from roslib.packages import get_pkg_dir from mapping import groups, msgs from os import path from sys import stdout pkg_dir = get_pkg_dir('applanix_generated_msgs') output = [] all_msgs_filename = path.join(pkg_dir, "msg", "AllMsgs.msg") with open(all_msgs_filename, "w") as all_msgs_f: all_msgs_f.write("time last_changed\n") all_msgs_f.write("time last_sent\n") for msg_num in msgs.keys(): msg_tuple = msgs[msg_num] if msg_tuple: name, msg_cls = msg_tuple if msg_cls.in_all_msgs: all_msgs_f.write("applanix_msgs/%s %s\n" % (msg_cls.__name__, name)) if name != "ack": msg_srv_filename = path.join(pkg_dir, "srv", "%s.srv" % msg_cls.__name__)
# ROS specific imports
import roslib
roslib.load_manifest('floor_nav')
import rospy
import igraph as ig
from math import *
from task_manager_lib.TaskClient import *
from roslib.packages import get_pkg_dir
rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/task_server")
default_period = rospy.get_param("~period", 0.05)
tc = TaskClient(server_node, default_period)
rospy.loginfo("Mission connected to server: " + server_node)
g = ig.Graph.Read_Picklez(get_pkg_dir('floor_graph') + "/graph.picklez")
tc.WaitForAuto()
try:
while True:
tc.Wait(duration=1.0)
# Hand-made hamiltonian path
node_seq = [2, 1, 4, 3, 5, 6, 0, 9, 8, 10, 7, 11]
for node in node_seq:
tc.GoTo(goal_x=g.vs[node]["x"],
goal_y=g.vs[node]["y"],
max_velocity=0.5,
k_v=1.0,
max_angular_velocity=1.0)
tc.Wait(duration=1.0)
node_seq.reverse()
def test_resolve_args(self):
from roslib.substitution_args import resolve_args, SubstitutionException
from roslib.packages import get_pkg_dir
rospy_dir = get_pkg_dir('rospy', required=True)
anon_context = {'foo': 'bar'}
arg_context = {'fuga': 'hoge', 'car': 'cdr'}
context = {'anon': anon_context, 'arg': arg_context }
tests = [
('$(find rospy)', rospy_dir),
('hello$(find rospy)', 'hello'+rospy_dir),
('$(find rospy )', rospy_dir),
('$$(find rospy )', '$'+rospy_dir),
('$( find rospy )', rospy_dir),
('$(find rospy )', rospy_dir),
('$(find rospy)$(find rospy)', rospy_dir+rospy_dir),
('$(find rospy)/foo/bar.xml', rospy_dir+os.sep+'foo'+os.sep+'bar.xml'),
(r'$(find rospy)\foo\bar.xml $(find rospy)\bar.xml', rospy_dir+os.sep+'foo'+os.sep+'bar.xml '+rospy_dir+os.sep+'bar.xml'),
('$(find rospy)\\foo\\bar.xml more/stuff\\here', rospy_dir+os.sep+'foo'+os.sep+'bar.xml more/stuff\\here'),
('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
('$(env ROS_ROOT )', os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT)', os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT)$(optenv ROS_ROOT)', os.environ['ROS_ROOT']+os.environ['ROS_ROOT']),
('$(optenv ROS_ROOT alternate text)', os.environ['ROS_ROOT']),
('$(optenv NOT_ROS_ROOT)', ''),
('$(optenv NOT_ROS_ROOT)more stuff', 'more stuff'),
('$(optenv NOT_ROS_ROOT alternate)', 'alternate'),
('$(optenv NOT_ROS_ROOT alternate text)', 'alternate text'),
# #1776
('$(anon foo)', 'bar'),
('$(anon foo)/baz', 'bar/baz'),
('$(anon foo)/baz/$(anon foo)', 'bar/baz/bar'),
# arg
('$(arg fuga)', 'hoge'),
('$(arg fuga)$(arg fuga)', 'hogehoge'),
('$(arg car)$(arg fuga)', 'cdrhoge'),
('$(arg fuga)hoge', 'hogehoge'),
]
for arg, val in tests:
self.assertEquals(val, resolve_args(arg, context=context))
# more #1776
r = resolve_args('$(anon foo)/bar')
self.assert_('/bar' in r)
self.failIf('$(anon foo)' in r)
# test against strings that should not match
noop_tests = [
'$(find rospy', '$find rospy', '', ' ', 'noop', 'find rospy', 'env ROS_ROOT', '$$', ')', '(', '()',
None,
]
for t in noop_tests:
self.assertEquals(t, resolve_args(t))
failures = [
'$((find rospy))', '$(find $rospy)',
'$(find)', '$(find rospy roslib)', '$(export rospy)',
'$(env)', '$(env ROS_ROOT alternate)',
'$(env NOT_SET)',
'$(optenv)',
'$(anon)',
'$(anon foo bar)',
]
for f in failures:
try:
resolve_args(f)
self.fail("resolve_args(%s) should have failed"%f)
except SubstitutionException: pass
########################################
# Module(s) declaration
########################################
import rospy
import os
from roslib.packages import get_pkg_dir
from python_qt_binding.QtGui import *
from python_qt_binding.QtCore import *
from python_qt_binding import loadUi
########################################
# Constante(s) and Variable(s) declaration
########################################
DIR_SSMPLUGIN_RESOURCES = os.path.join(get_pkg_dir('ssm_plugin'), 'resources')
DIR_SSMPLUGIN_VALUES = DIR_SSMPLUGIN_RESOURCES + '/values'
DIR_SSMPLUGIN_IMAGES = DIR_SSMPLUGIN_RESOURCES + '/images'
DIR_SSMPLUGIN_LAYOUTS = DIR_SSMPLUGIN_RESOURCES + '/layouts'
########################################
# Class(ies) declaration
########################################
class SsmPluginValues():
def __init__(self):
class SsmPluginStrings():
def __init__(self):
self.uuid = self.__class__.__name__
from pal_smach_utils.navigation.track_operator import TrackOperator
from pal_smach_utils.navigation.track_operator import LearnPerson
from pal_navigation_msgs.srv import DisableEmergency, DisableEmergencyRequest
from pal_control_msgs.msg import MotionManagerGoal, MotionManagerAction
from smach_ros import ServiceState, SimpleActionState
from std_srvs.srv import Empty
import os
from roslib import packages
ros_master_uri = os.environ.get('ROS_MASTER_URI')
remotelly_executing = ros_master_uri.rfind('localhost')
MOTION_FOLDER_PATH = packages.get_pkg_dir('pal_smach_utils') + '/config/interact_1.xml'
if remotelly_executing == -1:
MOTION_FOLDER_PATH = "/mnt_flash/robocup2013/reem_at_iri/state_machines/common/config/interact_1.xml"
inside_robot = False
robot = os.environ.get('PAL_ROBOT')
if robot == 'rh2c' or robot == 'rh2m' or robot == 'reemh3c' or robot == 'reemh3m' or robot == 'reemh3':
inside_robot = True
from pr2_controllers_msgs.msg import PointHeadGoal, PointHeadAction
from actionlib import SimpleActionClient
from smach import CBState
#!/usr/bin/env python
from roslib.packages import get_pkg_dir
import sys
sys.path = [get_pkg_dir('ardrone2_mudd')+"/nodes"]+sys.path
import ardrone2
from ardrone2 import rospy
from std_msgs.msg import String
class myArdrone(ardrone2.Ardrone):
def __init__(self):
ardrone2.Ardrone.__init__(self)
self.state = "keyboard"
self.boxX = 0
self.boxHeight = 0
self.tarHeight = .23
self.tarX = .46
self.toleranceX = 40/320.0
self.toleranceHeight = .03
self.toleranceArea = .001
self.noBox = True
self.spinPower = .4
self.forwardPower = .25
def loop(self):
while not rospy.is_shutdown():
char = self.getKeyPress(1000)
if char == 'n':
self.state = "start"
#! /usr/bin/env python
import roslib
roslib.load_manifest('wait_to_send_movement_test')
import os
import rospy
import smach
import smach_ros
from roslib import packages
from pal_smach_utils.utils.global_common import succeeded, preempted, aborted
from pal_smach_utils.utils.time_out import TimeOut
from wait_to_send_next_movement import WaitToSendNextMovement
#DUMB_MOVEMENT_XML_NAME = os.path.join(packages.get_pkg_dir('dancing_reem'), 'dancing_movements_database/testing_movement_files/transition_from_middle_to_sides_without_parameter.xml')
DUMB_MOVEMENT_XML_NAME = os.path.join(packages.get_pkg_dir('dancing_reem'), 'dancing_movements_database/testing_movement_files/waiting_to_send_movement.xml')
#DUMB_MOVEMENT_XML_NAME = '/mnt_flash/robocup2013/reem_at_iri/dancing_reem/dancing_movements_database/testing_movement_files/transition_from_middle_to_sides_without_parameter.xml'
HARMONICS = 1.0
# Send a movement every 3 seconds.
BPM_TEST = 20
MOVING_MOCK_TIME = 2.0
class InitVars(smach.State):
def __init__(self):
smach.State.__init__(self,
outcomes=[succeeded, preempted, aborted],
def setUp(self):
from roslib.packages import get_pkg_dir
self.xml_dir = os.path.join(get_pkg_dir('test_roslaunch'), 'test', 'xml')
