Elderly Fall Detection Using Web Camera and Mobile Robot

robot.cpp

#include <stdio.h>
#include <opencv/highgui.h>
#include <opencv/cv.h>
#include <opencv/cxcore.h>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/String.h>

IplImage* imgTracking;

int posX = 0;
int posY = 0;
int area01 = 1000;

IplImage* GetThresholdedImage(IplImage* imgHSV) {
	IplImage* imgThresh=cvCreateImage(cvGetSize(imgHSV),IPL_DEPTH_8U, 1);
 	cvInRangeS(imgHSV, cvScalar(165,160,60), cvScalar(185,2556,256), imgThresh);
	return imgThresh;
}

void trackObject(IplImage* imgThresh) {
  	CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
	cvMoments(imgThresh, moments, 1);
	double moment10 = cvGetSpatialMoment(moments, 1, 0);
	double moment01 = cvGetSpatialMoment(moments, 0, 1);
	double area = cvGetCentralMoment(moments, 0, 0);

	if(area>500) {
		posX = moment10/area;   //x<300,turn left,x>300, turn right
		posY = moment01/area;
		area01 = area;
		printf("x = %d,y = %d,area = %d \n",posX,posY,area01);
	}

	free(moments);
}


int main(int argc, char** argv) {
	ros::init(argc, argv, "robotrun");
	ros::NodeHandle nh;
	ros::Publisher command_publisher;
	ros::Rate loop_rate(10);
	command_publisher = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);

  	while(1) {
		if (nh.hasParam("private_int")) {
			break;
		} 
  	}

	CvCapture* capture =0;
  	capture = cvCaptureFromCAM(0);
  	if(!capture) {
		printf("Capture failure\n");
    		return -1;
  	}

	IplImage* frame=0;
	frame = cvQueryFrame(capture);
	if(!frame) {
		return -1;
	}

	imgTracking=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U, 3);
	cvZero(imgTracking);

  	cvNamedWindow("Video");
  	cvMoveWindow("video", 800, 0);
  	cvNamedWindow("Ball");
  	cvMoveWindow("Ball", 200, 0);

  	while(true) {
		frame = cvQueryFrame(capture);
		if(!frame) {
			break;
		}
    		frame=cvCloneImage(frame);

    		cvSmooth(frame, frame, CV_GAUSSIAN,3,3);
    		IplImage* imgHSV = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3);
    		cvCvtColor(frame, imgHSV, CV_BGR2HSV);
    		IplImage* imgThresh = GetThresholdedImage(imgHSV)
		cvSmooth(imgThresh, imgThresh, CV_GAUSSIAN,3,3);

    		trackObject(imgThresh); //track!!

    		if(posX>380 && area01<40000) {
			geometry_msgs::Twist twist_msg;
      			twist_msg.linear.x = 0;
      			twist_msg.linear.y = 0;
      			twist_msg.linear.z = 0;
      			twist_msg.angular.x = 0;
      			twist_msg.angular.y = 0;
      			twist_msg.angular.z = -0.1;
      			command_publisher.publish(twist_msg);
      			printf("right turn\n");
		}
		else if(posX<=220 && area01<40000) {
			geometry_msgs::Twist twist_msg;
      			twist_msg.linear.x = 0;
      			twist_msg.linear.y = 0;
      			twist_msg.linear.z = 0;
      			twist_msg.angular.x = 0;
      			twist_msg.angular.y = 0;
      			twist_msg.angular.z = 0.1;
      			command_publisher.publish(twist_msg);
      			printf("left turn\n");
		}
		else if(posX>220 && posX<380 && area01<40000) {
			geometry_msgs::Twist twist_msg;
      			twist_msg.linear.x = 0.2;
      			twist_msg.linear.y = 0;
      			twist_msg.linear.z = 0;
      			twist_msg.angular.x = 0;
      			twist_msg.angular.y = 0;
      			twist_msg.angular.z = 0;
      			command_publisher.publish(twist_msg);
      			printf("go forward\n");
		}
		else if(area01>40000) {
			geometry_msgs::Twist twist_msg;
      			twist_msg.linear.x = 0;
      			twist_msg.linear.y = 0;
      			twist_msg.linear.z = 0;
      			twist_msg.angular.x = 0;
      			twist_msg.angular.y = 0;
      			twist_msg.angular.z = 0;
      			command_publisher.publish(twist_msg);
      			printf("stop\n");  //  break;
		}

		cvShowImage("Ball", imgThresh);
		cvShowImage("Video", frame);
		cvReleaseImage(&imgHSV);
		cvReleaseImage(&imgThresh);
		cvReleaseImage(&frame);
		int c = cvWaitKey(10);
		if((char)c==27 ) {
			break;
		}
	}
	cvDestroyAllWindows() ;
  	cvReleaseImage(&imgTracking);
  	cvReleaseCapture(&capture);
  	return 0;
}