def do_security():
    global STATUS_LOCK, CAR_UPDATE_FLAG, CAMERA_UPDATE_FLAG
    global CAMERA_CURRENT_TIME, CAR_CURRENT_TIME
    global GLOBAL_MOVE_PUB, VEL_PUB
    global NAV_FLAG_PUB, ENABLE_MOVE_FLAG, BAR_FLAG

    time_now = rospy.Time.now()

    STATUS_LOCK.acquire()

    if CAR_UPDATE_FLAG:
        time1_diff = time_now - CAR_CURRENT_TIME

    if CAMERA_UPDATE_FLAG:
        time2_diff = time_now - CAMERA_CURRENT_TIME

    # 视觉丢失超时保险
    # 里程计丢失超时保险
    if (CAMERA_UPDATE_FLAG and time2_diff.to_sec() > 180 and not BAR_FLAG) or time1_diff.to_sec() > 5.:
        # 发布全局禁止flag
        global_move_flag = Bool()
        global_move_flag.data = False
        GLOBAL_MOVE_PUB.publish(global_move_flag)
        car_twist = Twist()
        VEL_PUB.publish(car_twist)
        CAMERA_UPDATE_FLAG = False
        CAR_UPDATE_FLAG = False
        CAMERA_CURRENT_TIME = rospy.Time.now()
        CAR_CURRENT_TIME = rospy.Time.now()
    STATUS_LOCK.release()

def talker():
    rospy.init_node("frobit_test_set_speed")

    deadman_pub = rospy.Publisher("/fmSignals/deadman", Bool)
    deadman = Bool()

    cmd_vel_left_pub = rospy.Publisher("/fmSignals/cmd_vel_left", TwistStamped)
    twist_left = TwistStamped()

    cmd_vel_right_pub = rospy.Publisher("/fmSignals/cmd_vel_right", TwistStamped)
    twist_right = TwistStamped()

    r = rospy.Rate(node_upd_freq)  # set talker update frequency

    while not rospy.is_shutdown():
        deadman.data = 1  # publish deadman topic
        deadman_pub.publish(deadman)

        twist_left.header.stamp = rospy.Time.now()
        twist_left.twist.linear.x = spd_left
        # publish cmd_vel topic for left wheel
        cmd_vel_left_pub.publish(twist_left)

        twist_right.header.stamp = rospy.Time.now()
        twist_right.twist.linear.x = spd_right
        # publish cmd_vel topic for right wheel
        cmd_vel_right_pub.publish(twist_right)

        r.sleep()

 def run(self):
     while not rospy.is_shutdown():
         if time.time() - self.current_time > self.time:
             shutoff = Bool()
             shutoff.data = True
             self.pub_shutoff.publish(shutoff)
         time.sleep(1)

 def status_updated_callback(self, msg):  # GoalStatusArray
     intermediate_statuses = [GoalStatus.PENDING, GoalStatus.ACTIVE, GoalStatus.RECALLING, GoalStatus.PREEMPTING]
     for goal in msg.status_list:
         # status = GoalManager.status_list[goal.status]
         # rospy.logerr("Goal %s. %s" % (status, goal.text))
         goal_idx = self.get_index_of(goal.goal_id.id)
         if goal.status == GoalStatus.SUCCEEDED:  # do a little dance to celebrate
             rospy.loginfo("Goal %s reached!" % goal.goal_id.id)
             last_goal_reached = Bool()
             last_goal_reached.data = not self.loop_goals and goal_idx + 1 == len(self.goals)
             self.done_pub.publish(last_goal_reached)
             self.wait_for_goal()
             if goal_idx + 1 == len(self.goals):
                 if self.loop_goals:
                     rospy.loginfo("Now looping back to first goal")
                     self.reset_goal_list()
                     self.next_goal()
             else:
                 self.next_goal(goal_idx + 1)
         elif goal.status in intermediate_statuses:  # transitional state, so just wait
             pass
         elif goal.status == GoalStatus.PREEMPTED:
             rospy.logwarn("Goal %s was preempted. %s" % (goal.goal_id.id, goal.text))
         else:  # the robot's dead
             status = GoalManager.status_list[goal.status]
             rospy.logerr("Goal %s %s. %s" % (goal.goal_id.id, status, goal.text))

    def model_stateCB(self, msg):
        if not self.active:
            return 
        quaternion_msg = [msg.pose[1].orientation.x, msg.pose[1].orientation.y, msg.pose[1].orientation.z, msg.pose[1].orientation.w]
        euler = tf.transformations.euler_from_quaternion(quaternion_msg)
        self.robot_pose = (msg.pose[1].position.x, msg.pose[1].position.y, euler[2])
        # print self.robot_pose

        # print 'robot_pose:', self.robot_pose#, 'waypoints:', self.waypoints
        self.destination_pose = self.circleIntersect(self.lookahead_distance)

        if self.destination_pose == None:
            self.active = False
            print "approach destination  "
            self.gazebo_cmd(0,0)
            msg = Bool()
            msg.data = True
            self.pub_finish.publish(msg)

        else:
            # print 'destination_pose:', self.destination_pose
            # print 'waypoint_index:', self.current_waypoint_index    
            self.speed = self.default_speed
            distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose)
            angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose)       
            # self.steering_angle = np.arctan((2 * self.robot_length * np.sin(angle_to_destination)) / distance_to_destination)         
            # print "robot_head",euler[2]*180/math.pi,"angle_to_destination", angle_to_destination*180/math.pi
            w = (angle_to_destination + math.pi) / (2 * math.pi) - 0.5
            self.gazebo_cmd(self.speed,w)

 def grab_close_ball(self):
     self.switch_to_cam("arm")
     mode = Bool()
     mode.data = True
     self.arm_mode_pub.publish(mode)
     req = False
     self.grab_request(req)

