python类is_shutdown()的实例源码

def talker():
    pub = rospy.Publisher('chatter', String, queue_size=10)
    rospy.init_node('talker', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        hello_str = "hello world %s" % rospy.get_time()
        rospy.loginfo(hello_str)
        pub.publish(hello_str)
        rate.sleep()

#    rate = rospy.Rate(10)
#    hello_str = "hello world"
 #   rospy.loginfo(hello_str)
 #   pub.publish(hello_str)
 #   rospy.spin()
 #   exit(0)

def main():
    rospy.init_node("whatsapp_service")
    cred = credentials.WHATSAPP
    stackBuilder = YowStackBuilder()
    stack = (stackBuilder
             .pushDefaultLayers(True)
             .push(AideRosLayer)
             .build())
    loginfo("Stack built...")
    stack.setCredentials(cred)
    stack.broadcastEvent(YowLayerEvent(YowNetworkLayer.EVENT_STATE_CONNECT))  # sending the connect signal
    loginfo("Connected...")
    atexit.register(lambda: stack.broadcastEvent(YowLayerEvent(YowNetworkLayer.EVENT_STATE_DISCONNECT)))

    th = threading.Thread(target=stack.loop)
    th.daemon = True
    th.start()
    loginfo("Running in background.")
    loginfo("All done. spinning.")
    while not rospy.is_shutdown():
        rospy.spin()

def keyboard_loop(self):
        while not rospy.is_shutdown():
            acc = 0
            yaw = 0

            keys = pygame.key.get_pressed()
            for event in pygame.event.get():
                if event.type==pygame.QUIT:
                    sys.exit()

            if(keys[pygame.K_UP]):
                acc = self.acc
            elif(keys[pygame.K_DOWN]):
                acc = -self.acc
            if(keys[pygame.K_LEFT]):
                yaw = self.yaw
            elif(keys[pygame.K_RIGHT]):
                yaw = -self.yaw

            self.send_control(acc, yaw)
            self.rate.sleep()

def record_motion(self):
        for countdown in ['ready?', 3, 2, 1, "go"]:
            self.say('{}'.format(countdown), blocking=False)
            rospy.sleep(1)
        self.arm.recorder.start(10)
        rospy.loginfo("Recording...")

        choice = ""
        while choice != 'stop' and not rospy.is_shutdown():
            choice = self.read_user_input(['stop'])

        joints, eef = self.arm.recorder.stop()
        self.say('Recorded', blocking=True)
        if len(joints.joint_trajectory.points) == 0:
            self.say('This demo is empty, please retry')
        else:
            target_id = self.promp.add_demonstration(joints, eef)
            self.say('Added to Pro MP {}'.format(target_id), blocking=False)

def main(args):
    rospy.init_node("publish_calib_file", args)

    files = glob.glob("left-*.png");
    files.sort()

    print("All {} files".format(len(files)))

    image_pub = rospy.Publisher("image",Image, queue_size=10)
    bridge = CvBridge();


    for f in files:
        if rospy.is_shutdown():
            break
        raw_input("Publish {}?".format(f))
        img = cv2.imread(f, 0)
        image_pub.publish(bridge.cv2_to_imgmsg(img, "mono8"))

def attach_springs(self):
        """
        Switches to joint torque mode and attached joint springs to current
        joint positions.
        """
        # record initial joint angles
        self._start_angles = self._limb.joint_angles()

        # set control rate
        control_rate = rospy.Rate(self._rate)

        # for safety purposes, set the control rate command timeout.
        # if the specified number of command cycles are missed, the robot
        # will timeout and return to Position Control Mode
        self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)

        # loop at specified rate commanding new joint torques
        while not rospy.is_shutdown():
            if not self._rs.state().enabled:
                rospy.logerr("Joint torque example failed to meet "
                             "specified control rate timeout.")
                break
            self._update_forces()
            control_rate.sleep()

