Ejemplo n.º 1
0
    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()
Ejemplo n.º 2
0
 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!')
Ejemplo n.º 3
0
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
Ejemplo n.º 5
0
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
Ejemplo n.º 7
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 ''
Ejemplo n.º 8
0
    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()
Ejemplo n.º 9
0
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)
Ejemplo n.º 10
0
    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)
Ejemplo n.º 11
0
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
Ejemplo n.º 12
0
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'))
Ejemplo n.º 13
0
 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
Ejemplo n.º 15
0
    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}
Ejemplo n.º 16
0
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)
Ejemplo n.º 17
0
    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()
Ejemplo n.º 19
0
 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)
Ejemplo n.º 20
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'])
Ejemplo n.º 21
0
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)
Ejemplo n.º 22
0
    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()
Ejemplo n.º 23
0
    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
Ejemplo n.º 24
0
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}
Ejemplo n.º 25
0
    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)
Ejemplo n.º 26
0
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
Ejemplo n.º 27
0
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))
Ejemplo n.º 29
0
    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