Python rospy.get_time() Examples

def main():
    rospy.init_node('easy_handeye')
    while rospy.get_time() == 0.0:
        pass

    print('Handeye Calibration Commander')
    print('connecting to: {}'.format((rospy.get_namespace().strip('/'))))

    cmder = HandeyeCalibrationCommander()

    if cmder.client.eye_on_hand:
        print('eye-on-hand calibration')
        print('robot effector frame: {}'.format(cmder.client.robot_effector_frame))
    else:
        print('eye-on-base calibration')
        print('robot base frame: {}'.format(cmder.client.robot_base_frame))
    print('tracking base frame: {}'.format(cmder.client.tracking_base_frame))
    print('tracking target frame: {}'.format(cmder.client.tracking_marker_frame))

    cmder.spin_interactive() 

def callback(data):
        """Callback function for subscribing to an Image topic and creating a buffer
        """
        global dtype
        global images_so_far
            
        # Get cv image (which is a numpy array) from data
        cv_image = bridge.imgmsg_to_cv2(data)
        # Save dtype before we float32-ify it
        dtype = str(cv_image.dtype)
        # Insert and roll buffer
        buffer.insert(0, (np.asarray(cv_image, dtype='float32'), rospy.get_time()))
        if(len(buffer) > bufsize):
            buffer.pop()
        
        # for showing framerate
        images_so_far += 1
                
    # Initialize subscriber with our callback 

def get_digital_io_state(self):
            """
            Get Digital IO state : Names, modes, states

            :return: Infos contains in a DigitalIOState object (see niryo_one_msgs)
            :rtype: DigitalIOState
            """
            timeout = rospy.get_time() + 2.0
            while self.digital_io_state is None:
                rospy.sleep(0.05)
                if rospy.get_time() > timeout:
                    raise NiryoOneException(
                        'Timeout: could not get digital io state (/niryo_one/rpi/digital_io_state topic)')
            return self.digital_io_state

        # End effectors 

def initialize_estimate(self, estimate_id, initial_state):
        """Initialize a state estimate with identity covariance.

        The initial estimate is saved in the `self.estimates` dictionary.
        The timestamp in the `self.estimate_times` is updated.

        Args:
             estimate_id (int): ID of the tracked target.
             initial_state (int): Initial state of the estimate.

        Returns:
             X (numpy.ndarray): Solution of equation.
        """
        x = initial_state
        P = self.initial_position_covariance * np.eye(6)
        P[3:6, 3:6] = 0
        estimate = UWBTracker.StateEstimate(x, P)
        self.estimates[estimate_id] = estimate
        self.estimate_times[estimate_id] = rospy.get_time()
        self.ikf_prev_outlier_flags[estimate_id] = False
        self.ikf_outlier_counts[estimate_id] = 0 

def __init__(self):
        self._read_configuration()

        if self.show_plots:
            self._setup_plots()

        rospy.loginfo("Receiving timestamp messages from {}".format(self.uwb_timestamps_topic))
        rospy.loginfo("Publishing multi-range messages to {}".format(self.uwb_multi_range_topic))
        rospy.loginfo("Publishing raw multi-range messages to {}".format(self.uwb_multi_range_raw_topic))
        rospy.loginfo("Publishing multi-range-with-offsets messages to {}".format(
            self.uwb_multi_range_with_offsets_topic))

        # ROS Publishers
        self.uwb_pub = rospy.Publisher(self.uwb_multi_range_topic, uwb.msg.UWBMultiRange, queue_size=1)
        self.uwb_raw_pub = rospy.Publisher(self.uwb_multi_range_raw_topic, uwb.msg.UWBMultiRange, queue_size=1)
        self.uwb_with_offsets_pub = rospy.Publisher(self.uwb_multi_range_with_offsets_topic,
                                                    uwb.msg.UWBMultiRangeWithOffsets, queue_size=1)
        self.uwb_timestamps_sub = rospy.Subscriber(self.uwb_timestamps_topic, uwb.msg.UWBMultiRangeTimestamps,
                                                   self.handle_timestamps_message)

        # Variables for rate display
        self.msg_count = 0
        self.last_now = rospy.get_time() 

