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 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 __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!