def __init__(self):
self.rospack = RosPack()
with open(
join(self.rospack.get_path('apex_playground'), 'config',
'recorder.json')) as f:
self.params = json.load(f)
self.cameras = []
for camera in self.params['cameras']:
self.cameras.append(
CameraRecorder(camera, self.params['duration'],
self.params['max_rate'], self.params['video']))
self.cameras[-1].start()
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)
self.torso = Torso()
self.ergo = Ergo()
self.learning = Learning()
self.perception = Perception()
self.iteration = -1 # -1 means "not up to date"
rospy.Subscriber('/apex_playground/iteration', UInt32,
self._cb_iteration)
rospy.loginfo('Controller fully started!')
def generate_wholebody_load(cfg, cs, fullBody=None, viewer=None):
rp = RosPack()
urdf = rp.get_path(
cfg.Robot.packageName
) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf'
logger.info("load robot : %s", urdf)
robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
logger.info("robot loaded.")
cs_wb = ContactSequence()
logger.warning("Load wholebody contact sequence from file : %s",
cfg.WB_FILENAME)
cs_wb.loadFromBinary(cfg.WB_FILENAME)
return cs_wb
def _get_robot_move_command(self):
"""Build command to move the robot (to be called as subprocess)
:returns list containing executable with path and appropriate arguments
"""
ptp_goal = [
str(x) for x in self.test_data.get_joints(self._PTP_TEST_NAME,
_GROUP_NAME)
]
movecmd = RosPack().get_path(
"pilz_robot_programming"
) + '/test/integrationtests/movecmd.py ptp joint'
return movecmd.split(" ") + ptp_goal
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 get_data(self, experiment_id):
csv_file = os.path.join(RosPack().get_path("mppi_ros"), "log",
"record.csv")
# df = pd.read_csv(csv_file, converters={'stage_cost': eval, 'step_count': eval, 'weights': eval, 'time': eval,
# 'effective_samples': eval})
df = pd.read_csv(csv_file)
df['tree_search'] = df['tree_search'].apply(lambda x: "Tree"
if x else "Monte Carlo")
df = df.rename(columns={'tree_search': 'Sampling Strategy'})
print("Looking for experiments with id: {}".format(experiment_id))
df = df[df['id'].str.contains(experiment_id)]
print("Found {}".format(len(df)))
return df
def getPkgRoot(filename):
pkg_name = getPkgName(filename)
if not pkg_name:
return ''
try:
from rospkg import RosPack
except ImportError:
return ''
try:
p = RosPack()
path = p.get_path(pkg_name)
return path
except ResourceNotFound:
return ''
def __init__(self):
self.rospack = RosPack()
with open(
join(self.rospack.get_path('apex_playground'), 'config',
'ergo.json')) as f:
self.params = json.load(f)
self.button = Button(self.params)
self.rate = rospy.Rate(self.params['publish_rate'])
self.button.switch_led(False)
# Service callers
self.robot_reach_srv_name = '{}/reach'.format(
self.params['robot_name'])
self.robot_compliant_srv_name = '{}/set_compliant'.format(
self.params['robot_name'])
rospy.loginfo("Ergo node is waiting for poppy controllers...")
rospy.wait_for_service(self.robot_reach_srv_name)
rospy.wait_for_service(self.robot_compliant_srv_name)
self.reach_proxy = rospy.ServiceProxy(self.robot_reach_srv_name,
ReachTarget)
self.compliant_proxy = rospy.ServiceProxy(
self.robot_compliant_srv_name, SetCompliant)
rospy.loginfo("Controllers connected!")
self.state_pub = rospy.Publisher('ergo/state',
CircularState,
queue_size=1)
self.button_pub = rospy.Publisher('sensors/buttons/help',
Bool,
queue_size=1)
self.goals = []
self.goal = 0.
self.joy_x = 0.
self.joy_y = 0.
self.motion_started_joy = 0.
self.js = JointState()
rospy.Subscriber(
'sensors/joystick/{}'.format(self.params["control_joystick_id"]),
Joy, self.cb_joy)
rospy.Subscriber('{}/joint_state'.format(self.params['robot_name']),
JointState, self.cb_js)
rospy.Subscriber('sensors/button_leds/pause', Bool, self.cb_bt_led)
self.t = rospy.Time.now()
self.srv_reset = None
self.extended = False
self.standby = False
self.last_activity = rospy.Time.now()
self.delta_t = rospy.Time.now()
def test_RosPack_get_path():
from rospkg import RosPack, ResourceNotFound, get_ros_root
path = get_package_test_path()
foo_path = os.path.join(path, 'p1', 'foo')
foo_path_alt = os.path.join(path, 'p2', 'foo')
bar_path = os.path.join(path, 'p1', 'bar')
baz_path = os.path.join(path, 'p2', 'baz')
# point ROS_ROOT at top, should spider entire tree
print("ROS path: %s" % (path))
r = RosPack(ros_paths=[path])
# precedence in this case is undefined as there are two 'foo's in the same path
assert r.get_path('foo') in [foo_path, foo_path_alt]
assert bar_path == r.get_path('bar')
assert baz_path == r.get_path('baz')
try:
r.get_path('fake')
assert False
except ResourceNotFound:
pass
# divide tree in half to test precedence
print("ROS_PATH 1: %s" % (os.path.join(path, 'p1')))
print("ROS_PATH 2: %s" % (os.path.join(path, 'p2')))
r = RosPack(ros_paths=[os.path.join(path, 'p1'), os.path.join(path, 'p2')])
assert foo_path == r.get_path('foo'), "%s vs. %s" % (foo_path, r.get_path('foo'))
assert bar_path == r.get_path('bar')
assert baz_path == r.get_path('baz')
if get_ros_root() and rospack_is_available():
# stresstest against rospack
r = RosPack()
for p in rospack_list():
retval = r.get_path(p)
rospackval = rospack_find(p)
assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval, rospackval)
def __init__(self):
self.rospack = RosPack()
with open(
join(self.rospack.get_path('nips2016'), 'config',
'bounds.json')) as f:
self.bounds = json.load(f)
self.bounds_motors_min = np.array(
[float(bound[0]) for bound in self.bounds['motors']['positions']])
self.bounds_motors_max = np.array(
[float(bound[1]) for bound in self.bounds['motors']['positions']])
self.bounds_sensory_min = [
d for space in [
'hand', 'joystick_1', 'joystick_2', 'ergo', 'ball', 'light',
'sound'
] for d in
[float(bound[0]) for bound in self.bounds['sensory'][space]] * 10
]
self.bounds_sensory_min = np.array([
float(self.bounds['sensory']['ergo'][0][0]),
float(self.bounds['sensory']['ball'][0][0])
] + self.bounds_sensory_min)
self.bounds_sensory_max = [
d for space in [
'hand', 'joystick_1', 'joystick_2', 'ergo', 'ball', 'light',
'sound'
] for d in
[float(bound[1]) for bound in self.bounds['sensory'][space]] * 10
]
self.bounds_sensory_max = np.array([
float(self.bounds['sensory']['ergo'][0][1]),
float(self.bounds['sensory']['ball'][0][1])
] + self.bounds_sensory_max)
self.bounds_sensory_diff = self.bounds_sensory_max - self.bounds_sensory_min
# DMP PARAMETERS
self.n_dmps = 4
self.n_bfs = 7
self.timesteps = 30
self.max_params = np.array([300.] * self.n_bfs * self.n_dmps +
[1.] * self.n_dmps)
self.motor_dmp = MyDMP(n_dmps=self.n_dmps,
n_bfs=self.n_bfs,
timesteps=self.timesteps,
max_params=self.max_params)
self.context = {}
self.config = dict(m_mins=[-1.] * 32,
m_maxs=[1.] * 32,
s_mins=[-1.] * 132,
s_maxs=[1.] * 132)
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 and nodelet explicitely
pluginlib_users = rp.get_depends_on('pluginlib', implicit=False)
nodelet_users = rp.get_depends_on('nodelet', implicit=False)
# Concatenate both lists removing the duplicates
pluginlib_users += list(set(nodelet_users) - set(pluginlib_users))
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 test_RosPack_get_rosdeps():
from rospkg import RosPack
path = get_package_test_path()
r = RosPack(ros_paths=[os.path.join(path, 'p1'), os.path.join(path, 'p2')])
# repeat tests due to caching
assert set(['foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3']) == set(r.get_rosdeps('foo', implicit=True)), r.get_rosdeps('foo', implicit=True)
assert set(['foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3']) == set(r.get_rosdeps('foo', implicit=True))
assert set(['foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3']) == set(r.get_rosdeps('foo', implicit=False))
assert set(['bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('bar', implicit=False))
assert set(['foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('bar', implicit=True))
assert set(['foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('bar', implicit=True))
assert set(['foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('bar'))
assert ['baz_rosdep1'] == r.get_rosdeps('baz', implicit=False)
assert set(['baz_rosdep1', 'foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('baz'))
assert set(['baz_rosdep1', 'foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('baz'))
# create a brand new instance to test with brand new cache
r = RosPack(ros_paths=[os.path.join(path, 'p1'), os.path.join(path, 'p2')])
assert set(['baz_rosdep1', 'foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('baz'))
assert set(['baz_rosdep1', 'foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('baz'))
def optimize_pose(self):
"""
Run g2o
"""
self.write_g2o_data()
package = RosPack().get_path('navigation_prototypes')
# path to compiled g2o file
g2o_data_path = path.join(package, 'data/data_g2o/data.g2o')
g2o_data_copy_path = path.join(
package, 'data/data_g2o/data_cp.g2o') # copy of the unedit data
g2o_result_path = path.join(package, 'data/data_g2o/result.g2o')
system("g2o -o %s %s" % (g2o_result_path, g2o_data_path)) # Run G2o
# Copy Original Data
system("cp %s %s" % (g2o_data_path, g2o_data_copy_path))
print("OPTIMIZATION COMPLETED")
def generateWholeBodyMotion(cs, fullBody=None, viewer=None):
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
robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer(), False)
if cfg.WB_VERBOSE:
print "robot loaded."
filename = cfg.EXPORT_PATH + "/npz/" + cfg.DEMO_NAME + ".npz"
print "Load npz file : ", filename
res = wholebody_result.loadFromNPZ(filename)
return res, robot
def __init__(self, outside_ros=False):
self.rospack = RosPack()
self.num_workers = 0
self.outside_ros = rospy.get_param(
'/use_sim_time', outside_ros
) # True if work manager <-> controller comm must use ZMQ
self.file_experiment = join(self.rospack.get_path('apex_playground'),
'config', 'experiment.json')
with open(self.file_experiment) as f:
experiment = self.check(json.load(f))
rospy.set_param('/work', experiment)
self.lock_experiment = RLock()
self.failing_workers = {
} # association {num_worker: remaining attempts before blacklist}
def soundFinder(gen, param_name = "rewardSound"):
top = RosPack().get_path("mobility_games")
sounds = [];
firstsound = None;
for dirname, dirnames, filenames in os.walk(top + '/auditory/sound_files'):
first = True
for fname in filenames:
if fname.endswith('.wav'):
if first:
firstsound = os.path.join(dirname, fname)
first = False
sounds.append(gen.const(fname[:-4], str_t, os.path.join(dirname, fname), fname))
print(fname[:-4])
sound_enum = gen.enum(sounds, "An enum to set .wav Sound")
gen.add(param_name, str_t, 0, "Filename of Sound file to be used as reward", firstsound, edit_method=sound_enum)
def __init__(self):
self.rospack = RosPack()
with open(
join(self.rospack.get_path('nips2016'), 'config',
'ergo.json')) as f:
self.params = json.load(f)
self.button = Button(self.params)
self.rate = rospy.Rate(self.params['publish_rate'])
self.eef_pub = rospy.Publisher('/nips2016/ergo/end_effector_pose',
PoseStamped,
queue_size=1)
self.state_pub = rospy.Publisher('/nips2016/ergo/state',
CircularState,
queue_size=1)
self.button_pub = rospy.Publisher('/nips2016/ergo/button',
Bool,
queue_size=1)
self.joy_pub = rospy.Publisher('/nips2016/ergo/joysticks/1',
Joy,
queue_size=1)
self.joy_pub2 = rospy.Publisher('/nips2016/ergo/joysticks/2',
Joy,
queue_size=1)
self.srv_reset = None
self.ergo = None
self.extended = False
self.standby = False
self.last_activity = rospy.Time.now()
if pygame.joystick.get_count() < 2:
rospy.logerr(
"Ergo: Expecting 2 joysticks but found only {}, exiting".
format(pygame.joystick.get_count()))
sys.exit(0)
else:
self.joystick = pygame.joystick.Joystick(0)
self.joystick2 = pygame.joystick.Joystick(1)
self.joystick.init()
self.joystick2.init()
if self.params['useless_joystick_id'] != int(
self.joystick2.get_name()[-1]):
useless_joy = self.joystick
self.joystick = self.joystick2
self.joystick2 = useless_joy
rospy.loginfo('Initialized Joystick 1: {}'.format(
self.joystick.get_name()))
rospy.loginfo('Initialized Joystick 2: {}'.format(
self.joystick2.get_name()))
def __init__(self, robot_name):
"""
:param robot_name: Robot name and ROS topics/services namespace
"""
self.rospack = RosPack()
with open(
join(self.rospack.get_path('poppy_torso_controllers'),
'config', 'torso.json')) as f:
self.params = json.load(f)
self.publish_rate = rospy.Rate(self.params['publish_rate'])
self.robot_name = robot_name
self.eef_pub_l = rospy.Publisher('left_arm/end_effector_pose',
PoseStamped,
queue_size=1)
self.eef_pub_r = rospy.Publisher('right_arm/end_effector_pose',
PoseStamped,
queue_size=1)
self.js_pub_l = rospy.Publisher('left_arm/joints',
JointState,
queue_size=1)
self.js_pub_r = rospy.Publisher('right_arm/joints',
JointState,
queue_size=1)
self.js_pub_full = rospy.Publisher('joint_state',
JointState,
queue_size=1)
# Services
self.srv_execute = None
self.srv_reach = None
self.srv_set_torque = None
self.srv_left_arm_set_compliant = None
self.srv_right_arm_set_compliant = None
self.srv_robot_set_compliant = None
self.srv_set_head_idle = None
self.srv_set_left_idle = None
self.srv_set_right_idle = None
# Behaviors
self.right_idle = None
self.left_idle = None
self.head_idle = None
# Protected resources
self.torso = None
self.robot_lock = RLock()
def __init__(self):
self.rospack = RosPack()
self.port = 80 if os.getuid() == 0 else 5000
self.app = Flask(__name__)
getLogger(__name__).setLevel(INFO)
#self.cors = CORS(self.app, resources={r'/api/*': {'origins': '*'}})
self.app.route('/ready', methods=['GET'])(self.get_ready)
self.app.route('/go', methods=['POST'])(self.post_go)
self.app.route('/rate', methods=['POST'])(self.post_rate)
self.app.route('/reset', methods=['POST'])(self.post_reset)
self._commanding_sub = rospy.Subscriber("/iiwa/commanding_status",
Bool,
self._cb_commanding_status,
queue_size=1)
self._last_commanding_status = False
self._last_commanding_status_date = rospy.Time(0)
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('nips2016'), '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('/nips2016/environment/ball', CircularState, queue_size=1)
self.light_pub = rospy.Publisher('/nips2016/environment/light', UInt8, queue_size=1)
self.sound_pub = rospy.Publisher('/nips2016/environment/sound', Float32, queue_size=1)
self.rate = rospy.Rate(self.params['rate'])
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)
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 create_from_rospkg(rospack=None,
rosstack=None,
sources_loader=None,
verbose=False):
"""
Create :class:`RosdepLookup` based on current ROS package
environment.
:param rospack: (optional) Override :class:`rospkg.RosPack`
instance used to crawl ROS packages.
:param rosstack: (optional) Override :class:`rospkg.RosStack`
instance used to crawl ROS stacks.
:param sources_loader: (optional) Override SourcesLoader used
for managing sources.list data sources.
"""
# initialize the loader
if rospack is None:
rospack = RosPack()
if rosstack is None:
rosstack = RosStack()
if sources_loader is None:
sources_loader = SourcesListLoader.create_default(verbose=verbose)
rosdep_db = RosdepDatabase()
# Use sources list to initialize rosdep_db. Underlay has no
# notion of specific resources, and its view keys are just the
# individual sources it can load from. SourcesListLoader
# cannot do delayed evaluation of OS setting due to matcher.
underlay_key = SourcesListLoader.ALL_VIEW_KEY
# Create the rospkg loader on top of the underlay
loader = RosPkgLoader(rospack=rospack,
rosstack=rosstack,
underlay_key=underlay_key)
# create our actual instance
lookup = RosdepLookup(rosdep_db, loader)
# load in the underlay
lookup._load_all_views(loader=sources_loader)
# use dependencies to implement precedence
view_dependencies = sources_loader.get_loadable_views()
rosdep_db.set_view_data(underlay_key, {}, view_dependencies,
underlay_key)
return lookup
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 __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 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 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, 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 __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 __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