def _continuous_data(self,start_cont):
        ret = False
        
        if (True == start_cont):
            r = rospy.Rate(10)
            start_time = rospy.get_time()
            while ((rospy.get_time() - start_time) < 3.0) and (False == ret):
                self._add_config_command_to_queue(0,1)
                r.sleep()
                if  ((rospy.get_time() - self.last_rsp_rcvd) < 0.05):
                    ret = True
        else:
            start_time = rospy.get_time()
            while ((rospy.get_time() - start_time) < 3.0) and (False == ret):
                self._add_config_command_to_queue(0,0)
                rospy.sleep(0.6)
                if  ((rospy.get_time() - self.last_rsp_rcvd) > 0.5):
                    ret = True

        return ret 

def _goto_mode_and_indicate(self,requested):
        """
        define the commands for the function
        """
        config_cmd = ConfigCmd()

        """
        Send the mode command
        """
        r = rospy.Rate(10)
        start_time = rospy.get_time()
        while ((rospy.get_time() - start_time) < 30.0) and (MOVO_MODES_DICT[requested] != self.movo_operational_state):
            config_cmd.header.stamp = rospy.get_rostime()
            config_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
            config_cmd.gp_param = requested
            self.cmd_config_cmd_pub.publish(config_cmd)
            r.sleep()

        if (MOVO_MODES_DICT[requested] != self.movo_operational_state):
            rospy.logerr("Could not set operational Mode")
            rospy.loginfo("The platform did not respond, ")
            return False 

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 get_conveyor_2_feedback(self):
            """
            Give conveyor 2 feedback

            :return: ID, connection_state, running, speed, direction
            :rtype: (int, bool, bool, int, int)
            """
            timeout = rospy.get_time() + 2.0
            while self.conveyor_2_feedback is None:
                rospy.sleep(0.05)
                if rospy.get_time() > timeout:
                    raise NiryoOneException(
                        'Timeout: could not get conveyor 2 feedback (/niryo_one/kits/conveyor_2_feedback topic)')
            fb = self.conveyor_2_feedback
            return fb.conveyor_id, fb.connection_state, fb.running, fb.speed, fb.direction

        # Will highlight a block on a Blockly interface
        # This is just graphical, no real functionality here 

def set_vel(self, fwd_speed, turn_speed, exe_time=1):
        """
        Set the moving velocity of the base

        :param fwd_speed: forward speed
        :param turn_speed: turning speed
        :param exe_time: execution time
        """
        fwd_speed = min(fwd_speed, self.configs.BASE.MAX_ABS_FWD_SPEED)
        fwd_speed = max(fwd_speed, -self.configs.BASE.MAX_ABS_FWD_SPEED)
        turn_speed = min(turn_speed, self.configs.BASE.MAX_ABS_TURN_SPEED)
        turn_speed = max(turn_speed, -self.configs.BASE.MAX_ABS_TURN_SPEED)

        msg = Twist()
        msg.linear.x = fwd_speed
        msg.angular.z = turn_speed

        start_time = rospy.get_time()
        self.ctrl_pub.publish(msg)
        while rospy.get_time() - start_time < exe_time:
            self.ctrl_pub.publish(msg)
            rospy.sleep(1.0 / self.configs.BASE.BASE_CONTROL_RATE) 

def compute_output(self, error):
        """
        Performs a PID computation and returns a control value based on
        the elapsed time (dt) and the error signal from a summing junction
        (the error parameter).
        """
        self._cur_time = rospy.get_time()  # get t
        dt = self._cur_time - self._prev_time  # get delta t
        de = error - self._prev_err  # get delta error

        self._cp = error  # proportional term
        self._ci += error * dt  # integral term

        self._cd = 0
        if dt > 0:  # no div by zero
            self._cd = de / dt  # derivative term

        self._prev_time = self._cur_time  # save t for next pass
        self._prev_err = error  # save t-1 error

        # sum the terms and return the result
        return ((self._kp * self._cp) + (self._ki * self._ci) +
                (self._kd * self._cd)) 

