diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 42ce8d4097e43359ee289a14ea731ef3c3b53add..bda465fbd9e44cb96f88861590cb3edaa7dd6b31 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -41,8 +41,8 @@
using namespace cv;
using namespace aruco;
using namespace std;
-double ImageConverter::Pos_Px = .05;
-double ImageConverter::Pos_Ix = 0.002;
+double ImageConverter::Pos_Px = .08;
+double ImageConverter::Pos_Ix = 0.0025;
double ImageConverter::Pos_Dx = 0;
double ImageConverter::Pos_Py = 5 * ImageConverter::Pos_Px;
@@ -474,7 +474,7 @@ camPose[3] = CamFB->pose.orientation.x;
)
{
- ROS_INFO("--------/*******//----- Dock is completed ! -----/*********---- \n ");
+ ROS_INFO("----/*****//----- Dock is completed successfully ! -----/******---- \n ");
keepMoving = false;
// to make sure that y & theta are within the threshold...
} else if (abs(RefPose[2] - camPose[2]) <= safety_margin_X)
@@ -492,7 +492,7 @@ camPose[3] = CamFB->pose.orientation.x;
(abs(RefPose[3] - abs(camPose[3])) <= A_eps)
)
{
- ROS_INFO("Y & THETA HAVE FIXED SUCCESSFULLY, MOVING STRAIGHT AHEAD ... \n");
+ ROS_INFO("y & theta have been fixed successfully, MOVING STRAIGHT AHEAD ... \n");
Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.01);
}
}else
@@ -625,7 +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");
+ //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");
@@ -633,7 +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");
+ //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 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;