Python rospy.sleep() Examples

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 

def visualize(data, fig_num, title):
    input_tensor = data.cpu()
    input_tensor = torch.squeeze(input_tensor)
    in_grid = input_tensor.detach().numpy()

    fig=plt.figure(num = fig_num)
    plt.imshow(in_grid, cmap='gray', interpolation='none')
    plt.title(title)
    figManager = plt.get_current_fig_manager()
    figManager.resize(*figManager.window.maxsize())
    plt.show(block=False)
    # time.sleep(4)
    # plt.close() 

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

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) 

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

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

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

def get_state(self):
        while self.quad_pos is None or self.quad_rot is None:
            rospy.sleep(.1)
        quad_pos, quad_rot = self.quad_pos, self.quad_rot
        quad_aa = transformations.axis_angle_from_quaternion(np.r_[quad_rot[3], quad_rot[:3]])
        up = np.array([0, 0, 1])
        return np.r_[quad_pos, quad_aa.dot(up)] 

def reset(self, state=None):
        while self.quad_to_obj_pos is None or \
                self.quad_to_obj_rot is None or \
                self.image is None:
            rospy.sleep(.1)
        obs = np.array(self.quad_to_obj_pos), np.array(self.quad_to_obj_rot), self.image
        return obs 

def rosMarker(loc,idx,color = "green",dur=0.2):
    val = 0.1
    if (color == "green"):
        val = 0.9

    # print(loc[3])
    quat = quaternion_from_euler(0, 0, loc[3])
    marker = Marker()
    # marker.header.frame_id = "map"
    marker.header.frame_id = "/zoe/base_link"
    marker.type = marker.CUBE
    marker.action = marker.ADD

    marker.scale.x = 2.5
    marker.scale.y = 5
    marker.scale.z = 2
    if (len(loc) > 4):
        marker.scale.x = loc[5]
        marker.scale.y = loc[6]
        marker.scale.z = loc[4]

    marker.color.a = 0.4
    marker.color.r = 1 - val
    marker.color.g = val
    marker.color.b = 0.2
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]
    marker.pose.position.x = loc[0]
    marker.pose.position.y = loc[1]
    marker.pose.position.z = loc[2]
    marker.lifetime = rospy.Duration(dur)
    marker.id = idx
    return marker
    # rospy.sleep(0.008)
    # # while (True):
    # for i in range(2):
    #     marker_pub.publish(marker)
    # # rospy.sleep(0.1) 

def reset(self, state=None):
        if state is None:
            state = self.state_space.sample()
        self.pr2.head.goto_joint_positions(state)
        rospy.sleep(1.0) 

def get_relative_target_position(self):
        while self.quad_to_obj_pos is None:
            rospy.sleep(.1)
        return self.quad_to_obj_pos 

def ros_pub(points,topic,color):
    pcl_pub = rospy.Publisher("/all_points", PointCloud2, queue_size=10)
    points = points.cpu().detach().numpy()
    cloud_msg = xyzrgb_array_to_pointcloud2(points,color,stamp =rospy.Time.now(), frame_id = "zoe/base_link" )
    rospy.loginfo("happily publishing sample pointcloud.. !")
    pcl_pub.publish(cloud_msg)
    rospy.sleep(0.1) 

def ros_pub_marker(loc):
    # print(loc[3])
    quat = quaternion_from_euler(0, 0, loc[3])
    marker = Marker()
    marker.header.frame_id = "/zoe/base_link"
    marker.type = marker.CUBE
    marker.action = marker.ADD
    marker.scale.x = 2.5
    marker.scale.y = 5
    marker.scale.z = 2
    marker.color.a = 0.4
    marker.color.r = 0.9
    marker.color.g = 0.1
    marker.color.b = 0.2
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]
    marker.pose.position.x = loc[0]
    marker.pose.position.y = loc[1]
    marker.pose.position.z = loc[2]
    # marker.lifetime = rospy.Duration(1)
    rospy.sleep(0.2)
    # while (True):
    for i in range(5):
        marker_pub.publish(marker)
    # rospy.sleep(0.1) 

def __init__(self, action_space, observation_space, state_space, sensor_names, dt=None):
        super(Pr2Env, self).__init__(action_space, observation_space, state_space, sensor_names)
        self._dt = 0.2 if dt is None else dt
        self.pr2 = PR2.PR2()
        self.pr2.larm.goto_posture('side')
        self.pr2.rarm.goto_posture('side')
        self.pr2.torso.go_down()
        gains = {'head_pan_joint': {'d': 2.0, 'i': 12.0, 'i_clamp': 0.5, 'p': 50.0},
                 'head_tilt_joint': {'d': 3.0, 'i': 4.0, 'i_clamp': 0.2, 'p': 1000.0}}
        rospy.set_param('/head_traj_controller/gains', gains)
        self.pr2.head.set_pan_tilt(*((self.state_space.low + self.state_space.high) / 2.0))

        self.msg_and_camera_sensor = camera_sensor.MessageAndCameraSensor()
        rospy.sleep(5.0) 