def wobble(self):
        self.set_neutral()
        """
        Performs the wobbling
        """
        command_rate = rospy.Rate(1)
        control_rate = rospy.Rate(100)
        start = rospy.get_time()
        while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0):
            angle = random.uniform(-2.0, 0.95)
            while (not rospy.is_shutdown() and
                   not (abs(self._head.pan() - angle) <=
                       intera_interface.HEAD_PAN_ANGLE_TOLERANCE)):
                self._head.set_pan(angle, speed=0.3, timeout=0)
                control_rate.sleep()
            command_rate.sleep()

        self._done = True
        rospy.signal_shutdown("Example finished.")

def revalidate(self, timeout, invalidate_state=True, invalidate_config=True):
        """
        invalidate the state and config topics, then wait up to timeout
        seconds for them to become valid again.
        return true if both the state and config topic data are valid
        """
        if invalidate_state:
            self.invalidate_state()
        if invalidate_config:
            self.invalidate_config()
        timeout_time = rospy.Time.now() + rospy.Duration(timeout)
        while not self.is_state_valid() and not rospy.is_shutdown():
            rospy.sleep(0.1)
            if timeout_time < rospy.Time.now():
                rospy.logwarn("Timed out waiting for node interface valid...")
                return False
        return True

def robot_listener(self):
        '''
        rospy.wait_for_service('spawn')
        spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
        spawner(4, 2, 0, 'turtle2')
        '''
        robot_vel_pub = rospy.Publisher('robot_0/cmd_vel', geometry_msgs.msg.Twist, queue_size=1)

        rate = rospy.Rate(10.0)
        while not rospy.is_shutdown():
            try:
                (trans,rot) = self.listener.lookupTransform('/robot_3', '/robot_0', rospy.Time(0))
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
            angular = 4 * math.atan2(trans[1], trans[0])
            linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
            cmd = geometry_msgs.msg.Twist()
            cmd.linear.x = linear
            cmd.angular.z = angular
            robot_vel_pub.publish(cmd)

            rate.sleep()

def compute_static_stability_thread():
    rate = rospy.Rate(60)
    global kron_com_vertices
    while not rospy.is_shutdown():
        if 'COM-static' in support_area_handles \
                and not gui.show_com_support_area:
            delete_support_area_display('COM-static')
        if not motion_plan or not motion_plan.started \
                or 'COM-static' in support_area_handles \
                or not gui.show_com_support_area:
            rate.sleep()
            continue
        color = (0.1, 0.1, 0.1, 0.5)
        req = contact_stability.srv.StaticAreaRequest(
            contacts=convert_contacts_to_ros(motion_plan.cur_stance.contacts),
            mass=robot.mass, z_out=motion_plan.com_height)
        try:
            res = compute_com_area(req)
            vertices = [array([v.x, v.y, v.z]) for v in res.vertices]
            update_support_area_display('COM-static', vertices, [], color)
        except rospy.ServiceException:
            delete_support_area_display('COM-static')
        rate.sleep()

def monitor(self):
        # Wait until subscriber on instruct message is present
        notified = False
        while not rospy.is_shutdown():
            _, subscribers, _ = Master('/needybot').getSystemState()
            if dict(subscribers).get(nb_channels.Messages.instruct.value) is not None:
                if self.is_connected == False:
                    self.connected_pub.publish()
                self.is_connected = True
            else:
                if self.is_connected or not notified:
                    notified = True
                    self.disconnected_pub.publish()
                self.is_connected = False 

            rospy.sleep(0.1)

def get_cylinder_status(self):
        self.cylinder_laser_client.wait_for_service()
        self.cylinder_opencv_client.wait_for_service()
        flag = 0
        r = rospy.Rate(2)
        while not rospy.is_shutdown() and flag != 1:
            res_opencv = self.cylinder_opencv_client()
            res_laser = self.cylinder_laser_client()
            x_laser = res_laser.x
            theta_laser = res_laser.theta
            theta_opencv = res_opencv.theta
            if abs(theta_laser - theta_opencv) < 0.01:
                flag = 1
                break
            r.sleep()
        return (x_laser,theta_laser)

