diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h index 42d85258a9e57dc334a2749e0fb21d0babb4e1c4..cc66ff08d4a993d988a747ae93ea07d3a06d07c5 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h @@ -122,11 +122,9 @@ public: static double Pos_Px,Pos_Ix,Pos_Dx; static double Pos_Py,Pos_Iy,Pos_Dy; - static double S_yPos_P,S_yPos_I,S_yPos_D; - static double L_yPos_P,L_yPos_I,L_yPos_D; - 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 ------ // static double prev_errorX; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ index 545a6ce6939f9e060dbbe001376fb58171f0358d..42d85258a9e57dc334a2749e0fb21d0babb4e1c4 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ @@ -122,8 +122,8 @@ public: static double Pos_Px,Pos_Ix,Pos_Dx; static double Pos_Py,Pos_Iy,Pos_Dy; - //static double S_yPos_P,S_yPos_I,S_yPos_D; - //static double L_yPos_P,L_yPos_I,L_yPos_D; + static double S_yPos_P,S_yPos_I,S_yPos_D; + static double L_yPos_P,L_yPos_I,L_yPos_D; static double S_Ang_P,S_Ang_I,S_Ang_D; static double L_Ang_P,L_Ang_I,L_Ang_D; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp index 20e7ad16cf91ab2d43c8d1ab6ababeb1f297a86b..42ce8d4097e43359ee289a14ea731ef3c3b53add 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp @@ -41,30 +41,21 @@ using namespace cv; using namespace aruco; using namespace std; - -double ImageConverter::Pos_Px = .00005; -double ImageConverter::Pos_Ix = 0.0015; +double ImageConverter::Pos_Px = .05; +double ImageConverter::Pos_Ix = 0.002; double ImageConverter::Pos_Dx = 0; -double ImageConverter::Pos_Py = 2.3 * ImageConverter::Pos_Px; -double ImageConverter::Pos_Iy = 2.3 * ImageConverter::Pos_Ix; -double ImageConverter::Pos_Dy = 2.3 * ImageConverter::Pos_Dx; - -double ImageConverter::S_yPos_P = 2.3 * ImageConverter::Pos_Px; -double ImageConverter::S_yPos_I = 2.3 * ImageConverter::Pos_Ix; -double ImageConverter::S_yPos_D = 2.3 * ImageConverter::Pos_Dx; +double ImageConverter::Pos_Py = 5 * ImageConverter::Pos_Px; +double ImageConverter::Pos_Iy = 5 * ImageConverter::Pos_Ix; +double ImageConverter::Pos_Dy = 5 * ImageConverter::Pos_Dx; -double ImageConverter::L_yPos_P = 15 * ImageConverter::Pos_Px; -double ImageConverter::L_yPos_I = 15 * ImageConverter::Pos_Ix; -double ImageConverter::L_yPos_D = 15 * ImageConverter::Pos_Dx; +double ImageConverter::S_Ang_P = .2 * ImageConverter::Pos_Px; +double ImageConverter::S_Ang_I = .2 * ImageConverter::Pos_Ix; +double ImageConverter::S_Ang_D = .2 * ImageConverter::Pos_Dx; -double ImageConverter::S_Ang_P = .062 * ImageConverter::Pos_Px; -double ImageConverter::S_Ang_I = .062 * ImageConverter::Pos_Ix; -double ImageConverter::S_Ang_D = .062 * ImageConverter::Pos_Dx; - -double ImageConverter::L_Ang_P = .7 * ImageConverter::Pos_Px; -double ImageConverter::L_Ang_I = .7 * ImageConverter::Pos_Ix; -double ImageConverter::L_Ang_D = .7 * ImageConverter::Pos_Dx; +double ImageConverter::L_Ang_P = .8 * ImageConverter::Pos_Px; +double ImageConverter::L_Ang_I = .8 * ImageConverter::Pos_Ix; +double ImageConverter::L_Ang_D = .8 * ImageConverter::Pos_Dx; float ImageConverter::TheMarkerSize = .1; // Default marker size @@ -116,10 +107,13 @@ double ImageConverter::control_signalYAW; double ImageConverter::zeroMax = .0000000000000000001; double ImageConverter::zeroMin = -.0000000000000000001; + +// ------ offsets for X, Y, theta double ImageConverter::Pz_eps = .001; -double ImageConverter::Py_eps = .005; -double ImageConverter::A_eps = (CV_PI/180) * 1; // 1 deg. +double ImageConverter::Py_eps = .002; +double ImageConverter::A_eps = (CV_PI/180) * .6; // 1 deg. +double ImageConverter::safety_margin_X = .15; // 10 cm ImageConverter::ImageConverter() : it_(nh_) @@ -136,10 +130,6 @@ double ImageConverter::A_eps = (CV_PI/180) * 1; // 1 deg. keepMoving = true; commandPub = nh_.advertise<geometry_msgs::Twist>("/base_controller/command",1); Sub = nh_.subscribe("/marker_pose",1,&ImageConverter::camCB,this); - - - - } ImageConverter::~ImageConverter() @@ -443,7 +433,7 @@ void ImageConverter::ContStart() void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) { - +// Ref. Values RefPose[0] = -.0957; RefPose[1] = -0.0193642; RefPose[2] = 0.307922; @@ -478,16 +468,34 @@ camPose[3] = CamFB->pose.orientation.x; ROS_INFO_STREAM(" ------------------------------------------------------------- \n"); if ( - (abs(RefPose[1] - camPose[1]) <= Py_eps) && // Y - (abs(RefPose[2] - camPose[2]) <= Pz_eps) && // Z - (abs(RefPose[3] - camPose[3]) <= A_eps) // Yaw + (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 ! -----/*********---- \n "); keepMoving = false; - - }else + // to make sure that y & theta are within the threshold... + } else if (abs(RefPose[2] - camPose[2]) <= safety_margin_X) + { + if( + (abs(RefPose[1] - camPose[1]) > Py_eps) || + (abs(RefPose[3] - abs(camPose[3])) > A_eps) + ) + { + ROS_INFO_STREAM(" X < " << safety_margin_X << " m. , Fixing Y and theta. \n "); + Controller(RefPose[2], RefPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.01); + + } else if( + (abs(RefPose[1] - camPose[1]) <= Py_eps) && + (abs(RefPose[3] - abs(camPose[3])) <= A_eps) + ) + { + ROS_INFO("Y & THETA HAVE FIXED SUCCESSFULLY, MOVING STRAIGHT AHEAD ... \n"); + Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.01); + } + }else { Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.01); } @@ -571,7 +579,8 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl 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; @@ -606,6 +615,8 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl // differentiation diffYAW = ((curr_errorYAW - prev_errorYAW) / dt); + + // YAW offset... int yaw_offset = 15; if (curr_errorYAW < ((CV_PI/180) * yaw_offset) && curr_errorYAW > ((CV_PI/180) * -yaw_offset)) // -5 < err < +5 { @@ -614,6 +625,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl p_termYAW = S_Ang_P * curr_errorYAW; i_termYAW = S_Ang_I * int_errorYAW; d_termYAW = S_Ang_D * diffYAW; + ROS_INFO_STREAM("prop_gainYAW = " << p_termYAW << ", integ_gainYAW = " << i_termYAW << " . \n"); } else if (curr_errorYAW > ((CV_PI/180) * yaw_offset) || curr_errorYAW < ((CV_PI/180) * -yaw_offset)) // err > +5 or err < -5 { ROS_INFO_STREAM(" robot is OUTSIDE Reference boundry of " << yaw_offset << " deg. \n"); @@ -621,6 +633,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl p_termYAW = L_Ang_P * curr_errorYAW; i_termYAW = L_Ang_I * int_errorYAW; d_termYAW = L_Ang_D * diffYAW; + ROS_INFO_STREAM("prop_gainYAW = " << p_termYAW << ", integ_gainYAW = " << i_termYAW << " . \n"); } // control signal control_signalYAW = p_termYAW + i_termYAW + d_termYAW; diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ index 30278f8a3b40e3ed1e7ae6b6eb4b17aa0da15d9f..20e7ad16cf91ab2d43c8d1ab6ababeb1f297a86b 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ @@ -42,29 +42,25 @@ using namespace cv; using namespace aruco; using namespace std; - double ImageConverter::Pos_Px = .00005; -double ImageConverter::Pos_Ix = 0.002; +double ImageConverter::Pos_Ix = 0.0015; double ImageConverter::Pos_Dx = 0; +double ImageConverter::Pos_Py = 2.3 * ImageConverter::Pos_Px; +double ImageConverter::Pos_Iy = 2.3 * ImageConverter::Pos_Ix; +double ImageConverter::Pos_Dy = 2.3 * ImageConverter::Pos_Dx; -double ImageConverter::Pos_Py = .39 * ImageConverter::Pos_Px; -double ImageConverter::Pos_Iy = .39 * ImageConverter::Pos_Ix; -double ImageConverter::Pos_Dy = .39 * ImageConverter::Pos_Dx; - -/* -double ImageConverter::S_yPos_P = .16 * ImageConverter::Pos_Px; -double ImageConverter::S_yPos_I = .16 * ImageConverter::Pos_Ix; -double ImageConverter::S_yPos_D = .16 * ImageConverter::Pos_Dx; +double ImageConverter::S_yPos_P = 2.3 * ImageConverter::Pos_Px; +double ImageConverter::S_yPos_I = 2.3 * ImageConverter::Pos_Ix; +double ImageConverter::S_yPos_D = 2.3 * ImageConverter::Pos_Dx; -double ImageConverter::L_yPos_P = .3 * ImageConverter::Pos_Px; -double ImageConverter::L_yPos_I = .3 * ImageConverter::Pos_Ix; -double ImageConverter::L_yPos_D = .3 * ImageConverter::Pos_Dx; -*/ +double ImageConverter::L_yPos_P = 15 * ImageConverter::Pos_Px; +double ImageConverter::L_yPos_I = 15 * ImageConverter::Pos_Ix; +double ImageConverter::L_yPos_D = 15 * ImageConverter::Pos_Dx; -double ImageConverter::S_Ang_P = .065 * ImageConverter::Pos_Px; -double ImageConverter::S_Ang_I = .065 * ImageConverter::Pos_Ix; -double ImageConverter::S_Ang_D = .065 * ImageConverter::Pos_Dx; +double ImageConverter::S_Ang_P = .062 * ImageConverter::Pos_Px; +double ImageConverter::S_Ang_I = .062 * ImageConverter::Pos_Ix; +double ImageConverter::S_Ang_D = .062 * ImageConverter::Pos_Dx; double ImageConverter::L_Ang_P = .7 * ImageConverter::Pos_Px; double ImageConverter::L_Ang_I = .7 * ImageConverter::Pos_Ix; @@ -72,8 +68,8 @@ double ImageConverter::L_Ang_D = .7 * ImageConverter::Pos_Dx; float ImageConverter::TheMarkerSize = .1; // Default marker size -int ImageConverter::Thresh1_min = 20; -int ImageConverter::Thresh2_min = 20; +int ImageConverter::Thresh1_min = 10; +int ImageConverter::Thresh2_min = 10; int ImageConverter::Thresh1_max = 300; int ImageConverter::Thresh2_max = 300; @@ -120,8 +116,8 @@ double ImageConverter::control_signalYAW; double ImageConverter::zeroMax = .0000000000000000001; double ImageConverter::zeroMin = -.0000000000000000001; -double ImageConverter::Pz_eps = .005; -double ImageConverter::Py_eps = .007; +double ImageConverter::Pz_eps = .001; +double ImageConverter::Py_eps = .005; double ImageConverter::A_eps = (CV_PI/180) * 1; // 1 deg. @@ -140,6 +136,9 @@ double ImageConverter::A_eps = (CV_PI/180) * 1; // 1 deg. keepMoving = true; commandPub = nh_.advertise<geometry_msgs::Twist>("/base_controller/command",1); Sub = nh_.subscribe("/marker_pose",1,&ImageConverter::camCB,this); + + + } @@ -446,7 +445,7 @@ void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) { RefPose[0] = -.0957; -RefPose[1] = -0.0203153; +RefPose[1] = -0.0193642; RefPose[2] = 0.307922; RefPose[3] = 0.705028; @@ -516,7 +515,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl diffX = ((curr_errorX - prev_errorX) / dt); // scalling - p_termX = Pos_Px * curr_errorX; + p_termX = Pos_Px * curr_errorX; i_termX = Pos_Ix * int_errorX; d_termX = Pos_Dx * diffX; @@ -532,8 +531,8 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl control_signalX = 0; } // -----------------Y--------------------- // - - if(abs(RefY - MarPoseY) > Py_eps) + + if((RefY - MarPoseY) <= -Py_eps || (RefY - MarPoseY) >= Py_eps) { // e(t) = setpoint - actual value; curr_errorY = RefY - MarPoseY; @@ -555,31 +554,35 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl i_termY = Pos_Iy * int_errorY; d_termY = Pos_Dy * diffY; - /*int y_offset = 5; - if (curr_errorY < y_offset && curr_errorY > -y_offset) // -5 < err < +5 + /*double x_offset = .25; + if (abs(curr_errorY) >= abs(curr_errorX)) // -5 < err < +5 { - ROS_INFO_STREAM(" INSIDE boundry of " << y_offset << " m. \n"); + ROS_INFO_STREAM("robot movement in Y-direction is dominant! \n"); + control_signalX = 0; // scalling - p_termYAW = S_yPos_P * curr_errorY; - i_termYAW = S_yPos_I * int_errorY; - d_termYAW = S_yPos_D * diffY; - } else if (curr_errorY > y_offset || curr_errorYAW < y_offset) // err > +5 or err < -5 + p_termY = L_yPos_P * curr_errorY; + i_termY = L_yPos_I * int_errorY; + d_termY = L_yPos_D * diffY; + } else { - ROS_INFO_STREAM(" OUTSIDE boundry of " << y_offset << " m. \n"); + ROS_INFO_STREAM("robot movement in X-direction is dominant! \n"); // scalling - p_termYAW = L_yPos_P * curr_errorY; - i_termYAW = L_yPos_I * int_errorY; - d_termYAW = L_yPos_D * diffY; + p_termY = S_yPos_P * curr_errorY; + i_termY = S_yPos_I * int_errorY; + d_termY = S_yPos_D * diffY; }*/ // control signal control_signalY = p_termY + i_termY + d_termY; - - + + // robot & marker coordinates conversion + control_signalY = - control_signalY; + // save the current error as the previous one // for the next iteration. - prev_errorY = curr_errorY; - } else + prev_errorY = curr_errorY; + + }else if ((RefY - MarPoseY) < Py_eps && (RefY - MarPoseY) > -Py_eps) { control_signalY = 0; } @@ -644,7 +647,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); } @@ -692,4 +695,4 @@ void ImageConverter::move2docking(double VelX_est, double VelY_est, double omega ROS_INFO_STREAM(" Current ESTIMATED speed of robot: \n" << msg << ".\n"); } -// ---- Controller part ----------- END -------- +// ---- Controller part -------- END ------