cob_people_detection: cob_people_detection: people_detection_action

00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_vision
00012  * ROS package name: cob_people_detection
00013  * Description:
00014  *
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *
00017  * Author: Richard Bormann, email:richard.bormann@ipa.fhg.de
00018  * Supervised by:
00019  *
00020  * Date of creation: 03/2011
00021  * ToDo:
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Fraunhofer Institute for Manufacturing
00034  *       Engineering and Automation (IPA) nor the names of its
00035  *       contributors may be used to endorse or promote products derived from
00036  *       this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as
00040  * published by the Free Software Foundation, either version 3 of the
00041  * License, or (at your option) any later version.
00042  *
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  *
00048  * You should have received a copy of the GNU Lesser General Public
00049  * License LGPL along with this program.
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 //##################
00055 //#### includes ####
00056 
00057 // ROS includes
00058 #include <ros/ros.h>
00059 
00060 // Actions
00061 #include <cob_people_detection/RecognizeAction.h>
00062 #include <cob_people_detection/TrainContinuousAction.h>
00063 #include <cob_people_detection/TrainCaptureSampleAction.h>
00064 #include <cob_people_detection/ShowAction.h>
00065 #include <actionlib/client/simple_action_client.h>
00066 
00067 // standard includes
00068 #ifdef __LINUX__
00069         #include <termios.h>
00070 
00071     int getch();
00072 
00073     int getch()
00074     {
00075        static int ch = -1, fd = 0;
00076        struct termios neu, alt;
00077        fd = fileno(stdin);
00078        tcgetattr(fd, &alt);
00079        neu = alt;
00080        neu.c_lflag &= ~(ICANON|ECHO);
00081        tcsetattr(fd, TCSANOW, &neu);
00082        ch = getchar();
00083        tcsetattr(fd, TCSANOW, &alt);
00084        return ch;
00085     }
00086 #endif
00087 
00088 //####################
00089 //#### node class ####
00090 
00091 typedef actionlib::SimpleActionClient<cob_people_detection::RecognizeAction> RecognizeClient;
00092 typedef actionlib::SimpleActionClient<cob_people_detection::TrainContinuousAction> TrainContinuousClient;
00093 typedef actionlib::SimpleActionClient<cob_people_detection::TrainCaptureSampleAction> TrainCaptureSampleClient;
00094 typedef actionlib::SimpleActionClient<cob_people_detection::ShowAction> ShowClient;
00095 
00096 
00097 void train(TrainContinuousClient& trainContinuousClient, TrainCaptureSampleClient& trainCaptureSampleClient)
00098 {
00099         std::string id;
00100         std::cout << "Input the ID of the captured person: ";
00101         std::cin >> id;
00102 
00103         cob_people_detection::TrainContinuousGoal goal;
00104         // Fill in goal here
00105         goal.running = true;
00106         goal.doPCA = true;
00107         goal.appendData = true;
00108         goal.numberOfImagesToCapture = 0;
00109         goal.trainingID = id;
00110         trainContinuousClient.sendGoal(goal);
00111         trainContinuousClient.waitForResult(ros::Duration(3.0));
00112         if (trainContinuousClient.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00113                 printf("Training on!\n");
00114         printf("Current State: %s\n", trainContinuousClient.getState().toString().c_str());
00115 
00116         std::cout << "Hit 'q' key to quit or 'c' key to capture an image.\n";
00117         char key;
00118         while ((key=getch()) != 'q')
00119         {
00120                 if (key == 'c')
00121                 {
00122                         cob_people_detection::TrainCaptureSampleGoal captureGoal;
00123                         trainCaptureSampleClient.sendGoal(captureGoal);
00124                         trainContinuousClient.waitForResult(ros::Duration(3.0));
00125                         if (trainContinuousClient.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00126                                 printf("Image capture initiated.\n");
00127                 }
00128         }
00129 
00130         goal.running = false;
00131         trainContinuousClient.sendGoal(goal);
00132         trainContinuousClient.waitForResult(ros::Duration(10.0));
00133         if (trainContinuousClient.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00134                 printf("Training off!\n");
00135         printf("Current State: %s\n", trainContinuousClient.getState().toString().c_str());
00136 }
00137 
00138 
00139 void train_continuous(TrainContinuousClient& trainContinuousClient, TrainCaptureSampleClient& trainCaptureSampleClient)
00140 {
00141         std::string id;
00142         std::cout << "Input the ID of the captured person: ";
00143         std::cin >> id;
00144 
00145         cob_people_detection::TrainContinuousGoal goal;
00146         // Fill in goal here
00147         goal.running = true;
00148         goal.doPCA = true;
00149         goal.appendData = true;
00150         goal.numberOfImagesToCapture = 30;
00151         goal.trainingID = id;
00152         trainContinuousClient.sendGoal(goal);
00153         trainContinuousClient.waitForResult(ros::Duration(120.0));
00154 
00155 
00156         goal.running = false;
00157         trainContinuousClient.sendGoal(goal);
00158         trainContinuousClient.waitForResult(ros::Duration(10.0));
00159         if (trainContinuousClient.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00160                 printf("Training off!\n");
00161         printf("Current State: %s\n", trainContinuousClient.getState().toString().c_str());
00162 }
00163 
00164 
00165 void recognize(RecognizeClient& recognizeClient)
00166 {
00167         cob_people_detection::RecognizeGoal goal;
00168         // Fill in goal here
00169         goal.running = true;
00170         goal.doRecognition = true;
00171         goal.display = true;
00172         recognizeClient.sendGoal(goal);
00173         recognizeClient.waitForResult(ros::Duration(3.0));
00174         if (recognizeClient.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00175                 printf("Recognition on!\n");
00176         printf("Current State: %s\n", recognizeClient.getState().toString().c_str());
00177 
00178         std::cout << "hit any key to quit.\n";
00179         getch();
00180 
00181         goal.running = false;
00182         recognizeClient.sendGoal(goal);
00183         recognizeClient.waitForResult(ros::Duration(2.0));
00184         if (recognizeClient.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
00185                 printf("Recognition off!\n");
00186         printf("Current State: %s\n", recognizeClient.getState().toString().c_str());
00187 }
00188 
00189 
00190 void show(ShowClient& showClient, int mode)
00191 {
00192         cob_people_detection::ShowGoal goal;
00193         // Fill in goal here
00194         goal.mode = mode;
00195         showClient.sendGoal(goal);
00196         showClient.waitForResult(ros::Duration(3.0));
00197         printf("Current State: %s\n", showClient.getState().toString().c_str());
00198 }
00199 
00200 
00201 int main(int argc, char** argv)
00202 {
00203         ros::init(argc, argv, "cob_people_detection_client");
00204         RecognizeClient recognizeClient("cob_people_detection/face_detection/recognize_server", true); // true -> don't need ros::spin()
00205         TrainContinuousClient trainContinuousClient("cob_people_detection/face_detection/train_continuous_server", true);
00206         TrainCaptureSampleClient trainCaptureSampleClient("cob_people_detection/face_detection/train_capture_sample_server", true);
00207         ShowClient showClient("cob_people_detection/face_detection/show_server", true);
00208         if (!recognizeClient.waitForServer(ros::Duration(2.0)))
00209         {
00210                 std::cout << "No connection to server 'recognize_server'.\n";
00211                 return 0;
00212         }
00213         if (!trainContinuousClient.waitForServer(ros::Duration(2.0)))
00214         {
00215                 std::cout << "No connection to server 'train_continuous_server'.\n";
00216                 return 0;
00217         }
00218         if (!trainCaptureSampleClient.waitForServer(ros::Duration(2.0)))
00219         {
00220                 std::cout << "No connection to server 'train_capture_sample_server'.\n";
00221                 return 0;
00222         }
00223         if (!showClient.waitForServer(ros::Duration(2.0)))
00224         {
00225                 std::cout << "No connection to server 'show_server'.\n";
00226                 return 0;
00227         }
00228 
00229         std::cout << "Connected to servers.\n";
00230 
00231 
00232         char key = 'q';
00233         do
00234         {
00235                 std::cout << "\n\nChoose an option:\n1 - train with manual capture\n2 - recognize\n3 - train continuously (30 images)\n4 - Show average image\n5 - Show Eigenfaces\nq - Quit\n\n";
00236                 key = getch();
00237                 if (key == '1') train(trainContinuousClient, trainCaptureSampleClient);
00238                 else if (key == '2') recognize(recognizeClient);
00239                 else if (key == '3') train_continuous(trainContinuousClient, trainCaptureSampleClient);
00240                 else if (key == '4') show(showClient, 0);
00241                 else if (key == '5') show(showClient, 1);
00242         }while(key != 'q');
00243 
00244 
00245         return 0;
00246 }
00247 
00248
people_detection_action_client.cpp
