diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp index c30473c2c114ddb863b300c90d5033a936f79d39..933df333ab0f9ae73fd04550d31cef12f83af6ad 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp @@ -103,7 +103,7 @@ double ImageConverter::Pos_Ix = 0.0028; double ImageConverter::Pos_Dx = 0; double ImageConverter::Pos_Py = 4 * Pos_Px; -double ImageConverter::Pos_Iy = .5 * Pos_Ix; +double ImageConverter::Pos_Iy = 1.5 * Pos_Ix; double ImageConverter::Pos_Dy = 4 * Pos_Dx; /*double ImageConverter::S_Ang_P = .2 * Pos_Px; @@ -125,13 +125,13 @@ double ImageConverter::zeroMin = -.0000000000000000001; // ------ offsets X, Y, theta for Docking --------- double ImageConverter::Pz_eps = .001; -double ImageConverter::Py_eps = .0015; +double ImageConverter::Py_eps = .002; double ImageConverter::A_eps = (CV_PI/180) * .5; // 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 = .01; +double ImageConverter::x_thresh_undock = .02; double ImageConverter::y_thresh_undock = .01; double ImageConverter::theta_thresh_undock = (CV_PI/180) * 3; @@ -151,11 +151,11 @@ double ImageConverter::docking_counter = 1; // Ref. Values RefPose[0] = -.0957; -RefPose[1] = -0.0196805; -RefPose[2] = 0.306154; -RefPose[3] = 0.702366; +RefPose[1] = -0.0196804; +RefPose[2] = 0.305155; +RefPose[3] = 0.703702; -x_rand_SM = RefPose[2] + .45; // 45 cm spreading domain in the x-axis while moving towards the random pose +x_rand_SM = RefPose[2] + .55; // 55 cm spreading domain in the x-axis while moving towards the random pose //marSub = nh_.subscribe("/Marker_pose",1,&ImageConverter::marCB,this); @@ -320,7 +320,7 @@ void ImageConverter::ProgStart(int argc,char** argv) { found = false; keepMoving = false; - ROS_INFO_STREAM("damn, marker is lost, but " << docking_counter<< " successful docking trials ... \n"); + ROS_INFO_STREAM("Marker is lost, successful docking trials : " << (docking_counter - 1) << "\n"); //RandomPose(x_new,y_new,theta_new); //move2docking(-control_signalX, -control_signalY, control_signalYAW); } @@ -494,13 +494,10 @@ camPose[3] = CamFB->pose.orientation.x; ROS_INFO_STREAM(" Kp = " << Pos_Px << " , Ki = " << Pos_Ix << " , Kd = " << Pos_Dx << "\n"); ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n"); - ROS_INFO_STREAM(" Xmar = " << camPose[2] << " m. \n"); - ROS_INFO_STREAM(" Xref = " << RefPose[2] << " m. \n"); + ROS_INFO_STREAM(" X_mar = " << camPose[2] << " vs X_ref = " << RefPose[2] << " \n"); + ROS_INFO_STREAM(" Y_mar = " << camPose[1] << " vs Y_ref = " << RefPose[1] << " \n"); - ROS_INFO_STREAM(" Ymar = " << camPose[1] << " m. \n"); - ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n"); - - ROS_INFO_STREAM(" theta_rob = " << camPose[3] << " rad. =~ " << (180/CV_PI) * camPose[3] << " deg. \n"); + ROS_INFO_STREAM(" theta_mar = " << 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 "); @@ -512,11 +509,10 @@ camPose[3] = CamFB->pose.orientation.x; //(abs(RefPose[1] - camPose[1]) <= Py_eps) && // Y //(abs(RefPose[3] - camPose[3]) <= A_eps) // Yaw ) - { - //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("docking No. " << docking_counter << " is finished in "<< docking_duration <<" sec, Moving 2 new Random Pose\n"); + ROS_INFO_STREAM("docking No. " << docking_counter << " is finished in "<< docking_duration <<" sec, moving to new Random Pose\n"); //keepMoving = false; GenerateRandomVal(); docking_counter ++; @@ -546,7 +542,7 @@ camPose[3] = CamFB->pose.orientation.x; } } else { - ROS_INFO(" moving towards Generated Random pose which is : \n"); + ROS_INFO(" Random pose : \n"); RandomPose(x_new,y_new,theta_new); } } @@ -630,7 +626,6 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl } // -------------------YAW--------------------------// - if(abs(RefYAW - abs(MarPoseYAW)) > A_eps) { // e(t) = setpoint - actual value; @@ -669,7 +664,6 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl //ROS_INFO_STREAM("prop_gainYAW = " << p_termYAW << ", integ_gainYAW = " << i_termYAW << " . \n"); }*/ - ROS_INFO_STREAM(" adjusting orientation! \n"); // scalling p_termYAW = Ang_P * curr_errorYAW; @@ -702,7 +696,6 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n"); dock(-control_signalX, control_signalY, control_signalYAW); - } void ImageConverter::dock(double VelX, double VelY, double omegaZ) @@ -759,16 +752,16 @@ void ImageConverter::GenerateRandomVal() Pos_Dx = ((double) rand() / (RAND_MAX)) * .02; // 0 < Kd < .01 // ------------------ Generating Random Pose ------------------ - x_new = ((double) rand() / (RAND_MAX)) * (1.05 - x_rand_SM) + x_rand_SM; - y_new = ((double) rand() / (RAND_MAX)) * (.5 - (-.24)) + (-.24); + x_new = ((double) rand() / (RAND_MAX)) * (1.1 - x_rand_SM) + x_rand_SM; + y_new = ((double) rand() / (RAND_MAX)) * (.48 - (-.24)) + (-.24); theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) + (-.5*RefPose[3]); } 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(" Xr = " << X_rand << ", Yr = " << Y_rand << ", Thetar = " << theta_rand << " rad ~ " << theta_rand * (180/CV_PI) << " deg\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; @@ -779,11 +772,11 @@ geometry_msgs::Twist msg_new; if (X_rand - camPose[2] > x_thresh_undock) { ROS_INFO_STREAM(" Adjusting X, moving backward ... \n"); - vel_x = -.02; + vel_x = -.04; } else if (X_rand - camPose[2] < -x_thresh_undock) { ROS_INFO_STREAM(" Adjusting X, moving forward ... \n"); - vel_x = .02; + vel_x = .04; }else if (abs(X_rand - camPose[2]) <= x_thresh_undock) { ROS_INFO(" X-axis is fixed, adjusting Y & theta - axes ... \n"); @@ -888,4 +881,3 @@ geometry_msgs::Twist msg_new; commandPub.publish(msg_new); } - diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ index 7866151018fa7c2cd8d9107b875eeaa8f4dce567..c30473c2c114ddb863b300c90d5033a936f79d39 100644 --- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ +++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ @@ -103,16 +103,20 @@ double ImageConverter::Pos_Ix = 0.0028; double ImageConverter::Pos_Dx = 0; double ImageConverter::Pos_Py = 4 * Pos_Px; -double ImageConverter::Pos_Iy = 4 * Pos_Ix; +double ImageConverter::Pos_Iy = .5 * Pos_Ix; double ImageConverter::Pos_Dy = 4 * Pos_Dx; -double ImageConverter::S_Ang_P = .2 * Pos_Px; +/*double ImageConverter::S_Ang_P = .2 * Pos_Px; double ImageConverter::S_Ang_I = .2 * Pos_Ix; double ImageConverter::S_Ang_D = .2 * Pos_Dx; 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::L_Ang_D = .8 * Pos_Dx;*/ + +double ImageConverter::Ang_P = .8 * Pos_Px; +double ImageConverter::Ang_I = .8 * Pos_Ix; +double ImageConverter::Ang_D = .8 * Pos_Dx; double ImageConverter::dock_started,ImageConverter::dock_finished,ImageConverter::docking_duration; @@ -121,8 +125,8 @@ double ImageConverter::zeroMin = -.0000000000000000001; // ------ 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. +double ImageConverter::Py_eps = .0015; +double ImageConverter::A_eps = (CV_PI/180) * .5; // 1 deg. double ImageConverter::safety_margin_X = .1; // safety margin X axis in docking process : 10 cm @@ -134,6 +138,7 @@ double ImageConverter::theta_thresh_undock = (CV_PI/180) * 3; // random pose initialized double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new; +double ImageConverter::docking_counter = 1; ImageConverter::ImageConverter() : it_(nh_) { @@ -146,13 +151,12 @@ double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new; // Ref. Values RefPose[0] = -.0957; -RefPose[1] = -0.0194805; -RefPose[2] = 0.306654; +RefPose[1] = -0.0196805; +RefPose[2] = 0.306154; RefPose[3] = 0.702366; x_rand_SM = RefPose[2] + .45; // 45 cm spreading domain in the x-axis while moving towards the random pose - //marSub = nh_.subscribe("/Marker_pose",1,&ImageConverter::marCB,this); // Publish pose message and buffer up to 100 messages @@ -246,7 +250,6 @@ bool ImageConverter::readArguments ( int argc,char **argv ) void ImageConverter::ProgStart(int argc,char** argv) { - // Show images, press "SPACE" to diable image // rendering to save CPU time @@ -317,7 +320,7 @@ void ImageConverter::ProgStart(int argc,char** argv) { found = false; keepMoving = false; - ROS_INFO("SORRY, BUT MARKER IS LOST, Starting again ... \n"); + ROS_INFO_STREAM("damn, marker is lost, but " << docking_counter<< " successful docking trials ... \n"); //RandomPose(x_new,y_new,theta_new); //move2docking(-control_signalX, -control_signalY, control_signalYAW); } @@ -487,9 +490,7 @@ 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("--------- PID gains in trial no. " << docking_counter << " : ---------\n"); ROS_INFO_STREAM(" Kp = " << Pos_Px << " , Ki = " << Pos_Ix << " , Kd = " << Pos_Dx << "\n"); ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n"); @@ -515,9 +516,10 @@ camPose[3] = CamFB->pose.orientation.x; //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"); + ROS_INFO_STREAM("docking No. " << docking_counter << " is finished in "<< docking_duration <<" sec, Moving 2 new Random Pose\n"); //keepMoving = false; GenerateRandomVal(); + docking_counter ++; Go2RandomPose = true; // to make sure that y & theta are within the threshold... @@ -527,16 +529,15 @@ camPose[3] = CamFB->pose.orientation.x; (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 "); + { + ROS_INFO_STREAM(" delta_X < " << safety_margin_X << " m. , Fixing Y or 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 been fixed successfully, MOVING STRAIGHT AHEAD ... \n"); + ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n"); Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.01); } }else @@ -590,7 +591,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl } // -----------------Y--------------------- // - if((RefY - MarPoseY) <= -Py_eps || (RefY - MarPoseY) >= Py_eps) + if((RefY - MarPoseY) < -Py_eps || (RefY - MarPoseY) > Py_eps) { // e(t) = setpoint - actual value; curr_errorY = RefY - MarPoseY; @@ -611,7 +612,8 @@ 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; - //ROS_INFO_STREAM("prop_gainY = " << p_termY << ", integ_gainY = " << i_termY << " . \n"); + + ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_termY<< " \n"); // control signal control_signalY = p_termY + i_termY + d_termY; @@ -622,7 +624,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl // for the next iteration. prev_errorY = curr_errorY; - }else if ((RefY - MarPoseY) < Py_eps && (RefY - MarPoseY) > -Py_eps) + }else if ((RefY - MarPoseY) <= Py_eps && (RefY - MarPoseY) >= -Py_eps) { control_signalY = 0; } @@ -647,7 +649,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl diffYAW = ((curr_errorYAW - prev_errorYAW) / dt); - // YAW offset... + /*// YAW offset... int yaw_offset = 15; if (curr_errorYAW < ((CV_PI/180) * yaw_offset) && curr_errorYAW > ((CV_PI/180) * -yaw_offset)) // -5 < err < +5 { @@ -665,8 +667,16 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl 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 + }*/ + + + ROS_INFO_STREAM(" adjusting orientation! \n"); + // scalling + p_termYAW = Ang_P * curr_errorYAW; + i_termYAW = Ang_I * int_errorYAW; + d_termYAW = Ang_D * diffYAW; + + // control signal control_signalYAW = p_termYAW + i_termYAW + d_termYAW; if(MarPoseYAW < 0) @@ -744,13 +754,13 @@ void ImageConverter::move2docking(double VelX_est, double VelY_est, double omega void ImageConverter::GenerateRandomVal() { // ---------------- PID gains ------------------ - Pos_Px = ((double) rand() / (RAND_MAX)) * (.16 - .09) + .09; // .09 < Kp < .16 + Pos_Px = ((double) rand() / (RAND_MAX)) * (.19 - .1) + .1; // .1 < Kp < .16 Pos_Ix = ((double) rand() / (RAND_MAX)) * .01; // 0 < Ki < .01 - Pos_Dx = ((double) rand() / (RAND_MAX)) * .02; // 0 < Kd < .02 + Pos_Dx = ((double) rand() / (RAND_MAX)) * .02; // 0 < Kd < .01 // ------------------ 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); + y_new = ((double) rand() / (RAND_MAX)) * (.5 - (-.24)) + (-.24); theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) + (-.5*RefPose[3]); }