def _callback_mqtt(self, client, userdata, mqtt_msg):
        u""" callback from MQTT

        :param mqtt.Client client: MQTT client used in connection
        :param userdata: user defined data
        :param mqtt.MQTTMessage mqtt_msg: MQTT message
        """
        rospy.logdebug("MQTT received from {}".format(mqtt_msg.topic))
        now = rospy.get_time()

        if self._interval is None or now - self._last_published >= self._interval:
            try:
                ros_msg = self._create_ros_message(mqtt_msg)
                self._publisher.publish(ros_msg)
                self._last_published = now
            except Exception as e:
                rospy.logerr(e) 

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 run_step(self, target_speed, current_speed, current_pose, waypoint):
        """
        Execute one step of control invoking both lateral and longitudinal
        PID controllers to reach a target waypoint at a given target_speed.

        :param target_speed: desired vehicle speed
        :param waypoint: target location encoded as a waypoint
        :return: distance (in meters) to the waypoint
        """
        current_time = rospy.get_time()
        dt = current_time-self._last_control_time
        if dt == 0.0:
            dt = 0.000001
        control = CarlaEgoVehicleControl()
        throttle = self._lon_controller.run_step(target_speed, current_speed, dt)
        steering = self._lat_controller.run_step(current_pose, waypoint, dt)
        self._last_control_time = current_time
        control.steer = steering
        control.throttle = throttle
        control.brake = 0.0
        control.hand_brake = False
        control.manual_gear_shift = False

        return control 

def _update_gripper_joint_state(self,dev=0):
        js = JointState()
        js.header.frame_id = ''
        js.header.stamp = rospy.get_rostime()
        js.header.seq = self._seq[dev]
        if 0==dev:
            prefix = 'right_'
        else:
            prefix='left_'
        js.name = ['%srobotiq_85_left_knuckle_joint'%prefix]
        pos = np.clip(0.8 - ((0.8/0.085) * self._gripper.get_pos(dev)), 0., 0.8)
        js.position = [pos]
        dt = rospy.get_time() - self._prev_js_time[dev]
        self._prev_js_time[dev] = rospy.get_time()
        js.velocity = [(pos-self._prev_js_pos[dev])/dt]
        self._prev_js_pos[dev] = pos
        return js 

