fix merges

Author: Frida

contour_filter/src/contour_filter.cpp

@@ -45,13 +45,45 @@ enum Filter_mode
 Filter_mode mode;
 
 //object contour parameters
-//double minArea = 1000.0;
-double minArea = 10.0;
+double minArea = 1000.0;
+//double minArea = 10.0;
 //double maxArea = 100000.0;
-double maxArea = 1000.0;
+double maxArea = 17000.0;
+
+//object rectangle parameter
+//double minRatio = 4.3;
+const double minRatio = 5;
+
+struct ObjectRectangle
+{
+  unsigned int ROI_id;
+  cv::Rect boundRect;
+};
+
+struct DetectedObject : ObjectRectangle
+{
+  std::vector<cv::Point> contours_poly;
+  cv::RotatedRect rotatedRect;
+  cv::Point mc;
+
+//    friend std::ostream & operator<<(std::ostream & stream, const DetectedObject &a)
+//    {
+//    	stream 	<< "id: "<<a.ROI_id<<"\ncontours_poly: "<<a.contours_poly.size()<<"\nboundRect: ["<<a.boundRect.tl()<<", "<<a.boundRect.br()<<"]"
+//    			<<"\nrotatedRect: "<<a.rotatedRect.angle<<"\nmc: "<<a.mc;
+//    	return stream;
+//    }
+
+};
 
 //previous ROI
 cv::Rect prev_rect;
 int ROI_id_counter = 0;
 
+//parameters for the shadow filter
+int SHADOW_FILTER_MASK_SIZE = 7;
+float SHADOW_FILTER_THRESHOLD = 0.02;
+
+//contour translation between depth to color image
+int DEPTH_TO_COLOR_DX = -30;
+int DEPTH_TO_COLOR_DY = 0;
 #endif /* CONTOUR_FILTER_H_ */

contour_filter/src/contour_filter.h

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 2.8.1, 2013-12-04T22:47:17. -->
+<!-- Written by QtCreator 2.8.1, 2013-12-05T18:34:39. -->
 <qtcreator>
  <data>
   <variable>ProjectExplorer.Project.ActiveTarget</variable>
@@ -158,8 +158,8 @@
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeRunConfiguration.EKF</value>
     <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
-    <value type="bool" key="RunConfiguration.UseCppDebugger">true</value>
-    <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">false</value>
+    <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
+    <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
     <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
     <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
     <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>

explorer/CMakeLists.txt.user

@@ -2,7 +2,7 @@
 
   <node name="serial_node" pkg="rosserial_python" type="serial_node.py"/>
   <node pkg="explorer" name="motors" type="motors"/>
-  <node pkg="explorer" name="EKF" type="EKF">
+  <node pkg="explorer" name="EKF" type="EKF" output="screen">
   	<param name="mode" value="1"/>
   </node>
   <node pkg="explorer" name="controller" type="controller" output="screen">

explorer/launch/goto_targets.launch

@@ -0,0 +1,16 @@
+<launch>
+
+  <node name="serial_node" pkg="rosserial_python" type="serial_node.py"/>
+  <node pkg="explorer" name="motors" type="motors"/>
+  <node pkg="explorer" name="EKF" type="EKF">
+  	<param name="mode" value="2"/>
+  </node>
+  <node pkg="explorer" name="controller" type="controller" output="screen">
+  	<param name="mode" value="2"/>
+  </node>	
+  
+  <node pkg="obj_detection" name="recogntion_master" type="recognition_master"/>
+  
+  <node pkg="robot_talk" name="robot_talk" type="robot_talk"/>
+  
+</launch>

explorer/launch/test.launch

@@ -55,21 +55,23 @@ void receive_sensors(const AnalogC::ConstPtr &msg)
 	double s2 = a_short*pow(msg->ch2,b_short); // right
 
 	static double s0,x_s0,y_s0;
-	if((s1 < 0.2) & !wall & (s1 < s2))
+	if((s1 < 0.15) & !wall & (s1 < s2)) //0.2
 	{
 		init();
 		x_s0 = x_s1;
 		y_s0 = y_s1;
 		right_sensor = false;
 		wall = true;
+		//y_wall_bar = 0;
 	}
-	if((s2 < 0.2) & !wall & (s2 < s1))
+	if((s2 < 0.15) & !wall & (s2 < s1)) //0.2
 	{
 		init();
 		x_s0 = x_s2;
 		y_s0 = y_s2;
 		right_sensor = true;
 		wall = true;
+		//y_wall_bar = 0;
 	}
 
 	// Sensor measurement
@@ -92,7 +94,7 @@ void receive_sensors(const AnalogC::ConstPtr &msg)
 	double s0_hat = (y_wall_bar-y_bar-x_s0*sin(theta_bar)-y_s0*cos(theta_bar))/cos(theta_bar);
 	double diff = s0 - s0_hat;
 
-	if((diff*diff > 0.03*0.03) | (y_wall*y_wall > 0.3*0.3))
+	if((diff*diff > 0.03*0.03) | (y_wall*y_wall > 0.25*0.25))
 	{
 		wall = false;
 	}
@@ -219,7 +221,7 @@ double angle(double th)
 void init()
 {
 	sigma = 1E-10 * MatrixXd::Identity(4,4);
-	sigma(2,2) = 1E-6; //-4
+	sigma(2,2) = 1E-6; //-4 //-6
 	sigma(3,3) = 1E-6;
 	R = 1E-10 * MatrixXd::Identity(4,4);
 	R(3,3) = 1E-8;
@@ -246,6 +248,7 @@ int main(int argc, char** argv)
 	// Init
 	init();
 
+	printf("mode = %d\n",mode);
 
 	if(mode == GOTO_TARGETS)
 	{
@@ -259,6 +262,17 @@ int main(int argc, char** argv)
 		x_true = odometry.at(0);
 		y_true = odometry.at(1);
 		theta_true = odometry.at(2);
+
+		// Failed to go back to (0,0)
+		double distance = sqrt((x_true)*(x_true)+(y_true)*(y_true));
+		if(distance > 0.1)
+		{
+			x_true = 0;
+			y_true = 0;
+			theta_true = 0;
+		}
+
+		printf("x = %f, y = %f, theta = %f\n",x_true,y_true,theta_true);
 	}
 
 
@@ -275,10 +289,10 @@ int main(int argc, char** argv)
 	{
 		// Save last odometry
     	std::vector<double> odometry;
+    	odometry.push_back(x_true + x*cos(theta_true) - y*sin(theta_true));
+    	odometry.push_back(y_true + x*sin(theta_true) + y*cos(theta_true));
+    	odometry.push_back(angle(theta_true + theta));
     	odometry.resize(3);
-    	odometry.push_back(x_true);
-    	odometry.push_back(y_true);
-    	odometry.push_back(theta_true);
 
     	std::ofstream os("/home/robo/explorer/last_odometry.dat",std::ios::binary);
     	os.write(reinterpret_cast<const char*>(&(odometry[0])),odometry.size()*sizeof(double));