void HardwareSensorCameraRos::initCameraRos(double init_time, cv::Size &imgSize)
{
ros::NodeHandle nh_for_camera_info;
ros::Subscriber camera_info_sub = nh_for_camera_info.subscribe("camera_info", 1, &HardwareSensorCameraRos::cam_info_callback, this);
received_cam_info = false;
while(!received_cam_info){
ros::spinOnce();
}
imgSize.width = CAMERA_IMG_WIDTH;
imgSize.height = CAMERA_IMG_HEIGHT;
// Initialize real frequency
ros::Subscriber sub = nh.subscribe("image_raw", 500, &HardwareSensorCameraRos::init_callback, this);
bool first = true;
double time_first = 0, time_last = 0;
int msg_count = 0;
callback_img = rawimage_ptr_t(new RawImage);
while(time_last - time_first < init_time){
if(camera_callback_queue.callOne(ros::WallDuration()) != ros::CallbackQueue::Called) continue;
if(first){
first = false;
time_first = callback_img->timestamp;
}
time_last = callback_img->timestamp;
msg_count++;
}
realFreq = (time_last - time_first)/msg_count;
}
void HardwareSensorCameraRos::initCameraRos(double init_time, cv::Size &imgSize)
{
ROS_INFO("A INICIAR CAMARA ROS...");
ros::NodeHandle nh_for_camera_info;
// ros::Subscriber camera_info_sub;
camInfo_sub = nh_for_camera_info.subscribe("/camera/camera_info", 1, &HardwareSensorCameraRos::cam_info_callback, this);
received_cam_info = false;
while(!received_cam_info){
// ROS_INFO("TESTE CAMARA ROS");
ros::spinOnce();
}
//ROS_INFO("imgSize.width = %d",imgSize.width );
//ROS_INFO("imgSize.height = %d", imgSize.height);
imgSize.width = CAMERA_IMG_WIDTH;
imgSize.height = CAMERA_IMG_HEIGHT;
//ROS_INFO("imgSize.width = %d",imgSize.width );
//ROS_INFO("imgSize.height = %d", imgSize.height);
// Initialize real frequency
camData_sub = nh.subscribe("/camera/image_raw", 1500, &HardwareSensorCameraRos::init_callback, this);
bool first = true;
double time_first = 0, time_last = 0;
int msg_count = 0;
callback_img = rawimage_ptr_t(new RawImage);
ROS_INFO("Ciclo while para iniciar a camara ROS");
while(time_last - time_first < init_time){
// ROS_INFO("While da CAMARA ROS");
if(camera_callback_queue.callOne(ros::WallDuration()) != ros::CallbackQueue::Called) continue;
if(first){
first = false;
time_first = callback_img->timestamp;
}
time_last = callback_img->timestamp;
msg_count++;
// ROS_INFO("count: %d - first: %.5f \t last: %.5f\t init_time:%.5f",msg_count,time_first,time_last,init_time);
}
realFreq = (time_last - time_first)/msg_count;
ROS_INFO("FINAL DE INICIAR CAMARA ROS");
}
