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 _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 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 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 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 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_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 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 __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 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 _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 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 __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 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 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 _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 _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 _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 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 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 render(self):
        """ Grabs images form cameras.
        If returning multiple images asserts timestamps are w/in OBS_TOLERANCE, and raises Image_Exception otherwise

        - dual: renders both left and main cameras
        - left: renders only left camera
        - main: renders only main (front) camera
        :param mode: Mode to render with (dual by default)
        :return: uint8 numpy array with rendering from sim
        """
        time_stamps = []
        cam_imgs = []
        cur_time = rospy.get_time()

        for recorder in self._cameras:
            stamp, image = recorder.get_image()
            print("stamp:", stamp)
            logging.getLogger('robot_logger').error("Checking for time difference:  Current time {} camera time {}".format(cur_time, stamp))
            if abs(stamp - cur_time) > 10 * self._obs_tol:    # no camera ping in half second => camera failure
                logging.getLogger('robot_logger').error("DeSYNC - no ping in more than {} seconds!".format(10 * self._obs_tol))
                raise Image_Exception
            time_stamps.append(stamp)
            cam_imgs.append(image)

        if self.ncam > 1:
            for index, i in enumerate(time_stamps[:-1]):
                for j in time_stamps[index + 1:]:
                    if abs(i - j) > self._obs_tol:
                        logging.getLogger('robot_logger').error('DeSYNC- Cameras are out of sync!')
                        raise Image_Exception

        images = np.zeros((self.ncam, self._height, self._width, 3), dtype=np.uint8)
        for c, img in enumerate(cam_imgs):
            images[c] = img[:, :, ::-1]

        return images 

def _proc_image(self, latest_obsv, data):
        latest_obsv.img_msg = data
        latest_obsv.tstamp_img = rospy.get_time()

        cv_image = self.bridge.imgmsg_to_cv2(data, self._image_dtype)[:, :, :3]
        latest_obsv.img_cv2 = copy.deepcopy(self._topic_data.process_image(cv_image))

        if self._tracking_enabled and self._is_tracking:
            if latest_obsv.track_itr % self.TRACK_SKIP == 0:
                _, bbox = latest_obsv.cv2_tracker.update(latest_obsv.img_cv2)
                latest_obsv.bbox = np.array(bbox).astype(np.int32).reshape(-1)
            latest_obsv.track_itr += 1 

def wait_for(test, timeout=1.0, raise_on_error=True, rate=100,
             timeout_msg="timeout expired", body=None):
    """
    Waits until some condition evaluates to true.

    @param test: zero param function to be evaluated
    @param timeout: max amount of time to wait. negative/inf for indefinitely
    @param raise_on_error: raise or just return False
    @param rate: the rate at which to check
    @param timout_msg: message to supply to the timeout exception
    @param body: optional function to execute while waiting
    """
    end_time = rospy.get_time() + timeout
    rate = rospy.Rate(rate)
    notimeout = (timeout < 0.0) or timeout == float("inf")
    while not test():
        if rospy.is_shutdown():
            if raise_on_error:
                raise OSError(errno.ESHUTDOWN, "ROS Shutdown")
            return False
        elif (not notimeout) and (rospy.get_time() >= end_time):
            if raise_on_error:
                raise OSError(errno.ETIMEDOUT, timeout_msg)
            return False
        if callable(body):
            body()
        rate.sleep()
    return True 

def _update_feedback(self, cmd_point, jnt_names, cur_time):
        self._fdbk.header.stamp = rospy.Duration.from_sec(rospy.get_time())
        self._fdbk.joint_names = jnt_names
        self._fdbk.desired = cmd_point
        self._fdbk.desired.time_from_start = rospy.Duration.from_sec(cur_time)
        self._fdbk.actual.positions = self._get_current_position(jnt_names)
        self._fdbk.actual.time_from_start = rospy.Duration.from_sec(cur_time)
        self._fdbk.error.positions = map(operator.sub,
                                         self._fdbk.desired.positions,
                                         self._fdbk.actual.positions
                                        )
        self._fdbk.error.time_from_start = rospy.Duration.from_sec(cur_time)
        self._server.publish_feedback(self._fdbk) 

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

        self._prev_err = 0.0

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

def __init__(self, filename, rate, side="right"):
        """
        Records joint data to a file at a specified rate.
        """
        self.gripper_name = '_'.join([side, 'gripper'])
        self._filename = filename
        self._raw_rate = rate
        self._rate = rospy.Rate(rate)
        self._start_time = rospy.get_time()
        self._done = False

        self._limb_right = intera_interface.Limb(side)
        try:
            self._gripper = intera_interface.Gripper(self.gripper_name)
            rospy.loginfo("Electric gripper detected.")
        except Exception as e:
            self._gripper = None
            rospy.loginfo("No electric gripper detected.")

        # Verify Gripper Have No Errors and are Calibrated
        if self._gripper:
            if self._gripper.has_error():
                self._gripper.reboot()
            if not self._gripper.is_calibrated():
                self._gripper.calibrate()

        self._cuff = intera_interface.Cuff(side) 

def _execute_gripper_commands(self):
        start_time = rospy.get_time() - self._trajectory_actual_offset.to_sec()
        grip_cmd = self.grip.trajectory.points
        pnt_times = [pnt.time_from_start.to_sec() for pnt in grip_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.gripper:
                self.gripper.set_position(grip_cmd[idx].positions[0])
            rate.sleep()
            now_from_start = rospy.get_time() - start_time 

def get_hz(self,use_cached = False):
        if use_cached:
            return self.last_rate
        if not self.times:
            rate = 0
        elif self.msg_tn == self.last_printed_tn:
            rate = 0
        else:
            n = len(self.times)
            #rate = (n - 1) / (rospy.get_time() - self.msg_t0)
            mean = sum(self.times) / n
            rate = 1./mean if mean > 0. else 0
            self.last_printed_tn = self.msg_tn
        self.last_rate = rate
        return rate