def go_to(self, radius, angular, mode):
        # ??????????
        symbol_y,symbol_w = self.MODE[mode]
        ang_has_moved = 0.0
        self.reach_goal = False
        move_vel = g_msgs.Twist()
        start_yaw = self.get_position.get_robot_current_w()
        while not rospy.is_shutdown() and self.reach_goal != True:
            current_yaw = self.get_position.get_robot_current_w()
            ang_has_moved += abs(abs(current_yaw) - abs(start_yaw))
            start_yaw = current_yaw
            if abs(ang_has_moved - abs(angular)) <= self.stop_tolerance:
                self.reach_goal = True
                break
            move_vel.linear.y = self.move_speed*symbol_y
            # ???????, ???? dw*dt = dt*atan2(dv*dt/2.0, radius)
            move_vel.angular.z = self.rate*atan2(self.move_speed/self.rate,radius)*symbol_w
            print ang_has_moved
            self.move_cmd_pub.publish(move_vel)
            self.R.sleep()
        self.brake()

        print ang_has_moved

def correct_angle(self):
        # rospy.loginfo("sadasdafasf")
        (x,theta,if_close_line) = self.close_line_cmd.find_line()
        r = rospy.Rate(50)
        move_velocity = g_msgs.Twist()
        if theta > 0:
            move_velocity.angular.z = -0.10
        else:
            move_velocity.angular.z = 0.10
        while not rospy.is_shutdown():
            (x,theta,if_close_line) = self.close_line_cmd.find_line()
            self.cmd_move_pub.publish(move_velocity)
            #????????????????

            if theta < 0.02 and theta > -0.02:
                rospy.loginfo("will Stop!!!!!!!!!!")
                self.brake()
                break
            r.sleep()


    #??3??????

def go_to_home(self):
        (x,theta,if_close_line) = self.close_line_cmd.find_line()
        r = rospy.Rate(50)
        move_velocity = g_msgs.Twist()
        while not rospy.is_shutdown():
            (x,theta,if_close_line) = self.close_line_cmd.find_line()
            move_velocity.linear.y = -0.3
            self.cmd_move_pub.publish(move_velocity)
            rospy.loginfo("python: y = %s",move_velocity.linear.y)
            if if_close_line != 0:
                rospy.loginfo("will Stop!!!!!!!!!!")
                self.brake()
                break
            r.sleep()

    #????????

def turn(self, goal_angular):
        # ???????????????,??????????????
        # ????,????
        rospy.loginfo('[robot_move_pkg]->move_in_robot.turn will turn %s'%goal_angular)
        current_angular = start_angular = self.robot_state.get_robot_current_w()#??????????

        r = rospy.Rate(100)
        delta_angular = current_angular - start_angular
        move_velocity = g_msgs.Twist()
        while not rospy.is_shutdown() :
            if abs(delta_angular - abs(goal_angular)) < self.turn_move_stop_tolerance:
                break
            current_angular = self.robot_state.get_robot_current_w()
            move_velocity.angular.z = math.copysign(self.w_speed, goal_angular)
            delta_angular += abs(abs(current_angular) - abs(start_angular) )
            start_angular = current_angular
            self.cmd_move_pub.publish(move_velocity) #???????????
            r.sleep()
        self.brake()

