Python rospy.sleep方法代码示例

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def create_linear_motion_task_planner(self, target_pose, time=4.0, steps=400.0):
        """ An *incredibly simple* linearly-interpolated Cartesian move """

        # self.trajectory_planner.enable_collision_table1 = False
        # self.trajectory_planner.clear_parameters()
        # self.trajectory_planner.scene.remove_world_object("table1")
        # rospy.sleep(1.0)

        self.trajectory_planner.table1_z = -1.0
        rospy.logwarn("Targetpose:" + str(target_pose))
        jnts = self.sawyer_robot._limb.ik_request(target_pose, self.sawyer_robot._tip_name)
        rospy.logwarn("JNTS:" + str(jnts))
        success = self.safe_goto_joint_position(jnts).result()
        # success = self.trajectory_planner.move_to_joint_target(jnts.values(),attempts=300)
        self.trajectory_planner.table1_z = 0.0

        if not success:
            self.create_wait_forever_task().result()

        return True 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def execute_task(self, fn, args=[]):
        """
        :param fn:
        :return:
        """
        # INTERRUPT HERE CURRENT TASK AND SAVE STATE

        self.stop()

        while len(self.tasks) >0:
            rospy.sleep(0.5)

        #launch asynchronosuly
        fn(*args)

        self.robot_sayt2s("REQUESTED TASK COMPLETED")


        # self.create_main_loop_task().result() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def connect(self):
        try:
            print "Connecting to Arduino on port", self.port, "..."
            self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
            # The next line is necessary to give the firmware time to wake up.
            time.sleep(1)
            test = self.get_baud()
            if test != self.baudrate:
                time.sleep(1)
                test = self.get_baud()   
                if test != self.baudrate:
                    raise SerialException
            print "Connected at", self.baudrate
            print "Arduino is ready."

        except SerialException:
            print "Serial Exception:"
            print sys.exc_info()
            print "Traceback follows:"
            traceback.print_exc(file=sys.stdout)
            print "Cannot connect to Arduino!"
            os._exit(1) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def move_to_eep(self, target_pose, duration=1.5):
        """
        :param target_pose: Cartesian pose (x,y,z, quat). 
        :param duration: Total time trajectory will take before ending
        """
        p1, q1 = self.get_xyz_quat()
        p2, q2 = target_pose[:3], target_pose[3:]

        last_pos = self.get_joint_angles()
        last_cmd = self._limb.joint_angles()
        joint_names = self._limb.joint_names()

        interp_jas = precalculate_interpolation(p1, q1, p2, q2, duration, last_pos, last_cmd, joint_names)

        i = 0
        self._control_rate.sleep()
        start_time = rospy.get_time()
        t = rospy.get_time()
        while t - start_time < duration:
            lookup_index = min(int(min((t - start_time), duration) / CONTROL_PERIOD), len(interp_jas) - 1)
            self._send_pos_command(interp_jas[lookup_index])
            i += 1
            self._control_rate.sleep()
            t = rospy.get_time()
        logging.getLogger('robot_logger').debug('Effective rate: {} Hz'.format(i / (rospy.get_time() - start_time))) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def visualize_stn(data, title):
    input_tensor = data.cpu()
    input_tensor = torch.squeeze(input_tensor)
    input_tensor = input_tensor.detach().numpy()
    N = len(input_tensor)

    fig=plt.figure(num = 2)

    columns = N
    rows = 1
    for i in range(1, columns*rows +1):
        img = input_tensor[i-1]
        fig.add_subplot(rows, columns, i)
        plt.imshow(img, cmap='gray', interpolation='none')

    figManager = plt.get_current_fig_manager()
    figManager.resize(*figManager.window.maxsize())
    # figManager.window.state('zoomed')
    plt.show(block=False)
    # time.sleep(4)
    # plt.close() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def evaluate_training(self, save_path):
        """
        Evaluates an agent during training. Results are saved in <evaluation_data_path>/evaluation_data/train
        :param agent_name: name of the agent (.pkl)
        """

        while True:
            self.__timestep -= 1

            #Waiting for new task
            while not self.__new_task_started:
                time.sleep(0.001)
            self.__done = False
            self.__new_task_started = False
            result = self.evaluate_episode(True)
            with open('%s.pickle' % (save_path), 'ab') as handle:
                pickle.dump(result, handle, protocol=pickle.HIGHEST_PROTOCOL) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def __init__(self):
	
        # Create a publisher for our custom message.
        self.pub = rospy.Publisher('from_ros_to_slack', String, queue_size=10)
	rospy.Subscriber("from_slack_to_ros", String, self.callback)

        # Main while loop.
        while not rospy.is_shutdown():
	    # Sleep for a while before publishing new messages. Division is so rate != period.
            rospy.sleep(5.0) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def __init__(self):
        # Get the ~private namespace parameters from command line or launch file.
        self.token = rospy.get_param('~token', 'xoxp-123456789')
        self.channel = rospy.get_param('~channel', 'G1234567Q')
        self.username = rospy.get_param('~username', 'ros-bot')
	
        # Create a publisher for our custom message.
        pub = rospy.Publisher('from_slack_to_ros', String, queue_size=10)
	rospy.Subscriber("from_ros_to_slack", String, self.callback)
	rospy.Subscriber("send_file_to_slack", String, self.filecallback)

	# Create the slack client
	self.sc = SlackClient(self.token)

	if self.sc.rtm_connect():

            # Main while loop.
            while not rospy.is_shutdown():
                for reply in self.sc.rtm_read():
                    if "type" in reply and "user" in reply:
                        #print reply
                        if reply["type"] == "message" and reply["channel"] == self.channel:
                            pub.publish(reply["text"])
                
	        # Sleep for a while before publishing new messages. Division is so rate != period.
                rospy.sleep(2.0) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def reset(self):
        self._SerialDataGateway.Reset()
        rospy.sleep(3) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def start_countdown(seconds):
    def timeout_countdown(seconds):
        time.sleep(seconds)
        rospy.logfatal("COUNTDOWN ERROR RUNTIME TEST")
        os.kill(os.getpid(), signal.SIGUSR1)
        sys.exit(1)
        
    t= Thread(target = lambda: timeout_countdown(seconds))
    t.start()
