int main(int argc, char** argv)
{
ros::init(argc, argv, "image_publisher");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
image_transport::Publisher pub_L = it.advertise("camera_left/image", 1);
image_transport::Publisher pub_R = it.advertise("camera_right/image", 1);
int serial_L=14481155;
int serial_R=14435551;
// Get the location of our camera config yaml
std::string camera_info_url ;
// std::string camera_info_url = "/home/sasha/hydro_ws/src/alvar_ptgrey/info/ptgrey_calib.yaml";
std::string frame_id_;
sensor_msgs::CameraInfoPtr ci_;
boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_L;
boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_R;
nh.param<std::string>("camera_info_url", camera_info_url, "");
// Get the desired frame_id, set to 'camera' if not found
// nh.param<std::string>("frame_id", frame_id_, "camera");
std::cout << camera_info_url << std::endl;
// Start the camera info manager and attempt to load any configurations
std::stringstream cinfo_name_L;
std::stringstream cinfo_name_R;
cinfo_name_L << serial_L;
cinfo_name_R << serial_R;
ros::Publisher camera_info_pub_L = nh.advertise<sensor_msgs::CameraInfo>("/camera_left/camera_info",1);
ros::Publisher camera_info_pub_R = nh.advertise<sensor_msgs::CameraInfo>("/camera_right/camera_info",1);
// sensor_msgs::CameraInfo cinfo;
cinfo_L.reset(new camera_info_manager::CameraInfoManager(nh, cinfo_name_L.str(), camera_info_url));
cinfo_R.reset(new camera_info_manager::CameraInfoManager(nh, cinfo_name_R.str(), camera_info_url));
BusManager busMgr;
// unsigned int numCameras = 2;
// error = busMgr.GetNumOfCameras(&numCameras);
// if (error != PGRERROR_OK) {
// std::cout << "Bus Manager error" << std::endl;
// return false;
// }
PGRGuid guid_L;
// error = busMgr.GetCameraFromIndex(1, &guid_L);
error = busMgr.GetCameraFromSerialNumber(14481155, &guid_L);
if (error != PGRERROR_OK) {
std::cout << "Index error" << std::endl;
return false;
}
PGRGuid guid_R;
// error = busMgr.GetCameraFromIndex(2, &guid_L);
error = busMgr.GetCameraFromSerialNumber(14435551, &guid_R);
if (error != PGRERROR_OK) {
std::cout << "Index error" << std::endl;
return false;
}
Camera pt_grey_L, pt_grey_R;
cv::Mat image_L, image_R;
// connect to camera
// init_cam_capture(pt_grey, serialNum);
init_cam_capture(pt_grey_L, guid_L);
init_cam_capture(pt_grey_R, guid_R);
// set camera parameters
// set_cam_param(pt_grey);
int frame_no = 0;
// capture loop
while(ros::ok())
{
// Get the image
Image rawImage;
error = pt_grey_L.RetrieveBuffer( &rawImage );
if ( error != PGRERROR_OK )
{
std::cout << "capture error" << std::endl;
return false;
}
// convert to bgr (default format for OpenCV)
Image rgbImage;
rawImage.Convert( FlyCapture2::PIXEL_FORMAT_BGR, &rgbImage );
// convert to OpenCV Mat
unsigned int rowBytes = (double)rgbImage.GetReceivedDataSize()/(double)rgbImage.GetRows();
cv::Mat image = cv::Mat(rgbImage.GetRows(), rgbImage.GetCols(), CV_8UC3, rgbImage.GetData(),rowBytes);
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
msg->header.frame_id = "/camera_left";
msg->header.stamp = ros::Time::now();
pub_L.publish(msg);
// sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
ci_.reset(new sensor_msgs::CameraInfo(cinfo_L->getCameraInfo()));
ci_->header.stamp = msg->header.stamp;
ci_->header.frame_id = msg->header.frame_id ;
camera_info_pub_L.publish(ci_);
ros::spinOnce();
// Get the image
error = pt_grey_R.RetrieveBuffer( &rawImage );
if ( error != PGRERROR_OK )
{
std::cout << "capture error" << std::endl;
return false;
}
// convert to bgr (default format for OpenCV)
rawImage.Convert( FlyCapture2::PIXEL_FORMAT_BGR, &rgbImage );
// convert to OpenCV Mat
rowBytes = (double)rgbImage.GetReceivedDataSize()/(double)rgbImage.GetRows();
image = cv::Mat(rgbImage.GetRows(), rgbImage.GetCols(), CV_8UC3, rgbImage.GetData(),rowBytes);
msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
msg->header.frame_id = "/camera_right";
msg->header.stamp = ros::Time::now();
pub_R.publish(msg);
ci_.reset(new sensor_msgs::CameraInfo(cinfo_R->getCameraInfo()));
ci_->header.stamp = msg->header.stamp;
ci_->header.frame_id = msg->header.frame_id ;
camera_info_pub_R.publish(ci_);
ros::spinOnce();
}
error = pt_grey_L.StopCapture();
if (error != PGRERROR_OK) {
// PrintError( error );
return false;
}
error = pt_grey_R.StopCapture();
if (error != PGRERROR_OK) {
// PrintError( error );
return false;
}
pt_grey_L.Disconnect();
pt_grey_R.Disconnect();
}
int main()
{
BusManager busMgr;
unsigned int numCameras = 2;
error = busMgr.GetNumOfCameras(&numCameras);
if (error != PGRERROR_OK) {
std::cout << "Bus Manager error" << std::endl;
return false;
}
PGRGuid guid_L;
// error = busMgr.GetCameraFromIndex(1, &guid_L);
error = busMgr.GetCameraFromSerialNumber(14481155, &guid_L);
if (error != PGRERROR_OK) {
std::cout << "Index error" << std::endl;
return false;
}
PGRGuid guid_R;
// error = busMgr.GetCameraFromIndex(2, &guid_L);
error = busMgr.GetCameraFromSerialNumber(14435551, &guid_R);
if (error != PGRERROR_OK) {
std::cout << "Index error" << std::endl;
return false;
}
Camera pt_grey_L, pt_grey_R;
cv::Mat image_L, image_R;
cv::namedWindow("image_L", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO);
cvMoveWindow("image_L", 3000, 10);
cvResizeWindow("image_L",700, 500);
cv::namedWindow("image_R", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO);
cvMoveWindow("image_R", 3000, 10);
cvResizeWindow("image_R",700, 500);
// connect to camera
// init_cam_capture(pt_grey, serialNum);
init_cam_capture(pt_grey_L, guid_L);
init_cam_capture(pt_grey_R, guid_R);
// set camera parameters
// set_cam_param(pt_grey);
int frame_no = 0;
// capture loop
char key = 0;
while(key != 'q')
{
// Get the image
Image rawImage;
error = pt_grey_L.RetrieveBuffer( &rawImage );
if ( error != PGRERROR_OK )
{
std::cout << "capture error" << std::endl;
return false;
}
// convert to bgr (default format for OpenCV)
Image rgbImage;
rawImage.Convert( FlyCapture2::PIXEL_FORMAT_BGR, &rgbImage );
// convert to OpenCV Mat
unsigned int rowBytes = (double)rgbImage.GetReceivedDataSize()/(double)rgbImage.GetRows();
cv::Mat image = cv::Mat(rgbImage.GetRows(), rgbImage.GetCols(), CV_8UC3, rgbImage.GetData(),rowBytes);
cv::imshow("image_L", image);
key = cv::waitKey(1);
// Get the image
error = pt_grey_R.RetrieveBuffer( &rawImage );
if ( error != PGRERROR_OK )
{
std::cout << "capture error" << std::endl;
return false;
}
// convert to bgr (default format for OpenCV)
rawImage.Convert( FlyCapture2::PIXEL_FORMAT_BGR, &rgbImage );
// convert to OpenCV Mat
rowBytes = (double)rgbImage.GetReceivedDataSize()/(double)rgbImage.GetRows();
image = cv::Mat(rgbImage.GetRows(), rgbImage.GetCols(), CV_8UC3, rgbImage.GetData(),rowBytes);
cv::imshow("image_R", image);
key = cv::waitKey(1);
// key = cv::waitKey(50);
// if (key == 'c') {
// char img_name[50];
// sprintf (img_name, "./frame_%03d.png", frame_no);
// // sprintf (img_name, "./images/frame_%03d.png", frame_no);
// // sprintf (img_name, "../cam_calib/images/frame_%03d.png", frame_no);
// // sprintf (img_name, "../homography2d/images/frame_%03d.png", frame_no);
// std::cout << "Writing to " << img_name << std::endl;
// cv::imwrite(img_name, image);
// frame_no++;
// }
}
error = pt_grey_L.StopCapture();
if (error != PGRERROR_OK) {
// PrintError( error );
return false;
}
error = pt_grey_R.StopCapture();
if (error != PGRERROR_OK) {
// PrintError( error );
return false;
}
pt_grey_L.Disconnect();
pt_grey_R.Disconnect();
}