00001 00011 #include <iostream> 00012 #include <lfd_common/classification_point.h> 00013 #include <lfd_common/conf_classification.h> 00014 #include <ros/ros.h> 00015 #include <time.h> 00016 #include <vector> 00017 00018 using namespace std; 00019 00020 vector<float*> data; 00029 bool classify_callback(lfd_common::conf_classification::Request &req, lfd_common::conf_classification::Response &resp) 00030 { 00031 // break them into 5 labels 00032 if (req.s.state_vector.at(2) < 5) 00033 resp.l = 5; 00034 else if (req.s.state_vector.at(2) < 10) 00035 resp.l = 10; 00036 else if (req.s.state_vector.at(2) < 15) 00037 resp.l = 15; 00038 else if (req.s.state_vector.at(2) < 20) 00039 resp.l = 20; 00040 else 00041 resp.l = 25; 00042 00043 // choose their decision boundary 00044 if (req.s.state_vector.at(1) < 13) 00045 resp.db = 1; 00046 else 00047 resp.db = 2; 00048 00049 // let confidence be based on the number of points out of the 1562575 possible states 00050 resp.c = ((float)data.size()) / 1562575.0; 00051 00052 // now randomly change the label to create some noise 00053 if ((rand() % 50) == 0) 00054 { 00055 resp.l = ((rand() % 5) + 1) * 5; 00056 resp.c /= 2.0; 00057 } 00058 00059 return true; 00060 } 00061 00066 void add_point_callback(const lfd_common::classification_point::ConstPtr &msg) 00067 { 00068 00069 // check if we have a new point 00070 for (uint i = 0; i < data.size(); i++) 00071 if (data.at(i)[0] == msg->s.state_vector.at(0) && data.at(i)[1] == msg->s.state_vector.at(1) 00072 && data.at(i)[2] == msg->s.state_vector.at(2)) 00073 return; 00074 00075 // new point 00076 float *data_point = (float *)malloc(sizeof(float) * msg->s.state_vector.size()); 00077 for (uint i = 0; i < msg->s.state_vector.size(); i++) 00078 data_point[i] = msg->s.state_vector.at(i); 00079 data.push_back(data_point); 00080 00081 // check if the labels match (-100 is the default correction value) 00082 if (msg->l != -100) { 00083 if (msg->s.state_vector.at(2) < 5) 00084 assert(msg->l == 5); 00085 else if (msg->s.state_vector.at(2) < 10) 00086 assert(msg->l == 10); 00087 else if (msg->s.state_vector.at(2) < 15) 00088 assert(msg->l == 15); 00089 else if (msg->s.state_vector.at(2) < 20) 00090 assert(msg->l == 20); 00091 else 00092 assert(msg->l == 25); 00093 } 00094 } 00095 00100 void change_point_callback(const lfd_common::classification_point::ConstPtr &msg) 00101 { 00102 // check if we have this point 00103 for (uint i = 0; i < data.size(); i++) 00104 if (data.at(i)[0] == msg->s.state_vector.at(0) && data.at(i)[1] == msg->s.state_vector.at(1) 00105 && data.at(i)[2] == msg->s.state_vector.at(2)) 00106 { 00107 cout << "\"Changed\" data point successfully!" << endl; 00108 return; 00109 } 00110 00111 // point never found 00112 cout << "[ERROR]: Data point provided in 'change_point' does not exist!"; 00113 exit(-1); 00114 } 00115 00123 int main(int argc, char **argv) 00124 { 00125 // initialize ROS and the node 00126 ros::init(argc, argv, "mock_classify"); 00127 // a handle for this ROS node 00128 ros::NodeHandle node; 00129 00130 // create services and topics 00131 ros::ServiceServer classify = node.advertiseService("classify", classify_callback); 00132 ros::Subscriber add_point = node.subscribe<lfd_common::classification_point>("add_point", -1, add_point_callback); 00133 ros::Subscriber change_point = node.subscribe<lfd_common::classification_point>("change_point", -1, 00134 change_point_callback); 00135 00136 // used to randomly classify incorrectly 00137 srand(time(NULL)); 00138 00139 ROS_INFO("Mock Classifier Initialized"); 00140 00141 // run the node 00142 ros::spin(); 00143 00144 return EXIT_SUCCESS; 00145 }