def turn_to(self,goal_angular):
        rospy.on_shutdown(self.brake) #???????????
        current_angular = start_angular = self.robot_state.get_robot_current_w()#??????????
        is_arrive_goal = False
        r = rospy.Rate(100)
        delta_angular = current_angular - start_angular
        delta_upper_limit = abs(goal_angular) + 0.02 #????
        delta_lower_limit = abs(goal_angular) - 0.02 #????
        move_velocity = g_msgs.Twist()
        while not rospy.is_shutdown() and not is_arrive_goal:
            if abs(delta_angular)<=delta_upper_limit and abs(delta_angular) >= delta_lower_limit: #????
                self.brake()
                is_arrive_goal = True
                break
            current_angular = self.robot_state.get_robot_current_w()
            if goal_angular > 0:
                move_velocity.angular.z = 0.1
            else:
                move_velocity.angular.z = -0.1
            delta_angular += abs(abs(current_angular) - abs(start_angular) )
            start_angular = current_angular
            self.cmd_move_pub.publish(move_velocity) #???????????
            r.sleep()
        self.brake()

def main():
    rospy.init_node("evaluation_ac")
    ac = ACControllerSimulator()
    rospy.Subscriber("/aide/ac_control", Bool, callback=lambda msg: ac.increase_ac() if msg.data else ac.decrease_ac())

    console = Console()
    console.preprocess = lambda source: source[3:]
    atexit.register(loginfo, "Going down by user-input.")
    ac_thread = Thread(target=ac.simulate)
    ac_thread.daemon = True
    pub_thread = Thread(target=publish, args=(ac, ))
    pub_thread.daemon = True
    pub_thread.start()
    while not rospy.is_shutdown():
        try:
            command = console.raw_input("ac> ")
            if command == "start":
                ac_thread.start()
            if command == "end":
                return

        except EOFError:
            print("")
            ac.finished = True
            rospy.signal_shutdown("Going down.")

def waitForInitialPose(self, next_topic, timeout=None):
        counter = 0
        while not rospy.is_shutdown():
            counter = counter + 1
            if timeout and counter >= timeout:
                return False
            try:
                self.marker_lock.acquire()
                self.initialize_poses = True
                topic_suffix = next_topic.split("/")[-1]
                if self.initial_poses.has_key(topic_suffix):
                    self.pre_pose = PoseStamped(pose=self.initial_poses[topic_suffix])
                    self.initialize_poses = False
                    return True
                else:
                    rospy.logdebug(self.initial_poses.keys())
                    rospy.loginfo("Waiting for pose topic of '%s' to be initialized",
                                  topic_suffix)
                    rospy.sleep(1)
            finally:
                self.marker_lock.release()

def attach_springs(self):
        """
        Switches to joint torque mode and attached joint springs to current
        joint positions.
        """
        # record initial joint angles
        self._des_angles = self._limb.joint_angles()

        # set control rate
        control_rate = rospy.Rate(self._rate)

        # for safety purposes, set the control rate command timeout.
        # if the specified number of command cycles are missed, the robot
        # will timeout and disable
        self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)

        # loop at specified rate commanding new joint torques
        while not rospy.is_shutdown():
            if not self._rs.state().enabled:
                rospy.logerr("Joint torque example failed to meet "
                             "specified control rate timeout.")
                break
            self._update_forces()
            control_rate.sleep()

def __init__(self):
        State.__init__(self, outcomes=['success'], input_keys=['waypoints'], output_keys=['waypoints'])
        # Create publsher to publish waypoints as pose array so that you can see them in rviz, etc.
        self.poseArray_publisher = rospy.Publisher('/waypoints', PoseArray, queue_size=1)

        # Start thread to listen for reset messages to clear the waypoint queue
        def wait_for_path_reset():
            """thread worker function"""
            global waypoints
            while not rospy.is_shutdown():
                data = rospy.wait_for_message('/path_reset', Empty)
                rospy.loginfo('Recieved path RESET message')
                self.initialize_path_queue()
                rospy.sleep(3) # Wait 3 seconds because `rostopic echo` latches
                               # for three seconds and wait_for_message() in a
                               # loop will see it again.
        reset_thread = threading.Thread(target=wait_for_path_reset)
        reset_thread.start()

