/** device poll */ void Camera1394Driver::poll(void) { // Do not run concurrently with reconfig(). // // The mutex lock should be sufficient, but the Linux pthreads // implementation does not guarantee fairness, and the reconfig() // callback thread generally suffers from lock starvation for many // seconds before getting to run. So, we avoid acquiring the lock // if there is a reconfig() pending. bool do_sleep = true; if (!reconfiguring_) { boost::mutex::scoped_lock lock(mutex_); do_sleep = (state_ == Driver::CLOSED); if (!do_sleep) { // driver is open, read the next image still holding lock sensor_msgs::ImagePtr left_image(new sensor_msgs::Image); sensor_msgs::ImagePtr right_image(new sensor_msgs::Image); if (read(left_image, right_image)) { publish(left_image, right_image); } } } if (do_sleep) { // device was closed or poll is not running, sleeping avoids // busy wait (DO NOT hold the lock while sleeping) cycle_.sleep(); } }
void dialog_frame::draw_border() { if(have_border_ == false) { return; } surface top_image(scale_surface(top_, dim_.interior.w, top_->h)); if(top_image != NULL) { video_.blit_surface(dim_.interior.x, dim_.exterior.y, top_image); } surface bot_image(scale_surface(bot_, dim_.interior.w, bot_->h)); if(bot_image != NULL) { video_.blit_surface(dim_.interior.x, dim_.interior.y + dim_.interior.h, bot_image); } surface left_image(scale_surface(left_, left_->w, dim_.interior.h)); if(left_image != NULL) { video_.blit_surface(dim_.exterior.x, dim_.interior.y, left_image); } surface right_image(scale_surface(right_, right_->w, dim_.interior.h)); if(right_image != NULL) { video_.blit_surface(dim_.interior.x + dim_.interior.w, dim_.interior.y, right_image); } update_rect(dim_.exterior); if(top_left_ == NULL || bot_left_ == NULL || top_right_ == NULL || bot_right_ == NULL) { return; } video_.blit_surface(dim_.interior.x - left_->w, dim_.interior.y - top_->h, top_left_); video_.blit_surface(dim_.interior.x - left_->w, dim_.interior.y + dim_.interior.h + bot_->h - bot_left_->h, bot_left_); video_.blit_surface(dim_.interior.x + dim_.interior.w + right_->w - top_right_->w, dim_.interior.y - top_->h, top_right_); video_.blit_surface(dim_.interior.x + dim_.interior.w + right_->w - bot_right_->w, dim_.interior.y + dim_.interior.h + bot_->h - bot_right_->h, bot_right_); }
int main(int argc, char **argv) { // Setup signal handlers setup_sig_handler(); // Initialize CV images int width = 480; int height = 640; cv::Mat left_image(height, width, CV_8UC3); cv::Mat right_image(height, width, CV_8UC3); cv::Mat disp_image(height, width, CV_16SC3); // Create VidereCamera object vc = new VidereCamera(); // Setup ROS structures ros::init(argc, argv, "videre_camera_node", ros::init_options::AnonymousName); ros::NodeHandle vc_nh; // image_transport deals with image topics image_transport::ImageTransport it(vc_nh); image_transport::Publisher pub_left = it.advertise("videre_camera/left/image_raw", 1); image_transport::Publisher pub_right = it.advertise("videre_camera/right/image_raw", 1); // cv_bridge deals with conversion between cv::Mat and sensor_msgs::Image cv_bridge::CvImage msg_left; cv_bridge::CvImage msg_right; // regular publishers for camera info topics ros::Publisher pub_info_left = vc_nh.advertise<sensor_msgs::CameraInfo>("videre_camera/left/camera_info", 1); ros::Publisher pub_info_right = vc_nh.advertise<sensor_msgs::CameraInfo>("videre_camera/right/camera_info", 1); // camera info messages sensor_msgs::CameraInfo info_left; sensor_msgs::CameraInfo info_right; // We can set encoding prior to loop because it doesn't change msg_left.encoding = sensor_msgs::image_encodings::BGR8; msg_right.encoding = sensor_msgs::image_encodings::BGR8; // We can set camera info params prior to loop because they don't change info_left.height = vc->cp()->height(); info_left.width = vc->cp()->width(); info_left.distortion_model = vc->cp()->distortion_model(); vc->cp()->left_D(info_left.D); vc->cp()->left_K(info_left.K); vc->cp()->left_R(info_left.R); vc->cp()->left_P(info_left.P); info_right.height = vc->cp()->height(); info_right.width = vc->cp()->width(); info_right.distortion_model = vc->cp()->distortion_model(); vc->cp()->right_D(info_right.D); vc->cp()->right_K(info_right.K); vc->cp()->right_R(info_right.R); vc->cp()->right_P(info_right.P); // Set frame_ids msg_left.header.frame_id = "/videre_camera/left"; msg_right.header.frame_id = "/videre_camera/left"; info_left.header.frame_id = "/videre_camera/left"; info_right.header.frame_id = "/videre_camera/left"; // TO DO: USE CAMERAINFO_MANAGER ros::Rate loop_rate(15); while(ros::ok()) { ros::Time ts = ros::Time::now(); bool got_image = vc->GetData(); left_image = vc->left(); right_image = vc->right(); if(!got_image) { ROS_INFO("Error in camera_getimagepair, exiting program"); break; } // Set image timestamps msg_left.header.stamp = ts; msg_right.header.stamp = ts; // Set image data msg_left.image = left_image; msg_right.image = right_image; // Publish images pub_left.publish(msg_left.toImageMsg()); pub_right.publish(msg_right.toImageMsg()); // Set camera info timestamps info_left.header.stamp = ts; info_right.header.stamp = ts; // Publish camera info pub_info_left.publish(info_left); pub_info_right.publish(info_right); ros::spinOnce(); loop_rate.sleep(); } return 0; }
void dialog_frame::draw_border() { if(have_border_ == false) { return; } #ifdef SDL_GPU top_.set_hscale(float(dim_.interior.w )/ top_.base_width()); video_.draw_texture(top_, dim_.interior.x, dim_.exterior.y); bot_.set_hscale(float(dim_.interior.w) / bot_.base_width()); video_.draw_texture(bot_, dim_.interior.x, dim_.interior.y + dim_.interior.h); left_.set_vscale(float(dim_.interior.h) / left_.base_height()); video_.draw_texture(left_, dim_.exterior.x, dim_.interior.y); right_.set_vscale(float(dim_.interior.h) / right_.base_height()); video_.draw_texture(right_, dim_.interior.x + dim_.interior.w, dim_.interior.y); if(top_left_.null() || bot_left_.null() || top_right_.null() || bot_right_.null()) { return; } video_.draw_texture(top_left_, dim_.interior.x - left_.width(), dim_.interior.y - top_.height()); video_.draw_texture(bot_left_, dim_.interior.x - left_.width(), dim_.interior.y + dim_.interior.h + bot_.height() - bot_left_.height()); video_.draw_texture(top_right_, dim_.interior.x + dim_.interior.w + right_.width() - top_right_.width(), dim_.interior.y - top_.height()); video_.draw_texture(bot_right_, dim_.interior.x + dim_.interior.w + right_.width() - bot_right_.width(), dim_.interior.y + dim_.interior.h + bot_.height() - bot_right_.height()); #else surface top_image(scale_surface(top_, dim_.interior.w, top_->h)); if(top_image != NULL) { video_.blit_surface(dim_.interior.x, dim_.exterior.y, top_image); } surface bot_image(scale_surface(bot_, dim_.interior.w, bot_->h)); if(bot_image != NULL) { video_.blit_surface(dim_.interior.x, dim_.interior.y + dim_.interior.h, bot_image); } surface left_image(scale_surface(left_, left_->w, dim_.interior.h)); if(left_image != NULL) { video_.blit_surface(dim_.exterior.x, dim_.interior.y, left_image); } surface right_image(scale_surface(right_, right_->w, dim_.interior.h)); if(right_image != NULL) { video_.blit_surface(dim_.interior.x + dim_.interior.w, dim_.interior.y, right_image); } update_rect(dim_.exterior); if(top_left_ == NULL || bot_left_ == NULL || top_right_ == NULL || bot_right_ == NULL) { return; } video_.blit_surface(dim_.interior.x - left_->w, dim_.interior.y - top_->h, top_left_); video_.blit_surface(dim_.interior.x - left_->w, dim_.interior.y + dim_.interior.h + bot_->h - bot_left_->h, bot_left_); video_.blit_surface(dim_.interior.x + dim_.interior.w + right_->w - top_right_->w, dim_.interior.y - top_->h, top_right_); video_.blit_surface(dim_.interior.x + dim_.interior.w + right_->w - bot_right_->w, dim_.interior.y + dim_.interior.h + bot_->h - bot_right_->h, bot_right_); #endif }