From f9b25c709623dc5d91feca6f7c9fcb2ad6fbb472 Mon Sep 17 00:00:00 2001
From: Farid Alijani <farid.alijani@student.lut.fi>
Date: Tue, 22 Mar 2016 19:54:21 +0100
Subject: [PATCH] leaving the docking station problem

---
 .../CamMark/camtomar/include/VisionControl.h  |   8 +-
 .../CamMark/camtomar/include/VisionControl.h~ |  23 ++++-
 .../CamMark/camtomar/src/VisionControl.cpp    |  95 ++++++++++++++----
 .../CamMark/camtomar/src/VisionControl.cpp~   |  22 ++--
 .../Machine_Learning/Practice/rand_func.cpp   |  20 +++-
 .../Machine_Learning/Practice/rand_func.cpp~  |  22 +++-
 MobileRobot/Machine_Learning/Practice/random  | Bin 8536 -> 8536 bytes
 7 files changed, 153 insertions(+), 37 deletions(-)

diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index 61091070..c0c473d7 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -74,7 +74,6 @@ private:
         // --- Robot listener ---
         tf::TransformListener RobListener;
   
-        
         static int Thresh1_min,Thresh2_min;
         
         static int Thresh1_max,Thresh2_max;
@@ -87,7 +86,7 @@ private:
         static float TheMarkerSize;
         
         void ContStart();
-        static bool update_images,found;
+        static bool update_images,found,Go2RandomPose;
         
         bool readArguments ( int argc,char **argv );
         
@@ -118,7 +117,7 @@ public:
   void createTrackbars();
   
   void myhandler(int value);
-  void RandomPose();
+  void RandomPose(double X_rand, double Y_rand, double theta_rand);
 
          double Pos_Px,Pos_Ix,Pos_Dx;
 	 double Pos_Py,Pos_Iy,Pos_Dy;
@@ -126,7 +125,10 @@ public:
          double S_Ang_P,S_Ang_I,S_Ang_D;
          double L_Ang_P,L_Ang_I,L_Ang_D;
 
+	double x_new,y_new,theta_new;
+
 	static double safety_margin_X;
+	double x_rand_SM;
         
         // ---- CONTROLL PARAMETERS ------ //
         static double prev_errorX;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index cc66ff08..b0056ea5 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
@@ -118,12 +118,16 @@ public:
   void createTrackbars();
   
   void myhandler(int value);
-  
-        static double Pos_Px,Pos_Ix,Pos_Dx;
-	static double Pos_Py,Pos_Iy,Pos_Dy;
+  void RandomPose();
+
+         double Pos_Px,Pos_Ix,Pos_Dx;
+	 double Pos_Py,Pos_Iy,Pos_Dy;
+
+         double S_Ang_P,S_Ang_I,S_Ang_D;
+         double L_Ang_P,L_Ang_I,L_Ang_D;
+
+	double x_new,y_new,theta_new;
 
-        static double S_Ang_P,S_Ang_I,S_Ang_D;
-        static double L_Ang_P,L_Ang_I,L_Ang_D;
 	static double safety_margin_X;
         
         // ---- CONTROLL PARAMETERS ------ //
@@ -156,6 +160,15 @@ public:
         // ---- CONTROLL PARAMETERS ------ //
         
         
+	// ---- TIMER PARAMETERS ------ //
+
+	static int msec,sec,min,hr;
+
+
+	// ---- TIMER PARAMETERS ------ //
+
+
+
         static double zeroMin,zeroMax;
 	static double Py_eps,Pz_eps,A_eps;  	
 
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 0aaf906c..a6b11027 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -68,6 +68,7 @@ int ImageConverter::ThePyrDownLevel = 0;
 bool ImageConverter::update_images = true;
 
 bool ImageConverter::found = false;
+bool ImageConverter::Go2RandomPose = false;
 
 int ImageConverter::ThresParam1 = 0;
 int ImageConverter::ThresParam2 = 0;
