int main(int argc,char *argv[])
{
std::cout << "Initializing video capture device... " << std::endl;
// start camera
capture = cvCaptureFromCAM(0);
if(!capture){
std::cerr << "No camera detected!!!" << std::endl;
return -1;
}
std::cout << "Done!" << std::endl;
// Construct a signal set registered for process termination.
boost::asio::io_service io;
boost::asio::signal_set ctrlC(io, SIGINT, SIGTERM);
// Start an asynchronous wait for one of the signals to occur.
ctrlC.async_wait(ctrlC_handler);
// io.run();
ros::init(argc, argv, "camera");
ros::NodeHandle nodeHandle;
image_transport::ImageTransport it(nodeHandle);
image_transport::Publisher pub;
pub = it.advertise(CAMERA_IMG, 1);
cvNamedWindow("src", 1);
// cvStartWindowThread();
std::cout << std::endl << "Publishing video in " << CAMERA_IMG << std::endl;
while(true){
IplImage *frame = cvQueryFrame(capture);
if( !frame ){
std::cerr << "Can't read from capture device." << std::endl;
break;
}
cvShowImage("src", frame);
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(
std_msgs::Header(),
"bgr8",
frame).toImageMsg();
pub.publish(msg);
io.poll();
ros::spinOnce();
char key = cvWaitKey( 10 );
if( key >= 27 )
break;
}
std::cout << "Stopping capture device... ";
cvDestroyAllWindows();
cvReleaseCapture(&capture);
std::cout << "Stopped." << std::endl;
return 0;
}
int sock_init(struct hostent *server, int portno){
int netfd = socket(AF_INET, SOCK_STREAM, 0);
/* open socket and connect to server */
if (netfd < 0)
printf("ERROR opening socket");
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
bzero((char *) &serv_addr, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
bcopy((char *)server->h_addr,
(char *)&serv_addr.sin_addr.s_addr,
server->h_length);
serv_addr.sin_port = htons(portno);
if (connect(netfd,(struct sockaddr *) &serv_addr,sizeof(serv_addr)) < 0) {
printf("ERROR connecting to server\n");
ctrlC(0);
exit(-1);
}
return netfd;
}
void check_char(char* feed)
{
int i;
char* output;
switch (feed[0])
{
case 1:
// my_str("Ctrl+A pressed.\n");
ctrlA();
break;
case 3:
// my_str("Ctrl+C pressed.\n");
ctrlC();
break;
case 5:
// my_str("Ctrl+E pressed.\n");
ctrlE();
break;
case 11:
// my_str("Ctrl+K pressed.\n");
ctrlK();
break;
case 12:
// my_str("Ctrl+L pressed.\n");
ctrlL();
break;
case 25:
// my_str("Ctrl+Y pressed.\n");
ctrlY();
break;
case 27:
switch (feed[2])
{
case 'B':
// my_str("Down arrow pressed.\n");
down();
break;
case 'A':
// my_str("Up arrow pressed.\n");
up();
break;
case 'D':
// my_str("Left arrow pressed.\n");
left();
break;
case 'C':
// my_str("Right arrow pressed.\n");
right();
break;
} break;
case 8:
// my_str("Backspace pressed.\n");
backspace();
break;
case 10:
// my_str("Enter pressed.\n");
enter();
break;
case 127:
// my_str("Delete pressed.\n");
backspace();
break;
default:
my_char(feed[0]);
gl_env.cmd[gl_env.cmdlength++] = feed[0];
gl_env.x++;
break;
}
}
int capture_and_send_image(int fd, int sockfd)
{
enum v4l2_buf_type type;
char camActive = 0;
char rec;
struct v4l2_buffer buf = {0};
int n,i;
char *bufferptr;
int framesize;
long long start, end;
for (i = 0; i < n_buffers; ++i) {
struct v4l2_buffer buf;
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
printf("VIDIOC_QBUF");
}
type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (-1 == xioctl(fd, VIDIOC_STREAMON, &type))
printf("VIDIOC_STREAMON");
while(1){
fd_set fds;
struct timeval tv;
int r;
FD_ZERO(&fds);
FD_SET(fd, &fds);
tv.tv_sec = 2;
tv.tv_usec = 0;
r = select(fd+1, &fds, NULL, NULL, &tv);
if(-1 == r)
{
perror("Waiting for Frame");
return 1;
}
CLEAR(buf);
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
if(-1 == xioctl(fd, VIDIOC_DQBUF, &buf))
{
switch (errno) {
case EAGAIN:
continue;
case EIO:
/* Could ignore EIO, see spec. */
/* fall through */
default:
perror("VIDIOC_DQBUF");
}
}
assert(buf.index < n_buffers);
n = write(sockfd,(char*)&buf.bytesused,4);
if (n < 0) {
printf("ERROR writing to socket");
ctrlC(0);
return -1;
}
bufferptr = buffers[buf.index].start;
framesize = buf.bytesused;
while( (n = write(sockfd,bufferptr,framesize)) != framesize){
if (n < 0){
printf("ERROR writing to socket");
ctrlC(0);
return -1;
}
bufferptr += n;
framesize -= n;
}
n = read(sockfd,&rec,1);
if (-1 == xioctl(fd, VIDIOC_QBUF, &buf))
printf("VIDIOC_QBUF");
}
return 0;
}
