diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 4f0cbb897dfeb536c9881d7f349b6effa0b11651..30278f8a3b40e3ed1e7ae6b6eb4b17aa0da15d9f 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -48,9 +48,9 @@ double ImageConverter::Pos_Ix = 0.002;
double ImageConverter::Pos_Dx = 0;
-double ImageConverter::Pos_Py = .37 * ImageConverter::Pos_Px;
-double ImageConverter::Pos_Iy = .37 * ImageConverter::Pos_Ix;
-double ImageConverter::Pos_Dy = .37 * 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;
@@ -62,13 +62,13 @@ double ImageConverter::L_yPos_I = .3 * ImageConverter::Pos_Ix;
double ImageConverter::L_yPos_D = .3 * ImageConverter::Pos_Dx;
*/
-double ImageConverter::S_Ang_P = .07 * ImageConverter::Pos_Px;
-double ImageConverter::S_Ang_I = .07 * ImageConverter::Pos_Ix;
-double ImageConverter::S_Ang_D = .07 * 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::L_Ang_P = .1 * ImageConverter::Pos_Px;
-double ImageConverter::L_Ang_I = .1 * ImageConverter::Pos_Ix;
-double ImageConverter::L_Ang_D = .1 * 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;
float ImageConverter::TheMarkerSize = .1; // Default marker size
@@ -122,7 +122,7 @@ double ImageConverter::zeroMax = .0000000000000000001;
double ImageConverter::zeroMin = -.0000000000000000001;
double ImageConverter::Pz_eps = .005;
double ImageConverter::Py_eps = .007;
-double ImageConverter::A_eps = .01; // ? deg.
+double ImageConverter::A_eps = (CV_PI/180) * 1; // 1 deg.
ImageConverter::ImageConverter() :
@@ -297,14 +297,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
}else
{
found = false;
-
- if(curr_errorYAW > RefPose[3])
- {
- move2docking(-control_signalX, -control_signalY, control_signalYAW); // CW (---|| \\)
- } else
- {
- move2docking(-control_signalX, -control_signalY, -control_signalYAW); // CCW (// || ---)
- }
+ move2docking(-control_signalX, -control_signalY, control_signalYAW);
}
if (ros::ok() && found)
@@ -455,7 +448,7 @@ void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
RefPose[0] = -.0957;
RefPose[1] = -0.0203153;
RefPose[2] = 0.307922;
-RefPose[3] = -0.705028;
+RefPose[3] = 0.705028;
camPose[0] = CamFB->pose.position.x;
camPose[1] = CamFB->pose.position.y;
@@ -480,8 +473,8 @@ camPose[3] = CamFB->pose.orientation.x;
ROS_INFO_STREAM(" Ymar = " << camPose[1] << " m. \n");
ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n");
- ROS_INFO_STREAM(" rollmar = " << camPose[3] << " rad. \n");
- ROS_INFO_STREAM(" rollref = " << RefPose[3] << " rad. \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(" ------------------------------------------------------------- \n");
@@ -593,11 +586,11 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
// -------------------YAW--------------------------//
- if(abs(RefYAW - MarPoseYAW) > A_eps)
+ if(abs(RefYAW - abs(MarPoseYAW)) > A_eps)
{
// e(t) = setpoint - actual value;
curr_errorYAW = RefYAW - MarPoseYAW;
-
+
// Integrated error
int_errorYAW += curr_errorYAW * dt;
/*
@@ -610,17 +603,17 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
// differentiation
diffYAW = ((curr_errorYAW - prev_errorYAW) / dt);
- int yaw_offset = 5;
+ int yaw_offset = 15;
if (curr_errorYAW < ((CV_PI/180) * yaw_offset) && curr_errorYAW > ((CV_PI/180) * -yaw_offset)) // -5 < err < +5
{
- ROS_INFO_STREAM(" INSIDE boundry of " << yaw_offset << " deg. \n");
+ ROS_INFO_STREAM(" robot is INSIDE Reference boundry of " << yaw_offset << " deg. \n");
// scalling
p_termYAW = S_Ang_P * curr_errorYAW;
i_termYAW = S_Ang_I * int_errorYAW;
d_termYAW = S_Ang_D * diffYAW;
} else if (curr_errorYAW > ((CV_PI/180) * yaw_offset) || curr_errorYAW < ((CV_PI/180) * -yaw_offset)) // err > +5 or err < -5
{
- ROS_INFO_STREAM(" OUTSIDE boundry of " << yaw_offset << " deg. \n");
+ ROS_INFO_STREAM(" robot is OUTSIDE Reference boundry of " << yaw_offset << " deg. \n");
// scalling
p_termYAW = L_Ang_P * curr_errorYAW;
i_termYAW = L_Ang_I * int_errorYAW;
@@ -628,7 +621,15 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
}
// control signal
control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
-
+
+ if(MarPoseYAW < 0)
+ {
+ ROS_INFO("Marker current orientation < 0, CW rotation. \n");
+ control_signalYAW = - control_signalYAW;
+ } else
+ {
+ ROS_INFO("Marker current orientation > 0, CCW rotation. \n");
+ }
// save the current error as the previous one
// for the next iteration.
@@ -643,13 +644,8 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
//ROS_INFO_STREAM("RAW Control signalYAW = "<< control_signalYAW <<". \n");
ROS_INFO_STREAM(" ------------------------------------------------------------- \n");
- if(curr_errorYAW > RefYAW)
- {
- dock(-control_signalX, -control_signalY, control_signalYAW); // CW (---|| \\)
- } else
- {
- dock(-control_signalX, -control_signalY, -control_signalYAW); // CCW (// || ---)
- }
+ dock(-control_signalX, -control_signalY, control_signalYAW);
+
}
void ImageConverter::dock(double VelX, double VelY, double omegaZ)
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 786e01e4e1875dc28691c55ccc4b5d019a2c6daa..30278f8a3b40e3ed1e7ae6b6eb4b17aa0da15d9f 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -42,23 +42,35 @@ using namespace cv;
using namespace aruco;
using namespace std;
-double ImageConverter::Pos_Px = .0001;
+
+double ImageConverter::Pos_Px = .00005;
double ImageConverter::Pos_Ix = 0.002;
double ImageConverter::Pos_Dx = 0;
-double ImageConverter::Pos_Py = .25 * ImageConverter::Pos_Px;
-double ImageConverter::Pos_Iy = .25 * ImageConverter::Pos_Ix;
-double ImageConverter::Pos_Dy = .25 * ImageConverter::Pos_Dx;
-double ImageConverter::S_Ang_P = 0.05 * ImageConverter::Pos_Px;
-double ImageConverter::S_Ang_I = 0.05 * ImageConverter::Pos_Ix;
-double ImageConverter::S_Ang_D = 0.05 * 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::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::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::L_Ang_P = 0.9 * ImageConverter::Pos_Px;
-double ImageConverter::L_Ang_I = 0.9 * ImageConverter::Pos_Ix;
-double ImageConverter::L_Ang_D = 0.9 * 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;
-float ImageConverter::TheMarkerSize = -1;
+float ImageConverter::TheMarkerSize = .1; // Default marker size
int ImageConverter::Thresh1_min = 20;
int ImageConverter::Thresh2_min = 20;
@@ -108,9 +120,9 @@ double ImageConverter::control_signalYAW;
double ImageConverter::zeroMax = .0000000000000000001;
double ImageConverter::zeroMin = -.0000000000000000001;
-double ImageConverter::Pz_eps = .01;
-double ImageConverter::Py_eps = .001;
-double ImageConverter::A_eps = .02; // ? deg.
+double ImageConverter::Pz_eps = .005;
+double ImageConverter::Py_eps = .007;
+double ImageConverter::A_eps = (CV_PI/180) * 1; // 1 deg.
ImageConverter::ImageConverter() :
@@ -255,7 +267,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
MDetector.pyrDown(ThePyrDownLevel);
}
-
MDetector.setCornerRefinementMethod(MarkerDetector::LINES);
char key=0;
@@ -286,14 +297,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
}else
{
found = false;
-
- if(curr_errorYAW > RefPose[3])
- {
- move2docking(-control_signalX, -control_signalY, control_signalYAW); // CW (---|| \\)
- } else
- {
- move2docking(-control_signalX, -control_signalY, -control_signalYAW); // CCW (// || ---)
- }
+ move2docking(-control_signalX, -control_signalY, control_signalYAW);
}
if (ros::ok() && found)
@@ -385,17 +389,24 @@ void ImageConverter::ProgStart(int argc,char** argv)
// -------------------------Removed----------------------------- //
}
- // Print other rectangles that contains no valid markers
+ /*// Print other rectangles that contains no valid markers
for (unsigned int i=0;i<MDetector.getCandidates().size();i++)
{
Marker m( MDetector.getCandidates()[i],10);
- m.draw(TheInputImageCopy,cv::Scalar(0,255,0),2);
- }
+ m.draw(TheInputImageCopy,Scalar(0,255,0),2);
+ }*/
for (unsigned int i=0;i<TheMarkers.size();i++)
{
+ int currentMarID = TheMarkers[i].id;
+ TheMarkers[i].draw(TheInputImageCopy,Scalar(0,255,0),2) ;
+
CvDrawingUtils::draw3dCube(TheInputImageCopy,TheMarkers[i],TheCameraParameters);
CvDrawingUtils::draw3dAxis(TheInputImageCopy,TheMarkers[i],TheCameraParameters);
+
+ //Marker ID to string
+ stringstream marker_id_string;
+ marker_id_string << "marker_ " << currentMarID;
}
if (update_images)
@@ -435,9 +446,9 @@ void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
{
RefPose[0] = -.0957;
-RefPose[1] = -0.0965533;
-RefPose[2] = 0.371;
-RefPose[3] = -0.69;
+RefPose[1] = -0.0203153;
+RefPose[2] = 0.307922;
+RefPose[3] = 0.705028;
camPose[0] = CamFB->pose.position.x;
camPose[1] = CamFB->pose.position.y;
@@ -462,8 +473,8 @@ camPose[3] = CamFB->pose.orientation.x;
ROS_INFO_STREAM(" Ymar = " << camPose[1] << " m. \n");
ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \n");
- ROS_INFO_STREAM(" rollmar = " << camPose[3] << " rad. \n");
- ROS_INFO_STREAM(" rollref = " << RefPose[3] << " rad. \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(" ------------------------------------------------------------- \n");
@@ -543,7 +554,24 @@ 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;
-
+
+ /*int y_offset = 5;
+ if (curr_errorY < y_offset && curr_errorY > -y_offset) // -5 < err < +5
+ {
+ ROS_INFO_STREAM(" INSIDE boundry of " << y_offset << " m. \n");
+ // 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
+ {
+ ROS_INFO_STREAM(" OUTSIDE boundry of " << y_offset << " m. \n");
+ // scalling
+ p_termYAW = L_yPos_P * curr_errorY;
+ i_termYAW = L_yPos_I * int_errorY;
+ d_termYAW = L_yPos_D * diffY;
+ }*/
+
// control signal
control_signalY = p_termY + i_termY + d_termY;
@@ -558,11 +586,11 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
// -------------------YAW--------------------------//
- if(abs(RefYAW - MarPoseYAW) > A_eps)
+ if(abs(RefYAW - abs(MarPoseYAW)) > A_eps)
{
// e(t) = setpoint - actual value;
curr_errorYAW = RefYAW - MarPoseYAW;
-
+
// Integrated error
int_errorYAW += curr_errorYAW * dt;
/*
@@ -575,16 +603,17 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
// differentiation
diffYAW = ((curr_errorYAW - prev_errorYAW) / dt);
- if (curr_errorYAW < .08726 && curr_errorYAW > -.08726) // -5 < err < +5
+ int yaw_offset = 15;
+ if (curr_errorYAW < ((CV_PI/180) * yaw_offset) && curr_errorYAW > ((CV_PI/180) * -yaw_offset)) // -5 < err < +5
{
- ROS_INFO(" -5 < err < +5 \n");
+ ROS_INFO_STREAM(" robot is INSIDE Reference boundry of " << yaw_offset << " deg. \n");
// scalling
p_termYAW = S_Ang_P * curr_errorYAW;
i_termYAW = S_Ang_I * int_errorYAW;
d_termYAW = S_Ang_D * diffYAW;
- } else if (curr_errorYAW > .08726 || curr_errorYAW < -.08726) // err > +5 or err < -5
+ } else if (curr_errorYAW > ((CV_PI/180) * yaw_offset) || curr_errorYAW < ((CV_PI/180) * -yaw_offset)) // err > +5 or err < -5
{
- ROS_INFO(" err > +5 or err < -5 \n");
+ ROS_INFO_STREAM(" robot is OUTSIDE Reference boundry of " << yaw_offset << " deg. \n");
// scalling
p_termYAW = L_Ang_P * curr_errorYAW;
i_termYAW = L_Ang_I * int_errorYAW;
@@ -592,7 +621,15 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
}
// control signal
control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
-
+
+ if(MarPoseYAW < 0)
+ {
+ ROS_INFO("Marker current orientation < 0, CW rotation. \n");
+ control_signalYAW = - control_signalYAW;
+ } else
+ {
+ ROS_INFO("Marker current orientation > 0, CCW rotation. \n");
+ }
// save the current error as the previous one
// for the next iteration.
@@ -607,13 +644,8 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
//ROS_INFO_STREAM("RAW Control signalYAW = "<< control_signalYAW <<". \n");
ROS_INFO_STREAM(" ------------------------------------------------------------- \n");
- if(curr_errorYAW > RefYAW)
- {
- dock(-control_signalX, -control_signalY, control_signalYAW); // CW (---|| \\)
- } else
- {
- dock(-control_signalX, -control_signalY, -control_signalYAW); // CCW (// || ---)
- }
+ dock(-control_signalX, -control_signalY, control_signalYAW);
+
}
void ImageConverter::dock(double VelX, double VelY, double omegaZ)