ergo.py 文件源码

def run(self):
        self.go_to_start()
        self.last_activity = rospy.Time.now()
        self.srv_reset = rospy.Service('ergo/reset', Reset, self._cb_reset)
        rospy.loginfo('Ergo is ready and starts joystick servoing...')
        self.t = rospy.Time.now()

        while not rospy.is_shutdown():
            now = rospy.Time.now()
            self.delta_t = (now - self.t).to_sec()
            self.t = now

            self.go_or_resume_standby()
            self.servo_robot(self.joy_y, self.joy_x)
            self.publish_state()
            self.publish_button()

            # Update the last activity
            if abs(self.joy_x) > self.params['min_joy_activity'] or abs(self.joy_y) > self.params['min_joy_activity']:
                self.last_activity = rospy.Time.now()

            self.rate.sleep()