@@ -32,10 +32,6 @@ ros::NodeHandle_<ArduinoHardware> nh;
3232ros::Subscriber<geometry_msgs::Point> sub_point (" ~look_at" , &callback_look_at);
3333ros::Subscriber<std_msgs::UInt16> sub_eye_status (" eye_status" , &callback_emotion);
3434
35- int eye_status = 0 ;
36- bool emotion_changed_flag = true ;
37- static int frame = 0 ;
38-
3935void callback_look_at (const geometry_msgs::Point &msg)
4036{
4137 look_x = (float )msg.x ;
@@ -46,8 +42,7 @@ void callback_look_at(const geometry_msgs::Point &msg)
4642void callback_emotion (const std_msgs::UInt16 &msg)
4743{
4844 nh.loginfo (" in the callback emotion func" );
49- eye_status = msg.data ;
50- emotion_changed_flag = true ;
45+ emotion.set_emotion (msg.data );
5146}
5247
5348void setup ()
@@ -89,19 +84,18 @@ void setup()
8984 eye.init (image_width, image_height, 1 );
9085 }
9186 eye.set_gaze_direction (look_x, look_y);
92- emotion.set_emotion (eye_status );
87+ emotion.set_emotion (); // emotionの初期化
9388 emotion.update_emotion ();
9489}
9590
9691void loop ()
9792{
9893 delay (100 );
99- frame ++;
94+
10095 eye.set_gaze_direction (look_x, look_y);
101- emotion.set_emotion (eye_status);
10296 emotion.update_emotion ();
10397 nh.spinOnce ();
10498 char log_msg[50 ];
105- sprintf (log_msg, " Eye status: %d, Emotion changed: %s " , eye_status, emotion_changed_flag ? " true " : " false " );
99+ sprintf (log_msg, " Eye status: %d" , emotion. get_emotion () );
106100 nh.loginfo (log_msg);
107101}
0 commit comments