FaceDetector::~FaceDetector() {
ros::start();
deleteROSParams();
ROS_INFO("Qbo face tracking successfully ended");
//Publisher of the nose color
nose_color_pub_ = private_nh_.advertise<qbo_arduqbo::Nose>("/cmd_Nose", 4);
/*
* Creating structure to turn off the nose
*/
qbo_arduqbo::Nose nose;
nose.header.stamp = ros::Time::now();
nose.color=0;
ros::Time time_saved;
time_saved = ros::Time::now();
/*
* Waiting for nose to turn off
*/
while(1)
{
ros::Duration time_diff = ros::Time::now() - time_saved;
//Publish nose color to turn off the light
nose_color_pub_.publish(nose);
if(time_diff.toSec()>2.0)
break;
}
printf("Qbo face tracking successfully ended\n");
}
StereoSelector::~StereoSelector() {
deleteROSParams();
printf("Qbo face tracking successfully ended\n");
}
void FaceDetector::onInit()
{
/*
* Setting ROS Parameters
*/
setROSParams();
cvNamedWindow("Kalman predictor", CV_WINDOW_AUTOSIZE); // output screen
cvInitFont(&font_,CV_FONT_HERSHEY_PLAIN, 1.0, 4.0, 2,2,CV_AA);
textColor_ = CV_RGB(0,255,255);
first_time_=true;
notFoundCount_=0;
found_=false;
random_=true;
if(!face_classifier_.load(face_classifier_path_))
{
ROS_ERROR("Error importing face Haar cascade classifier from the specified path: %s", face_classifier_path_.c_str());
deleteROSParams();
exit(-1);
}
else
{
ROS_INFO("Haar cascade classifier successfully loaded from %s", face_classifier_path_.c_str());
}
if(!alternative_face_classifier_.load(alternative_face_classifier_path_))
{
ROS_WARN("Error importing alternative face Haar cascade classifier from the specified path: %s", alternative_face_classifier_path_.c_str());
exist_alternative_=false;
}
else
{
ROS_INFO("Alternative Haar cascade classifier successfully loaded from %s", alternative_face_classifier_path_.c_str());
exist_alternative_=true;
}
/*
* Subscribers of the node
*/
info_sub_=private_nh_.subscribe<sensor_msgs::CameraInfo>("/usb_cam/camera_info",10,&FaceDetector::infoCallback, this);
joint_states_sub_=private_nh_.subscribe<sensor_msgs::JointState>("/joint_states",10,&FaceDetector::jointStateCallback, this);
/*
* Publishers of the node
*/
//Publisher of the head joint positions
joint_pub_=private_nh_.advertise<sensor_msgs::JointState>("/cmd_joints",1);
//Publisher of the body movements
//base_control_pub_=private_nh_.advertise<geometry_msgs::Twist>("/cmd_vel",1);
base_control_pub_=private_nh_.advertise<geometry_msgs::Twist>("/pre_cmd_vel",1);
face_distance_pub_=private_nh_.advertise<std_msgs::Float32>("/head_distance",1);
/*
* Initialize some internal parameters values
*/
face_detected_bool_ = false;
image_size_ = cv::Size(0,0);
//Initialize Kalman filter
initializeKalmanFilter();
}
void FaceDetector::onInit()
{
/*
* Setting ROS Parameters
*/
setROSParams();
if(!face_classifier_.load(face_classifier_path_))
{
ROS_ERROR("Error importing face Haar cascade classifier from the specified path: %s", face_classifier_path_.c_str());
deleteROSParams();
exit(-1);
}
else
{
ROS_INFO("Haar cascade classifier successfully loaded from %s", face_classifier_path_.c_str());
}
if(!alternative_face_classifier_.load(alternative_face_classifier_path_))
{
ROS_WARN("Error importing alternative face Haar cascade classifier from the specified path: %s", alternative_face_classifier_path_.c_str());
exist_alternative_=false;
}
else
{
ROS_INFO("Alternative Haar cascade classifier successfully loaded from %s", alternative_face_classifier_path_.c_str());
exist_alternative_=true;
}
/*
* Subscribers of the node
*/
info_sub_=private_nh_.subscribe<sensor_msgs::CameraInfo>("/stereo/left/camera_info",10,&FaceDetector::infoCallback, this);
/*
* Publishers of the node
*/
//Publisher of the face tracking position and size
face_position_and_size_pub_=private_nh_.advertise<qbo_face_msgs::FacePosAndDist>("/qbo_face_tracking/face_pos_and_dist", 1);
//Publisher of the face image
face_pub_ = private_nh_.advertise<sensor_msgs::Image>("/qbo_face_tracking/face_image", 1);
//Publisher of the viewer, using image transport for compression
viewer_image_pub_ = it_.advertise("/qbo_face_tracking/viewer", 1);
//Publisher of the nose color
nose_color_pub_ = private_nh_.advertise<qbo_arduqbo::Nose>("/cmd_nose", 1);
/*
* Initialize some internal parameters values
*/
face_detected_bool_ = false;
image_size_ = cv::Size(0,0);
//Initialize Kalman filter
initializeKalmanFilter();
loop_counter_ = 0;
undetected_count_ = undetected_threshold_;
/*
* Set dynamic Haar Cascade check
*/
cam_shift_detected_count_ = 0;
dynamic_check_haar_ = false;
if(check_Haar_<=0)
{ dynamic_check_haar_ = true;
check_Haar_=50;
track_object_ = check_Haar_;
}
/*
* Service clients for face recognition
*/
client_recognize_ = private_nh_.serviceClient<qbo_face_msgs::RecognizeFace>("/qbo_face_recognition/recognize_with_stabilizer");
client_get_name = private_nh_.serviceClient<qbo_face_msgs::GetName>("/qbo_face_recognition/get_name");
}
FaceDetector::~FaceDetector()
{
deleteROSParams();
printf("Qbo face detector successfully ended\n");
}
