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