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 store_latest_im(self, data):
        self._latest_image.mutex.acquire()
        self._proc_image(self._latest_image, data)

        current_hash = hashlib.sha256(self._latest_image.img_cv2.tostring()).hexdigest()
        if self._is_first_status:
            self._cam_height, self._cam_width = self._latest_image.img_cv2.shape[:2]
            self._is_first_status = False
            self._status_sem.release()
        elif self._last_hash == current_hash:
            if self._num_repeats < self.MAX_REPEATS:
                self._num_repeats += 1
            else:
                logging.getLogger('robot_logger').error('Too many repeated images. Check camera!')
                rospy.signal_shutdown('Too many repeated images. Check camera!')
        else:
            self._num_repeats = 0
        self._last_hash = current_hash

        if self._save_vides and self._saving:
            if self._latest_image.save_itr % self.TRACK_SKIP == 0:
                self._buffers.append(copy.deepcopy(self._latest_image.img_cv2)[:, :, ::-1])
            self._latest_image.save_itr += 1
        self._latest_image.mutex.release() 

def __init__(self, limb="right"):
        """
        @param limb: Limb to run CalibrateArm on arm side.
        """
        self._limb=limb
        self._client = actionlib.SimpleActionClient('/calibration_command',
                                   CalibrationCommandAction)
        # Waits until the action server has started up and started
        # listening for goals.
        server_up = self._client.wait_for_server(rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Calibration"
                         " Server to connect. Check your ROS networking"
                         "  and/or reboot the robot.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1) 

def __init__(self, limb, joint_names):
        self._joint_names = joint_names
        ns = 'robot/limb/' + limb + '/'
        self._client = actionlib.SimpleActionClient(
            ns + "follow_joint_trajectory",
            FollowJointTrajectoryAction,
        )
        self._goal = FollowJointTrajectoryGoal()
        self._goal_time_tolerance = rospy.Time(0.1)
        self._goal.goal_time_tolerance = self._goal_time_tolerance
        server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Joint Trajectory"
                         " Action Server to connect. Start the action server"
                         " before running example.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1)
        self.clear(limb) 

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 closeEvent(self, event):
        """ Intercept shutdowns to cleanly disconnect from the flie and cleanly shut ROS down too """
        rospy.loginfo("Close Event")
        #self.autoRetryTimer.stop()

        # Shut Down the Flie
        if self.state < STATE.GEN_DISCONNECTED:
            rospy.loginfo("Triggering flie disconnect for shutdown")
            self.ui.pushButton_connect.setText("Shutting Down...")
            self.flie.requestDisconnect()

        # Clean up ROS
        rospy.signal_shutdown("User closed window")

        # Save State
        self.writeSettings()

        # Commence shutdown
        event.accept() 

def __init__(self, limb):
        self.waiting = False
        self.jointnames = ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']
        ns = 'robot/limb/' + limb + '/'
        self._client = actionlib.SimpleActionClient(
            ns + "follow_joint_trajectory",
            FollowJointTrajectoryAction,
        )
        self._goal = FollowJointTrajectoryGoal()
        server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Joint Trajectory"
                         " Action Server to connect. Start the action server"
                         " before running example.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1)
        self.clear(limb) 

def __init__(self):
        # ROS initialization:
        rospy.init_node('pose_manager')

        self.poseLibrary = dict()
        self.readInPoses()
        self.poseServer = actionlib.SimpleActionServer("body_pose", BodyPoseAction,
                                                       execute_cb=self.executeBodyPose,
                                                       auto_start=False)
        self.trajectoryClient = actionlib.SimpleActionClient("joint_trajectory", JointTrajectoryAction)
        if self.trajectoryClient.wait_for_server(rospy.Duration(3.0)):
            try:
                rospy.wait_for_service("stop_walk_srv", timeout=2.0)
                self.stopWalkSrv = rospy.ServiceProxy("stop_walk_srv", Empty)
            except:
                rospy.logwarn("stop_walk_srv not available, pose_manager will not stop the walker before executing a trajectory. "
                          +"This is normal if there is no nao_walker running.")
            self.stopWalkSrv = None
            self.poseServer.start()

            rospy.loginfo("pose_manager running, offering poses: %s", self.poseLibrary.keys());

        else:
            rospy.logfatal("Could not connect to required \"joint_trajectory\" action server, is the nao_controller node running?");
            rospy.signal_shutdown("Required component missing"); 

def destroy(self):
        """
        Function to destroy this object.

        :return:
        """
        rospy.signal_shutdown("")
        self.debug_helper.destroy()
        self.shutdown.set()
        self.carla_weather_subscriber.unregister()
        self.carla_control_queue.put(CarlaControl.STEP_ONCE)
        if not self.carla_settings.synchronous_mode:
            if self.on_tick_id:
                self.carla_world.remove_on_tick(self.on_tick_id)
            self.update_actor_thread.join()
        self._update_actors(set())

        rospy.loginfo("Exiting Bridge") 

def __init__(self, arm='right', dof=''):
        if ''==dof:
            rospy.logerr('DoF parameter needs to be set 6 or 7')
            return

        self._client = actionlib.SimpleActionClient(
            'movo/%s_arm_controller/follow_joint_trajectory'%arm,
            FollowJointTrajectoryAction,
        )
        self._goal = FollowJointTrajectoryGoal()
        self._goal_time_tolerance = rospy.Time(0.1)
        self._goal.goal_time_tolerance = self._goal_time_tolerance
        self.dof = dof
        server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Joint Trajectory"
                         " Action Server to connect. Start the action server"
                         " before running example.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1)
        self.clear(arm) 

def __init__(self):
        self._client = actionlib.SimpleActionClient(
            'movo/head_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction,
        )
        self._goal = FollowJointTrajectoryGoal()
        self._goal_time_tolerance = rospy.Time(0.1)
        self._goal.goal_time_tolerance = self._goal_time_tolerance
        self.total_time = 0.0
        server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Joint Trajectory"
                         " Action Server to connect. Start the action server"
                         " before running example.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1)
        self.clear() 

def __init__(self):
        self._client = actionlib.SimpleActionClient(
            'movo/torso_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction,
        )
        self._goal = FollowJointTrajectoryGoal()
        self._goal_time_tolerance = rospy.Time(0.1)
        self._goal.goal_time_tolerance = self._goal_time_tolerance
        self.total_time = 0.0
        server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Joint Trajectory"
                         " Action Server to connect. Start the action server"
                         " before running example.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1)
        self.clear() 

def __init__(self,prefix="right"):
        
        self._prefix = prefix
        self._client = actionlib.SimpleActionClient(
            '/movo/%s_gripper_controller/gripper_cmd'%self._prefix,
            GripperCommandAction,
        )
        self._goal = GripperCommandGoal()
        server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Gripper Command"
                         " Action Server to connect. Start the action server"
                         " before running example.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1)
        self.clear() 

def __init__(self):
        self._client = actionlib.SimpleActionClient(
            'movo/head_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction,
        )
        self._goal = FollowJointTrajectoryGoal()
        self._goal_time_tolerance = rospy.Time(0.1)
        self._goal.goal_time_tolerance = self._goal_time_tolerance
        self.total_time = 0.0
        server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Joint Trajectory"
                         " Action Server to connect. Start the action server"
                         " before running example.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1)
        self.clear() 

def __init__(self, arm='right', dof='6dof'):
        self._client = actionlib.SimpleActionClient(
            'movo/%s_arm_controller/follow_joint_trajectory'%arm,
            FollowJointTrajectoryAction,
        )
        self._goal = FollowJointTrajectoryGoal()
        self._goal_time_tolerance = rospy.Time(0.1)
        self._goal.goal_time_tolerance = self._goal_time_tolerance
        self.dof = dof
        server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Joint Trajectory"
                         " Action Server to connect. Start the action server"
                         " before running example.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1)
        self.clear(arm) 

def __init__(self):
        self._client = actionlib.SimpleActionClient(
            'movo/torso_controller/follow_joint_trajectory',
            FollowJointTrajectoryAction,
        )
        self._goal = FollowJointTrajectoryGoal()
        self._goal_time_tolerance = rospy.Time(0.1)
        self._goal.goal_time_tolerance = self._goal_time_tolerance
        self.total_time = 0.0
        server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
        if not server_up:
            rospy.logerr("Timed out waiting for Joint Trajectory"
                         " Action Server to connect. Start the action server"
                         " before running example.")
            rospy.signal_shutdown("Timed out waiting for Action Server")
            sys.exit(1)
        self.clear() 

def exit_properly_runtime_test():
    f = open("result.txt", "w")
    f.write("0")
    f.close()
    rospy.signal_shutdown("SUCCESS TEST")
    os.kill(os.getpid(), signal.SIGUSR1)
    sys.exit(0) 

def main():
    rospy.init_node('Demo_Stats')
    rs = intera_interface.RobotEnable(CHECK_VERSION)
    state = rs.state()
    print state
    rospy.signal_shutdown("Demo_Stats finished.") 

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

def shutdown(self):
        """
        Shuts down the node
        """
        rospy.signal_shutdown("See ya!") 

def shutdown(self):
        """
        Shuts down the node
        """
        rospy.signal_shutdown("See ya!") 

def shutdown(self):
        """
        Shuts down the node
        """
        rospy.signal_shutdown("See ya!") 

def shutdown(self):
        """
        Shuts down the node
        """
        rospy.signal_shutdown("See ya!") 

def shutdown(self):
        """
        Shuts down the node
        """
        rospy.signal_shutdown("See ya!") 

def signal_handler(signal, frame):
    rospy.signal_shutdown("Order 66 Received")
    exit("Order 66 Received") 

def __init__(self):
        """
        Constructor - creates a new motion controller action client and
        waits for a connection to the motion controller action server.
        """
        self._waypointSequenceId = 0
        self._client = actionlib.SimpleActionClient(
            '/motion/motion_command',
            intera_motion_msgs.msg.MotionCommandAction)
        if not self._client.wait_for_server(rospy.Duration(10.0)):
            rospy.logerr("Timed out waiting for Motion Controller"
                         " Server to connect. Check your ROS networking"
                         "  and/or reboot the robot.")
            rospy.signal_shutdown("Timed out waiting for Action Server") 

def ros2cv(self, data, dt="mono8"):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, dt)
        except CvBridgeError, err:
            rospy.logerr("Converting image error: %s", err)
            rospy.signal_shutdown("Converting image error: "+ str(err)) 

def quit_sequence(self):
        self.wrap_up()
        if self.args.jay1 or self.args.gazebo:
            rospy.logwarn("Quit")
            rospy.signal_shutdown("Quit")
        exit() 

def quit_sequence(self):
        self.wrap_up()
        if self.args.jay1 or self.args.gazebo:
            rospy.logwarn("Quit")
            rospy.signal_shutdown("Quit")
        exit() 

def stop(self):
        if self.show_plots:
            import pyqtgraph
            pyqtgraph.QtGui.QApplication.quit()
        else:
            rospy.signal_shutdown('User request')