@@ -131,21 +132,31 @@ double ImageConverter::safety_margin_X = .15; // 20 cm
     
 	/* initialize random seed: */
   	srand (time(NULL));
+// Ref. Values
+RefPose[0] = -.0957;
+
+RefPose[1] = -0.0194805;
+RefPose[2] = 0.308654;
+RefPose[3] = 0.702366;
 
+x_rand_SM = RefPose[2] - .1; // 10 cm safety margin in the x-axis while moving towards the random pose
 
 Pos_Px = ((double) rand() / (RAND_MAX)) * .15;
 Pos_Ix = ((double) rand() / (RAND_MAX)) * .01;
 Pos_Dx = ((double) rand() / (RAND_MAX)) * .01;
 
-
+x_new = ((double) rand() / (RAND_MAX)) * (.98 - RefPose[2]) + RefPose[2];
+y_new = ((double) rand() / (RAND_MAX)) * (.52 - (-.52)) -.52;
+theta_new = ((double) rand() / (RAND_MAX)) * (RefPose[3] - (-RefPose[3])) -RefPose[3];
+  
 /* -----------------  Default Gains which dock the robot successfully ----------
 Pos_Px = .1;
 Pos_Ix = 0.0028;
 Pos_Dx = 0;*/
 
-Pos_Py = 5 * Pos_Px;
-Pos_Iy = 5 * Pos_Ix;
-Pos_Dy = 5 * Pos_Dx;
+Pos_Py = 4 * Pos_Px;
+Pos_Iy = 4 * Pos_Ix;
+Pos_Dy = 4 * Pos_Dx;
 
 S_Ang_P = .2 * Pos_Px;
 S_Ang_I = .2 * Pos_Ix;
@@ -160,7 +171,6 @@ L_Ang_D = .8 * Pos_Dx;
     // Publish pose message and buffer up to 100 messages
     MarPose_pub = nh_.advertise<geometry_msgs::PoseStamped>("/marker_pose", 100);
     
-    //Move2RandPos = false;
     keepMoving = true;
     commandPub = nh_.advertise<geometry_msgs::Twist>("/base_controller/command",1);
     Sub = nh_.subscribe("/marker_pose",1,&ImageConverter::camCB,this);
@@ -337,7 +347,8 @@ void ImageConverter::ProgStart(int argc,char** argv)
 			{
 			        found = false;
 				keepMoving = false;
-				ROS_INFO("SORRY, BUT MARKER IS LOST ... \n");					
+				ROS_INFO("SORRY, BUT MARKER IS LOST, Starting again ... \n");					
+				//RandomPose(x_new,y_new,theta_new);			
 				//move2docking(-control_signalX, -control_signalY, control_signalYAW);
 			}
 			