def zero_sensor(self):
        rospy.loginfo("Zeroing sensor...")
        # Wait a little bit until we get a message from the sensor
        rospy.sleep(1)
        self.tip_offset, self.inside_offset = (np.zeros_like(self.tip),
                                               np.zeros_like(self.inside))
        inside_vals, tip_vals = [], []
        r = rospy.Rate(10)
        while not rospy.is_shutdown() and len(inside_vals) < 10:
            inside, tip = self.inside, self.tip
            # If there are zero values (most likely becase a message
            # has not yet been received), skip that. We could also
            # initialize them with nans to find out if there's a
            # problem
            if all(inside) and all(tip):
                inside_vals.append(inside)
                tip_vals.append(tip)
            r.sleep()
        # Center around 5000, so ranges are similar to when not centering
        self.inside_offset = np.min(inside_vals, axis=0) - 5000
        self.tip_offset = np.min(tip_vals, axis=0) - 5000
        rospy.loginfo("Zeroing finished")

def limb_pose(limb_name):
    """Get limb pose at time of OK cuff button press."""
    button = CuffOKButton(limb_name)
    rate = rospy.Rate(20)  # rate at which we check whether button was
                           # pressed or not
    rospy.loginfo(
        'Waiting for %s OK cuff button press to save pose' % limb_name)
    while not button.pressed and not rospy.is_shutdown():
        rate.sleep()
    joint_pose = baxter_interface.Limb(limb_name).joint_angles()
    # Now convert joint coordinates to end effector cartesian
    # coordinates using forward kinematics.
    kinematics = baxter_kinematics(limb_name)
    endpoint_pose = kinematics.forward_position_kinematics(joint_pose)
    # How is this different from
    # baxter_interface.Limb(limb_name).endpoint_pose()
    print()
    print('baxter_interface endpoint pose:')
    print(baxter_interface.Limb(limb_name).endpoint_pose())
    print('pykdl forward kinematics endpoint pose:')
    print(endpoint_pose)
    print()
    return endpoint_pose

def zero_sensor(self):
        rospy.loginfo("Zeroing sensor...")
        # Wait a little bit until we get a message from the sensor
        rospy.sleep(1)
        self.tip_offset, self.inside_offset = (np.zeros_like(self.tip),
                                               np.zeros_like(self.inside))
        inside_vals, tip_vals = [], []
        r = rospy.Rate(10)
        while not rospy.is_shutdown() and len(inside_vals) < 10:
            inside, tip = self.inside, self.tip
            # If there are zero values (most likely becase a message
            # has not yet been received), skip that. We could also
            # initialize them with nans to find out if there's a
            # problem
            if all(inside) and all(tip):
                inside_vals.append(inside)
                tip_vals.append(tip)
            r.sleep()
        # Center around 5000, so ranges are similar to when not centering
        self.inside_offset = np.min(inside_vals, axis=0) - 5000
        self.tip_offset = np.min(tip_vals, axis=0) - 5000
        rospy.loginfo("Zeroing finished")

def attach_springs(self):
        self._start_angles = self._get_cmd_positions()
        control_rate = rospy.Rate(self._rate)

        # for safety purposes, set the control rate command timeout.
        # if the specified number of command cycles are missed, the robot
        # will timeout and disable
        self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds)

        error = [self._limb.joint_angles()[key] - self._start_angles[key] for key in self._limb.joint_angles().keys()]
        self.sum_sqr_error = sum([error[i]**2 for i in range(len(error))])
        print ('-------new set of joint position---------')
        print (self.sum_sqr_error)
        tolerance = 0.020
        # loop at specified rate commanding new joint torques
        while self.sum_sqr_error>tolerance and not rospy.is_shutdown():

            if not self._rs.state().enabled:
                    rospy.logerr("Joint torque example failed to meet "
                            "specified control rate timeout.")
                    break
            self._update_forces()
            control_rate.sleep()

