ergo.py 文件源码

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()