diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index 0eb38b81a8975e98f5c9a2f0b06e1b786ac52451..bd7bc80dac32f065b866deb9866f5c7f3355725a 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -170,7 +170,8 @@ public:
static double zeroMin,zeroMax;
- static double Py_eps,Pz_eps,A_eps;
+ static double Py_eps,Pz_eps,A_eps;
+ static double x_thresh_undock,y_thresh_undock,theta_thresh_undock;
static double rand_X_SM,rand_A_esp,rand_Y_esp;
double marpose[6];
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index c0c473d7940e18657b30a496a1531c8a531726e0..0eb38b81a8975e98f5c9a2f0b06e1b786ac52451 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
@@ -171,6 +171,7 @@ public:
static double zeroMin,zeroMax;
static double Py_eps,Pz_eps,A_eps;
+ static double rand_X_SM,rand_A_esp,rand_Y_esp;
double marpose[6];
double camPose[6];
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 7121dbb2dcb2f84f5c6f5f0e6e989cd830747dff..a950d5168511402d73f9e3da5a02bcd7f2d670b3 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -55,8 +55,8 @@ double ImageConverter::Pos_Dx = 0;*/
float ImageConverter::TheMarkerSize = .1; // Default marker size
-int ImageConverter::Thresh1_min = 6;
-int ImageConverter::Thresh2_min = 6;
+int ImageConverter::Thresh1_min = 10;
+int ImageConverter::Thresh2_min = 10;
int ImageConverter::Thresh1_max = 300;
int ImageConverter::Thresh2_max = 300;
@@ -117,12 +117,19 @@ int ImageConverter::hr = 0;
double ImageConverter::zeroMax = .0000000000000000001;
double ImageConverter::zeroMin = -.0000000000000000001;
-// ------ offsets for X, Y, theta
+// ------ offsets X, Y, theta for Docking ---------
double ImageConverter::Pz_eps = .001;
double ImageConverter::Py_eps = .002;
-
double ImageConverter::A_eps = (CV_PI/180) * .6; // 1 deg.
+
+// ------ offsets X, Y, theta for Undocking ---------
+double ImageConverter::x_thresh_undock = .005;
+double ImageConverter::y_thresh_undock = .01;
+double ImageConverter::theta_thresh_undock = (CV_PI/180) * 5;
+
+
+
double ImageConverter::rand_A_esp = (CV_PI/180) * 10; // 10 deg. threshold for random theta_pose
double ImageConverter::rand_X_SM = .0005; // 0.5 mm threshold for random X_pose_SM
@@ -144,14 +151,14 @@ RefPose[1] = -0.0194805;
RefPose[2] = 0.308654;
RefPose[3] = 0.702366;
-x_rand_SM = RefPose[2] + .2; // 20 cm safety margin in the x-axis while moving towards the random pose
+x_rand_SM = RefPose[2] + .35; // 35 cm safety margin in the x-axis while moving towards the random pose
-Pos_Px = ((double) rand() / (RAND_MAX)) * (.15 - .08) + .09; // .08 < Kp < .15
+Pos_Px = ((double) rand() / (RAND_MAX)) * (.16 - .09) + .09; // .09 < Kp < .15
Pos_Ix = ((double) rand() / (RAND_MAX)) * .01; // 0 < Ki < .01
Pos_Dx = ((double) rand() / (RAND_MAX)) * .01; // 0 < Kd < .01
-x_new = ((double) rand() / (RAND_MAX)) * (.9 - x_rand_SM) + x_rand_SM;
-y_new = ((double) rand() / (RAND_MAX)) * (.52 - (-.52)) -.52;
+x_new = ((double) rand() / (RAND_MAX)) * (.98 - x_rand_SM) + x_rand_SM;
+y_new = ((double) rand() / (RAND_MAX)) * (.4 - (-.4)) -.4;
theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) - (.5*RefPose[3]);
/* ----------------- Default Gains which dock the robot successfully ----------
@@ -809,6 +816,9 @@ void ImageConverter::RandomPose(double X_rand, double Y_rand, double theta_rand)
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 << " m. \n");
ROS_INFO_STREAM(" Y_rand = " << Y_rand << " m. \n");
@@ -817,49 +827,107 @@ theta_new = ((double) rand() / (RAND_MAX)) * (RefPose[3] - (-RefPose[3])) -RefPo
double vel_x,vel_y,omega_z;
geometry_msgs::Twist msg_new;
-
+ // CCW ==>> w > 0 , CW ==>> w < 0
// Leaving docking station, moving towards x-axis SM
- if (camPose[2] - /*x_rand_SM*/ X_rand > .002)
+ if (X_rand - camPose[2] > x_thresh_undock)
{
- //keepMoving = false;
- ROS_INFO(" X-axis OK, adjusting Y-axis... \n");
- if (camPose[1] - Y_rand > Py_eps )
+ ROS_INFO_STREAM(" Adjusting X_SM pose ... \n");
+ //msg_new.linear.x = -.05;
+ vel_x = -.05;
+ } else
+ {
+ ROS_INFO(" X-axis is fixed, adjusting Y & theta - axes ... \n");
+ if ((camPose[1] - Y_rand > y_thresh_undock) && (theta_rand > 0))
{
- ROS_INFO("Robot is in the Right side of Y_rand => Moving to Left ...\n");
- //msg_new.linear.y = .05;
- vel_y = .05;
- } else if (camPose[1] - Y_rand < -Py_eps)
+ if(abs(abs(camPose[3]) - abs(theta_rand)) > theta_thresh_undock)
+ {
+ ROS_INFO("moving 2 left side & CW rot. \n");
+ vel_y = .05;
+ omega_z = -.05;
+ } else
+ {
+ ROS_INFO("CW rot. is fixed, only moving 2 left side ...\n");
+ vel_y = .05;
+ }
+ } else if ((camPose[1] - Y_rand < -y_thresh_undock) && (theta_rand < 0))
{
- ROS_INFO("Robot is in the Left side of Y_rand => Moving to Right ...\n");
- //msg_new.linear.y = -.05;
- vel_y = -.05;
- } else if (abs(abs(camPose[3]) - abs(theta_rand)) > A_eps)
+ if(abs(abs(camPose[3]) - abs(theta_rand)) > theta_thresh_undock)
+ {
+ ROS_INFO("moving 2 right side & CCW rot. \n");
+ vel_y = -.05;
+ omega_z = .05;
+ }else
+ {
+ ROS_INFO("CCW rot. is fixed, only moving 2 right side ... \n");
+ vel_y = -.05;
+ }
+ }else if (abs(camPose[1] - Y_rand) <= y_thresh_undock)
{
- // ang speed is larger than linears
- // CCW ==>> w > 0 , CW ==>> w < 0
-
- if(theta_rand > 0)
+ ROS_INFO(" Y-axis is fixed, adjusting theta-axis ... ! \n");
+ if (abs(abs(camPose[3]) - abs(theta_rand)) <= theta_thresh_undock)
+ {
+ ROS_INFO(" theta & Y-axes adjusted! \n");
+ keepMoving = false;
+ } else
+ {
+ if(theta_rand > 0)
+ {
+ ROS_INFO_STREAM(" theta > 0 => Rob rot. is CW(-) \n");
+ omega_z = -.01;
+ } else
+ {
+ ROS_INFO_STREAM(" theta < 0 => Rob rot. is CCW(+) \n");
+ omega_z = .01;
+ }
+ }
+ } else if ((camPose[1] - Y_rand > y_thresh_undock) && (theta_rand < 0))
+ {
+ if(abs(abs(camPose[3]) - abs(theta_rand)) > theta_thresh_undock)
+ {
+ ROS_INFO("moving 2 left side & CCW rot., chance of losing marker \n");
+ vel_y = .05;
+ omega_z = .01;
+ } else
+ {
+ ROS_INFO("CCW rot. is fixed, only moving 2 left side ... \n");
+ vel_y = .05;
+ }
+ } else if ((camPose[1] - Y_rand < -y_thresh_undock) && (theta_rand > 0))
+ {
+ if(abs(abs(camPose[3]) - abs(theta_rand)) > theta_thresh_undock)
{
- ROS_INFO_STREAM(" theta > 0 => Rob rot. is CW(-) \n");
- //msg_new.angular.z = -.05;
- omega_z = -.05;
+ ROS_INFO("moving 2 right side & CW rot., chance of losing marker \n");
+ vel_y = -.05;
+ omega_z = -.01;
} else
{
- ROS_INFO_STREAM(" theta < 0 => Rob rot. is CCW(+) \n");
- //msg_new.angular.z = .05;
- omega_z = .05;
+ ROS_INFO("CW rot. is fixed, only moving 2 right side ... \n");
+ vel_y = .05;
}
- }else if (abs(abs(camPose[3]) - abs(theta_rand)) <= A_eps && abs(camPose[1] - Y_rand) <= Py_eps)
+
+ }/* else if ((Y_rand < 0) && (theta_rand < 0) && (camPose[1] - Y_rand > y_thresh_undock))
+ { // extream case on the right side, move 2 right, CCW
+
+ ROS_INFO(" EXTREAM case on right side, move 2 right, CCW \n");
+ if ((abs(Y_rand) - camPose[1]) > y_thresh_undock)
+ {
+ vel_y = -.01;
+ omega_z = .05;
+ }
+ } else if ((Y_rand > 0) && (theta_rand > 0) && (camPose[1] - Y_rand < -y_thresh_undock))
+ { // extream case on the left side
+
+ ROS_INFO(" EXTREAM case on left side, move 2 left, CW \n");
+ if ((abs(Y_rand) - camPose[1]) > y_thresh_undock)
+ {
+ vel_y = .01;
+ omega_z = -.05;
+ }
+ }*/ else
{
- ROS_INFO(" Angle & Y-axis adjusted! \n");
- keepMoving = false;
-
- }
- } else
- {
- ROS_INFO_STREAM(" Adjusting X_SM pose ... \n");
- //msg_new.linear.x = -.05;
- vel_x = -.05;
+ ROS_INFO(" New condition should be added! \n");
+ keepMoving = false;
+ }
}
msg_new.linear.x = vel_x;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index a6b1102747540b65fcc2509d4d1378c05086e538..7121dbb2dcb2f84f5c6f5f0e6e989cd830747dff 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -55,8 +55,8 @@ double ImageConverter::Pos_Dx = 0;*/
float ImageConverter::TheMarkerSize = .1; // Default marker size
-int ImageConverter::Thresh1_min = 10;
-int ImageConverter::Thresh2_min = 10;
+int ImageConverter::Thresh1_min = 6;
+int ImageConverter::Thresh2_min = 6;
int ImageConverter::Thresh1_max = 300;
int ImageConverter::Thresh2_max = 300;
@@ -122,7 +122,12 @@ double ImageConverter::Pz_eps = .001;
double ImageConverter::Py_eps = .002;
double ImageConverter::A_eps = (CV_PI/180) * .6; // 1 deg.
-double ImageConverter::safety_margin_X = .15; // 20 cm
+
+double ImageConverter::rand_A_esp = (CV_PI/180) * 10; // 10 deg. threshold for random theta_pose
+
+double ImageConverter::rand_X_SM = .0005; // 0.5 mm threshold for random X_pose_SM
+
+double ImageConverter::safety_margin_X = .15; // 15 cm
ImageConverter::ImageConverter() :
it_(nh_)
@@ -139,15 +144,15 @@ 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
+x_rand_SM = RefPose[2] + .2; // 20 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;
+Pos_Px = ((double) rand() / (RAND_MAX)) * (.15 - .08) + .09; // .08 < Kp < .15
+Pos_Ix = ((double) rand() / (RAND_MAX)) * .01; // 0 < Ki < .01
+Pos_Dx = ((double) rand() / (RAND_MAX)) * .01; // 0 < Kd < .01
-x_new = ((double) rand() / (RAND_MAX)) * (.98 - RefPose[2]) + RefPose[2];
+x_new = ((double) rand() / (RAND_MAX)) * (.9 - x_rand_SM) + x_rand_SM;
y_new = ((double) rand() / (RAND_MAX)) * (.52 - (-.52)) -.52;
-theta_new = ((double) rand() / (RAND_MAX)) * (RefPose[3] - (-RefPose[3])) -RefPose[3];
+theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) - (.5*RefPose[3]);
/* ----------------- Default Gains which dock the robot successfully ----------
Pos_Px = .1;
@@ -537,8 +542,6 @@ camPose[3] = CamFB->pose.orientation.x;
ROS_INFO_STREAM(" rollmar = " << camPose[3] << " rad. =~ " << (180/CV_PI) * camPose[3] << " deg. \n");
ROS_INFO_STREAM(" rollref = " << RefPose[3] << " rad. =~ " << (180/CV_PI) * RefPose[3] << " deg. \n");
-
- ROS_INFO_STREAM(" ------------------------------------------------------------- \n");
if(Go2RandomPose == false)
{
@@ -579,7 +582,7 @@ camPose[3] = CamFB->pose.orientation.x;
}
} else
{
- ROS_INFO_STREAM("---------- Moving to Random POSE --------- \n ");
+ ROS_INFO_STREAM("------------------- Moving to Random POSE ---------------------- \n ");
RandomPose(x_new,y_new,theta_new);
}
@@ -806,23 +809,62 @@ void ImageConverter::RandomPose(double X_rand, double Y_rand, double theta_rand)
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");
+ ROS_INFO_STREAM(" ---------------- new random pose ---------------- \n");
+ ROS_INFO_STREAM(" X_rand = " << X_rand << " m. \n");
+ ROS_INFO_STREAM(" Y_rand = " << Y_rand << " m. \n");
+ ROS_INFO_STREAM(" theta_rand = " << theta_rand << " rad. =~ " << theta_rand * (180/CV_PI) << " deg. \n");
+double vel_x,vel_y,omega_z;
geometry_msgs::Twist msg_new;
// Leaving docking station, moving towards x-axis SM
- if (abs(camPose[2] - x_rand_SM) <= .0005)
+ if (camPose[2] - /*x_rand_SM*/ X_rand > .002)
{
- keepMoving = false;
+ //keepMoving = false;
+ ROS_INFO(" X-axis OK, adjusting Y-axis... \n");
+ if (camPose[1] - Y_rand > Py_eps )
+ {
+ ROS_INFO("Robot is in the Right side of Y_rand => Moving to Left ...\n");
+ //msg_new.linear.y = .05;
+ vel_y = .05;
+ } else if (camPose[1] - Y_rand < -Py_eps)
+ {
+ ROS_INFO("Robot is in the Left side of Y_rand => Moving to Right ...\n");
+ //msg_new.linear.y = -.05;
+ vel_y = -.05;
+ } else if (abs(abs(camPose[3]) - abs(theta_rand)) > A_eps)
+ {
+ // ang speed is larger than linears
+ // CCW ==>> w > 0 , CW ==>> w < 0
+
+ if(theta_rand > 0)
+ {
+ ROS_INFO_STREAM(" theta > 0 => Rob rot. is CW(-) \n");
+ //msg_new.angular.z = -.05;
+ omega_z = -.05;
+ } else
+ {
+ ROS_INFO_STREAM(" theta < 0 => Rob rot. is CCW(+) \n");
+ //msg_new.angular.z = .05;
+ omega_z = .05;
+ }
+ }else if (abs(abs(camPose[3]) - abs(theta_rand)) <= A_eps && abs(camPose[1] - Y_rand) <= Py_eps)
+ {
+ ROS_INFO(" Angle & Y-axis adjusted! \n");
+ keepMoving = false;
+
+ }
} else
{
- msg_new.linear.x = -.05;
+ ROS_INFO_STREAM(" Adjusting X_SM pose ... \n");
+ //msg_new.linear.x = -.05;
+ vel_x = -.05;
}
+ msg_new.linear.x = vel_x;
+ msg_new.linear.y = vel_y;
+ msg_new.angular.z = omega_z;
/*if(X_rand - camPose[2] > Pz_eps)
{