#---------------------------------------------
# launch countdown 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def move_gripper(limb, action):
    # initialize interfaces
    print("Getting robot state...")
    rs = intera_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state()
    gripper = None
    original_deadzone = None
    
    def clean_shutdown():
        if gripper and original_deadzone:
            gripper.set_dead_zone(original_deadzone)
        print("Finished.")
    
    try:
        gripper = intera_interface.Gripper(limb + '_gripper')
    except (ValueError, OSError) as e:
        rospy.logerr("Could not detect an electric gripper attached to the robot.")
        clean_shutdown()
        return
    rospy.on_shutdown(clean_shutdown)

    original_deadzone = gripper.get_dead_zone()
    # WARNING: setting the deadzone below this can cause oscillations in
    # the gripper position. However, setting the deadzone to this
    # value is required to achieve the incremental commands in this example
    gripper.set_dead_zone(0.001)
    rospy.loginfo("Gripper deadzone set to {}".format(gripper.get_dead_zone()))
    rospy.loginfo("Enabling robot...")
    rs.enable()
    print("Controlling grippers.")
    if (action == "open"):
        gripper.open()
        rospy.sleep(1.0)
        print("Opened grippers")
    elif (action == "close"):
        gripper.close()
        rospy.sleep(1.0)
        print("Closed grippers")
    
    # force shutdown call if caught by key handler
    rospy.signal_shutdown("Example finished.") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def gripper_open(self):
        """
        :return:
        """
        rospy.logwarn("OPENING GRIPPER")
        self._gripper.open()
        while self._gripper.is_moving() and not rospy.is_shutdown():
            rospy.sleep(0.4) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def gripper_close(self):
        """
        :return:
        """
        rospy.logwarn("CLOSING GRIPPER")
        self._gripper.close()

        while self._gripper.is_moving() and not rospy.is_shutdown():
            rospy.sleep(0.1) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def create_go_vision_head_pose_task(self):
        """
        :return:
        """

        self.sawyer_robot.gripper_open()

        self.trajectory_planner.ceilheight = 0.95
        self.trajectory_planner.update_ceiling_obstacle()

        # joint_angles = self.sawyer_robot._limb.joint_angles()

        oldceil = self.trajectory_planner.ceilheight

        joint_angles = self.head_pose_joints

        self.trajectory_planner.ceilheight = 2.0

        # self.sawyer_robot._limb.move_to_joint_positions(joint_angles)
        found = self.safe_goto_joint_position(joint_angles).result()
        # self.delay_task(0.2).result()
        rospy.sleep(0.5)
        self.trajectory_planner.ceilheight = oldceil
        """
        rospy.logwarn(self.sawyer_robot._limb.endpoint_pose())

        targetpose = Pose()
        targetpose.position.x = 0.4595219280890743
        targetpose.position.y = 0.1473752184292072
        targetpose.position.z = 0.019578584407653032

        targetpose.orientation.x = -0.011325648436031916
        targetpose.orientation.y = 0.9998115142702567
        targetpose.orientation.z = -0.006101035043221461
        targetpose.orientation.w = 0.014541079448283218

        found = self.trajectory_planner.move_to_cartesian_target(targetpose)
        """

        if not found:
            self.create_wait_forever_task().result() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import sleep [as 别名]