@@ -488,12 +499,7 @@ void ImageConverter::ContStart()
 
 void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
 {
-// Ref. Values
-RefPose[0] = -.0957;
 
-RefPose[1] = -0.0194805;
-RefPose[2] = 0.308654;
-RefPose[3] = 0.702366;
 
 camPose[0] = CamFB->pose.position.x;
 camPose[1] = CamFB->pose.position.y;
@@ -514,8 +520,9 @@ camPose[3] = CamFB->pose.orientation.x;
         
 	/*ROS_INFO_STREAM(" --------------------- TIMER ------------------ \n");
 	ROS_INFO_STREAM("Docking duration : "<< hr << " : " << min << " : " << sec << " . " << msec << "\n");*/
+	
 
-
+	
 	ROS_INFO_STREAM(" --------------------- GAINS ------------------ \n");
 	ROS_INFO_STREAM(" Kp = " << Pos_Px << " . \n");
 	ROS_INFO_STREAM(" Ki = " << Pos_Ix << " . \n");
@@ -533,15 +540,20 @@ camPose[3] = CamFB->pose.orientation.x;
         
         ROS_INFO_STREAM(" ------------------------------------------------------------- \n");
 
+	if(Go2RandomPose == false)
+	{
+		ROS_INFO_STREAM("---------- MOVE TOWARDS THE DOCKING STATION ... ---------  \n ");
 		if (
             		(abs(RefPose[2] - camPose[2]) <= Pz_eps) //&& // Z
             		//(abs(RefPose[1] - camPose[1]) <= Py_eps) && // Y
             		//(abs(RefPose[3] - camPose[3]) <= A_eps) // Yaw
             	)
         	{
-                
-                        ROS_INFO("----/*****//----- Dock is completed successfully ! -----/******---- \n ");                        
-			keepMoving = false;
+                        ROS_INFO("----/*****----- Dock is completed successfully ! -----/******---- \n ");                        
+			//keepMoving = false;
+			Go2RandomPose = true;
+			
+
                  // to make sure that y & theta are within the threshold...
         	} else if (abs(RefPose[2] - camPose[2]) <= safety_margin_X)
 		{
@@ -565,10 +577,22 @@ camPose[3] = CamFB->pose.orientation.x;
         	{
                 	Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.01);
 		}
+	} else
+	{
+		ROS_INFO_STREAM("---------- Moving to Random POSE ---------  \n ");		
+		RandomPose(x_new,y_new,theta_new);
+	}
+
+/*else if (Go2RandomPose == false)
+		{
+			ROS_INFO_STREAM("---------- MOVE TOWARDS THE DOCKING STATION ... ---------  \n "); 
+		}*/
+
 }
 
 void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt)
 {
+	ROS_INFO_STREAM("---------- Controller started ---------  \n "); 
         // -----------------X--------------------- //
         
 	if(abs(RefX - MarPoseX) > Pz_eps)
@@ -724,7 +748,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
         //ROS_INFO_STREAM("Control signalX = " << control_signalX <<" . \n");
 	//ROS_INFO_STREAM("RAW Control signalY = " << control_signalY << ". \n");
 	//ROS_INFO_STREAM("RAW Control signalYAW = "<< control_signalYAW <<". \n");
-	ROS_INFO_STREAM(" ------------------------------------------------------------- \n");	
+	ROS_INFO_STREAM(" ---------------- Controller ended --------------------- \n");	
 	
 	dock(-control_signalX, control_signalY, control_signalYAW);
 	
@@ -776,10 +800,47 @@ void ImageConverter::move2docking(double VelX_est, double VelY_est, double omega
 }
 // ---- Controller part -------- END ------
 
-void ImageConverter::RandomPose()
+void ImageConverter::RandomPose(double X_rand, double Y_rand, double theta_rand)
 {
+/*x_new = ((double) rand() / (RAND_MAX)) * (.98 - RefPose[2]) + RefPose[2];
+y_new = ((double) rand() / (RAND_MAX)) * (.52 - (-.52)) -.52;
+theta_new = ((double) rand() / (RAND_MAX)) * (RefPose[3] - (-RefPose[3])) -RefPose[3];*/
+
+	ROS_INFO_STREAM(" --------------------- new random pose ------------------ \n");
+	ROS_INFO_STREAM(" X_rand	= " << X_rand << " . \n");
+	ROS_INFO_STREAM(" Y_rand	= " << Y_rand << " . \n");
+	ROS_INFO_STREAM(" theta_rand	= " << theta_rand << " . \n");
 
 
+geometry_msgs::Twist msg_new;
 
+	// Leaving docking station, moving towards x-axis SM
+	if (abs(camPose[2] - x_rand_SM) <= .0005)
+	{
+		keepMoving = false;			
+	} else
+	{
+		msg_new.linear.x = -.05;	
+	}
+
+
+	/*if(X_rand - camPose[2] > Pz_eps)
+	{
+		msg_new.linear.x = -.05;
+				
+	}else if (Y_rand > camPose[1] )
+	{
+		msg_new.linear.y = +.05;
+	}else if (Y_rand < camPose[1])
+	{
+		msg_new.linear.y = -.05;
+	} else if (X_rand - camPose[2] <= Pz_eps && abs(Y_rand - camPose[1]) <= Py_eps )
+	{
+		ROS_INFO("Random pose has achieved!\n");		
+		keepMoving = false;
+	}*/
+
+commandPub.publish(msg_new);
+	
 }
 
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 0aaf906c..984a38ce 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -131,13 +131,21 @@ double ImageConverter::safety_margin_X = .15; // 20 cm
     
 	/* initialize random seed: */
   	srand (time(NULL));
+// Ref. Values
+RefPose[0] = -.0957;
 
+RefPose[1] = -0.0194805;
+RefPose[2] = 0.308654;
+RefPose[3] = 0.702366;
 
 Pos_Px = ((double) rand() / (RAND_MAX)) * .15;
 Pos_Ix = ((double) rand() / (RAND_MAX)) * .01;
 Pos_Dx = ((double) rand() / (RAND_MAX)) * .01;
 
-
+x_new = ((double) rand() / (RAND_MAX)) * (.98 - RefPose[2]) + RefPose[2];
+y_new = ((double) rand() / (RAND_MAX)) * (.52 - (-.52)) -.52;
+theta_new = ((double) rand() / (RAND_MAX)) * (RefPose[3] - (-RefPose[3])) -RefPose[3];
+  
 /* -----------------  Default Gains which dock the robot successfully ----------
 Pos_Px = .1;
 Pos_Ix = 0.0028;
@@ -488,12 +496,7 @@ void ImageConverter::ContStart()
 
 void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
 {
-// Ref. Values
-RefPose[0] = -.0957;
 
-RefPose[1] = -0.0194805;
-RefPose[2] = 0.308654;
-RefPose[3] = 0.702366;
 
 camPose[0] = CamFB->pose.position.x;
 camPose[1] = CamFB->pose.position.y;
@@ -514,7 +517,12 @@ camPose[3] = CamFB->pose.orientation.x;
         
 	/*ROS_INFO_STREAM(" --------------------- TIMER ------------------ \n");
 	ROS_INFO_STREAM("Docking duration : "<< hr << " : " << min << " : " << sec << " . " << msec << "\n");*/
+	
 
+	/*ROS_INFO_STREAM(" --------------------- new random pose ------------------ \n");
+	ROS_INFO_STREAM(" X_new 	= " << x_new << " . \n");
+	ROS_INFO_STREAM(" Y_new 	= " << y_new << " . \n");
+	ROS_INFO_STREAM(" theta_new 	= " << theta_new << " . \n");*/
 
 	ROS_INFO_STREAM(" --------------------- GAINS ------------------ \n");
 	ROS_INFO_STREAM(" Kp = " << Pos_Px << " . \n");
@@ -726,7 +734,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
 	//ROS_INFO_STREAM("RAW Control signalYAW = "<< control_signalYAW <<". \n");
 	ROS_INFO_STREAM(" ------------------------------------------------------------- \n");	
 	
-	dock(-control_signalX, control_signalY, control_signalYAW);
+	//dock(-control_signalX, control_signalY, control_signalYAW);
 	
 }
 
