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/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index b0056ea595686fa060380d3edc09cf5e2af20457..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;
@@ -129,6 +128,7 @@ public:
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/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)
{
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 984a38ce657476cd08ef682071c2aebc17a8359a..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;
@@ -138,6 +139,8 @@ 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;
@@ -151,9 +154,9 @@ 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;
@@ -168,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);
@@ -345,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);
}
@@ -519,11 +522,7 @@ camPose[3] = CamFB->pose.orientation.x;
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");
ROS_INFO_STREAM(" Ki = " << Pos_Ix << " . \n");
@@ -541,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)
{
@@ -573,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)
@@ -732,9 +748,9 @@ 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);
+ dock(-control_signalX, control_signalY, control_signalYAW);
}
@@ -784,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);
+
}