From 48f0133170823aaa5a9ab64f549a0f57535a4683 Mon Sep 17 00:00:00 2001
From: Farid Alijani <farid.alijani@student.lut.fi>
Date: Thu, 31 Mar 2016 20:58:14 +0200
Subject: [PATCH] normal modification
---
.../CamMark/camtomar/src/VisionControl.cpp | 50 ++++++---------
.../CamMark/camtomar/src/VisionControl.cpp~ | 64 +++++++++++--------
2 files changed, 58 insertions(+), 56 deletions(-)
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index c30473c2..933df333 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 78661510..c30473c2 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]);
}
--
GitLab