gaze_node.py 文件源码

def __init__(self):
        rospy.init_node('gaze', anonymous=False)

        self.lock = threading.RLock()
        with self.lock:
            self.current_state = GazeState.IDLE
            self.last_state = self.current_state

        # Initialize Variables
        self.glance_timeout = 0
        self.glance_timecount = 0
        self.glance_played = False

        self.idle_timeout = 0
        self.idle_timecount = 0
        self.idle_played = False


        rospy.loginfo('\033[92m[%s]\033[0m waiting for bringup social_mind...'%rospy.get_name())
        rospy.wait_for_service('environmental_memory/read_data')
        rospy.wait_for_service('social_events_memory/read_data')

        self.rd_memory = {}
        self.rd_memory['environmental_memory'] = rospy.ServiceProxy('environmental_memory/read_data', ReadData)
        self.rd_memory['social_events_memory'] = rospy.ServiceProxy('social_events_memory/read_data', ReadData)

        rospy.Subscriber('raising_events', RaisingEvents, self.handle_raising_events)
        rospy.Subscriber('gaze_focusing', String, self.handle_gaze_focusing)
        self.pub_gaze_cmd = rospy.Publisher('gaze_command', GazeCommand, queue_size=10)
        self.pub_viz_gaze_cmd = rospy.Publisher('visualization_gaze_cmd', PointStamped, queue_size=10)

        rospy.Timer(rospy.Duration(GAZE_CONTROLLER_PERIOD), self.handle_gaze_controller)
        rospy.loginfo('\033[92m[%s]\033[0m initialized...'%rospy.get_name())
        rospy.spin()