diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index bd7bc80dac32f065b866deb9866f5c7f3355725a..8fa1d913a4aed34afa7e7b2adbf24d4cde6f96b2 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -117,15 +117,17 @@ public:
void createTrackbars();
void myhandler(int value);
+
+ void GenerateRandomVal();
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;
+ static double Pos_Px,Pos_Ix,Pos_Dx;
+ static 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;
+ static double S_Ang_P,S_Ang_I,S_Ang_D;
+ static double L_Ang_P,L_Ang_I,L_Ang_D;
- double x_new,y_new,theta_new;
+ static double x_new,y_new,theta_new;
static double safety_margin_X;
double x_rand_SM;
@@ -161,13 +163,11 @@ public:
// ---- TIMER PARAMETERS ------ //
-
static int msec,sec,min,hr;
-
-
// ---- TIMER PARAMETERS ------ //
+ static double dock_started,dock_finished,docking_duration;
static double zeroMin,zeroMax;
static double Py_eps,Pz_eps,A_eps;
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/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index a950d5168511402d73f9e3da5a02bcd7f2d670b3..7866151018fa7c2cd8d9107b875eeaa8f4dce567 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -47,12 +47,6 @@ using namespace aruco;
using namespace std;
-
-/*double ImageConverter::Pos_Px = .1;
-double ImageConverter::Pos_Ix = 0.0028;
-double ImageConverter::Pos_Dx = 0;*/
-
-
float ImageConverter::TheMarkerSize = .1; // Default marker size
int ImageConverter::Thresh1_min = 10;
@@ -102,17 +96,25 @@ double ImageConverter::d_termYAW = 0;
double ImageConverter::control_signalYAW;
// ---- CONTROLL PARAMETERS ------ //
+// ---------------- Initial Gains which dock the robot successfully ---------------- //
-// ---- TIMER PARAMETERS ------ //
+double ImageConverter::Pos_Px = .1;
+double ImageConverter::Pos_Ix = 0.0028;
+double ImageConverter::Pos_Dx = 0;
-int ImageConverter::msec = 0;
-int ImageConverter::sec = 0;
-int ImageConverter::min = 0;
-int ImageConverter::hr = 0;
+double ImageConverter::Pos_Py = 4 * Pos_Px;
+double ImageConverter::Pos_Iy = 4 * Pos_Ix;
+double ImageConverter::Pos_Dy = 4 * Pos_Dx;
+double ImageConverter::S_Ang_P = .2 * Pos_Px;
+double ImageConverter::S_Ang_I = .2 * Pos_Ix;
+double ImageConverter::S_Ang_D = .2 * Pos_Dx;
-// ---- TIMER PARAMETERS ------ //
+double ImageConverter::L_Ang_P = .8 * Pos_Px;
+double ImageConverter::L_Ang_I = .8 * Pos_Ix;
+double ImageConverter::L_Ang_D = .8 * Pos_Dx;
+double ImageConverter::dock_started,ImageConverter::dock_finished,ImageConverter::docking_duration;
double ImageConverter::zeroMax = .0000000000000000001;
double ImageConverter::zeroMin = -.0000000000000000001;
@@ -122,19 +124,15 @@ 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 = .1; // safety margin X axis in docking process : 10 cm
// ------ offsets X, Y, theta for Undocking ---------
-double ImageConverter::x_thresh_undock = .005;
+double ImageConverter::x_thresh_undock = .01;
double ImageConverter::y_thresh_undock = .01;
-double ImageConverter::theta_thresh_undock = (CV_PI/180) * 5;
-
+double ImageConverter::theta_thresh_undock = (CV_PI/180) * 3;
-
-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
+// random pose initialized
+double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new;
ImageConverter::ImageConverter() :
it_(nh_)
@@ -144,39 +142,16 @@ double ImageConverter::safety_margin_X = .15; // 15 cm
/* initialize random seed: */
srand (time(NULL));
+
// Ref. Values
RefPose[0] = -.0957;
RefPose[1] = -0.0194805;
-RefPose[2] = 0.308654;
+RefPose[2] = 0.306654;
RefPose[3] = 0.702366;
-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)) * (.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)) * (.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 ----------
-Pos_Px = .1;
-Pos_Ix = 0.0028;
-Pos_Dx = 0;*/
+x_rand_SM = RefPose[2] + .45; // 45 cm spreading domain in the x-axis while moving towards the random pose
-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;
-S_Ang_D = .2 * Pos_Dx;
-
-L_Ang_P = .8 * Pos_Px;
-L_Ang_I = .8 * Pos_Ix;
-L_Ang_D = .8 * Pos_Dx;
//marSub = nh_.subscribe("/Marker_pose",1,&ImageConverter::marCB,this);
@@ -296,7 +271,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
cerr<<"Could not open video!!"<<endl;
nh_.shutdown();
}
-
+ dock_started = ros::Time::now().toSec();
// Read first image to get the dimensions
TheVideoCapturer>>TheInputImage;
@@ -318,23 +293,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
int index=0;
// Capture until press ESC or until the end of the video
while ((key != 'x') && (key!=27) && TheVideoCapturer.grab() && ros::ok() && keepMoving){
-
- /*if(msec == 10)
- {
- ++sec;
- msec = 0;
- }
- if(sec == 60)
- {
- ++min;
- sec = 0;
- }
- if(min == 60)
- {
- ++hr;
- min = 0;
- }*/
-
ros::spinOnce();
if (TheVideoCapturer.retrieve(TheInputImage))
@@ -450,8 +408,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
MarPose_pub.publish(msg);
ros::spinOnce();
-
- // -------------------------Removed----------------------------- //
}
/*// Print other rectangles that contains no valid markers
for (unsigned int i=0;i<MDetector.getCandidates().size();i++)
@@ -511,8 +467,6 @@ void ImageConverter::ContStart()
void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
{
-
-
camPose[0] = CamFB->pose.position.x;
camPose[1] = CamFB->pose.position.y;
camPose[2] = CamFB->pose.position.z;
@@ -535,36 +489,38 @@ camPose[3] = CamFB->pose.orientation.x;
- ROS_INFO_STREAM(" --------------------- GAINS ------------------ \n");
- ROS_INFO_STREAM(" Kp = " << Pos_Px << " . \n");
- ROS_INFO_STREAM(" Ki = " << Pos_Ix << " . \n");
- ROS_INFO_STREAM(" Kd = " << Pos_Dx << " . \n");
+ ROS_INFO_STREAM(" ------------------------- GAINS ---------------------- \n");
+ ROS_INFO_STREAM(" Kp = " << Pos_Px << " , Ki = " << Pos_Ix << " , Kd = " << Pos_Dx << "\n");
ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n");
- ROS_INFO_STREAM(" Zmar = " << camPose[2] << " m. \n");
- ROS_INFO_STREAM(" Zref = " << RefPose[2] << " m. \n");
+ ROS_INFO_STREAM(" Xmar = " << camPose[2] << " m. \n");
+ ROS_INFO_STREAM(" Xref = " << RefPose[2] << " m. \n");
ROS_INFO_STREAM(" Ymar = " << camPose[1] << " m. \n");
ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n");
- 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(" theta_rob = " << camPose[3] << " rad. =~ " << (180/CV_PI) * camPose[3] << " deg. \n");
+ ROS_INFO_STREAM(" theta_ref = " << RefPose[3] << " rad. =~ " << (180/CV_PI) * RefPose[3] << " deg. \n");
+ ROS_INFO_STREAM("------------------------------------------------------ \n ");
if(Go2RandomPose == false)
{
- ROS_INFO_STREAM("---------- MOVE TOWARDS THE DOCKING STATION ... --------- \n ");
+ ROS_INFO_STREAM("---------- MOVING 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 ");
+ //ROS_INFO("---- ***** Robot is docked successfully, Moving 2 new Random Pose ***** ---- \n ");
+ dock_finished = ros::Time::now().toSec();
+ docking_duration = dock_finished - dock_started;
+ ROS_INFO_STREAM("Robot is docked in " << docking_duration <<" sec, Moving 2 new Random Pose\n");
//keepMoving = false;
+ GenerateRandomVal();
Go2RandomPose = true;
-
- // to make sure that y & theta are within the threshold...
+ // to make sure that y & theta are within the threshold...
} else if (abs(RefPose[2] - camPose[2]) <= safety_margin_X)
{
if(
@@ -589,20 +545,14 @@ camPose[3] = CamFB->pose.orientation.x;
}
} else
{
- ROS_INFO_STREAM("------------------- Moving to Random POSE ---------------------- \n ");
+ ROS_INFO(" moving towards Generated Random pose which is : \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 ");
+ ROS_INFO_STREAM("--------------------- Controller started ----------------------\n ");
// -----------------X--------------------- //
if(abs(RefX - MarPoseX) > Pz_eps)
@@ -661,25 +611,6 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
p_termY = Pos_Py * curr_errorY;
i_termY = Pos_Iy * int_errorY;
d_termY = Pos_Dy * diffY;
-
- /*double x_offset = .25;
- if (abs(curr_errorY) >= abs(curr_errorX)) // -5 < err < +5
- {
- ROS_INFO_STREAM("robot movement in Y-direction is dominant! \n");
- control_signalX = 0;
- // scalling
- p_termY = L_yPos_P * curr_errorY;
- i_termY = L_yPos_I * int_errorY;
- d_termY = L_yPos_D * diffY;
- } else
- {
- ROS_INFO_STREAM("robot movement in X-direction is dominant! \n");
- // scalling
- p_termY = S_yPos_P * curr_errorY;
- i_termY = S_yPos_I * int_errorY;
- d_termY = S_yPos_D * diffY;
- }*/
-
//ROS_INFO_STREAM("prop_gainY = " << p_termY << ", integ_gainY = " << i_termY << " . \n");
// control signal
control_signalY = p_termY + i_termY + d_termY;
@@ -758,7 +689,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(" ---------------- Controller ended --------------------- \n");
+ ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n");
dock(-control_signalX, control_signalY, control_signalYAW);
@@ -810,19 +741,25 @@ void ImageConverter::move2docking(double VelX_est, double VelY_est, double omega
}
// ---- Controller part -------- END ------
-void ImageConverter::RandomPose(double X_rand, double Y_rand, double theta_rand)
+void ImageConverter::GenerateRandomVal()
{
-/*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];*/
-
-
-
+ // ---------------- PID gains ------------------
+ Pos_Px = ((double) rand() / (RAND_MAX)) * (.16 - .09) + .09; // .09 < Kp < .16
+ Pos_Ix = ((double) rand() / (RAND_MAX)) * .01; // 0 < Ki < .01
+ Pos_Dx = ((double) rand() / (RAND_MAX)) * .02; // 0 < Kd < .02
+
+ // ------------------ Generating Random Pose ------------------
+ x_new = ((double) rand() / (RAND_MAX)) * (1.05 - x_rand_SM) + x_rand_SM;
+ y_new = ((double) rand() / (RAND_MAX)) * (.59 - (-.24)) + (-.24);
+ theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) + (-.5*RefPose[3]);
+}
- ROS_INFO_STREAM(" ---------------- new random pose ---------------- \n");
+void ImageConverter::RandomPose(double X_rand, double Y_rand, double theta_rand)
+{
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");
+ ROS_INFO_STREAM(" -------------------------------------------------------------- \n");
double vel_x,vel_y,omega_z;
@@ -831,10 +768,13 @@ geometry_msgs::Twist msg_new;
// Leaving docking station, moving towards x-axis SM
if (X_rand - camPose[2] > x_thresh_undock)
{
- ROS_INFO_STREAM(" Adjusting X_SM pose ... \n");
- //msg_new.linear.x = -.05;
- vel_x = -.05;
- } else
+ ROS_INFO_STREAM(" Adjusting X, moving backward ... \n");
+ vel_x = -.02;
+ } else if (X_rand - camPose[2] < -x_thresh_undock)
+ {
+ ROS_INFO_STREAM(" Adjusting X, moving forward ... \n");
+ vel_x = .02;
+ }else if (abs(X_rand - camPose[2]) <= x_thresh_undock)
{
ROS_INFO(" X-axis is fixed, adjusting Y & theta - axes ... \n");
if ((camPose[1] - Y_rand > y_thresh_undock) && (theta_rand > 0))
@@ -842,32 +782,33 @@ geometry_msgs::Twist msg_new;
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;
+ vel_y = .03;
+ omega_z = -.01;
} else
{
ROS_INFO("CW rot. is fixed, only moving 2 left side ...\n");
- vel_y = .05;
+ vel_y = .03;
}
} 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 right side & CCW rot. \n");
- vel_y = -.05;
- omega_z = .05;
+ vel_y = -.03;
+ omega_z = .01;
}else
{
ROS_INFO("CCW rot. is fixed, only moving 2 right side ... \n");
- vel_y = -.05;
+ vel_y = -.03;
}
}else if (abs(camPose[1] - Y_rand) <= y_thresh_undock)
{
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;
+ ROS_INFO(" Robot is in a new random Pose! \n");
+ //keepMoving = false;
+ Go2RandomPose = false;
} else
{
if(theta_rand > 0)
@@ -885,24 +826,24 @@ geometry_msgs::Twist msg_new;
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;
+ vel_y = .03;
omega_z = .01;
} else
{
ROS_INFO("CCW rot. is fixed, only moving 2 left side ... \n");
- vel_y = .05;
+ vel_y = .03;
}
} 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 right side & CW rot., chance of losing marker \n");
- vel_y = -.05;
+ vel_y = -.03;
omega_z = -.01;
} else
{
ROS_INFO("CW rot. is fixed, only moving 2 right side ... \n");
- vel_y = .05;
+ vel_y = -.03;
}
}/* else if ((Y_rand < 0) && (theta_rand < 0) && (camPose[1] - Y_rand > y_thresh_undock))
@@ -934,22 +875,6 @@ geometry_msgs::Twist msg_new;
msg_new.linear.y = vel_y;
msg_new.angular.z = omega_z;
- /*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 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/Machine_Learning/Practice/rand_func.cpp b/MobileRobot/Machine_Learning/Practice/rand_func.cpp
index d7d406a3689d5750df775c37b2945d2606436ff6..232fdf95cf543aa3f473b57f30160637be947068 100644
--- a/MobileRobot/Machine_Learning/Practice/rand_func.cpp
+++ b/MobileRobot/Machine_Learning/Practice/rand_func.cpp
@@ -24,9 +24,9 @@ int main ()
// 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;
+ x_new = ((double) rand() / (RAND_MAX)) * (.98 - 0.658654) + 0.658654;
+ y_new = ((double) rand() / (RAND_MAX)) * (.4 - (-.4)) + (-.4);
+ theta_new = ((double) rand() / (RAND_MAX)) * (.5*.702366 - (-.5*.702366)) + (-.5*.702366);
/*printf("Kp : %1.12f \n",Kp);
printf("Ki : %1.12f \n",Ki);
diff --git a/MobileRobot/Machine_Learning/Practice/rand_func.cpp~ b/MobileRobot/Machine_Learning/Practice/rand_func.cpp~
index fb84c71900624800ef03186a808dcf3353219b85..9e5ae918d9665ad21670aabb0008d2e75378bdd6 100644
--- a/MobileRobot/Machine_Learning/Practice/rand_func.cpp~
+++ b/MobileRobot/Machine_Learning/Practice/rand_func.cpp~
@@ -25,8 +25,8 @@ int main ()
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;
+ y_new = ((double) rand() / (RAND_MAX)) * (.4 - (-.4)) + (-.4);
+ theta_new = ((double) rand() / (RAND_MAX)) * (.5*.702366 - (-.5*.702366)) + (-.5*.702366);
/*printf("Kp : %1.12f \n",Kp);
printf("Ki : %1.12f \n",Ki);
diff --git a/MobileRobot/Machine_Learning/Practice/random b/MobileRobot/Machine_Learning/Practice/random
index 810c88d4d444c005e7fa0418a92b0fe54f60c8d8..c90b3992244b94af4a6f1714e9b1dd9ec82b71d7 100755
Binary files a/MobileRobot/Machine_Learning/Practice/random and b/MobileRobot/Machine_Learning/Practice/random differ