void StereoSelector::onInit()
{
/*
* Initialize ROS parameters
*/
setROSParams();
//ROS Subscribers
info_sub_=private_nh_.subscribe<sensor_msgs::CameraInfo>("/stereo/left/camera_info",10,&StereoSelector::infoCallback, this);
joint_states_=private_nh_.subscribe<sensor_msgs::JointState>("/joint_states",10,&StereoSelector::newJointState, this);
//ROS Publishers - images
viewer_image_pub_ = it_.advertise("/qbo_stereo_selector/viewer", 1);
viewer_object_image_pub_ = it_.advertise("/qbo_stereo_selector/object", 1);
object_mask_pub_ = private_nh_.advertise<sensor_msgs::Image>("/qbo_stereo_selector/object_mask", 1);
//ROS Publishers - nose color and head movement
joint_pub_ = private_nh_.advertise<sensor_msgs::JointState>("/cmd_joints", 1);
nose_color_pub_ = private_nh_.advertise<qbo_arduqbo::Nose>("/cmd_nose", 1);
//Initializing values
image_size_ = cv::Size(320,240);
object_detected_bool_ = false;
init_roi_width_=80;
init_roi_height_=80;
object_roi_.x = image_size_.width/2-init_roi_width_/2;
object_roi_.y = image_size_.height/2-init_roi_height_/2;
object_roi_.width = init_roi_width_;
object_roi_.height = init_roi_height_;
undetected_count_ = 0;
yaw_from_joint_ = 0;
tilt_from_joint_ = 0;
u_act_=0;
u_prev_=0;
diff_u_=0;
kp_u_=0.0030; //0.0050
ki_u_=0.00;
kd_u_=0.02;
v_act_=0;
v_prev_=0;
diff_v_=0;
kp_v_=0.0035;
ki_v_=0.00;
kd_v_=0.00;
}
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");
}
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();
}