def create_linear_motion_task(self, target_pose, time=4.0, steps=500):
        """ An *incredibly simple* linearly-interpolated Cartesian move """

        r = rospy.Rate(1 / (time / steps))  # Defaults to 100Hz command rate
        current_pose = self.sawyer_robot._limb.endpoint_pose()
        ik_delta = Pose()
        ik_delta.position.x = (current_pose['position'].x - target_pose.position.x) / steps
        ik_delta.position.y = (current_pose['position'].y - target_pose.position.y) / steps
        ik_delta.position.z = (current_pose['position'].z - target_pose.position.z) / steps
        ik_delta.orientation.x = (current_pose['orientation'].x - target_pose.orientation.x) / steps
        ik_delta.orientation.y = (current_pose['orientation'].y - target_pose.orientation.y) / steps
        ik_delta.orientation.z = (current_pose['orientation'].z - target_pose.orientation.z) / steps
        ik_delta.orientation.w = (current_pose['orientation'].w - target_pose.orientation.w) / steps
        for d in range(int(steps), -1, -1):
            if rospy.is_shutdown():
                return
            ik_step = Pose()
            ik_step.position.x = d * ik_delta.position.x + target_pose.position.x
            ik_step.position.y = d * ik_delta.position.y + target_pose.position.y
            ik_step.position.z = d * ik_delta.position.z + target_pose.position.z
            ik_step.orientation.x = d * ik_delta.orientation.x + target_pose.orientation.x
            ik_step.orientation.y = d * ik_delta.orientation.y + target_pose.orientation.y
            ik_step.orientation.z = d * ik_delta.orientation.z + target_pose.orientation.z
            ik_step.orientation.w = d * ik_delta.orientation.w + target_pose.orientation.w
            joint_angles = self.sawyer_robot._limb.ik_request(ik_step, self.sawyer_robot._tip_name)
            if joint_angles:
                self.sawyer_robot._limb.set_joint_positions(joint_angles)
            else:
                rospy.logerr("No Joint Angles provided for move_to_joint_positions. Staying put.")

            r.sleep()
        r.sleep()