walktest.py 文件源码

def run(self, distance, angle, snap_to, options):
        """ Run our code """
        rospy.loginfo("Start test code")
        self.task_subscriber = rospy.Subscriber("/srcsim/finals/task", Task, self.task_status)

        rate = rospy.Rate(1) # 10hz
        if self.task_subscriber.get_num_connections() == 0:
            rospy.loginfo('waiting for task publisher...')
            while self.task_subscriber.get_num_connections() == 0:
                rate.sleep()

        if distance > 0.0001 or distance < -0.005:
            self.zarj_os.walk.forward(distance, True)
            while not self.zarj_os.walk.walk_is_done():
                rospy.sleep(0.01)

        if abs(angle) > 0.0 or "turn" in options:
            align = "align" in options
            small_forward = 0.005 if align else None
            self.zarj_os.walk.turn(angle, snap_to, align_to_snap = align, small_forward = small_forward)
            while not self.zarj_os.walk.walk_is_done():
                rospy.sleep(0.01)