/** 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
}
