-
-
Save ngoluuduythai/a406ade2cbc7ece87813 to your computer and use it in GitHub Desktop.
Elderly Fall Detection Using Web Camera and Mobile Robot
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment