[Personalrobots-commit] SF.net SVN: personalrobots:[19989] pkg/trunk/stacks

[<mar...@us...>]

Revision: 19989
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19989&view=rev
Author:   mariusmuja
Date:     2009-07-29 08:33:01 +0000 (Wed, 29 Jul 2009)

Log Message:
-----------
Changes to outlet_spotting and handle_detection

Modified Paths:
--------------
    pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp
    pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp

Modified: pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp	2009-07-29 07:47:56 UTC (rev 19988)
+++ pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp	2009-07-29 08:33:01 UTC (rev 19989)
@@ -163,7 +163,7 @@
 	bool got_images_;
 
 
-	double image_timeout_;
+	double timeout_;
 	ros::Time start_image_wait_;
 
 
@@ -182,7 +182,7 @@
     	nh_.param("~min_height", min_height_, 0.8);
     	nh_.param("~max_height", max_height_, 1.2);
     	nh_.param("~frames_no", frames_no_, 10);
-    	nh_.param("~timeout", image_timeout_, 3.0);		// timeout (in seconds) until an image must be received, otherwise abort
+    	nh_.param("~timeout", timeout_, 3.0);		// timeout (in seconds) until an image must be received, otherwise abort
     	nh_.param("~display", display_, false);
         stringstream ss;
         ss << getenv("ROS_ROOT") << "/../ros-pkg/mapping/door_handle_detector/data/";
@@ -836,7 +836,10 @@
         	while (!got_images_ && !preempted_) {
         		data_cv_.wait(lock);
         	}
-        	if (preempted_) break;
+        	if (preempted_) {
+        		ROS_INFO("OutletSpotter: detect loop preempted, stereo data not received");
+        		return false;
+        	}
 
         	if(display_){
         		// show original disparity
@@ -1007,6 +1010,16 @@
 		ros::Rate r(10);
 		while (nh_.ok())
 		{
+			data_lock_.lock();
+			if (!(have_images_ && have_cloud_point_)) {
+				if ((ros::Time::now()-start_image_wait_) > ros::Duration(timeout_)) {
+					preempted_ = true;
+					data_cv_.notify_all();
+				}
+			}
+			data_lock_.unlock();
+
+
 			if (display_) {
 				int key = cvWaitKey(10)&0x00FF;
 				if(key == 27) //ESC

Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp	2009-07-29 07:47:56 UTC (rev 19988)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp	2009-07-29 08:33:01 UTC (rev 19989)
@@ -1457,7 +1457,7 @@
 				data_cv_.wait(images_lock);
 			}
 			if (preempt_) {
-				ROS_INFO("OutletSpotter: detect loop preempted");
+				ROS_INFO("OutletSpotter: detect loop preempted, stereo or base laser not received");
 				return false;
 			}
 			ROS_INFO("OutletSpotter: received images and base laser data, performing detection");
@@ -1497,7 +1497,7 @@
 		while (nh_.ok())
 		{
 			data_lock_.lock();
-			if (!have_images_ && !have_cloud_point_) {
+			if (!(have_images_ && have_cloud_point_)) {
 				if ((ros::Time::now()-start_image_wait_) > ros::Duration(timeout_)) {
 					preempt_ = true;
 					data_cv_.notify_all();


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.