def limb_pose(limb_name):
    """Get limb pose at time of OK cuff button press."""
    button = CuffOKButton(limb_name)
    rate = rospy.Rate(20)  # rate at which we check whether button was
                           # pressed or not
    rospy.loginfo(
        'Waiting for %s OK cuff button press to save pose' % limb_name)
    while not button.pressed and not rospy.is_shutdown():
        rate.sleep()
    joint_pose = baxter_interface.Limb(limb_name).joint_angles()
    # Now convert joint coordinates to end effector cartesian
    # coordinates using forward kinematics.
    kinematics = baxter_kinematics(limb_name)
    endpoint_pose = kinematics.forward_position_kinematics(joint_pose)
    #print (endpoint_pose)
    return endpoint_pose

def _execute_gripper_commands(self):
        start_time = rospy.get_time() - self._trajectory_actual_offset.to_sec()
        r_cmd = self._r_grip.trajectory.points
        l_cmd = self._l_grip.trajectory.points
        pnt_times = [pnt.time_from_start.to_sec() for pnt in r_cmd]
        end_time = pnt_times[-1]
        rate = rospy.Rate(self._gripper_rate)
        now_from_start = rospy.get_time() - start_time
        while(now_from_start < end_time + (1.0 / self._gripper_rate) and
              not rospy.is_shutdown()):
            idx = bisect(pnt_times, now_from_start) - 1
            if self._r_gripper.type() != 'custom':
                self._r_gripper.command_position(r_cmd[idx].positions[0])
            if self._l_gripper.type() != 'custom':
                self._l_gripper.command_position(l_cmd[idx].positions[0])
            rate.sleep()
            now_from_start = rospy.get_time() - start_time

def goto_cup(self):
        # self.calibrate_obj_det_pub.publish(True)
        #
        # while self.calibrated == False:
        #     pass
        #
        # print("Finger Sensors calibrated")
        rate = rospy.Rate(100)
        while not rospy.is_shutdown():
            try:
                trans = self.tfBuffer.lookup_transform('root', 'teaCup_position', rospy.Time())
                break
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
                rate.sleep()
            # continue

        translation  = [trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z]
        rotation = [trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w]
        pose_value = translation + rotation
        #second arg=0 (absolute movement), arg = '-r' (relative movement)
        self.cmmnd_CartesianPosition(pose_value, 0)

def cmmd_touchBlock(self,current_finger_position):
        ii = 0
        rate = rospy.Rate(100)
        while self.touch_finger_1 != True and not rospy.is_shutdown():
            current_finger_position[0] += 5 # slowly close finger_1 until contact is made
            # print (current_finger_position[0])
            self.cmmnd_FingerPosition([current_finger_position[0], current_finger_position[1], current_finger_position[2]])
            rate.sleep()
        self.touch_finger_1 = False

        while self.touch_finger_2 != True and not rospy.is_shutdown():
            current_finger_position[1] += 5 # slowly close finger_1 until contact is made
            # print (current_finger_position[0])
            self.cmmnd_FingerPosition([current_finger_position[0], current_finger_position[1], current_finger_position[2]])
            rate.sleep()
        self.touch_finger_2 = False

        return current_finger_position

def main():
    rospy.init_node('issue_com')
    pub = rospy.Publisher(POS_COM_TOPIC, PositionCommand, queue_size=10)
    test_pub = rospy.Publisher(TEST_TOPIC, Empty, queue_size=10)
    sub = rospy.Subscriber(POS_COM_TOPIC, PositionCommand, listen)
    #sub = rospy.Subscriber('/joint_states', JointState, listen)

    pc = PositionCommand()
    pc.mode = JOINT_SPACE
    #pc.arm = PositionCommand.LEFT_ARM
    pc.arm = 1#PositionCommand.RIGHT_ARM
    pc.data = np.zeros(7)

    r = rospy.Rate(1)
    #while not rospy.is_shutdown():
    #    pub.publish(pc)
    #    r.sleep()
    #    print 'published!'
    r.sleep()
    test_pub.publish(Empty())
    pub.publish(pc)