def send_command(self, msg, pub_rate):
        """
        @param msg: either an InteractionControlCommand message or
                    InteractionOptions object to be published
        @param pub_rate: the rate in Hz to publish the command

        Note that this function is blocking for non-zero pub_rates until
        the node is shutdown (e.g. via cntl+c) or the robot is disabled.
        A pub_rate of zero will publish the function once and return.
        """
        repeat = False
        if pub_rate > 0:
            rate = rospy.Rate(pub_rate)
            repeat = True
        elif pub_rate < 0:
            rospy.logerr('Invalid publish rate!')

        if isinstance(msg, InteractionOptions):
            msg = msg.to_msg()

        try:
            self.pub.publish(msg)
            while repeat and not rospy.is_shutdown() and self.enable.state().enabled:
                rate.sleep()
                self.pub.publish(msg)
        except rospy.ROSInterruptException:
            rospy.logerr('Keyboard interrupt detected from the user. %s',
                         'Exiting the node...')
        finally:
            if repeat:
                self.send_position_mode_cmd() 

def send_position_mode_cmd(self):
        '''
        Send a message to put the robot back into position mode
        '''
        position_mode = InteractionOptions()
        position_mode.set_interaction_control_active(False)
        self.pub.publish(position_mode.to_msg())
        rospy.loginfo('Sending position command')
        rospy.sleep(0.5) 

def step(self, action):
        twist_msg = geometry_msgs.Twist(
            linear=geometry_msgs.Point(action[1], -action[0], action[2]),  # forward, left, ascend
            angular=geometry_msgs.Point(0., 0., action[3])
        )
        self.cmd_vel_pub.publish(twist_msg)
        self.rate.sleep()
        while self.quad_to_obj_pos is None or \
                self.quad_to_obj_rot is None or \
                self.image is None:
            rospy.sleep(.1)
        obs = np.array(self.quad_to_obj_pos), np.array(self.quad_to_obj_rot), self.image
        return obs, None, None, None 

def publish_grasps_as_poses(grasps):
	rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
	graspmsg = Grasp()
	grasp_PA = PoseArray()
	header = Header()
	header.frame_id = "base_link"
	header.stamp = rospy.Time.now()
	grasp_PA.header = header
	for graspmsg in grasps:
		p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
		grasp_PA.poses.append(p)
	grasp_publisher.publish(grasp_PA)
	rospy.loginfo('Published ' + str(len(grasp_PA.poses)) + ' poses')
	rospy.sleep(2) 

def ros_pub_marker(loc,color = "green"):
    val = 0.1
    if (color == "green"):
        val = 0.9

    # print(loc[3])
    quat = quaternion_from_euler(0, 0, loc[3])
    marker = Marker()
    marker.header.frame_id = "/zoe/base_link"
    # marker.header.frame_id = "/zoe/base_link"
    marker.type = marker.CUBE
    marker.action = marker.ADD

    marker.scale.x = 2.5
    marker.scale.y = 5
    marker.scale.z = 2
    if (len(loc) > 4):
        marker.scale.x = loc[5]
        marker.scale.y = loc[6]
        marker.scale.z = loc[4]

    marker.color.a = 0.4
    marker.color.r = 1 - val
    marker.color.g = val
    marker.color.b = 0.2
    marker.pose.orientation.x = quat[0]
    marker.pose.orientation.y = quat[1]
    marker.pose.orientation.z = quat[2]
    marker.pose.orientation.w = quat[3]
    marker.pose.position.x = loc[0]
    marker.pose.position.y = loc[1]
    marker.pose.position.z = loc[2]
    # marker.lifetime = rospy.Duration(0.3)
    marker_pub.publish(marker)
    # rospy.sleep(0.008)
    # # while (True):
    # for i in range(2):
    #     marker_pub.publish(marker)
    # # rospy.sleep(0.1) 

def ros_pub_cloud(points,topic,color,pcl_pub):

    # points = points.cpu().detach().numpy()
    cloud_msg = xyzrgb_array_to_pointcloud2(points,color,stamp =rospy.Time.now(), frame_id = "/zoe/base_link" )
    # rospy.loginfo("happily publishing sample pointcloud.. !")
    pcl_pub.publish(cloud_msg)
    # rospy.sleep(0.1) 