def errorCheck(pub):
	r = rospy.Rate(10)
	global count
	precount = 0
	thread_unloop_count = 0
	while True:
		error_flag = Bool()
		if (count - precount) > 0:
			thread_unloop_count = 0
			precount = count
		elif (count - precount) == 0:
			thread_unloop_count += 1
			precount = count
		else:
			thread_unloop_count = 0
			precount = 0
			
		if thread_unloop_count > 2:
			error_flag.data = True
			print'********++++++++++------------------AMU is rebooting now!!------------------++++++++******'
		else:
			error_flag.data = False
			#commented out by hagiwara 0224
			#print'********++++++++++------------------AMU is OK!!------------------++++++++******'
			
		pub.publish(error_flag)
		r.sleep()

    def test_std_msgs_Bool(self):
        from std_msgs.msg import Bool
        self.assertEquals(Bool(), Bool())
        self._test_ser_deser(Bool(), Bool())
        # default value should be False
        self.assertEquals(False, Bool().data)
        # test various constructor permutations
        for v in [True, False]:
            self.assertEquals(Bool(v), Bool(v))
            self.assertEquals(Bool(v), Bool(data=v))
            self.assertEquals(Bool(data=v), Bool(data=v))
        self.assertNotEquals(Bool(True), Bool(False))            

        self._test_ser_deser(Bool(True), Bool())
        self._test_ser_deser(Bool(False), Bool())

        # validate type cast to bool
        blank = Bool()
        b = StringIO()
        Bool(True).serialize(b)
        blank.deserialize(b.getvalue())
        self.assert_(blank.data)
        self.assert_(type(blank.data) == bool)        

        b = StringIO()
        Bool(True).serialize(b)
        blank.deserialize(b.getvalue())
        self.assert_(blank.data)
        self.assert_(type(blank.data) == bool)

	def newSample(self, msg):
		newFFT = self.stft.ingestSample(msg.data)
		if newFFT is not None:
			self.FFT = newFFT

			# Mask and average data
			guard1 = np.mean(newFFT[self.G1_mask, :][:, self.channel_mask])
			alpha = np.mean(newFFT[self.Al_mask, :][:, self.channel_mask])
			guard2 = np.mean(newFFT[self.G2_mask, :][:, self.channel_mask])

			detected = self.movavg.step(alpha > (guard1 + guard2)*1.1) > 0.5
			if detected and not self.ignore.test():
				self.movavg.reset()
				self.ignore.reset(4)
			else:
				detected = False
				
			# Publish messages
			msg = Float32()
			msg.data = guard1
			self.pub_guard1.publish(msg)

			msg = Float32()
			msg.data = alpha
			self.pub_alpha.publish(msg)

			msg = Float32()
			msg.data = guard2
			self.pub_guard2.publish(msg)

			msg = Bool()
			msg.data = detected
			self.pub_eyes.publish(msg)

    def model_stateCB(self, msg):
        if not self.active:
            return

        quaternion_msg = [msg.pose[1].orientation.x, msg.pose[1].orientation.y, msg.pose[1].orientation.z, msg.pose[1].orientation.w]
        euler = tf.transformations.euler_from_quaternion(quaternion_msg)
        self.robot_pose = (msg.pose[1].position.x, msg.pose[1].position.y, euler[2])
            
        distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose)
        angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose)

        if angle_to_destination < 3 * (math.pi/180):
            print "correct straight line" 
            if distance_to_destination<= self.threshold_proximity:
                print "ready to grab"
                self.gazebo_cmd(0,0)
                self.active = False
                self.gripper_cmd(0,0.5)

                msg = Bool()
                msg.data = True
                self.pub_grab_object.publish(msg)
                
            else:
                self.gazebo_cmd(self.speed,0)
        else:
            print "correct orientation"    
            w = (angle_to_destination + math.pi) / (2 * math.pi) - 0.5
            self.gazebo_cmd(0,w)

	def cbJoy(self, joy_msg):

		model_state_msg = Twist()
		model_state_msg.linear.x = joy_msg.axes[1] * self.v_gain
		model_state_msg.linear.y = 0
		model_state_msg.linear.z = 0

		model_state_msg.angular.x = 0
		model_state_msg.angular.y = 0
		model_state_msg.angular.z = -joy_msg.axes[3] * self.w_gain

		self.pub_gazebo.publish(model_state_msg)

		model_state_msg.linear.x = 0
		model_state_msg.angular.z = 0
		if joy_msg.buttons[0] == 1:
			model_state_msg.angular.z = -1
		elif joy_msg.buttons[1] == 1:
			model_state_msg.angular.z = 1

		self.pub_gazebo_gripper.publish(model_state_msg)
		
		if (joy_msg.buttons[7] == 1):
			reset_msg = Bool()
			reset_msg.data = True
			self.pub_encoder_reset.publish(reset_msg)

 def callback_manipulator(self, controller):
     if controller.killswitch != self.controller.killswitch:
         shut_down = Bool()
         shut_down.data = self.controller.killswitch == 1
         self.callback_auto_shut_down(shut_down)
     self.controller = controller
     self.send_manipulator = True

 def pub_e( self , enable ):
     """ pub enable status """
     
     msg      = Bool()
     msg.data = enable
     
     self.pub_enable.publish( msg )

    def prepare_std_msg(self):
        rospy.loginfo("Preparing std_msgs......")
        '''
        Fill Bool message
        '''
        bool_msg = Bool()

        bool_msg.data = False

        '''
        Fill Float32 message
        '''
        float32_msg = Float32()

        float32_msg.data = 1.0

        '''
        Fill Int32 message
        Please, do it your self for practice
        '''

        '''
        Fill String message
        Please, do it your self for practice
        '''
        rospy.loginfo("std_msgs preparation done......")