diff --git a/MobileRobot/Machine_Learning/Practice/rand_func.cpp b/MobileRobot/Machine_Learning/Practice/rand_func.cpp
index ef870dc1..d7d406a3 100644
--- a/MobileRobot/Machine_Learning/Practice/rand_func.cpp
+++ b/MobileRobot/Machine_Learning/Practice/rand_func.cpp
@@ -9,6 +9,7 @@
 int main ()
 {
   double  Kp, Ki,Kd;
+  double x_new,y_new,theta_new;
 
   /* initialize random seed: */
   srand (time(NULL));
@@ -18,9 +19,24 @@ int main ()
   Ki = ((double) rand() / (RAND_MAX)) * .01;
   Kd = ((double) rand() / (RAND_MAX)) * .001;
   
-  printf("Kp : %1.12f \n",Kp);
+  /* How to make A <= x < B out of 0 <= x < 1 ????
+  // 0 <= x < 1         >>>>===*(B-A)===>>>     0 <= x < (B-A) 
+  // 0 <= x < (B-A)     >>>>=== + A ===>>>      A <= x < B */
+  
+  
+  x_new = ((double) rand() / (RAND_MAX)) * (.98 - 0.308654) + 0.308654;
+  y_new = ((double) rand() / (RAND_MAX)) * (.52 - (-.52)) -.52;
+  theta_new = ((double) rand() / (RAND_MAX)) * (0.702366 - (-0.702366)) -0.702366;
+  
+  /*printf("Kp : %1.12f \n",Kp);
   printf("Ki : %1.12f \n",Ki);
-  printf("Kd : %1.12f \n",Kd);
+  printf("Kd : %1.12f \n",Kd);*/
+  
+  printf("X     = %1.12f \n",x_new);
+  printf("Y     = %1.12f \n",y_new);
+  printf("theta = %1.12f \n",theta_new);
+  
+  
   /*do {
     printf ("Guess the number (0 to 1): ");
     scanf ("%lf",&iGuess);
diff --git a/MobileRobot/Machine_Learning/Practice/rand_func.cpp~ b/MobileRobot/Machine_Learning/Practice/rand_func.cpp~
index d66d2a1c..fb84c719 100644
--- a/MobileRobot/Machine_Learning/Practice/rand_func.cpp~
+++ b/MobileRobot/Machine_Learning/Practice/rand_func.cpp~
@@ -9,18 +9,34 @@
 int main ()
 {
   double  Kp, Ki,Kd;
+  double x_new,y_new,theta_new;
 
   /* initialize random seed: */
   srand (time(NULL));
 
   /* generate secret number between 0 and 1: */
-  Kp = ((double) rand() / (RAND_MAX)) * .2;
+  Kp = ((double) rand() / (RAND_MAX)) * .12;
   Ki = ((double) rand() / (RAND_MAX)) * .01;
   Kd = ((double) rand() / (RAND_MAX)) * .001;
   
-  printf("Kp : %1.12f \n",Kp);
+  /* How to make A <= x < B out of 0 <= x < 1 ????
+  // 0 <= x < 1         >>>>===*(B-A)===>>>     0 <= x < (B-A) 
+  // 0 <= x < (B-A)     >>>>=== + A ===>>>      A <= x < B */
+  
+  
+  x_new = ((double) rand() / (RAND_MAX)) * (.98 - 0.308654) + 0.308654;
+  y_new = ((double) rand() / (RAND_MAX)) * (.52 - (-.52)) -.52;
+  theta_new = ((double) rand() / (RAND_MAX)) * (0.702366 - (-.5)) -.5;
+  
+  /*printf("Kp : %1.12f \n",Kp);
   printf("Ki : %1.12f \n",Ki);
-  printf("Kd : %1.12f \n",Kd);
+  printf("Kd : %1.12f \n",Kd);*/
+  
+  printf("X     = %1.12f \n",x_new);
+  printf("Y     = %1.12f \n",y_new);
+  printf("theta = %1.12f \n",theta_new);
+  
+  
   /*do {
     printf ("Guess the number (0 to 1): ");
     scanf ("%lf",&iGuess);
diff --git a/MobileRobot/Machine_Learning/Practice/random b/MobileRobot/Machine_Learning/Practice/random
index e82aa5005931fe2ebc04c8922a3cad522a214cf0..810c88d4d444c005e7fa0418a92b0fe54f60c8d8 100755
GIT binary patch
delta 846
zcmccNbi--F1+E%S1~91MoOo-W&<YMPhhYVW1DLUbWAZA-TOvEdH^ya~Y91C^puKRB
zR!&BBv-0fCGnno(GKDfvu4Jj?JizY2;Bow*0sG|NEEgCJCZA!ItIuL$VEDuzckmOx
z0B;(Y9eMB*zo6@d7bpJx|Nn_!>i|f`1uO$m<pE|xRNZ*72uYP3SO%g>3(SV7dhntK
zN!1TVgpEA)aAg9#9AI_?*!mYQypXgUL(=jJ%mx`O!21l$jsffV@InGf#}u#(#3?7h
zY>18{U^Ya@k5Bvpajq}+D{we4>|kVIV0bb8FUZRRt{?Ur!o`}QVn6mf!Nv0b{{O!N
z6!a$#Pk;iC&K#8+KAkBl4?KExAG0Yi`1GpYVVk^-O`GZSpUIEeG8sK4`?4Fj%K!cU
z-=p~m$6*ErMg}I384M8$AYiMYYN%&ul%~MN5DDTdAPJOYq?RNi2~6I_t`^EP*K_u&
z>ZkTwo_yT$=fGwAB?Y!z*B5_)(huyX1pIP3QTYu@KeK0)X13O0U|?WjU|_iM`~QCr
z1_lP1zyJRiFfcH5{QLiZ0s{ks#AH?uZKjUjlZ`mkl$QMY|33v3E{=QxZA{L*Y|QbD
zJnS3{Dhvz^DGUq@AO23R;!tCZ*u0QqKg(ndz5pf;j?GnkZfr~%%##m^=y7pC(<cYV
z<X<9Yf)Y@!1T^_hnCvL3&xMqTt3=H>Z$K5@*t}D;o`sQPa-)<eGe3g_m=<7I0j32R
zZftIp`p(Ey!8y4~Mvw8r<dri0j0YxL%DOXga7=EJ-Nz)sG1*B@kI8_2a*<pbQwrne
ITXJ<w0Nzm{d;kCd

delta 701
zcmccNbi--F1+Fa|3}CQ@W8$rSLJ!!%9EJz%4q(Ou_Q|UlZ;2E;zNrqV;1rPhrYsxS
ze!FVRlSj6jXE5DoWID<;xss)l(}3N9!Q=SB1#FXlvs_?Qn0$s+uKp4u1H&i&xPzbg
z1$fVa*^vi7@e8`XcyZ$2|No!(wGMz})_`Rosy2Yx5LF*uEJ9M%29|-SngnJ;RQ>qG
zFW~xO{{(gih8>Iy3=A)H!I}hIKkWCN2N#rt3jWx?1}?_>@BjZDAU~cwJOKrGbmpks
z@aarZdEn8jTg<M&;M1#`!#>%KU7M--@8n|kOvW3N@3I@%uK)Z0zen>Cj>8NLj0{X5
zlNh`U6s#0f4fPC-(iFHDyfaaFDagFZK^$tTjMB{3It&a9EDQ_`3;z87@4>*p@Z|6R
z{{;*T3<v)G|386&fgxb>Sq^QcfIpLeai}Tf{Qdv`9LOd|K7lqSXI?g@lRWGk3=Ap^
z3=Am@3=A6oCOdJeu}=8+|3A~_LeA|hlN&hCOxEBFV5(r>T*c?c#x#Rz@*xpDt_o=M
zR<KY0C1NJn0p)f;V?ScDqo_U?QW~fdHRBZEfFucl%{xWwSv+AzfSd!v%nbYtFtcD{
zAexy$0G^^id>CeC5M&V8+$i;(k!b_R<SH3GMu*8OW%?NnCR@t7GgYupZj#-{)WJU4
XNluSx0o&vvxiqG844ZGs)iD7875mlA

-- 
GitLab