def __init__(self, args_lateral=None, args_longitudinal=None):
        """
        :param vehicle: actor to apply to local planner logic onto
        :param args_lateral: dictionary of arguments to set the lateral PID controller using
                             the following semantics:
                             K_P -- Proportional term
                             K_D -- Differential term
                             K_I -- Integral term
        :param args_longitudinal: dictionary of arguments to set the longitudinal PID
                                  controller using the following semantics:
                             K_P -- Proportional term
                             K_D -- Differential term
                             K_I -- Integral term
        """
        if not args_lateral:
            args_lateral = {'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.0}
        if not args_longitudinal:
            args_longitudinal = {'K_P': 1.0, 'K_D': 0.0, 'K_I': 0.0}

        self._lon_controller = PIDLongitudinalController(**args_longitudinal)
        self._lat_controller = PIDLateralController(**args_lateral)
        self._last_control_time = rospy.get_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 move_to_neutral(self, duration=2):
        i = 0
        if (self.trialcount % 50 == 0) and (self.trialcount > 0):
            self.redistribute_objects()

        self.recover()

        self._control_rate.sleep()
        start_time = rospy.get_time()
        t = rospy.get_time()
        while t - start_time < duration:
            self._send_pos_command([0.5, 0.0, 0.10, 0.0, 0.0, 1.0, 0.0])
            i += 1
            self._control_rate.sleep()
            t = rospy.get_time()
        self.trialcount += 1 

def _lerp_joints(self, target_joint_pos, duration):
        start_t, start_joints = rospy.get_time(), self.get_joint_angles()
        self._control_rate.sleep()
        cur_joints = self.get_joint_angles()
        while rospy.get_time() - start_t < 1.2 * duration and not np.isclose(target_joint_pos[:5], cur_joints[:5], atol=CONTROL_TOL).all():
            t = min(1.0, (rospy.get_time() - start_t) / duration)
            target_joints = (1 - t) * start_joints[:5] + t * target_joint_pos[:5]
            self._move_to_target_joints(target_joints)

            self._control_rate.sleep()
            cur_joints = self.get_joint_angles()
        logging.getLogger('robot_logger').debug('Lerped for {} seconds'.format(rospy.get_time() - start_t))
        self._reset_pybullet()

        delta = np.linalg.norm(target_joints[:5] - cur_joints[:5])
        if delta > MAX_FINAL_ERR:
            assert self._arbotix.syncWrite(TORQUE_LIMIT, [[servo_id, 255, 0] for servo_id in SERVO_IDS]) != -1, "failure during servo configuring"
            self._n_errors += 1
        if self._n_errors > MAX_ERRORS:
            logging.getLogger('robot_logger').error('More than {} errors! WidowX probably crashed.'.format(MAX_ERRORS))
            raise Environment_Exception
        
        logging.getLogger('robot_logger').debug('Delta at end of lerp is {}'.format(delta)) 

def _set_gripper(self, command_pos, wait=False):
        self._status_mutex.acquire()
        self._desired_gpos = command_pos
        if wait:
            if self.num_timeouts > MAX_TIMEOUT:
                rospy.signal_shutdown("MORE THAN {} GRIPPER TIMEOUTS".format(MAX_TIMEOUT))

            sem = Semaphore(value=0)  # use of semaphore ensures script will block if gripper dies during execution
            self.sem_list.append(sem)
            self._status_mutex.release()

            start = rospy.get_time()
            logging.getLogger('robot_logger').debug("gripper sem acquire, list len-{}".format(len(self.sem_list)))
            sem.acquire()
            logging.getLogger('robot_logger').debug("waited on gripper for {} seconds".format(rospy.get_time() - start))
        else:
            self._status_mutex.release() 

def handle_request(req):
        """Request-handling for returning a bunch of images stuck together
        """
        # Register time of request
        req_time = rospy.get_time()
        
        # Check if request fits in buffer
        if req.num_requested > len(buffer):
            raise RuntimeError("Number of images requested exceeds current buffer size")
        
        # Cut out the images and timestamps we're returning, save image shape
        ret_images, ret_times = zip(*buffer[:req.num_requested])
        image_shape = ret_images[0].shape
        images_per_frame = 1 if len(image_shape) == 2 else image_shape[2]
        
        # Get timestamps in desired mode
        if req.timing_mode == 0:
            ret_times = np.asarray(ret_times)
        elif req.timing_mode == 1:
            ret_times = np.asarray([req_time - time for time in ret_times])
        else:
            raise RuntimeError("{0} is not a value for timing_mode".format(timing_mode))
        
        # Stack and unravel images because ROS doesn't like multidimensional arrays
        ret_images = np.dstack(ret_images)
        
        return ImageBufferResponse(ret_times, ret_images.ravel(), images_per_frame, dtype, *ret_images.shape)
    
    # Initialize service with our request handler 

def __init__(self,cutoff_freq,num_sigs=1,sig_init=[0.0]):
        self._cutoff_freq = cutoff_freq
        self._sigout = deepcopy(sig_init)
        self._sigin = deepcopy(sig_init)
        self._numsigs = num_sigs
        self._prev_time = rospy.get_time() 

def initialize(self):
        """
        Initialize pid controller.
        """
        # reset delta t variables
        self._cur_time = rospy.get_time()
        self._prev_time = self._cur_time
        self._filt_error.Reset([0.0])

        self._prev_err = 0.0

        # reset result variables
        self._cp = 0.0
        self._ci = 0.0
        self._cd = 0.0 

def processFeedback(self,feedback):
        with self.lock:
        
            p = feedback.pose.position
            o = feedback.pose.orientation
            
            now_time = rospy.get_time()
            dt = now_time - self.last_marker_update
            self.last_feedback = feedback
            
                    
            if (dt >= 0.01):
                self.last_marker_update = now_time
                (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([o.x, o.y, o.z, o.w])
                vx = p.x * self.linear_scale
                vx = limit_f(vx,self.x_vel_limit_mps)
                vy = p.y * self.linear_scale
                vy = limit_f(vy,self.y_vel_limit_mps)
                wz = yaw * self.angular_scale          
                wz = limit_f(wz,self.yaw_rate_limit_rps)

                self.motion_cmd.linear.x = slew_limit(vx,
                                                      self.motion_cmd.linear.x,
                                                      self.accel_lim, dt)
                self.motion_cmd.linear.y = slew_limit(vy,
                                                      self.motion_cmd.linear.y,
                                                      self.accel_lim, dt)
                
                self.motion_cmd.angular.z = slew_limit(wz,
                                                       self.motion_cmd.angular.z,
                                                       self.yaw_accel_lim, dt)
            

                self.motion_pub.publish(self.motion_cmd)
                self.stop_on_timeout = True
                
            
            self._server.setPose(feedback.marker_name, Pose())
            self._server.applyChanges() 

def timeout_check(self,event):
    
        now_time = rospy.get_time()
        dt = now_time - self.last_marker_update
        
        if (dt > 0.4) and (None != self.last_feedback) and (True == self.stop_on_timeout):
            self.motion_cmd.linear.x = 0.0
            self.motion_cmd.linear.y = 0.0
            self.motion_cmd.angular.z = 0.0
            self.motion_pub.publish(self.motion_cmd)
            self._server.setPose(self.last_feedback.marker_name, Pose())
            self._server.applyChanges()
            self.stop_on_timeout = False 

def _shutdown(self):
        rospy.loginfo("Stopping the robot...")
        try:
            self.move_base_client.cancel_all_goals()
        except:
            pass

        try:
            r = rospy.Rate(10)
            start_time = rospy.get_time()
            while ((rospy.get_time() - start_time) < 2.0):
                self.cmd_vel_pub.publish(Twist())
                r.sleep()
        except:
            pass 

def setup(self, context):
	super(Dashboard, self).__init__(context)
        self.name = 'Movo Dashboard'
        self.message = None

        self._console = ConsoleDashWidget(self.context, minimal=False)
        self._monitor = MonitorDashWidget(self.context)
        self._bat = BatteryWidget("Battery Power")

        self._dashboard_message = None
        self._last_dashboard_message_time = 0.0
        self._bat_sub = rospy.Subscriber('/movo/feedback/battery', Battery, self._battery_callback)
        self._last_dashboard_message_time = rospy.get_time()
        self._system_charging = False 

def _basic(state):
        result = {'time': rospy.get_time()}
        if state is not None:
            result.update({
                'name': state.name,
                'state': state.__class__.__name__,
                'path': state._get_path()
            })
        return result 

def talker():
    pub = rospy.Publisher('/gibson_ros/sim_clock', Int64, queue_size=10)
    rospy.init_node('gibson_ros_clock')
    rate = rospy.Rate(1000) # 1000hz
    while not rospy.is_shutdown():
        pub.publish(rospy.get_time())
        rate.sleep() 

def main():
    rospy.init_node('easy_handeye')
    while rospy.get_time() == 0.0:
        pass

    cw = HandeyeServer()

    rospy.spin()