def navToPose(goal):
    """Drive to a goal subscribed to from /move_base_simple/goal"""
    #compute angle required to make straight-line move to desired pose
    global xPosition
    global yPosition
    global theta
    #capture desired x and y positions
    desiredY = goal.pose.position.y
    desiredX = goal.pose.position.x
    #capture desired angle
    quat = goal.pose.orientation
    q = [quat.x, quat.y, quat.z, quat.w]
    roll, pitch, yaw = euler_from_quaternion(q)
    desiredT = yaw * (180.0/math.pi)
    #compute distance to target
    goalDistance = math.sqrt(math.pow((desiredX - xPosition),2) + math.pow((desiredY-yPosition),2))#Distance formula
    #compute initial turn amount
    initialTurn = math.atan((desiredY-yPosition)/(desiredX-xPosition))
    

    print "spin!" #turn to calculated angle
    rotate(initialTurn)
    print "move!" #move in straight line specified distance to new pose
    driveStraight(0.25, goalDistance)
    rospy.sleep(2)
    print "spin!" #spin to final angle 
    rotate(desiredT)
    print "done"
    arrival = Bool()
    arrival.data = True
    arrival_pub.publish(arrival)

def publishOrderBegun():
    pubBegunMsg = Bool()
    if(orderProcess == True):
        pubBegunMsg.data = 1
    else:
        pubBegunMsg.data = 0
    global pubOrderBegun
    pubOrderBegun.publish(pubBegunMsg)

 def pub_e( self , enable ):
     """ pub actuator cmd """
     
     msg     = Bool()
     
     msg.data = enable
     
     self.pub_enable.publish( msg )

def talker(options):
  pub = rospy.Publisher("pr2_etherCAT/motors_halted", Bool)
  rospy.init_node(NAME, anonymous=True)
  while not rospy.is_shutdown():
    out = Bool()
    out.data = options.halt
    pub.publish(out)
    rospy.sleep(1.0)

def send_move_status(msg):
    """
    Send a movement status message.
    :param msg: A bool, true if moving, false if stopped.
    """
    global move_status_pub
    status_msg = Bool()
    status_msg.data = msg
    move_status_pub.publish(status_msg)

def vacuum_state_cb(msg, arm):
    print("state call back  %8s %7.2f %s"%(arm, g_force[arm], msg.data))
    pub_msg = Bool(False)
    if g_force[arm] > 15 and msg.data is True:
        pub_msg.data = True
    if arm == "left":
        left_vacuum_pub.publish(pub_msg)
    elif arm == "right":
        right_vacuum_pub.publish(pub_msg)