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