Python rospy.get_time方法代码示例

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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)) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [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 get_time [as 别名]
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)) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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.") 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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) 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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() 

# 需要导入模块: import rospy [as 别名]
# 或者: from rospy import get_time [as 别名]
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