def __init__(self, obj_map, obj_conf, roi_map, roi_conf, config_file=None):
self._soma_obj_roi_ids = dict()
self._soma_obj_type = dict()
self._soma_obj_pose = dict()
self.obj_map = obj_map
self.obj_conf = obj_conf
self.roi_map = roi_map
self.roi_conf = roi_conf
self._soma_roi = dict()
self._obj_msg_store=MessageStoreProxy(collection="soma")
self._roi_msg_store=MessageStoreProxy(collection="soma_roi")
self._retrieve_objects()
self._retrieve_rois()
self._server = InteractiveMarkerServer("soma_vis")
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_objects') + '/config/'
filename = 'default.json'
self._config_file=path+filename
self._init_types()
def __init__(self, config_file=None, blog=None):
soma_srv_name = '/soma2/query_db'
rospy.loginfo("Waiting for SOMA query service...")
rospy.wait_for_service(soma_srv_name)
rospy.loginfo("Done")
self.soma_srv = rospy.ServiceProxy(soma_srv_name, SOMA2QueryObjs)
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('change_detection_objects') + '/config/'
filename = 'default.json'
self._config_file=path+filename
rospy.loginfo("Use KB at: %s", self._config_file)
self._init_kb()
self._init_rois()
if blog:
self.blog_collection = blog
else:
self.blog_collection = 'soma_blog' # only commandline reporting
self.talk = RobotTalkProxy('robot_talk')
self._setup_services()
self._init_meta_room_count()
def __init__(self, config_name, srdf_file, move_group = "", robot_name = ""):
'''
Constructor
'''
super(GetJointsFromSrdfState, self).__init__(outcomes=['retrieved', 'file_error'],
output_keys=['joint_values'])
self._config_name = config_name
self._move_group = move_group
self._robot_name = robot_name
# Check existence of SRDF file to reduce risk of runtime failure.
# Anyways, values will only be read during runtime to allow modifications.
self._srdf_path = None
try:
rp = RosPack()
pkg_name = srdf_file.split('/')[0]
self._srdf_path = os.path.join(rp.get_path(pkg_name), '/'.join(srdf_file.split('/')[1:]))
if not os.path.isfile(self._srdf_path):
raise IOError('File "%s" does not exist!' % self._srdf_path)
except Exception as e:
Logger.logwarn('Unable to find given SRDF file: %s\n%s' % (srdf_file, str(e)))
self._file_error = False
self._srdf = None
def __init__(self, config_file=None):
self._map = dict()
self._obj = dict()
self._roi = dict()
self._roi_cnt = dict()
self._obj_cnt = dict()
self._num_of_objs = 0
self._num_of_rois = 0
self._soma_utils = dict()
self._obj_msg_store=MessageStoreProxy(collection="soma")
self._roi_msg_store=MessageStoreProxy(collection="soma_roi")
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_utils') + '/config/'
filename = 'maps.json'
self._config_file=path+filename
self._init_maps()
def __init__(self, config_file=None, blog=None):
soma_srv_name = '/soma/query_rois'
rospy.loginfo("Waiting for SOMA query service...")
rospy.wait_for_service(soma_srv_name)
rospy.loginfo("Done")
self.soma_srv = rospy.ServiceProxy(soma_srv_name, SOMAQueryROIs)
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('intrusion_detection_people') + '/config/'
filename = 'default.json'
self._config_file=path+filename
rospy.loginfo("Use KB at: %s", self._config_file)
self._init_kb()
self._init_rois()
self.res_uuids = []
self.unres_uuids = []
if blog:
self.blog_collection = blog
else:
self.blog_collection = 'soma_blog' # only commandline reporting
self.talk = RobotTalkProxy('robot_talk')
self._setup_callbacks()
def roscreatepkg_main():
from optparse import OptionParser
parser = OptionParser(usage="usage: %prog <package-name> [dependencies...]", prog=NAME)
options, args = parser.parse_args()
if not args:
parser.error("you must specify a package name and optionally also list package dependencies")
package = args[0]
if not roslib.names.is_legal_resource_base_name(package):
parser.error("illegal package name: %s\nNames must start with a letter and contain only alphanumeric characters\nand underscores."%package)
# validate dependencies and turn into XML
depends = args[1:]
uses_roscpp = 'roscpp' in depends
uses_rospy = 'rospy' in depends
rospack = RosPack()
for d in depends:
try:
rospack.get_path(d)
except ResourceNotFound:
print("ERROR: dependency [%s] cannot be found"%d, file=sys.stderr)
sys.exit(1)
depends = u''.join([u' <depend package="%s"/>\n'%d for d in depends])
if not on_ros_path(os.getcwd()):
print('!'*80+"\nWARNING: current working directory is not on ROS_PACKAGE_PATH!\nPlease update your ROS_PACKAGE_PATH environment variable.\n"+'!'*80, file=sys.stderr)
if type(package) == str:
package = package.decode('utf-8')
create_package(package, author_name(), depends, uses_roscpp=uses_roscpp, uses_rospy=uses_rospy)
def handle_Save_Data(self,req):
if req.ToSave == True:
# if GUI request data to be saved create file
# namespace, e.g. /Iris1/
ns = rospy.get_namespace()
# remove / symbol to namespace: e.g, we get ns= Iris1
ns = ns.replace("/", "")
# string for time: used for generating files
tt = str(int(rospy.get_time() - self.TimeSaveData))
# determine ROS workspace directory
rp = RosPack()
package_path = rp.get_path('quad_control')
self.file_handle = file(package_path+'/../../'+ns+'_data_'+tt+'.txt', 'w')
# if GUI request data to be saved, set falg to true
self.SaveDataFlag = True
else:
# if GUI request data NOT to be saved, set falg to False
self.SaveDataFlag = False
# return message to Gui, to let it know resquest has been fulfilled
return SaveDataResponse(True)
def __init__(self, soma_map, soma_conf, config_file=None):
self.soma_map = soma_map
self.soma_conf = soma_conf
if config_file:
self._config_file = config_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_roi_manager') + '/config/'
filename = 'default.json'
self._config_file=path+filename
self._soma_obj_ids = dict()
self._soma_obj_msg = dict()
self._soma_obj_roi_ids = dict()
self._soma_obj_roi = dict()
self._soma_obj_type = dict()
self._soma_obj_pose = dict()
self._interactive = True
self._msg_store=MessageStoreProxy(collection="soma_roi")
self._gs_store=GeoSpatialStoreProxy(db="geospatial_store", collection="soma")
self._server = InteractiveMarkerServer("soma_roi")
self._init_types()
self._init_menu()
self.load_objects()
rospy.spin()
def __init__(self, kb_file=None):
self.octomaps = dict()
#self.pointclouds = dict()
self.labels = dict()
self.octomap_keys = dict()
self.octomap_key_idx = dict()
self.label_names = dict()
#self.labels = dict() # store p(l|d)
self.label_probs = dict()
self.label_freq = dict()
self.points = dict()
if kb_file:
self._kb_file = kb_file
else:
# default file
rp = RosPack()
path = rp.get_path('soma_pcl_segmentation') + '/data/'
filename = 'object_kb.json'
self._kb_file=path+filename
self._init_object_kb()
self._waypoint_service = rospy.Service('soma_probability_at_waypoint', GetProbabilityAtWaypoint, self.get_probability_at_waypoint)
self._view_service = rospy.Service('soma_probability_at_view', GetProbabilityAtView, self.get_probability_at_view)
self._prob_marker_pub = rospy.Publisher('/object_search/object_probabilities', MarkerArray, queue_size = 1)
get_top_map_srv = rospy.ServiceProxy('/topological_map_publisher/get_topological_map', GetTopologicalMap)
self._topo_map = get_top_map_srv(rospy.get_param('topological_map_name')).map.nodes
rospy.spin()
def test_get_depends_on():
from rospkg import RosPack, get_ros_root
test_dir = get_package_test_path()
rp = RosPack(ros_paths=[test_dir])
# test direct depends
val = rp.get_depends_on("foo", implicit=False)
assert set(["bar", "baz"]) == set(val), val
val = rp.get_depends_on("bar", implicit=False)
assert ["baz"] == val, val
val = rp.get_depends_on("baz", implicit=False)
assert [] == val, val
# test implicit depends
val = rp.get_depends_on("foo", implicit=True)
assert set(["bar", "baz"]) == set(val), val
val = rp.get_depends_on("bar", implicit=True)
assert ["baz"] == val, val
val = rp.get_depends_on("baz", implicit=True)
assert [] == val, val
if get_ros_root() and rospack_is_available():
# stress test: test default environment against rospack
r = RosPack()
for p in rospack_list():
retval = set(r.get_depends_on(p, False))
rospackval = set(rospack_depends_on1(p))
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval, rospackval)
for p in rospack_list():
retval = set(r.get_depends_on(p, True))
rospackval = set(rospack_depends_on(p))
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval, rospackval)
def donot_test_linear_no_constant(self):
theta_dot_fi_function = 'Duty_fi_linear_no_constant'
v_fi_function = 'Duty_fi_linear_no_constant'
learner = Linear_learner(theta_dot_fi_function, v_fi_function)
rp = RosPack()
filepath = rp.get_path('kinematics') + "/tests/test_training_set.txt"
theta_dot_weights = learner.fit_theta_dot_from_file(filepath)
v_weights = learner.fit_v_from_file(filepath)
def __init__(self, description='human_description', prefix='human', control=False):
rospack = RosPack()
self.path = rospack.get_path('human_moveit_config')
self.description = description
self.robot_commander = moveit_commander.RobotCommander(description)
if control:
self.joint_publisher = rospy.Publisher('/human/set_joint_values', JointState, queue_size=1)
self.groups = {}
self.groups['head'] = moveit_commander.MoveGroupCommander('Head', description)
self.groups['right_arm'] = moveit_commander.MoveGroupCommander('RightArm', description)
self.groups['left_arm'] = moveit_commander.MoveGroupCommander('LeftArm', description)
self.groups['right_leg'] = moveit_commander.MoveGroupCommander('RightLeg', description)
self.groups['left_leg'] = moveit_commander.MoveGroupCommander('LeftLeg', description)
self.groups['upper_body'] = moveit_commander.MoveGroupCommander('UpperBody', description)
self.groups['lower_body'] = moveit_commander.MoveGroupCommander('LowerBody', description)
self.groups['whole_body'] = moveit_commander.MoveGroupCommander('WholeBody', description)
# initialize end-effectors dict
self.end_effectors = {}
# fill both dict
for key, value in self.groups.iteritems():
self.end_effectors[key] = value.get_end_effector_link()
# add the list of end-effectors for bodies
self.end_effectors['upper_body'] = [self.end_effectors['head'],
self.end_effectors['right_arm'],
self.end_effectors['left_arm']]
self.end_effectors['lower_body'] = [self.end_effectors['right_leg'],
self.end_effectors['left_leg']]
self.end_effectors['whole_body'] = self.end_effectors['upper_body'] + self.end_effectors['lower_body']
# initialize common links per group
self.group_links = {}
self.group_links['head'] = [prefix + '/shoulder_center', prefix + '/head']
# fill the disct of active joints by links
self.joint_by_links = {}
self.joint_by_links[prefix + '/shoulder_center'] = ['spine_0', 'spine_1', 'spine_2']
self.joint_by_links[prefix + '/head'] = ['neck_0', 'neck_1', 'neck_2']
sides = ['right', 'left']
for s in sides:
self.group_links[s + '_arm'] = [prefix + '/' + s + '_shoulder',
prefix + '/' + s + '_elbow',
prefix + '/' + s + '_hand']
self.group_links[s + '_leg'] = [prefix + '/' + s + '_hip',
prefix + '/' + s + '_knee',
prefix + '/' + s + '_foot']
# arm links
self.joint_by_links[prefix + '/' + s + '_shoulder'] = [s + '_shoulder_0',
s + '_shoulder_1',
s + '_shoulder_2']
self.joint_by_links[prefix + '/' + s + '_elbow'] = [s + '_elbow_0', s + '_elbow_1']
self.joint_by_links[prefix + '/' + s + '_hand'] = [s + '_wrist_0', s + '_wrist_1']
# leg links
self.joint_by_links[prefix + '/' + s + '_hip'] = [s + '_hip_0', s + '_hip_1', s + '_hip_2']
self.joint_by_links[prefix + '/' + s + '_knee'] = [s + '_knee']
self.joint_by_links[prefix + '/' + s + '_foot'] = [s + '_ankle_0', s + '_ankle_1']
self.prefix = prefix
rospy.wait_for_service('compute_fk')
self.compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
self.current_state = self.get_initial_state()
def test_unary():
from rospkg import RosStack, RosPack
path = get_unary_test_path()
rospack = RosPack(ros_paths=[path])
rosstack = RosStack(ros_paths=[path])
assert rospack.get_path('unary') == rosstack.get_path('unary')
assert rosstack.packages_of('unary') == ['unary']
assert rospack.stack_of('unary') == 'unary'
def __init__(self, name):
self.name = name
rospack = RosPack()
self.path = rospack.get_path(name)
self.extensions = {}
self.folders = list()
self.files_to_be_moved = list()
self.duplicated_files = list()
pass
def resolve_ros_path(path):
if path.startswith("package://"):
package_name = path.split("/")[2]
rest_path = path.split("/")[3:]
rp = RosPack()
pkg_path = rp.get_path(package_name)
return os.path.join(*([pkg_path] + rest_path))
else:
return path
def get_all_deps(packages):
rp = RosPack()
all_deps = set()
for pack in packages:
depends = rp.get_depends(pack)
for dep in depends:
all_deps.add(dep)
for pack in packages:
all_deps.add(pack)
return all_deps
def save_leg_positions_to_disc(self, file_name):
rp = RosPack()
save_path = rp.get_path('humanoid_driving_controller') + '/config/throttle/' + file_name
yaml_dict = dict()
yaml_dict.update({'leg_joints': leg_joints})
yaml_dict.update({'speed_control_joints': used_joints})
yaml_dict.update(self.leg_positions)
with open(save_path, 'w+') as outfile:
outfile.write(yaml.dump(yaml_dict, default_flow_style=False))
def save_key_positions_to_disc(self, file_name):
rp = RosPack()
save_path = rp.get_path('humanoid_driving_controller') + '/config/steering/'
yaml_dict = dict()
yaml_dict.update({'joints': joints})
yaml_dict.update({'angles': [int(angle) for angle in self.key_positions.iterkeys()]})
yaml_dict.update({'angle_' + str(angle).split('.')[0]: self.key_positions[angle] for angle in self.key_positions})
with open(save_path + file_name, 'w+') as outfile:
outfile.write(yaml.dump(yaml_dict, default_flow_style=False))
def __init__(self, name=None):
global id
self.group = self.Group(self, "Default", "", True, 0, 0)
id = 1
if name is None:
self.name = rospy.get_name() + "_dyn_rec"
else:
self.name = name
self.constants = []
rp = RosPack()
self.dynconfpath = rp.get_path('dynamic_reconfigure')
def test_linear_no_constant(self):
theta_dot_fi_function = 'Duty_fi_linear_no_constant'
v_fi_function = 'Duty_fi_linear_no_constant'
learner = Linear_learner(theta_dot_fi_function, v_fi_function)
rp = RosPack()
filepath = rp.get_path('kinematics') + "/tests/test_training_set.txt"
theta_dot_weights = learner.fit_theta_dot_from_file(filepath)
v_weights = learner.fit_v_from_file(filepath)
np.testing.assert_almost_equal(v_weights, np.matrix([1.0, 1.0]))
np.testing.assert_almost_equal(theta_dot_weights, np.matrix([-1, 1]))
def _get_packages_in_environment():
global _packages_in_environment
if _packages_in_environment is None:
if ROS_PACKAGE_PATH not in os.environ or not os.environ[ROS_PACKAGE_PATH]:
raise RuntimeError("The environment variable '%s' must be set when using '%s'" % (ROS_PACKAGE_PATH, ARG_CURRENT_ENVIRONMENT))
_packages_in_environment = set([])
rs = RosStack()
_packages_in_environment.update(set(rs.list()))
rp = RosPack()
_packages_in_environment.update(set(rp.list()))
return _packages_in_environment
def __init__(self, name,
device = None,
tracer = None):
AbstractHumanoidRobot.__init__ (self, name, tracer)
print "You ask for ", name, ". "
rospack = RosPack()
self.urdfDir = rospack.get_path('romeo_description') + '/urdf/'
if name == 'romeo_small':
print "Loaded model is romeo_small.urdf."
self.urdfName = 'romeo_small.urdf'
self.halfSitting = self.halfSittingSmall
else:
print "Loaded model is romeo.urdf."
self.urdfName = 'romeo.urdf'
self.halfSitting = self.halfSittingAll
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.OperationalPointsMap = {'left-wrist' : 'LWristPitch',
'right-wrist' : 'RWristPitch',
'left-ankle' : 'LAnkleRoll',
'right-ankle' : 'RAnkleRoll',
'gaze' : 'gaze_joint',
'waist' : 'waist',
'chest' : 'TrunkYaw'}
self.device = device
# correct the name of the body link
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
self.pinocchioModel = se3.buildModelFromUrdf(self.urdfDir + self.urdfName,
se3.JointModelFreeFlyer())
self.pinocchioData = self.pinocchioModel.createData()
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
# complete feet position (TODO: move it into srdf file)
#ankle =self.dynamic.getAnklePositionInFootFrame()
#self.ankleLength = 0.1935
#self.ankleWidth = 0.121
#self.dynamic.setFootParameters(True , self.ankleLength, self.ankleWidth, ankle)
#self.dynamic.setFootParameters(False, self.ankleLength, self.ankleWidth, ankle)
# check half sitting size
self.dimension = self.dynamic.getDimension()
if self.dimension != len(self.halfSitting):
raise RuntimeError("invalid half-sitting pose. HalfSitting Dimension:",
len(self.halfSitting), " .Robot Dimension:", self.dimension)
self.initializeRobot()
def GetRosIncludePaths():
"""Return a list of potential include directories
The directories are looked for in the ROS environment variables
"""
try:
from rospkg import RosPack
except ImportError:
return []
rospack = RosPack()
includes = GetWorkspaceIncludes()
for p in rospack.list():
if os.path.exists(rospack.get_path(p) + '/include'):
includes.append(rospack.get_path(p) + '/include')
return includes
def __init__(self):
super().__init__("scheduler", "ai")
self.running = False
self.current_action: Action = None
self.robot_pos = Pose2D()
self.points = 0
# ROS services / subscribers
self.control_srv = rospy.Service("/scheduler/do", SetSchedulerState,
self.set_state)
self.robot_pos_sub = rospy.Subscriber("/can_interface/in", CanData,
self.on_can_message)
self.can_out = rospy.Publisher("/can_interface/out",
CanData,
queue_size=10)
# Parse actions
try:
folder = RosPack().get_path("ai_description") + "/actions/"
self.root_action = ActionGroup.parse_file(
folder + rospy.get_param("/actions/main", default="main") +
".xml",
context={"folder": folder})
except ParsingException as e:
self.set_status(NodeStatus.ERROR, 1)
raise e
# Load requirements
self.save_required(self.root_action)
self.wait_for_nodes(5)
class image_converter:
def __init__(self):
# print ("passaggio: __init__")
self.bridge = CvBridge()
self.rp = RosPack()
self.pkg_path = self.rp.get_path('mask_detection')
self.img_path = self.pkg_path + "/image/detection_image.jpg"
self.image_topic = rospy.get_param('~image_topic')
# self.image_sub = rospy.Subscriber("chatter",Image,self.callback)
self.image_sub = rospy.Subscriber(self.image_topic, CompressedImage,
self.callback)
def callback(self, data):
# print ("passaggio: callback")
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(
data, "passthrough")
except CvBridgeError as e:
print("CvBridgeError: " + str(e))
cv2.imshow("Video stream", cv_image)
cv2.waitKey(3)
cv2.imwrite(self.img_path, cv_image)
def __init__(self, export_tag, base_class_type):
super(RospkgPluginProvider, self).__init__(export_tag, base_class_type)
self.setObjectName('RospkgPluginProvider')
if RospkgPluginProvider.rospack is None:
from rospkg import RosPack
RospkgPluginProvider.rospack = RosPack()
def main():
script_name = os.path.basename(__file__)
node_name = os.path.splitext(script_name)[0]
rospy.init_node(node_name)
model_default = RosPack().get_path('oit_navigation_microbot_01') + \
'/models/collision_avoidance/best_model.pth'
image_topic_in = "/jetbot_camera_image_modifier/mod_image"
classification_topic_out = "~classification"
process_rate = rospy.get_param("~process_rate", 20.0)
width = rospy.get_param("~width", 224)
height = rospy.get_param("~height", 224)
model_path = rospy.get_param("~model_path", model_default)
try:
rospy.loginfo("%s:Loading DNN model from %s..." %
(node_name, model_path))
model = torchvision.models.alexnet(pretrained=False)
model.classifier[6] = torch.nn.Linear(
model.classifier[6].in_features, 2)
model.load_state_dict(torch.load(model_path))
rospy.loginfo("%s:Done." % (node_name))
except Exception as e:
rospy.logerr(e)
return
node = DetectCollision(model, image_topic_in,
classification_topic_out, width, height)
rate = rospy.Rate(process_rate)
while not rospy.is_shutdown():
node.spin()
rate.sleep()
def __init__(self, experiment_id="exp"):
self.data_dict = {
"id": [],
"index": [],
"horizon": [],
"nr_rollouts": [],
"alpha": [],
"beta": [],
"filter_window": [],
"tree_search": [],
"stage_cost": [],
"effective_samples": [],
"time": [],
"opt_time": []
}
self.first_call = True
self.data_subscriber = rospy.Subscriber("/mppi_data",
Data,
self.data_callback,
queue_size=10)
self.initial_step_count = 0
self.initial_time = 0
self.idx = 0
self.experiment_id = experiment_id + str(uuid.uuid4())
self.csv_file = os.path.join(RosPack().get_path("mppi_ros"), "log",
"record.csv")
rospy.loginfo("Writing to {}".format(self.csv_file))
class Controller(object):
def __init__(self):
self.rospack = RosPack()
with open(
join(self.rospack.get_path('pobax_playground'), 'config',
'general.json')) as f:
self.params = json.load(f)
self.torso = Torso()
rospy.loginfo('Controller fully started!')
def run(self):
self.torso.go_to_rest()
try:
while not rospy.is_shutdown():
#trajectory = self.learning.produce(skill_to_demonstrate=self.demonstrate).torso_trajectory
self.torso.set_torque_max(15)
#self.recorder.record(task, method, trial, iteration)
#self.torso.execute_trajectory(trajectory) # TODO: blocking, non-blocking, action server?
#recording = self.perception.record(human_demo=False, nb_points=self.params['nb_points'])
#recording.demo.torso_demonstration = JointTrajectory()
#self.torso.set_torque_max(80)
#return self.learning.perceive(recording.demo)
rospy.sleep(1)
finally:
self.torso.close()
class AudioHelp(object):
def __init__(self):
self.rospack = RosPack()
# Sound setting
#define stream chunk
self.chunk = 1024
#open a wav format music
self.help_msg = dict()
#instantiate PyAudio
self.p = pyaudio.PyAudio()
def play(self,msg_name):
#import wave
#read data
f = wave.open(join(self.rospack.get_path('pobax_playground'), 'config', "help_"+msg_name+".wav"),"rb")
data = f.readframes(self.chunk)
stream = self.p.open(format = self.p.get_format_from_width(f.getsampwidth()),
channels = f.getnchannels(),
rate = f.getframerate(),
output = True)
#play stream
while data:
stream.write(data)
data = f.readframes(self.chunk)
#stop stream
stream.stop_stream()
stream.close()
def export(cs_com, cs_wb):
rp = RosPack()
urdf = rp.get_path(cfg.Robot.packageName) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
if cfg.WB_VERBOSE:
print("load robot : ", urdf)
#srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf'
robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False)
if cfg.WB_VERBOSE:
print("robot loaded in export OpenHRP")
results = computeWaistData(robot, res)
path = cfg.EXPORT_PATH + "/openHRP/" + cfg.DEMO_NAME
if not os.path.exists(path):
os.makedirs(path)
q_openhrp_l = generateOpenHRPMotion(results, path, cfg.DEMO_NAME, useRefZMP=cfg.openHRP_useZMPref)
writeKinematicsData(results, path, cfg.DEMO_NAME)
class Perception(object):
def __init__(self):
self.services = {'record': {'name': '/apex_playground/perception/record', 'type': Record}}
for service_name, service in self.services.items():
rospy.loginfo("Controller is waiting service {}...".format(service['name']))
rospy.wait_for_service(service['name'])
service['call'] = rospy.ServiceProxy(service['name'], service['type'])
rospy.Subscriber('/apex_playground/ergo/button', Bool, self._cb_help_pressed)
self.button_pressed = False
self.last_press = rospy.Time(0)
self.rospack = RosPack()
with open(join(self.rospack.get_path('apex_playground'), 'config', 'perception.json')) as f:
self.params = json.load(f)
def _cb_help_pressed(self, msg):
if msg.data and rospy.Time.now() - self.last_press > rospy.Duration(self.params['duration_between_presses']):
rospy.logwarn("User demonstration requested, wait till the end of this iteration...")
self.button_pressed = True
self.last_press = rospy.Time.now()
def help_pressed(self):
pressed = self.button_pressed
self.button_pressed = False
return pressed
def record(self, human_demo, nb_points):
call = self.services['record']['call']
return call(RecordRequest(human_demo=Bool(data=human_demo), nb_points=UInt8(data=nb_points)))
def __init__(self):
# Load parameters and hack the tuple conversions so that OpenCV is happy
self.rospack = RosPack()
with open(join(self.rospack.get_path('apex_playground'), 'config', 'environment.json')) as f:
self.params = json.load(f)
self.params['tracking']['ball']['lower'] = tuple(self.params['tracking']['ball']['lower'])
self.params['tracking']['ball']['upper'] = tuple(self.params['tracking']['ball']['upper'])
self.params['tracking']['arena']['lower'] = tuple(self.params['tracking']['arena']['lower'])
self.params['tracking']['arena']['upper'] = tuple(self.params['tracking']['arena']['upper'])
self.tracking = BallTracking(self.params)
self.conversions = EnvironmentConversions()
self.ball_pub = rospy.Publisher('/apex_playground/environment/ball', CircularState, queue_size=1)
self.light_pub = rospy.Publisher('/apex_playground/environment/light', UInt8, queue_size=1)
self.sound_pub = rospy.Publisher('/apex_playground/environment/sound', Float32, queue_size=1)
self.rate = rospy.Rate(self.params['rate'])
class Torso(object):
def __init__(self):
self.rospack = RosPack()
robot_config = join(self.rospack.get_path('pobax_playground'),
'config', 'torso.json')
self.torso = from_json(robot_config)
self.torso.compliant = False
def set_torque_max(self, torque_max=100):
for m in self.torso.motors:
m.torque_limit = torque_max
def go_to_rest(self, slow=True):
duration = 2 if slow else 0.25
#self.torso.goto_position({'l_shoulder_y': 13, 'l_shoulder_x': 20, 'l_elbow_y': -25}, duration)
#rospy.sleep(duration)
self.go_to([0, 0, 0, 0, 20, 0, 0, 0, 0, 0, 0, 0, 0], duration)
self.in_rest_pose = True
def go_to(self, motors, duration):
motors_dict = dict(zip([m.name for m in self.torso.motors], motors))
self.torso.goto_position(motors_dict, duration)
rospy.sleep(duration)
def close(self):
self.torso.close()
def get_message_file_paths_via_gendeps(types_list, rp=RosPack()):
"""
Given a list of types as ['geometry_msgs/PoseStamped', ...]
return the list of messages that are dependend with their full
file path. Also returns a list of non processed files as gendeps
sometimes fails parsing them.
"""
message_file_paths = []
non_processed_files = []
for t in types_list:
pkg_name, msg_name = t.split('/')
pkg_path = rp.get_path(pkg_name)
msg_full_path = pkg_path + "/msg/" + msg_name + ".msg"
if msg_full_path not in message_file_paths:
message_file_paths.append(msg_full_path)
# Get list of files that this message depends on
try:
out = check_output(["rosrun", "roslib", "gendeps", msg_full_path])
except CalledProcessError as e:
print "\n Exception when checking message: " + msg_full_path
print " " + str(e) + "\n"
non_processed_files.append(msg_full_path)
out = out.replace('\n', '')
files = out.split(' ')
for f in files:
if f != '':
if f not in message_file_paths:
message_file_paths.append(f)
# if files != ['']:
# print "Message: " + t + " depends on: "
# print " " + str(files)
# else:
# print "Message: " + t + " has no dependencies."
return message_file_paths, non_processed_files
class RecorderBase(object):
MAX_POINTS = 10000
def __init__(self):
self._rospack = RosPack()
self._path = join(self._rospack.get_path("poppy_controllers"), "data")
self._data = []
def __init__(self):
self._sub = rospy.Subscriber("/flexbe/request_behavior", BehaviorRequest, self._callback)
self._pub = rospy.Publisher("/flexbe/start_behavior", BehaviorSelection, queue_size=100)
self._mirror_pub = rospy.Publisher("/flexbe/mirror/structure", ContainerStructure, queue_size=100)
self._rp = RosPack()
self._behavior_lib = dict()
behaviors_package = "flexbe_behaviors"
if rospy.has_param("behaviors_package"):
behaviors_package = rospy.get_param("behaviors_package")
else:
rospy.loginfo("Using default behaviors package: %s" % behaviors_package)
manifest_folder = os.path.join(self._rp.get_path(behaviors_package), 'behaviors/')
file_entries = [os.path.join(manifest_folder, filename) for filename in os.listdir(manifest_folder) if not filename.startswith('#')]
manifests = sorted([xmlpath for xmlpath in file_entries if not os.path.isdir(xmlpath)])
for i in range(len(manifests)):
m = ET.parse(manifests[i]).getroot()
e = m.find("executable")
self._behavior_lib[i] = {
"name": m.get("name"),
"package": e.get("package_path").split(".")[0],
"file": e.get("package_path").split(".")[1],
"class": e.get("class")
}
rospy.loginfo("%d behaviors available, ready for start request." % len(self._behavior_lib.items()))
class EnvironmentConversions(object):
def __init__(self):
self.rospack = RosPack()
with open(
join(self.rospack.get_path('apex_playground'), 'config',
'environment.json')) as f:
self.params = json.load(f)
def get_state(self, ball_center, arena_center, ring_radius):
"""
Reduce the current joint configuration of the ergo in (angle, theta)
:return: a CircularState
"""
x, y = (ball_center[0] - arena_center[0],
ball_center[1] - arena_center[1])
elongation = sqrt(x * x + y * y)
theta = arctan2(y, x)
state = CircularState()
state.extended = elongation > ring_radius
state.angle = theta
return state
@staticmethod
def ball_to_color(state):
"""
Reduce the given 2D ball position in color
:param state: the requested circular state of the ball
:return: hue value designating the color in [0, 255]
"""
hue = min(255, max(0, int((state.angle + pi) * 255 / (2 * pi))))
return hue
def __init__(self):
"""Parameters Inicialization """
self.poses_path = rospy.get_param(
"~poses_db_path",
RosPack().get_path("behavior") +
"/include/poses.csv") #Pose Data Base path
self.buttons_path = rospy.get_param(
"~buttons_db_path",
RosPack().get_path("behavior") +
"/include/buttons.csv") #Buttons Data Base path
r_nw_db = rospy.get_param(
"~r_nw_db", True) #Topic for the status of the Roombot behavior
save_request_service = rospy.get_param(
"~save_request_service",
"save_request") #Topic for the status of the Roombot behavior
pose_request_service = rospy.get_param(
"~pose_request_service",
"pose_request") #Topic for the status of the Roombot behavior
button_request_service = rospy.get_param(
"~button_request_service",
"button_request") #Topic for the status of the Roombot behavior
self.pose_frame_id = rospy.get_param("~pose_frame_id", "map")
"""Services"""
if not r_nw_db:
self.srv_save_request = rospy.Service(save_request_service,
SavePose,
self.callback_save_request)
rospy.loginfo("Data Bases in write mode")
else:
self.srv_pose_request = rospy.Service(pose_request_service,
GetPose,
self.callback_pose_request)
self.srv_button_request = rospy.Service(
button_request_service, GetButton,
self.callback_button_request)
rospy.loginfo("Data Bases in read mode")
"""Node Configuration"""
self.poses_db = pd.read_csv(self.poses_path, index_col=False)
print(self.poses_db)
self.buttons_db = pd.read_csv(self.buttons_path,
dtype={
"Nombres": str,
"Boton": str
},
index_col=False)
print(self.buttons_db)
rospy.spin()
def __init__(self, name, robotnumber, device=None, tracer=None):
AbstractHumanoidRobot.__init__(self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.device = device
self.AdditionalFrames.append(
("accelerometer", matrixToTuple(self.accelerometerPosition),
"chest"))
self.AdditionalFrames.append(
("gyrometer", matrixToTuple(self.gyrometerPosition), "chest"))
self.AdditionalFrames.append(
("leftFootForceSensor", self.forceSensorInLeftAnkle, "left-ankle"))
self.AdditionalFrames.append(
("rightFootForceSensor", self.forceSensorInRightAnkle,
"right-ankle"))
self.OperationalPointsMap = {
'left-wrist': 'LARM_JOINT5',
'right-wrist': 'RARM_JOINT5',
'left-ankle': 'LLEG_JOINT5',
'right-ankle': 'RLEG_JOINT5',
'gaze': 'HEAD_JOINT1',
'waist': 'WAIST',
'chest': 'CHEST_JOINT1'
}
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
rospack = RosPack()
self.urdfPath = rospack.get_path('hrp2_{0}_description'.format(
robotnumber)) + '/urdf/hrp2_{0}_reduced.urdf'.format(robotnumber)
self.pinocchioModel = se3.buildModelFromUrdf(self.urdfPath,
se3.JointModelFreeFlyer())
self.pinocchioData = self.pinocchioModel.createData()
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dimension = self.dynamic.getDimension()
self.plugVelocityFromDevice = True
if self.dimension != len(self.halfSitting):
raise RuntimeError(
"Dimension of half-sitting: {0} differs from dimension of robot: {1}"
.format(len(self.halfSitting), self.dimension))
self.initializeRobot()
self.dynamic.displayModel()
def GetRosIncludePaths():
"""Return a list of potential include directories
The directories are looked for in $ROS_WORKSPACE.
"""
try:
from rospkg import RosPack
except ImportError:
return []
rospack = RosPack()
includes = []
includes.append(os.path.expandvars('$ROS_WORKSPACE') + '/devel/include')
for p in rospack.list():
if os.path.exists(rospack.get_path(p) + '/include'):
includes.append(rospack.get_path(p) + '/include')
for distribution in os.listdir('/opt/ros'):
includes.append('/opt/ros/' + distribution + '/include')
return includes
def parse_srdf (srdf, packageName = None, prefix = None):
"""
parameters:
- packageName: if provided, the filename is considered relative to this ROS package
- prefix: if provided, the name of the elements will be prepended with
prefix + "/"
"""
import os
if packageName is not None:
from rospkg import RosPack
rospack = RosPack()
path = rospack.get_path(packageName)
srdfFn = os.path.join(path, srdf)
else:
srdfFn = srdf
# tree = ET.fromstring (srdfFn)
tree = ET.parse (srdfFn)
root = tree.getroot()
grippers = {}
for xml in root.iter('gripper'):
n = _read_name (xml)
g = { "robot": prefix,
"name": n,
"clearance": _read_clearance (xml),
"link": _read_link (xml),
"position": _read_position (xml),
"joints": _read_joints (xml),
}
tc = _read_torque_constant (xml)
if tc is not None: g["torque_constant"] = tc
grippers[ prefix + "/" + n if prefix is not None else n] = g
handles = {}
for xml in root.iter('handle'):
n = _read_name (xml)
h = { "robot": prefix,
"name": n,
"clearance": _read_clearance (xml),
"link": _read_link (xml),
"position": _read_position (xml),
"mask": _read_mask (xml),
}
handles[ prefix + "/" + n if prefix is not None else n] = h
return { "grippers": grippers, "handles": handles}
def test_RosPack_no_env():
# regression test for #3680
from rospkg import RosPack, ResourceNotFound
try:
environ_copy = os.environ.copy()
if 'ROS_ROOT' in os.environ:
del os.environ['ROS_ROOT']
if 'ROS_PACKAGE_PATH' in os.environ:
del os.environ['ROS_PACKAGE_PATH']
r = RosPack()
try:
r.get_depends('roscpp')
assert False, "should have raised"
except ResourceNotFound:
pass
finally:
os.environ = environ_copy
def getRosIncludePaths():
import os
try:
from rospkg import RosPack
except ImportError:
return []
rospack = RosPack()
includes = []
includes.append(os.path.expandvars('$ROS_WORKSPACE') + '/devel/include')
for p in rospack.list():
if os.path.exists(rospack.get_path(p) + '/include'):
includes.append(rospack.get_path(p) + '/include')
if os.path.exists('/opt/ros/indigo/include'):
includes.append('/opt/ros/indigo/include')
if os.path.exists('/opt/ros/hydro/include'):
includes.append('/opt/ros/hydro/include')
return includes
def GetRosIncludePaths():
"""Return a list of potential include directories
The directories are looked for in $ROS_WORKSPACE.
"""
try:
from rospkg import RosPack
except ImportError:
return []
rospack = RosPack()
includes = []
includes.append(os.path.expandvars('$ROS_WORKSPACE') + '/devel/include')
for p in rospack.list():
if os.path.exists(rospack.get_path(p) + '/include'):
includes.append(rospack.get_path(p) + '/include')
for distribution in os.listdir('/opt/ros'):
includes.append('/opt/ros/' + distribution + '/include')
return includes
def __init__(self, parent=None):
pyqtgraph.setConfigOption('background', 'w') # before loading widget
super(AudioTopicPlotter, self).__init__(parent)
# We load dynamically the ui file
rp = RosPack()
pkg_path = rp.get_path('audio_stream_arecord_hack')
uic.loadUi(pkg_path + '/scripts/ui_pcm_vol.ui', self)
self.grPCM.plotItem.showGrid(True, True, 0.7)
self.maxPCM = 0
self.blocksize = None
self.last_data = None
self.new_data = False
self.sub = rospy.Subscriber('/audio_stream/audio_data',
AudioData,
self.audio_cb,
queue_size=10)
rospy.loginfo('Subscribed to topic: ' + str(self.sub.resolved_name))
def test_RosPackage_get_depends_explicit():
from rospkg import RosPack, get_ros_root
path = get_package_test_path()
r = RosPack(ros_paths=[path])
implicit=False
assert set(r.get_depends('baz', implicit)) == set(['bar', 'foo'])
assert r.get_depends('bar', implicit) == ['foo']
assert r.get_depends('foo', implicit) == []
if get_ros_root() and rospack_is_available():
# stress test: test default environment against rospack
r = RosPack()
for p in rospack_list():
retval = set(r.get_depends(p, implicit))
rospackval = set(rospack_depends1(p))
assert retval == rospackval, "[%s]: %s vs. %s"%(p, retval, rospackval)
def save_model(self):
os.chdir(RosPack().get_path('panda_dagger'))
if not os.path.exists("./models"):
os.mkdir("./models")
os.chdir("./models")
tl.files.save_npz(self.n_test.all_params,
name=self.name + '.npz',
sess=self.sess)
def collect_plugins(root_scan_dir):
"""
Scan directories starting in the designated root to locate all
packages that depend on pluginlib. This will indirectly tell us
which packages potentially export plugins. Then we search for the
plugin manifest file and we parse it to obtain all the exported
plugin classes.
root_scan_dir indicates the starting point for the scan
returns the collected plugin classes
"""
rp = RosPack([root_scan_dir])
packages = rp.list()
debug_print("Found packages:\n")
#print packages
debug_print()
# Find all packages that depend on pluginlib explicitely
pluginlib_users = rp.get_depends_on('pluginlib', implicit=False)
debug_print("Packages that depend on pluginlib:\n")
debug_print(pluginlib_users)
debug_print()
# Within the packages that require pluginlib, search all their
# dependencies for plugins
plugin_classes = []
for p in pluginlib_users:
path = rp.get_path(p)
debug_print(p + ": ")
debug_print(path)
exports = rp.get_manifest(p).exports
debug_print("Exports: ")
for e in exports:
s = e.get("plugin")
if s:
s2 = string.replace(s, "${prefix}", path)
debug_print(s2)
f = open(s2, 'r')
xml_str = f.read()
xml_str = escape_xml(xml_str)
plugin_classes += parse_plugin_xml(xml_str, p, path)
#plugin_classes += parse_plugin_xml(s2)
debug_print(plugin_classes)
f.close()
debug_print()
return plugin_classes
def __init__(self, haar_cascade_file=None):
# Load the classifier
if haar_cascade_file:
self.classifier = cv2.CascadeClassifier(haar_cascade_file)
else:
rp = RosPack()
pkg_path = rp.get_path('head_depth')
self.classifier = cv2.CascadeClassifier(
pkg_path +
"/file/haarcascades/haarcascade_range_multiview_5p_bg.xml")
# Subscribe to the topics
self.bridge = CvBridge()
self.last_depth = None
self.depth_sub = rospy.Subscriber(
'/pepper/camera/depth/image_raw/compressedDepth',
CompressedImage, self.depth_cb, queue_size=1)
self.last_rgb = None
def test_RosPack_no_env():
# regression test for #3680
from rospkg import RosPack, ResourceNotFound
try:
environ_copy = os.environ.copy()
if 'ROS_ROOT' in os.environ:
del os.environ['ROS_ROOT']
if 'ROS_PACKAGE_PATH' in os.environ:
del os.environ['ROS_PACKAGE_PATH']
r = RosPack()
try:
r.get_depends('roscpp')
assert False, "should have raised"
except ResourceNotFound:
pass
finally:
os.environ = environ_copy
def __init__(self, name, robotnumber,
device = None, tracer = None):
AbstractHumanoidRobot.__init__ (self, name, tracer)
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.device = device
self.AdditionalFrames.append(
("accelerometer",
matrixToTuple(self.accelerometerPosition), "chest"))
self.AdditionalFrames.append(
("gyrometer",
matrixToTuple(self.gyrometerPosition), "chest"))
self.AdditionalFrames.append(
("leftFootForceSensor",
self.forceSensorInLeftAnkle, "left-ankle"))
self.AdditionalFrames.append(
("rightFootForceSensor",
self.forceSensorInRightAnkle, "right-ankle"))
self.OperationalPointsMap = {'left-wrist' : 'LARM_JOINT5',
'right-wrist' : 'RARM_JOINT5',
'left-ankle' : 'LLEG_JOINT5',
'right-ankle' : 'RLEG_JOINT5',
'gaze' : 'HEAD_JOINT1',
'waist' : 'WAIST',
'chest' : 'CHEST_JOINT1'}
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
rospack = RosPack()
self.urdfPath = rospack.get_path('hrp2_{0}_description'.format(robotnumber)) + '/urdf/hrp2_{0}.urdf'.format(robotnumber)
self.pinocchioModel = se3.buildModelFromUrdf(self.urdfPath, se3.JointModelFreeFlyer())
self.pinocchioData = self.pinocchioModel.createData()
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dimension = self.dynamic.getDimension()
self.plugVelocityFromDevice = True
if self.dimension != len(self.halfSitting):
raise RuntimeError("Dimension of half-sitting: {0} differs from dimension of robot: {1}".format (len(self.halfSitting), self.dimension))
self.initializeRobot()
self.dynamic.displayModel()
def __init__(self):
self.rospack = RosPack()
with open(join(self.rospack.get_path('apex_playground'), 'config', 'general.json')) as f:
self.params = json.load(f)
with open(join(self.rospack.get_path('apex_playground'), 'config', 'torso.json')) as f:
self.torso_params = json.load(f)
self.outside_ros = rospy.get_param('/use_sim_time', False) # True if work manager <-> controller comm must use ZMQ
id = search(r"(\d+)", rospy.get_namespace())
self.worker_id = 0 if id is None else int(id.groups()[0]) # TODO string worker ID
self.work = WorkManager(self.worker_id, self.outside_ros)
self.torso = Torso()
self.ergo = Ergo()
self.learning = Learning()
self.perception = Perception()
rospy.loginfo('Controller fully started!')
def get_test_rospkgs():
test_dir = get_test_tree_dir()
ros_root = os.path.join(test_dir, 'ros')
ros_package_path = os.path.join(test_dir, 'stacks')
ros_paths = [ros_root, ros_package_path]
rospack = RosPack(ros_paths=ros_paths)
rosstack = RosStack(ros_paths=ros_paths)
return rospack, rosstack
def __init__(self, parent, guicontext):
'''
A GUi panel to list common operation commands for Hironx / NEXTAGE Open robot.
@param guicontext: The plugin context to create the monitor in.
@type guicontext: qt_gui.plugin_context.PluginContext
'''
super(HironxoCommandPanel, self).__init__()
self._parent = parent
self._guicontext = guicontext
# RTM Client
rtm.nshost = self.get_rosmaster_domain().hostname
rtm.nsport = rospy.get_param('rtmnameserver_port', '15005')
robotname = rospy.get_param('rtmnameserver_robotname',
'HiroNX(Robot)0')
rospy.loginfo(
'Connecting to RTM nameserver. host={}, port={}, robotname={}'.
format(rtm.nshost, rtm.nsport, robotname))
self._rtm = HIRONX()
self._rtm.init(robotname=robotname, url='')
rospack = RosPack()
ui_file = os.path.join(rospack.get_path(PKG_NAME), 'resource',
'hironx_commandpanel_main.ui')
loadUi(ui_file, self, {'HironxoCommandPanel': HironxoCommandPanel})
self._precision_output = 4 # Default degree order to be used to print values
# Assign callback methods
self.pushButton_checkEncoders.clicked[bool].connect(
self._check_encoders)
self.pushButton_goInitial.clicked[bool].connect(self._go_initial)
self.pushButton_goInitial_factoryval.clicked[bool].connect(
self._go_initial_factoryval)
self.pushButton_goOffPose.clicked[bool].connect(self._go_offPose)
self.pushButton_startImpedance.clicked[bool].connect(
self._impedance_on)
self.pushButton_stopImpedance.clicked[bool].connect(
self._impedance_off)
self.pushButton_actualPose_l.clicked[bool].connect(self._actual_pose_l)
self.pushButton_actualPose_r.clicked[bool].connect(self._actual_pose_r)
self.spinBox_precision_output.valueChanged[int].connect(
self._get_precision_output)
self.pushButton_groups.clicked[bool].connect(self._show_groups)
def __init__(self):
top = RosPack().get_path('mobility_games')
self.sound_folder = os.path.join(top, 'auditory/sound_files')
rospy.init_node('audio_feedback')
# Set initial values for reconfigure
self.mode = rospy.get_param("~mode", "sound")
self.sweep_tolerance = rospy.get_param("~sweep_tolerance", "2.0")
self.sweep_range = rospy.get_param("~sweep_range", "0.6")
# Set initial positions to None
self.x = None
self.y = None
self.yaw = None
self.cane_x = None
self.cane_y = None
self.cane_z = None
self.start = False
self.num_sweeps = 0
self.count_sweeps = True
self.reward_at = {}
for num in [10, 20, 50, 100, 250, 500, 1000]:
self.reward_at[num] = True
# Associate April tag ID to different wav files.
self.tag_to_music_file = {3: "ambience2.wav", 12: "generic_music.wav"}
self.tag_to_music_object = {}
self.tag_to_sound_file = {3: "ding.wav", 12: "beep.wav"}
self.tag_to_sound_object = {}
for tag_id in self.tag_to_music_file:
# Create wav object of each track in music object dictionary
self.tag_to_music_object[tag_id] = \
pw.Wav(os.path.join(self.sound_folder,
self.tag_to_music_file[tag_id]))
for tag_id in self.tag_to_sound_file:
# Create wav obejct of each sound file in sound disctionary
self.tag_to_sound_object[tag_id] = \
pw.Wav(os.path.join(self.sound_folder,
self.tag_to_music_file[tag_id]))
self.reward_sound_file = None
self.reward_sound_object = None
self.engine = pyttsx.init()
self.hasSpoken = False
# Set up dynamic reconfigure server
Server(MusicalCaneConfig, self.config_callback)
rospy.Subscriber('/tango_pose', PoseStamped, self.process_pose)
rospy.Subscriber('/cane_tip', PoseStamped, self.process_cane_tip)
rospy.Subscriber('/fisheye_undistorted/tag_detections',
AprilTagDetectionArray, self.tag_array)
def roscreatepkg_main():
from optparse import OptionParser
parser = OptionParser(
usage="usage: %prog <package-name> [dependencies...]", prog=NAME)
options, args = parser.parse_args()
if not args:
parser.error(
"you must specify a package name and optionally also list package dependencies"
)
package = args[0]
if not roslib.names.is_legal_resource_base_name(package):
parser.error(
"illegal package name: %s\nNames must start with a letter and contain only alphanumeric characters\nand underscores."
% package)
# validate dependencies and turn into XML
depends = args[1:]
uses_roscpp = 'roscpp' in depends
uses_rospy = 'rospy' in depends
rospack = RosPack()
for d in depends:
try:
rospack.get_path(d)
except ResourceNotFound:
print("ERROR: dependency [%s] cannot be found" % d,
file=sys.stderr)
sys.exit(1)
depends = u''.join([u' <depend package="%s"/>\n' % d for d in depends])
if not on_ros_path(os.getcwd()):
print(
'!' * 80 +
"\nWARNING: current working directory is not on ROS_PACKAGE_PATH!\nPlease update your ROS_PACKAGE_PATH environment variable.\n"
+ '!' * 80,
file=sys.stderr)
if type(package) == str:
package = package.decode('utf-8')
create_package(package,
author_name(),
depends,
uses_roscpp=uses_roscpp,
uses_rospy=uses_rospy)
def __init__(self):
self.rospack = RosPack()
self.web_app_root = join(self.rospack.get_path('nips2016'), 'webapp',
'static')
self.app = Flask(__name__,
static_url_path='',
static_folder=self.web_app_root)
self.cors = CORS(self.app, resources={r'/api/*': {'origins': '*'}})
self.services = UserServices()
self.app.route('/')(self.root)
self.app.route('/api/interests',
methods=['GET'])(self.experiment_status)
self.app.route('/api/focus', methods=['POST'])(self.update_focus)
self.app.route('/api/time-travel', methods=['POST'])(self.time_travel)
self.app.route('/api/reset', methods=['POST'])(self.reset)
self.app.route('/api/assessment',
methods=['POST'])(self.update_assessment)
def __init__(self):
self.rospack = RosPack()
# Using this topic
self.topic_name_result = "/thr/robot_run_action/right/result"
self._baxter_result = None
# Serving these services
self.service_name_get_result = "/pobax_playground/baxter/get_result"
def get_rospkg():
# configure inside of the test tree
test_dir = get_test_dir()
ros_root = os.path.join(test_dir, 'ros')
ros_package_path = os.path.join(test_dir, 'stacks')
ros_paths = [ros_root, ros_package_path]
rospack = RosPack(ros_paths=ros_paths)
rosstack = RosStack(ros_paths=ros_paths)
return rospack, rosstack
