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 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, 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 __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 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, 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, 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 __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):
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 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 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 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 __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 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'
class SoundController(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)
with open(
join(self.rospack.get_path('apex_playground'), 'config',
'bounds.json')) as f:
self.bounds = json.load(f)["sensory"]["sound"][0]
self.p = pyaudio.PyAudio()
self.fs = 44100 # sampling rate, Hz, must be integer
self.duration = 1. / self.params['rate']
# for paFloat32 sample values must be in range [-1.0, 1.0]
self.stream = self.p.open(format=pyaudio.paFloat32,
channels=1,
rate=self.fs,
output=True)
def cb_sound(self, msg):
value = msg.data
if value != 0.:
f = (value-self.bounds[0])/(self.bounds[1]-self.bounds[0]) *\
(self.params["sound"]["freq"][1] - self.params["sound"]["freq"][0]) +\
self.params["sound"]["freq"][0]
self.beep(f)
def beep(self, f):
samples = (np.sin(2 * np.pi * np.arange(self.fs * self.duration) * f /
self.fs)).astype(np.float32)
self.stream.write(samples)
def run(self):
rospy.Subscriber("apex_playground/environment/sound", Float32,
self.cb_sound)
rospy.spin()
self.stream.stop_stream()
self.stream.close()
self.p.terminate()
def get_res_path(package_name, res_path=None):
rp = RosPack()
try:
dirpath = rp.get_path(package_name)
except ResourceNotFound as err:
rospy.logwarn('unable find given rospackage in "get_res_path"')
return None
if res_path is None:
res_path = 'res'
return os.path.join(dirpath, res_path)
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)
create_package(package,
author_name(),
depends,
uses_roscpp=uses_roscpp,
uses_rospy=uses_rospy)
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 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('/home/yuebenben/catkin_ws/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 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 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, robot
class EnvironmentConversions(object):
def __init__(self):
self.rospack = RosPack()
self.last_angle = None
with open(
join(self.rospack.get_path('apex_playground'), 'config',
'environment.json')) as f:
self.params = json.load(f)
self.filename = '/home/pi/ball_trajectory.csv'
def save_ball_pos(self, x, y):
with open(self.filename, 'a') as csvFile:
writer = csv.writer(csvFile)
writer.writerow([str(time.time()), str(x), str(y)])
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])
self.save_ball_pos(x, y)
elongation = sqrt(x * x + y * y)
theta = arctan2(y, x)
state = CircularState()
state.extended = elongation > ring_radius
state.angle = theta
return state
def ball_to_color(self, state):
"""
Reduce the given 2D ball position to color
:param state: the requested circular state of the ball
:return: hue value designating the color in [0, 255]
"""
max_speed = 0.25
min_speed = 0.07
if self.last_angle is None:
hue = 0
else:
distance = abs(state.angle - self.last_angle)
speed = max(min_speed,
min(max_speed, min(distance, 2 * pi - distance)))
hue = int((speed - min_speed) / (max_speed - min_speed) * 255)
self.last_angle = state.angle
return hue
def ball_to_sound(self, state):
"""
Reduce the given 2D ball position to sound
:param state: the requested circular state of the ball
:return: sound float designating the color within the same range than the circular state angle
"""
return state.angle if state.extended else 0.
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 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 __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 __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 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')
logging.ino("Found ROS package: " + p)
for distribution in os.listdir('/opt/ros'):
includes.append('/opt/ros/' + distribution + '/include')
logging.info("Found ROS distro: " + distribution)
return includes
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 initScenePinocchio(urdfName,
packageName,
envName=None,
envPackageName="hpp_environments"):
rp = RosPack()
urdf = rp.get_path(packageName) + '/urdf/' + urdfName + '.urdf'
robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(),
pin.JointModelFreeFlyer())
robot.initDisplay(loadModel=True)
robot.displayCollisions(False)
robot.displayVisuals(True)
robot.display(robot.model.neutralConfiguration)
cl = gepetto.corbaserver.Client()
gui = cl.gui
if envName:
urdfEnvPath = rp.get_path(envPackageName)
urdfEnv = urdfEnvPath + '/urdf/' + envName + '.urdf'
gui.addUrdfObjects(sceneName + "/environments", urdfEnv, urdfEnvPath,
True)
return robot, gui
def get_package_path(package_name, *paths):
"""
return the system path of a file or folder in a ROS package
Example: get_package_path('mas_perception_libs', 'ros', 'src', 'mas_perception_libs')
:type package_name: str
:param paths: chunks of the file path relative to the package
:return: file or directory full path
"""
rp = RosPack()
pkg_path = rp.get_path(package_name)
return os.path.join(pkg_path, *paths)
def save_key_positions_to_disc(self, dummy):
rp = RosPack()
save_path = rp.get_path(self.save_ros_package) + self.save_path
Logger.loginfo("Saving key positions to %s" % save_path)
yaml_dict = dict()
yaml_dict.update({'joints': self._state_machine.userdata.save_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, 'w+') as outfile:
outfile.write(yaml.dump(yaml_dict, default_flow_style=False))
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)
image_transport_users = rp.get_depends_on('image_transport',
implicit=False)
# Concatenate both lists removing the duplicates
pluginlib_users += list(set(nodelet_users) - set(pluginlib_users))
pluginlib_users += list(set(image_transport_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 generate_gzb_world(pedsim_file_name):
rospack1 = RosPack()
pkg_path = rospack1.get_path('pedsim_simulator')
xml_scenario = pkg_path + "/scenarios/" + pedsim_file_name # bo_airport.xml"
rospack2 = RosPack()
pkg_path = rospack2.get_path('pedsim_gazebo_plugin')
gazebo_world = pkg_path + "/worlds/" + \
pedsim_file_name.split('.')[0] + ".world" # bo_airport.xml"
global gzb_world
with open(gazebo_world, 'w') as gzb_world:
print >> gzb_world, "<?xml version=\"1.0\" ?>"
print >> gzb_world, '''
<!-- this file is auto generated using pedsim_gazebo_plugin pkg -->
<sdf version="1.5">
<world name="default">
<!-- Ground Plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
'''
# use the parse() function to load and parse an XML file
# xmlfile = pkg_path + "/scenarios/obstacle.xml"
parseXML(xml_scenario)
print >> gzb_world, '''
<plugin name="ActorPosesPlugin" filename="libActorPosesPlugin.so">
</plugin>
</world>
</sdf>
'''
print "gazbo world has been generated: {}".format(gazebo_world)
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)
image_transport_users = rp.get_depends_on('image_transport', implicit=False)
# Concatenate both lists removing the duplicates
pluginlib_users += list(set(nodelet_users) - set(pluginlib_users))
pluginlib_users += list(set(image_transport_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 __init__(self):
rospack = RosPack()
urdf_file = join(rospack.get_path('human_moveit_config'), 'urdf',
'human.urdf')
model = URDF.from_xml_file(urdf_file)
self.model_dict = {}
for j in model.joints:
self.model_dict[j.child] = {}
self.model_dict[j.child]['parent_joint'] = {
'name': j.name,
'type': j.type
}
self.model_dict[j.child]['parent_link'] = j.parent
def check_map_existance(self):
rp = RosPack()
pkg_path = rp.get_path('wheelchair_navigation')
map_path = pkg_path + "/maps/" + self.world_name + "/" + self.world_name + ".yaml"
if os.path.isfile(map_path):
arguments = map_path
else:
arguments = pkg_path + "/maps/empty_world/empty_world.yaml"
rospy.logwarn(
"Map Server Launcher: The map of %s.world does not exist. The empty_world one will be used instead."
% self.world_name)
return arguments
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
pinRobot = 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):
rospy.loginfo("Initializing TwistFromCan")
rp = RosPack()
pkg_path = rp.get_path('panda_bridge_ros')
path_to_dbc = pkg_path + '/config/honda_civic_touring_2016_can_for_cantools.dbc'
self.can_db = cantools.db.load_file(path_to_dbc)
self.wheel_speed = None
self.wheel_angle = None
self.sub = rospy.Subscriber("can_frame_msgs", Frame,
self.callback, queue_size=10)
self.pub = rospy.Publisher("twist_stamped",
TwistStamped, queue_size=10)
rospy.loginfo("Done")
class Environment(object):
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 update_light(self, state):
self.light_pub.publish(UInt8(data=self.conversions.ball_to_color(state)))
def update_sound(self, state):
self.sound_pub.publish(state.angle if state.extended else 0.) # TODO rescale
def run(self):
if not self.tracking.open():
rospy.logerr("Cannot open the webcam, exiting")
return
while not rospy.is_shutdown():
debug = rospy.get_param('/nips2016/perception/debug', False)
grabbed, frame = self.tracking.read()
if not grabbed:
rospy.logerr("Cannot grab image from webcam, exiting")
break
hsv, mask_ball, mask_arena = self.tracking.get_images(frame)
ball_center, _ = self.tracking.find_center('ball', frame, mask_ball, 20)
arena_center, arena_radius = self.tracking.find_center('arena', frame, mask_arena, 100)
ring_radius = int(arena_radius/self.params['tracking']['ring_divider']) if arena_radius is not None else None
if ball_center is not None and arena_center is not None:
circular_state = self.conversions.get_state(ball_center, arena_center, ring_radius)
self.update_light(circular_state)
self.update_sound(circular_state)
self.ball_pub.publish(circular_state)
if debug:
self.tracking.draw_images(frame, hsv, mask_ball, mask_arena, arena_center, ring_radius)
self.rate.sleep()
def __init__(self):
# Action Client
self.move_client = SimpleActionClient(MOVE_BASE_TOPIC, MoveBaseAction)
rospy.loginfo("BASE CLIENT: Waiting for base action server...")
base_client_running = self.move_client.wait_for_server(
rospy.Duration(3))
if base_client_running:
rospy.loginfo("BASE CLIENT: Base controller initialized.")
else:
rospy.loginfo("BASE CLIENT: Base controller is NOT initialized!")
# Omni Client
self.omni_client = SimpleActionClient(OMNI_BASE_CLIENT_TOPIC,
FollowJointTrajectoryAction)
omni_client_running = self.omni_client.wait_for_server(
rospy.Duration(3))
# Current Pose
rospy.Subscriber(CURRENT_POSE_TOPIC, PoseStamped, self._pose_cb)
self.current_pose = PoseStamped()
# Velocity
self.vel_pub = rospy.Publisher(BASE_VELOCITY_TOPIC,
Twist,
queue_size=10)
# Default move_base goal init
self.move_goal = MoveBaseGoal()
self.move_goal.target_pose.header.frame_id = 'map'
self.timeout = rospy.Duration(30)
# Default traj goal init
# Tolerances
self.tolx = JointTolerance(name='odom_x', position=0.1)
self.toly = JointTolerance(name='odom_y', position=0.1)
self.tolt = JointTolerance(name='odom_t', position=0.1)
self.traj_goal = FollowJointTrajectoryGoal()
self.traj_goal.goal_tolerance = [self.tolx, self.toly, self.tolt]
self.traj_goal.path_tolerance = [self.tolx, self.toly, self.tolt]
self.traj_goal.trajectory.joint_names = OMNI_BASE_JOINTS
self.traj_goal.trajectory.header.frame_id = 'map'
r = RosPack()
pkg_path = r.get_path('frasier_utilities')
self.yaml_filename = pkg_path + '/config/wrs_map.yaml'
with open(self.yaml_filename, 'r') as outfile:
try:
self.locations = load(outfile)
except YAMLError as e:
print e
def genmain_(argv, progname, gen):
rospack = RosPack()
options, args = parse_options(argv, progname)
try:
if len(args) < 2:
print("ERROR: please specify args")
if not os.path.exists(options.outdir):
# This script can be run multiple times in parallel. We
# don't mind if the makedirs call fails because somebody
# else snuck in and created the directory before us.
try:
os.makedirs(options.outdir)
except OSError as e:
if not os.path.exists(options.outdir):
raise
options.outdir = os.path.abspath(options.outdir)
search_path = genmsg.command_line.includepath_to_dict(
options.includepath)
search_path['std_msgs'] = [
os.path.join(rospack.get_path('std_msgs'), 'msg')
]
for d in rospack.get_depends(options.package, implicit=False):
search_path[d] = [os.path.join(rospack.get_path(d), 'msg')]
retcode = gen.generate_messages(options.package, args[1:],
options.outdir, search_path)
except genmsg.InvalidMsgSpec as e:
print("ERROR: ", e, file=sys.stderr)
retcode = 1
except MsgGenerationException as e:
print("ERROR: ", e, file=sys.stderr)
retcode = 2
except Exception as e:
traceback.print_exc()
print("ERROR: ", e)
retcode = 3
return (retcode or 0)
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 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 = []
paths = os.path.expandvars('$ROS_WORKSPACE')
words = paths.split()
for word in words:
includes.append(os.path.expanduser(word) + '/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
class Perception_controller(object):
def __init__(self):
self.services = {
'record': {
'name': '/pobax_playground/perception/record',
'type': Record
},
'get': {
'name': '/pobax_playground/perception/get',
'type': GetSensorialState
}
}
for service_name, service in self.services.items():
rospy.loginfo(
"Perception controller is waiting service {}...".format(
service['name']))
rospy.wait_for_service(service['name'])
service['call'] = rospy.ServiceProxy(service['name'],
service['type'])
self.rospack = RosPack()
with open(
join(self.rospack.get_path('pobax_playground'), 'config',
'perception.json')) as f:
self.params = json.load(f)
# Init Recording action server client for non blocking recording
self.record_server_name = '/pobax_playground/perception/record_server'
self.client = actionlib.SimpleActionClient(self.record_server_name,
RecordAction)
self.client.wait_for_server()
# Blocking record
def record(self, nb_points):
call = self.services['record']['call']
return call(RecordRequest(nb_points=UInt8(
data=nb_points))).sensorial_trajectory
def get(self):
call = self.services['get']['call']
return call(GetSensorialStateRequest())
# Starts non-blocking recording
def start_recording(self, nb_points):
self.client.send_goal(RecordGoal(nb_points=UInt8(data=nb_points)))
def stop_recording(self):
self.client.cancel_goal()
self.client.wait_for_result()
return self.client.get_result().sensorial_trajectory
def _load_trajectory_from_file(ros_package, filename):
"""Loads trajectory from *.json file. Converts unicode strings to Python str for ROS.
Args:
ros_package (str): Name of ros_package where trajectory file is located
filename (str): Name of *.json trajectory file
Returns:
obj: Python object from customized json.load(...) with an custom object_hook function
"""
rospack = RosPack()
package_path = rospack.get_path(ros_package)
file_path = os.path.join(package_path, "trajectories", filename)
with open(file_path, 'r') as loadfile:
return json_load_byteified(loadfile)
class Recorder(object):
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)
def run(self):
rospy.Service('recorder/record', RecordScene, self.cb_record)
rospy.spin()
def cb_record(self, request):
rospy.logerr(
'VRep camera recorder is fake, thus corresponding service is void')
return RecordSceneResponse(recording_duration=self.params['duration'])
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 __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, 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):
# frequency of controlling action!!
self.frequency = 35.0
# initialize counter for publishing to GUI
# we only publish when self.PublishToGUI =1
self.PublishToGUI = 1
# Frequency of publishing to GUI (Hz)
frequency_PubToGui = 10
# we reset counter when self.PublishToGUI >= PublishToGUIBound
self.PublishToGUIBound = int(self.frequency/frequency_PubToGui)
# for saving data
# determine ROS workspace directory
rp = RosPack()
# determine ROS workspace directory where data is saved
package_path = rp.get_path('quad_control')
self.package_save_path = package_path+'/experimental_data/data/'
def __init__(self):
# frequency of controlling action!!
self.frequency = 35.0
# state of quad: position, velocity and attitude
# ROLL, PITCH, AND YAW (EULER ANGLES IN DEGREES)
self.state_quad = numpy.zeros(3+3+3)
# dy default, desired trajectory is staying still in origin
TrajectoryClass = trajectories_dictionary.trajectories_dictionary['StayAtRest']
self.TrajGenerator = TrajectoryClass()
# initialize counter for publishing to GUI
# we only publish when self.PublishToGUI =1
self.PublishToGUI = 1
# Frequency of publishing to GUI (Hz)
frequency_PubToGui = 10
# we reset counter when self.PublishToGUI >= PublishToGUIBound
self.PublishToGUIBound = int(self.frequency/frequency_PubToGui)
# intiialization should be done in another way,
# but median will take care of minimizing effects
self.VelocityEstimator = Velocity_Filter(3,numpy.zeros(3),0.0)
# controllers selected by default
ControllerClass = controllers_dictionary.controllers_dictionary['PIDSimpleBoundedIntegralController']
self.ControllerObject = ControllerClass()
YawControllerClass = yaw_controllers_dictionary.yaw_controllers_dictionary['YawRateControllerTrackReferencePsi']
self.YawControllerObject = YawControllerClass()
# for reseting neutral value that makes iris+ stay at desired altitude
self.DesiredZForceMedian = Median_Filter(10)
# for saving data
# determine ROS workspace directory
rp = RosPack()
# determine ROS workspace directory where data is saved
package_path = rp.get_path('quad_control')
self.package_save_path = package_path+'/experimental_data/data/'
def resolve_paths(text):
'''
Searches in text for $(find ...) statements and replaces it by the package path.
@return: text with replaced statements.
'''
result = text
startIndex = text.find('$(')
if startIndex > -1:
endIndex = text.find(')', startIndex + 2)
script = text[startIndex + 2: endIndex].split()
if len(script) == 2 and (script[0] == 'find'):
pkg = ''
try:
from rospkg import RosPack
rp = RosPack()
pkg = rp.get_path(script[1])
except:
import roslib
pkg = roslib.packages.get_pkg_dir(script[1])
return result.replace(text[startIndex: endIndex + 1], pkg)
return result
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)
#!/usr/bin/env python
import rospy
import os
import logging
import json
from rospkg import RosPack
import webui.srv as srv
import requests
import urllib
rp = RosPack()
logger = logging.getLogger('hr.webui.bug_controller')
data_dir = os.path.join(rp.get_path('webui'), 'data')
class ChatbotController:
def __init__(self):
self.auth_token = 'AAAAB3NzaC'
rospy.init_node('chatnot_controller')
rospy.Service('~bot_names', srv.Json, self.bot_names_callback)
rospy.spin()
def bot_names_callback(self, request):
return self.get_json_response('bot_names')
def get_json_response(self, path, request=None):
params = {'Auth': self.auth_token}
if isinstance(request, dict):
params.update(request)
# YOUR CODE HERE
pass
# Let's draw a clown-nose for you, circle in the middle of the face
# http://docs.opencv.org/modules/core/doc/drawing_functions.html?highlight=rectangle#circle
# YOUR CODE HERE
pass
# Let's mark the middle of the image with a blue circle
cv2.circle(image, (image.shape[1]/2, image.shape[0]/2), 2, (255, 0, 0), 5)
# Pop up a window and visualize the image
cv2.imshow("Faces found", image)
cv2.waitKey(1)
if __name__ == "__main__":
# Initialize ROS
rospy.init_node("demo_face")
# Center the head
move_head(state)
print 'Load face detector'
rp = RosPack() # utility to find path of ROS packages from code
# Initialize OpenCV face detector with the offline trained dataset
faceCascade = cv2.CascadeClassifier(rp.get_path('aisoy_playground')+'/config/haarcascade_frontalface_default.xml')
# Initialize CvBridge which gives us conversion from ROS Image type to OpenCV image type
bridge = CvBridge()
# Subscribe to the image, for every new image the function "callback" will be executed once
sub = rospy.Subscriber("/airos4/camera/image_medium", Image, callback, queue_size=1)
print 'Subscribed to image'
rospy.spin() # Go!
