def __init__(self): self.rospack = RosPack() with open( join(self.rospack.get_path('apex_playground'), 'config', 'recorder.json')) as f: self.params = json.load(f) self.cameras = [] for camera in self.params['cameras']: self.cameras.append( CameraRecorder(camera, self.params['duration'], self.params['max_rate'], self.params['video'])) self.cameras[-1].start()
def __init__(self): self.rospack = RosPack() with open( join(self.rospack.get_path('apex_playground'), 'config', 'general.json')) as f: self.params = json.load(f) self.torso = Torso() self.ergo = Ergo() self.learning = Learning() self.perception = Perception() self.iteration = -1 # -1 means "not up to date" rospy.Subscriber('/apex_playground/iteration', UInt32, self._cb_iteration) rospy.loginfo('Controller fully started!')
def generate_wholebody_load(cfg, cs, fullBody=None, viewer=None): rp = RosPack() urdf = rp.get_path( cfg.Robot.packageName ) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf' logger.info("load robot : %s", urdf) robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False) logger.info("robot loaded.") cs_wb = ContactSequence() logger.warning("Load wholebody contact sequence from file : %s", cfg.WB_FILENAME) cs_wb.loadFromBinary(cfg.WB_FILENAME) return cs_wb
def _get_robot_move_command(self): """Build command to move the robot (to be called as subprocess) :returns list containing executable with path and appropriate arguments """ ptp_goal = [ str(x) for x in self.test_data.get_joints(self._PTP_TEST_NAME, _GROUP_NAME) ] movecmd = RosPack().get_path( "pilz_robot_programming" ) + '/test/integrationtests/movecmd.py ptp joint' return movecmd.split(" ") + ptp_goal
def _get_packages_in_environment(): global _packages_in_environment if _packages_in_environment is None: if ROS_PACKAGE_PATH not in os.environ or not os.environ[ ROS_PACKAGE_PATH]: raise RuntimeError( "The environment variable '%s' must be set when using '%s'" % (ROS_PACKAGE_PATH, ARG_CURRENT_ENVIRONMENT)) _packages_in_environment = set([]) rs = RosStack() _packages_in_environment.update(set(rs.list())) rp = RosPack() _packages_in_environment.update(set(rp.list())) return _packages_in_environment
def get_data(self, experiment_id): csv_file = os.path.join(RosPack().get_path("mppi_ros"), "log", "record.csv") # df = pd.read_csv(csv_file, converters={'stage_cost': eval, 'step_count': eval, 'weights': eval, 'time': eval, # 'effective_samples': eval}) df = pd.read_csv(csv_file) df['tree_search'] = df['tree_search'].apply(lambda x: "Tree" if x else "Monte Carlo") df = df.rename(columns={'tree_search': 'Sampling Strategy'}) print("Looking for experiments with id: {}".format(experiment_id)) df = df[df['id'].str.contains(experiment_id)] print("Found {}".format(len(df))) return df
def getPkgRoot(filename): pkg_name = getPkgName(filename) if not pkg_name: return '' try: from rospkg import RosPack except ImportError: return '' try: p = RosPack() path = p.get_path(pkg_name) return path except ResourceNotFound: return ''
def __init__(self): self.rospack = RosPack() with open( join(self.rospack.get_path('apex_playground'), 'config', 'ergo.json')) as f: self.params = json.load(f) self.button = Button(self.params) self.rate = rospy.Rate(self.params['publish_rate']) self.button.switch_led(False) # Service callers self.robot_reach_srv_name = '{}/reach'.format( self.params['robot_name']) self.robot_compliant_srv_name = '{}/set_compliant'.format( self.params['robot_name']) rospy.loginfo("Ergo node is waiting for poppy controllers...") rospy.wait_for_service(self.robot_reach_srv_name) rospy.wait_for_service(self.robot_compliant_srv_name) self.reach_proxy = rospy.ServiceProxy(self.robot_reach_srv_name, ReachTarget) self.compliant_proxy = rospy.ServiceProxy( self.robot_compliant_srv_name, SetCompliant) rospy.loginfo("Controllers connected!") self.state_pub = rospy.Publisher('ergo/state', CircularState, queue_size=1) self.button_pub = rospy.Publisher('sensors/buttons/help', Bool, queue_size=1) self.goals = [] self.goal = 0. self.joy_x = 0. self.joy_y = 0. self.motion_started_joy = 0. self.js = JointState() rospy.Subscriber( 'sensors/joystick/{}'.format(self.params["control_joystick_id"]), Joy, self.cb_joy) rospy.Subscriber('{}/joint_state'.format(self.params['robot_name']), JointState, self.cb_js) rospy.Subscriber('sensors/button_leds/pause', Bool, self.cb_bt_led) self.t = rospy.Time.now() self.srv_reset = None self.extended = False self.standby = False self.last_activity = rospy.Time.now() self.delta_t = rospy.Time.now()
def test_RosPack_get_path(): from rospkg import RosPack, ResourceNotFound, get_ros_root path = get_package_test_path() foo_path = os.path.join(path, 'p1', 'foo') foo_path_alt = os.path.join(path, 'p2', 'foo') bar_path = os.path.join(path, 'p1', 'bar') baz_path = os.path.join(path, 'p2', 'baz') # point ROS_ROOT at top, should spider entire tree print("ROS path: %s" % (path)) r = RosPack(ros_paths=[path]) # precedence in this case is undefined as there are two 'foo's in the same path assert r.get_path('foo') in [foo_path, foo_path_alt] assert bar_path == r.get_path('bar') assert baz_path == r.get_path('baz') try: r.get_path('fake') assert False except ResourceNotFound: pass # divide tree in half to test precedence print("ROS_PATH 1: %s" % (os.path.join(path, 'p1'))) print("ROS_PATH 2: %s" % (os.path.join(path, 'p2'))) r = RosPack(ros_paths=[os.path.join(path, 'p1'), os.path.join(path, 'p2')]) assert foo_path == r.get_path('foo'), "%s vs. %s" % (foo_path, r.get_path('foo')) assert bar_path == r.get_path('bar') assert baz_path == r.get_path('baz') if get_ros_root() and rospack_is_available(): # stresstest against rospack r = RosPack() for p in rospack_list(): retval = r.get_path(p) rospackval = rospack_find(p) assert retval == rospackval, "[%s]: %s vs. %s" % (p, retval, rospackval)
def __init__(self): self.rospack = RosPack() with open( join(self.rospack.get_path('nips2016'), 'config', 'bounds.json')) as f: self.bounds = json.load(f) self.bounds_motors_min = np.array( [float(bound[0]) for bound in self.bounds['motors']['positions']]) self.bounds_motors_max = np.array( [float(bound[1]) for bound in self.bounds['motors']['positions']]) self.bounds_sensory_min = [ d for space in [ 'hand', 'joystick_1', 'joystick_2', 'ergo', 'ball', 'light', 'sound' ] for d in [float(bound[0]) for bound in self.bounds['sensory'][space]] * 10 ] self.bounds_sensory_min = np.array([ float(self.bounds['sensory']['ergo'][0][0]), float(self.bounds['sensory']['ball'][0][0]) ] + self.bounds_sensory_min) self.bounds_sensory_max = [ d for space in [ 'hand', 'joystick_1', 'joystick_2', 'ergo', 'ball', 'light', 'sound' ] for d in [float(bound[1]) for bound in self.bounds['sensory'][space]] * 10 ] self.bounds_sensory_max = np.array([ float(self.bounds['sensory']['ergo'][0][1]), float(self.bounds['sensory']['ball'][0][1]) ] + self.bounds_sensory_max) self.bounds_sensory_diff = self.bounds_sensory_max - self.bounds_sensory_min # DMP PARAMETERS self.n_dmps = 4 self.n_bfs = 7 self.timesteps = 30 self.max_params = np.array([300.] * self.n_bfs * self.n_dmps + [1.] * self.n_dmps) self.motor_dmp = MyDMP(n_dmps=self.n_dmps, n_bfs=self.n_bfs, timesteps=self.timesteps, max_params=self.max_params) self.context = {} self.config = dict(m_mins=[-1.] * 32, m_maxs=[1.] * 32, s_mins=[-1.] * 132, s_maxs=[1.] * 132)
def collect_plugins(root_scan_dir): """ Scan directories starting in the designated root to locate all packages that depend on pluginlib. This will indirectly tell us which packages potentially export plugins. Then we search for the plugin manifest file and we parse it to obtain all the exported plugin classes. root_scan_dir indicates the starting point for the scan returns the collected plugin classes """ rp = RosPack([root_scan_dir]) packages = rp.list() debug_print("Found packages:\n") #print packages debug_print() # Find all packages that depend on pluginlib and nodelet explicitely pluginlib_users = rp.get_depends_on('pluginlib', implicit=False) nodelet_users = rp.get_depends_on('nodelet', implicit=False) # Concatenate both lists removing the duplicates pluginlib_users += list(set(nodelet_users) - set(pluginlib_users)) debug_print("Packages that depend on pluginlib:\n") debug_print(pluginlib_users) debug_print() # Within the packages that require pluginlib, search all their # dependencies for plugins plugin_classes = [] for p in pluginlib_users: path = rp.get_path(p) debug_print(p + ": ") debug_print(path) exports = rp.get_manifest(p).exports debug_print("Exports: ") for e in exports: s = e.get("plugin") if s: s2 = string.replace(s, "${prefix}", path) debug_print(s2) f = open(s2, 'r') xml_str = f.read() xml_str = escape_xml(xml_str) plugin_classes += parse_plugin_xml(xml_str, p, path) #plugin_classes += parse_plugin_xml(s2) debug_print(plugin_classes) f.close() debug_print() return plugin_classes
def test_RosPack_get_rosdeps(): from rospkg import RosPack path = get_package_test_path() r = RosPack(ros_paths=[os.path.join(path, 'p1'), os.path.join(path, 'p2')]) # repeat tests due to caching assert set(['foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3']) == set(r.get_rosdeps('foo', implicit=True)), r.get_rosdeps('foo', implicit=True) assert set(['foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3']) == set(r.get_rosdeps('foo', implicit=True)) assert set(['foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3']) == set(r.get_rosdeps('foo', implicit=False)) assert set(['bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('bar', implicit=False)) assert set(['foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('bar', implicit=True)) assert set(['foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('bar', implicit=True)) assert set(['foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('bar')) assert ['baz_rosdep1'] == r.get_rosdeps('baz', implicit=False) assert set(['baz_rosdep1', 'foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('baz')) assert set(['baz_rosdep1', 'foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('baz')) # create a brand new instance to test with brand new cache r = RosPack(ros_paths=[os.path.join(path, 'p1'), os.path.join(path, 'p2')]) assert set(['baz_rosdep1', 'foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('baz')) assert set(['baz_rosdep1', 'foo_rosdep1', 'foo_rosdep2', 'foo_rosdep3', 'bar_rosdep1', 'bar_rosdep2']) == set(r.get_rosdeps('baz'))
def optimize_pose(self): """ Run g2o """ self.write_g2o_data() package = RosPack().get_path('navigation_prototypes') # path to compiled g2o file g2o_data_path = path.join(package, 'data/data_g2o/data.g2o') g2o_data_copy_path = path.join( package, 'data/data_g2o/data_cp.g2o') # copy of the unedit data g2o_result_path = path.join(package, 'data/data_g2o/result.g2o') system("g2o -o %s %s" % (g2o_result_path, g2o_data_path)) # Run G2o # Copy Original Data system("cp %s %s" % (g2o_data_path, g2o_data_copy_path)) print("OPTIMIZATION COMPLETED")
def generateWholeBodyMotion(cs, fullBody=None, viewer=None): rp = RosPack() urdf = rp.get_path( cfg.Robot.packageName ) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf' if cfg.WB_VERBOSE: print "load robot : ", urdf robot = pin.RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False) if cfg.WB_VERBOSE: print "robot loaded." filename = cfg.EXPORT_PATH + "/npz/" + cfg.DEMO_NAME + ".npz" print "Load npz file : ", filename res = wholebody_result.loadFromNPZ(filename) return res, robot
def __init__(self, outside_ros=False): self.rospack = RosPack() self.num_workers = 0 self.outside_ros = rospy.get_param( '/use_sim_time', outside_ros ) # True if work manager <-> controller comm must use ZMQ self.file_experiment = join(self.rospack.get_path('apex_playground'), 'config', 'experiment.json') with open(self.file_experiment) as f: experiment = self.check(json.load(f)) rospy.set_param('/work', experiment) self.lock_experiment = RLock() self.failing_workers = { } # association {num_worker: remaining attempts before blacklist}
def soundFinder(gen, param_name = "rewardSound"): top = RosPack().get_path("mobility_games") sounds = []; firstsound = None; for dirname, dirnames, filenames in os.walk(top + '/auditory/sound_files'): first = True for fname in filenames: if fname.endswith('.wav'): if first: firstsound = os.path.join(dirname, fname) first = False sounds.append(gen.const(fname[:-4], str_t, os.path.join(dirname, fname), fname)) print(fname[:-4]) sound_enum = gen.enum(sounds, "An enum to set .wav Sound") gen.add(param_name, str_t, 0, "Filename of Sound file to be used as reward", firstsound, edit_method=sound_enum)
def __init__(self): self.rospack = RosPack() with open( join(self.rospack.get_path('nips2016'), 'config', 'ergo.json')) as f: self.params = json.load(f) self.button = Button(self.params) self.rate = rospy.Rate(self.params['publish_rate']) self.eef_pub = rospy.Publisher('/nips2016/ergo/end_effector_pose', PoseStamped, queue_size=1) self.state_pub = rospy.Publisher('/nips2016/ergo/state', CircularState, queue_size=1) self.button_pub = rospy.Publisher('/nips2016/ergo/button', Bool, queue_size=1) self.joy_pub = rospy.Publisher('/nips2016/ergo/joysticks/1', Joy, queue_size=1) self.joy_pub2 = rospy.Publisher('/nips2016/ergo/joysticks/2', Joy, queue_size=1) self.srv_reset = None self.ergo = None self.extended = False self.standby = False self.last_activity = rospy.Time.now() if pygame.joystick.get_count() < 2: rospy.logerr( "Ergo: Expecting 2 joysticks but found only {}, exiting". format(pygame.joystick.get_count())) sys.exit(0) else: self.joystick = pygame.joystick.Joystick(0) self.joystick2 = pygame.joystick.Joystick(1) self.joystick.init() self.joystick2.init() if self.params['useless_joystick_id'] != int( self.joystick2.get_name()[-1]): useless_joy = self.joystick self.joystick = self.joystick2 self.joystick2 = useless_joy rospy.loginfo('Initialized Joystick 1: {}'.format( self.joystick.get_name())) rospy.loginfo('Initialized Joystick 2: {}'.format( self.joystick2.get_name()))
def __init__(self, robot_name): """ :param robot_name: Robot name and ROS topics/services namespace """ self.rospack = RosPack() with open( join(self.rospack.get_path('poppy_torso_controllers'), 'config', 'torso.json')) as f: self.params = json.load(f) self.publish_rate = rospy.Rate(self.params['publish_rate']) self.robot_name = robot_name self.eef_pub_l = rospy.Publisher('left_arm/end_effector_pose', PoseStamped, queue_size=1) self.eef_pub_r = rospy.Publisher('right_arm/end_effector_pose', PoseStamped, queue_size=1) self.js_pub_l = rospy.Publisher('left_arm/joints', JointState, queue_size=1) self.js_pub_r = rospy.Publisher('right_arm/joints', JointState, queue_size=1) self.js_pub_full = rospy.Publisher('joint_state', JointState, queue_size=1) # Services self.srv_execute = None self.srv_reach = None self.srv_set_torque = None self.srv_left_arm_set_compliant = None self.srv_right_arm_set_compliant = None self.srv_robot_set_compliant = None self.srv_set_head_idle = None self.srv_set_left_idle = None self.srv_set_right_idle = None # Behaviors self.right_idle = None self.left_idle = None self.head_idle = None # Protected resources self.torso = None self.robot_lock = RLock()
def __init__(self): self.rospack = RosPack() self.port = 80 if os.getuid() == 0 else 5000 self.app = Flask(__name__) getLogger(__name__).setLevel(INFO) #self.cors = CORS(self.app, resources={r'/api/*': {'origins': '*'}}) self.app.route('/ready', methods=['GET'])(self.get_ready) self.app.route('/go', methods=['POST'])(self.post_go) self.app.route('/rate', methods=['POST'])(self.post_rate) self.app.route('/reset', methods=['POST'])(self.post_reset) self._commanding_sub = rospy.Subscriber("/iiwa/commanding_status", Bool, self._cb_commanding_status, queue_size=1) self._last_commanding_status = False self._last_commanding_status_date = rospy.Time(0)
def __init__(self): # Load parameters and hack the tuple conversions so that OpenCV is happy self.rospack = RosPack() with open(join(self.rospack.get_path('nips2016'), 'config', 'environment.json')) as f: self.params = json.load(f) self.params['tracking']['ball']['lower'] = tuple(self.params['tracking']['ball']['lower']) self.params['tracking']['ball']['upper'] = tuple(self.params['tracking']['ball']['upper']) self.params['tracking']['arena']['lower'] = tuple(self.params['tracking']['arena']['lower']) self.params['tracking']['arena']['upper'] = tuple(self.params['tracking']['arena']['upper']) self.tracking = BallTracking(self.params) self.conversions = EnvironmentConversions() self.ball_pub = rospy.Publisher('/nips2016/environment/ball', CircularState, queue_size=1) self.light_pub = rospy.Publisher('/nips2016/environment/light', UInt8, queue_size=1) self.sound_pub = rospy.Publisher('/nips2016/environment/sound', Float32, queue_size=1) self.rate = rospy.Rate(self.params['rate'])
def export(cs_com, cs_wb): rp = RosPack() urdf = rp.get_path(cfg.Robot.packageName) + '/urdf/' + cfg.Robot.urdfName + cfg.Robot.urdfSuffix + '.urdf' if cfg.WB_VERBOSE: print("load robot : ", urdf) #srdf = "package://" + package + '/srdf/' + cfg.Robot.urdfName+cfg.Robot.srdfSuffix + '.srdf' robot = RobotWrapper.BuildFromURDF(urdf, pin.StdVec_StdString(), pin.JointModelFreeFlyer(), False) if cfg.WB_VERBOSE: print("robot loaded in export OpenHRP") results = computeWaistData(robot, res) path = cfg.EXPORT_PATH + "/openHRP/" + cfg.DEMO_NAME if not os.path.exists(path): os.makedirs(path) q_openhrp_l = generateOpenHRPMotion(results, path, cfg.DEMO_NAME, useRefZMP=cfg.openHRP_useZMPref) writeKinematicsData(results, path, cfg.DEMO_NAME)
def __init__(self, name, robotnumber, device=None, tracer=None): AbstractHumanoidRobot.__init__(self, name, tracer) self.OperationalPoints.append('waist') self.OperationalPoints.append('chest') self.device = device self.AdditionalFrames.append( ("accelerometer", matrixToTuple(self.accelerometerPosition), "chest")) self.AdditionalFrames.append( ("gyrometer", matrixToTuple(self.gyrometerPosition), "chest")) self.AdditionalFrames.append( ("leftFootForceSensor", self.forceSensorInLeftAnkle, "left-ankle")) self.AdditionalFrames.append( ("rightFootForceSensor", self.forceSensorInRightAnkle, "right-ankle")) self.OperationalPointsMap = { 'left-wrist': 'LARM_JOINT5', 'right-wrist': 'RARM_JOINT5', 'left-ankle': 'LLEG_JOINT5', 'right-ankle': 'RLEG_JOINT5', 'gaze': 'HEAD_JOINT1', 'waist': 'WAIST', 'chest': 'CHEST_JOINT1' } self.dynamic = RosRobotModel("{0}_dynamic".format(name)) rospack = RosPack() self.urdfPath = rospack.get_path('hrp2_{0}_description'.format( robotnumber)) + '/urdf/hrp2_{0}_reduced.urdf'.format(robotnumber) self.pinocchioModel = se3.buildModelFromUrdf(self.urdfPath, se3.JointModelFreeFlyer()) self.pinocchioData = self.pinocchioModel.createData() self.dynamic.setModel(self.pinocchioModel) self.dynamic.setData(self.pinocchioData) self.dimension = self.dynamic.getDimension() self.plugVelocityFromDevice = True if self.dimension != len(self.halfSitting): raise RuntimeError( "Dimension of half-sitting: {0} differs from dimension of robot: {1}" .format(len(self.halfSitting), self.dimension)) self.initializeRobot() self.dynamic.displayModel()
def create_from_rospkg(rospack=None, rosstack=None, sources_loader=None, verbose=False): """ Create :class:`RosdepLookup` based on current ROS package environment. :param rospack: (optional) Override :class:`rospkg.RosPack` instance used to crawl ROS packages. :param rosstack: (optional) Override :class:`rospkg.RosStack` instance used to crawl ROS stacks. :param sources_loader: (optional) Override SourcesLoader used for managing sources.list data sources. """ # initialize the loader if rospack is None: rospack = RosPack() if rosstack is None: rosstack = RosStack() if sources_loader is None: sources_loader = SourcesListLoader.create_default(verbose=verbose) rosdep_db = RosdepDatabase() # Use sources list to initialize rosdep_db. Underlay has no # notion of specific resources, and its view keys are just the # individual sources it can load from. SourcesListLoader # cannot do delayed evaluation of OS setting due to matcher. underlay_key = SourcesListLoader.ALL_VIEW_KEY # Create the rospkg loader on top of the underlay loader = RosPkgLoader(rospack=rospack, rosstack=rosstack, underlay_key=underlay_key) # create our actual instance lookup = RosdepLookup(rosdep_db, loader) # load in the underlay lookup._load_all_views(loader=sources_loader) # use dependencies to implement precedence view_dependencies = sources_loader.get_loadable_views() rosdep_db.set_view_data(underlay_key, {}, view_dependencies, underlay_key) return lookup
def parse_srdf (srdf, packageName = None, prefix = None): """ parameters: - packageName: if provided, the filename is considered relative to this ROS package - prefix: if provided, the name of the elements will be prepended with prefix + "/" """ import os if packageName is not None: from rospkg import RosPack rospack = RosPack() path = rospack.get_path(packageName) srdfFn = os.path.join(path, srdf) else: srdfFn = srdf # tree = ET.fromstring (srdfFn) tree = ET.parse (srdfFn) root = tree.getroot() grippers = {} for xml in root.iter('gripper'): n = _read_name (xml) g = { "robot": prefix, "name": n, "clearance": _read_clearance (xml), "link": _read_link (xml), "position": _read_position (xml), "joints": _read_joints (xml), } tc = _read_torque_constant (xml) if tc is not None: g["torque_constant"] = tc grippers[ prefix + "/" + n if prefix is not None else n] = g handles = {} for xml in root.iter('handle'): n = _read_name (xml) h = { "robot": prefix, "name": n, "clearance": _read_clearance (xml), "link": _read_link (xml), "position": _read_position (xml), "mask": _read_mask (xml), } handles[ prefix + "/" + n if prefix is not None else n] = h return { "grippers": grippers, "handles": handles}
def __init__(self, parent, guicontext): ''' A GUi panel to list common operation commands for Hironx / NEXTAGE Open robot. @param guicontext: The plugin context to create the monitor in. @type guicontext: qt_gui.plugin_context.PluginContext ''' super(HironxoCommandPanel, self).__init__() self._parent = parent self._guicontext = guicontext # RTM Client rtm.nshost = self.get_rosmaster_domain().hostname rtm.nsport = rospy.get_param('rtmnameserver_port', '15005') robotname = rospy.get_param('rtmnameserver_robotname', 'HiroNX(Robot)0') rospy.loginfo( 'Connecting to RTM nameserver. host={}, port={}, robotname={}'. format(rtm.nshost, rtm.nsport, robotname)) self._rtm = HIRONX() self._rtm.init(robotname=robotname, url='') rospack = RosPack() ui_file = os.path.join(rospack.get_path(PKG_NAME), 'resource', 'hironx_commandpanel_main.ui') loadUi(ui_file, self, {'HironxoCommandPanel': HironxoCommandPanel}) self._precision_output = 4 # Default degree order to be used to print values # Assign callback methods self.pushButton_checkEncoders.clicked[bool].connect( self._check_encoders) self.pushButton_goInitial.clicked[bool].connect(self._go_initial) self.pushButton_goInitial_factoryval.clicked[bool].connect( self._go_initial_factoryval) self.pushButton_goOffPose.clicked[bool].connect(self._go_offPose) self.pushButton_startImpedance.clicked[bool].connect( self._impedance_on) self.pushButton_stopImpedance.clicked[bool].connect( self._impedance_off) self.pushButton_actualPose_l.clicked[bool].connect(self._actual_pose_l) self.pushButton_actualPose_r.clicked[bool].connect(self._actual_pose_r) self.spinBox_precision_output.valueChanged[int].connect( self._get_precision_output) self.pushButton_groups.clicked[bool].connect(self._show_groups)
def GetRosIncludePaths(): """Return a list of potential include directories The directories are looked for in $ROS_WORKSPACE. """ try: from rospkg import RosPack except ImportError: return [] rospack = RosPack() includes = [] includes.append(os.path.expandvars('$ROS_WORKSPACE') + '/devel/include') for p in rospack.list(): if os.path.exists(rospack.get_path(p) + '/include'): includes.append(rospack.get_path(p) + '/include') for distribution in os.listdir('/opt/ros'): includes.append('/opt/ros/' + distribution + '/include') return includes
def test_RosPack_no_env(): # regression test for #3680 from rospkg import RosPack, ResourceNotFound try: environ_copy = os.environ.copy() if 'ROS_ROOT' in os.environ: del os.environ['ROS_ROOT'] if 'ROS_PACKAGE_PATH' in os.environ: del os.environ['ROS_PACKAGE_PATH'] r = RosPack() try: r.get_depends('roscpp') assert False, "should have raised" except ResourceNotFound: pass finally: os.environ = environ_copy
def __init__(self, parent=None): pyqtgraph.setConfigOption('background', 'w') # before loading widget super(AudioTopicPlotter, self).__init__(parent) # We load dynamically the ui file rp = RosPack() pkg_path = rp.get_path('audio_stream_arecord_hack') uic.loadUi(pkg_path + '/scripts/ui_pcm_vol.ui', self) self.grPCM.plotItem.showGrid(True, True, 0.7) self.maxPCM = 0 self.blocksize = None self.last_data = None self.new_data = False self.sub = rospy.Subscriber('/audio_stream/audio_data', AudioData, self.audio_cb, queue_size=10) rospy.loginfo('Subscribed to topic: ' + str(self.sub.resolved_name))
def __init__(self): self.rospack = RosPack() with open(join(self.rospack.get_path('apex_playground'), 'config', 'general.json')) as f: self.params = json.load(f) with open(join(self.rospack.get_path('apex_playground'), 'config', 'torso.json')) as f: self.torso_params = json.load(f) self.outside_ros = rospy.get_param('/use_sim_time', False) # True if work manager <-> controller comm must use ZMQ id = search(r"(\d+)", rospy.get_namespace()) self.worker_id = 0 if id is None else int(id.groups()[0]) # TODO string worker ID self.work = WorkManager(self.worker_id, self.outside_ros) self.torso = Torso() self.ergo = Ergo() self.learning = Learning() self.perception = Perception() rospy.loginfo('Controller fully started!')
def __init__(self, haar_cascade_file=None): # Load the classifier if haar_cascade_file: self.classifier = cv2.CascadeClassifier(haar_cascade_file) else: rp = RosPack() pkg_path = rp.get_path('head_depth') self.classifier = cv2.CascadeClassifier( pkg_path + "/file/haarcascades/haarcascade_range_multiview_5p_bg.xml") # Subscribe to the topics self.bridge = CvBridge() self.last_depth = None self.depth_sub = rospy.Subscriber( '/pepper/camera/depth/image_raw/compressedDepth', CompressedImage, self.depth_cb, queue_size=1) self.last_rgb = None