00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <ros/node.h>
00036 #include <checkerboard_detector/ObjectDetection.h>
00037
00038 #include <tf/tf.h>
00039 #include <tf/transform_datatypes.h>
00040 #include <tf/transform_listener.h>
00041
00042 #include <door_msgs/Door.h>
00043 #include <std_msgs/String.h>
00044 #include <door_handle_detector/DoorDetector.h>
00045
00046 #define MIN_DIST_THRESHOLD 0.1
00047 #define MAX_COMPARE_TIME 20
00048
00049 using namespace checkerboard_detector;
00050 using namespace tf;
00051
00052 class TestDoorDetectionNode : public ros::Node
00053 {
00054 public:
00055 std::string frame_id_, joy_topic_;
00056 std_msgs::String joy_msg_;
00057 double door_width_;
00058 tf::TransformListener tf_;
00060 door_msgs::Door door_msg_from_checkerboard_;
00061 door_msgs::Door door_msg_from_detector_;
00062 door_msgs::Door door_msg_to_detector_;
00063
00064 TestDoorDetectionNode(std::string node_name):ros::Node(node_name),tf_(*this, false, ros::Duration(10))
00065 {
00066 this->param<std::string>("test_door_detection_node/joy_topic",joy_topic_,"annotation_msg");
00067 this->param<std::string>("test_door_detection_node/frame_id",frame_id_,"base_footprint");
00068
00069 double tmp; int tmp2;
00070 param("~/door_frame_p1_x", tmp, 1.5); door_msg_to_detector_.frame_p1.x = tmp;
00071 param("~/door_frame_p1_y", tmp, -0.5);door_msg_to_detector_.frame_p1.y = tmp;
00072 param("~/door_frame_p2_x", tmp, 1.5); door_msg_to_detector_.frame_p2.x = tmp;
00073 param("~/door_frame_p2_y", tmp, 0.5); door_msg_to_detector_.frame_p2.y = tmp;
00074 param("~/door_hinge" , tmp2, -1); door_msg_to_detector_.hinge = tmp2;
00075 param("~/door_rot_dir" , tmp2, -1); door_msg_to_detector_.rot_dir = tmp2;
00076 door_msg_to_detector_.header.frame_id = frame_id_;
00077
00078 subscribe(joy_topic_, joy_msg_, &TestDoorDetectionNode::joyCallback,1);
00079 }
00080
00081 ~TestDoorDetectionNode()
00082 {
00083 unsubscribe(joy_topic_);
00084 }
00085
00086 void transformTFPoint(const std::string &frame, const Stamped<tf::Point> &in, Stamped<tf::Point> &out)
00087 {
00088 try{
00089 tf_.transformPoint(frame,in,out);
00090 }
00091 catch(tf::LookupException& ex) {
00092 ROS_INFO("No Transform available Error\n");
00093 return;
00094 }
00095 catch(tf::ConnectivityException& ex) {
00096 ROS_INFO("Connectivity Error\n");
00097 return;
00098 }
00099 catch(tf::ExtrapolationException& ex) {
00100 ROS_INFO("Extrapolation Error %s\n", ex.what());
00101 return;
00102 }
00103 catch(tf::TransformException e) {
00104 return;
00105 }
00106 }
00107
00108 void pointTFToMsg32(const tf::Vector3 &in, geometry_msgs::Point32 &out)
00109 {
00110 out.x = in.x();
00111 out.y = in.y();
00112 out.z = in.z();
00113 }
00114
00115 void joyCallback()
00116 {
00117 std::string status_string;
00118 ROS_INFO("Joystick message: %s",joy_msg_.data.c_str());
00119 if (joy_msg_.data != std::string("detect_door"))
00120 {
00121 return;
00122 }
00123 if(detectDoor(door_msg_to_detector_, door_msg_from_detector_))
00124 {
00125 ROS_INFO("Door detected");
00126 if(detectDoorCheckerboard(door_msg_from_checkerboard_,door_msg_from_checkerboard_))
00127 {
00128 if(compareDoorMsgs(door_msg_from_detector_,door_msg_from_checkerboard_,status_string))
00129 {
00130
00131 }
00132 else
00133 {
00134 ROS_INFO("Failed to match door messages from laser and checkerboard");
00135 }
00136 }
00137 else
00138 {
00139 ROS_WARN("Could not see checkerboard");
00140 }
00141 }
00142 else
00143 {
00144 ROS_ERROR("No door detected");
00145 }
00146 return;
00147 }
00148
00149 double distancePoints(geometry_msgs::Point32 p1, geometry_msgs::Point32 p2)
00150 {
00151 return(sqrt(pow((p1.x-p2.x),2) + pow((p1.y-p2.y),2) + pow((p1.z-p2.z),2)));
00152 }
00153 double distancePointsXY(geometry_msgs::Point32 p1, geometry_msgs::Point32 p2)
00154 {
00155 return(sqrt(pow((p1.x-p2.x),2) + pow((p1.y-p2.y),2)));
00156 }
00157
00158 bool compareDoorMsgs(door_msgs::Door msg_1, door_msgs::Door msg_2, std::string &print_string)
00159 {
00160 if (fabs(msg_1.header.stamp.toSec() - msg_2.header.stamp.toSec()) > MAX_COMPARE_TIME)
00161 {
00162 ROS_ERROR("Comparison invalid since time stamps for the two door messages are %f seconds apart which is greater than max allowable difference %f", fabs(msg_1.header.stamp.toSec() - msg_2.header.stamp.toSec()), (double) MAX_COMPARE_TIME);
00163 return false;
00164 }
00165
00166 if(msg_1.header.frame_id != msg_2.header.frame_id)
00167 {
00168 ROS_ERROR("Comparison invalid since the two door messages do not have the same frame id");
00169 return false;
00170 }
00171 bool door_result(false), handle_result(false);
00172 if(distancePoints(msg_1.door_p1, msg_2.door_p1) < MIN_DIST_THRESHOLD)
00173 {
00174 print_string += "door_p1 from the detector matches door_p1 from ground truth\n";
00175 if(distancePoints(msg_1.door_p2, msg_2.door_p2) < MIN_DIST_THRESHOLD)
00176 {
00177 door_result = true;
00178 print_string += "door_p2 from the detector matches door_p2 from ground truth\n";
00179 }
00180 else
00181 {
00182 print_string += "door_p2 from the detector does not match door_p2 from ground truth\n";
00183 }
00184 }
00185 else if(distancePointsXY(msg_1.door_p2, msg_2.door_p1) < MIN_DIST_THRESHOLD)
00186 {
00187 print_string += "door_p2 from the detector matches door_p1 from ground truth\n";
00188 if(distancePointsXY(msg_1.door_p1, msg_2.door_p2) < MIN_DIST_THRESHOLD)
00189 {
00190 door_result = true;
00191 print_string += "door_p1 from the detector matches door_p2 from ground truth\n";
00192 }
00193 else
00194 {
00195 print_string += "door_p1 from the detector does not match door_p2 from ground truth\n";
00196 }
00197 }
00198 else
00199 {
00200 print_string += "No match found between detected door and ground truth door position\n";
00201 }
00202
00203 if(distancePoints(msg_1.handle, msg_2.handle) < MIN_DIST_THRESHOLD)
00204 {
00205 handle_result = true;
00206 print_string += "handle location from the detector matches handle location from ground truth\n";
00207 }
00208 else
00209 print_string += "handle location from the detector does not match handle location from ground truth\n";
00210 ROS_INFO("%s",print_string.c_str());
00211 return (door_result);
00212 }
00213
00214
00215 bool detectDoor(const door_msgs::Door& door_estimate, door_msgs::Door& door_detection)
00216 {
00217 door_handle_detector::DoorDetector::Request req;
00218 door_handle_detector::DoorDetector::Response res;
00219 req.door = door_estimate;
00220 if (ros::service::call("door_handle_detector", req, res)){
00221 door_detection = res.door;
00222 return true;
00223 }
00224 else
00225 return false;
00226 }
00227
00228 bool detectDoorCheckerboard(const door_msgs::Door& door_estimate, door_msgs::Door& door_detection)
00229 {
00230 door_handle_detector::DoorDetector::Request req;
00231 door_handle_detector::DoorDetector::Response res;
00232 req.door = door_estimate;
00233 if (ros::service::call("door_handle_checkerboard_detector", req, res)){
00234 door_detection = res.door;
00235 return true;
00236 }
00237 else
00238 return false;
00239 }
00240
00241 };
00242
00243 int main(int argc, char** argv)
00244 {
00245 ros::init(argc,argv);
00246
00247 TestDoorDetectionNode node("test_detection_node");
00248
00249 try {
00250 node.spin();
00251 }
00252 catch(char const* e){
00253 std::cout << e << std::endl;
00254 }
00255
00256 return(0);
00257 }