def move_to_ja(self, waypoints, duration=1.5):
        """
        :param waypoints: List of joint angle arrays. If len(waypoints) == 1: then go directly to point.
                                                      Otherwise: take trajectory that ends at waypoints[-1] and passes through each intermediate waypoint
        :param duration: Total time trajectory will take before ending
        """
        self._try_enable()

        jointnames = self._limb.joint_names()
        prev_joint = np.array([self._limb.joint_angle(j) for j in jointnames])
        waypoints = np.array([prev_joint] + waypoints)

        spline = CSpline(waypoints, duration)

        start_time = rospy.get_time()  # in seconds
        finish_time = start_time + duration  # in seconds

        time = rospy.get_time()
        while time < finish_time:
            pos, velocity, acceleration = spline.get(time - start_time)
            command = JointCommand()
            command.mode = JointCommand.POSITION_MODE
            command.names = jointnames
            command.command = pos
            #command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag)
            #command.acceleration = np.clip(acceleration, -max_accel_mag, max_accel_mag)
            self._cmd_publisher.publish(command)

            self._control_rate.sleep()
            time = rospy.get_time()

        for i in xrange(10):
            command = JointCommand()
            command.mode = JointCommand.POSITION_MODE
            command.names = jointnames
            #command.position = waypoints[-1]
            command.command = waypoints[-1]
            self._cmd_publisher.publish(command)

            self._control_rate.sleep() 

def start_tracking(self, start_points):
        assert self._tracking_enabled
        n_desig, xy_dim = start_points.shape
        if n_desig != 1:
            raise NotImplementedError("opencv_tracking requires 1 designated pixel")
        if xy_dim != 2:
            raise ValueError("Requires XY pixel location")

        self._latest_image.mutex.acquire()
        self._cam_start_tracking(self._latest_image, start_points[0])
        self._is_tracking = True
        self._latest_image.mutex.release()
        rospy.sleep(2)   # sleep a bit for first few messages to initialize tracker

        logging.getLogger('robot_logger').info("TRACKING INITIALIZED") 

def move_to_ja(self, waypoints, duration=1.5):
        """
        :param waypoints: List of joint angle arrays. If len(waypoints) == 1: then go directly to point.
                                                      Otherwise: take trajectory that ends at waypoints[-1] and passes through each intermediate waypoint
        :param duration: Total time trajectory will take before ending
        """
        self._try_enable()

        jointnames = self._limb.joint_names()
        prev_joint = np.array([self._limb.joint_angle(j) for j in jointnames])
        waypoints = np.array([prev_joint] + waypoints)

        spline = CSpline(waypoints, duration)

        start_time = rospy.get_time()  # in seconds
        finish_time = start_time + duration  # in seconds

        time = rospy.get_time()
        while time < finish_time:
            pos, velocity, acceleration = spline.get(time - start_time)
            command = JointCommand()
            command.mode = JointCommand.POSITION_MODE
            command.names = jointnames
            command.position = pos
            command.velocity = np.clip(velocity, -max_vel_mag, max_vel_mag)
            command.acceleration = np.clip(acceleration, -max_accel_mag, max_accel_mag)
            self._cmd_publisher.publish(command)

            self._control_rate.sleep()
            time = rospy.get_time()

        for i in xrange(10):
            command = JointCommand()
            command.mode = JointCommand.POSITION_MODE
            command.names = jointnames
            command.position = waypoints[-1]
            self._cmd_publisher.publish(command)

            self._control_rate.sleep() 

def _try_enable(self):
        """
        The start impedance script will try to re-enable the robot once it disables
        The script will wait until that occurs and throw an assertion if it doesn't
        """
        i = 0
        while not self._rs.state().enabled and i < 50:
            rospy.sleep(10)
            i += 1
        if not self._rs.state().enabled:
            logging.getLogger('robot_logger').error("Robot was disabled, please manually re-enable!")
            self.clean_shutdown() 

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
        """
        self._try_enable()


        # logging.getLogger('robot_logger').debug('Target Cartesian position in interface: {}'.format(target_pose))

        self.KukaObj.move_kuka_to_eep(target_pose)


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

#### ****************************************** Doubtful about this function. Still using position mode?  *******************************#### 

def _try_enable(self):
        """
        The start impedance script will try to re-enable the robot once it disables
        The script will wait until that occurs and throw an assertion if it doesn't
        """
        # i = 0
        # while not self._rs.state().enabled and i < 50:
        #     rospy.sleep(10)
        #     i += 1
        # if not self._rs.state().enabled:

        #*** Check how the kuka can be seen as enabled or not *************###
        if not self.KukaObj.Robot_State():
            logging.getLogger('robot_logger').error("Robot was disabled, please manually re-enable!")
            self.clean_shutdown() 

def get_kuka_joint_angles_effort(self):
        #returns current joint angle velocities
        # rospy.sleep(0.01)
        return self._joint_efforts 

def get_kuka_joint_angles_names(self):
        #returns current joint angle velocities
        # rospy.sleep(0.01)
        return self._joint_name 

def get_kuka_joint_angles_velocity(self):
        #returns current joint angle velocities
        # rospy.sleep(0.01)
        return self._joint_vel