diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index 61091070319204d56697eabfcefea319d405babe..c0c473d7940e18657b30a496a1531c8a531726e0 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 cc66ff08d4a993d988a747ae93ea07d3a06d07c5..b0056ea595686fa060380d3edc09cf5e2af20457 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 0aaf906c33956e36835c90201ba49e7b594e35b3..a6b1102747540b65fcc2509d4d1378c05086e538 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 0aaf906c33956e36835c90201ba49e7b594e35b3..984a38ce657476cd08ef682071c2aebc17a8359a 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 ef870dc1c658c671fdfcd8751d96e8327949dd4e..d7d406a3689d5750df775c37b2945d2606436ff6 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 d66d2a1cf5d5adcea12b1114516f5db769c719b2..fb84c71900624800ef03186a808dcf3353219b85 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
Binary files a/MobileRobot/Machine_Learning/Practice/random and b/MobileRobot/Machine_Learning/Practice/random differ