From 015436a25f00511a8df3641e7ae5245c05d8a2e7 Mon Sep 17 00:00:00 2001
From: Farid Alijani <farid.alijani@student.lut.fi>
Date: Mon, 28 Mar 2016 22:09:07 +0200
Subject: [PATCH] y axis slowness has fixed
---
.../CamMark/camtomar/include/VisionControl.h | 4 +-
.../CamMark/camtomar/include/VisionControl.h~ | 16 +-
.../CamMark/camtomar/src/VisionControl.cpp | 64 ++---
.../CamMark/camtomar/src/VisionControl.cpp~ | 223 ++++++------------
4 files changed, 122 insertions(+), 185 deletions(-)
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index 8fa1d913..d92b9ae7 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -127,6 +127,8 @@ public:
static double S_Ang_P,S_Ang_I,S_Ang_D;
static double L_Ang_P,L_Ang_I,L_Ang_D;
+ static double Ang_P,Ang_I,Ang_D;
+
static double x_new,y_new,theta_new;
static double safety_margin_X;
@@ -166,7 +168,7 @@ public:
static int msec,sec,min,hr;
// ---- TIMER PARAMETERS ------ //
-
+ static double docking_counter;
static double dock_started,dock_finished,docking_duration;
static double zeroMin,zeroMax;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index bd7bc80d..8fa1d913 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
@@ -117,15 +117,17 @@ public:
void createTrackbars();
void myhandler(int value);
+
+ void GenerateRandomVal();
void RandomPose(double X_rand, double Y_rand, double theta_rand);
- double Pos_Px,Pos_Ix,Pos_Dx;
- double Pos_Py,Pos_Iy,Pos_Dy;
+ static double Pos_Px,Pos_Ix,Pos_Dx;
+ static double Pos_Py,Pos_Iy,Pos_Dy;
- double S_Ang_P,S_Ang_I,S_Ang_D;
- double L_Ang_P,L_Ang_I,L_Ang_D;
+ static double S_Ang_P,S_Ang_I,S_Ang_D;
+ static double L_Ang_P,L_Ang_I,L_Ang_D;
- double x_new,y_new,theta_new;
+ static double x_new,y_new,theta_new;
static double safety_margin_X;
double x_rand_SM;
@@ -161,13 +163,11 @@ public:
// ---- TIMER PARAMETERS ------ //
-
static int msec,sec,min,hr;
-
-
// ---- TIMER PARAMETERS ------ //
+ static double dock_started,dock_finished,docking_duration;
static double zeroMin,zeroMax;
static double Py_eps,Pz_eps,A_eps;
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]);
}
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index a950d516..78661510 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -47,12 +47,6 @@ using namespace aruco;
using namespace std;
-
-/*double ImageConverter::Pos_Px = .1;
-double ImageConverter::Pos_Ix = 0.0028;
-double ImageConverter::Pos_Dx = 0;*/
-
-
float ImageConverter::TheMarkerSize = .1; // Default marker size
int ImageConverter::Thresh1_min = 10;
@@ -102,17 +96,25 @@ double ImageConverter::d_termYAW = 0;
double ImageConverter::control_signalYAW;
// ---- CONTROLL PARAMETERS ------ //
+// ---------------- Initial Gains which dock the robot successfully ---------------- //
-// ---- TIMER PARAMETERS ------ //
+double ImageConverter::Pos_Px = .1;
+double ImageConverter::Pos_Ix = 0.0028;
+double ImageConverter::Pos_Dx = 0;
-int ImageConverter::msec = 0;
-int ImageConverter::sec = 0;
-int ImageConverter::min = 0;
-int ImageConverter::hr = 0;
+double ImageConverter::Pos_Py = 4 * Pos_Px;
+double ImageConverter::Pos_Iy = 4 * Pos_Ix;
+double ImageConverter::Pos_Dy = 4 * Pos_Dx;
+double ImageConverter::S_Ang_P = .2 * Pos_Px;
+double ImageConverter::S_Ang_I = .2 * Pos_Ix;
+double ImageConverter::S_Ang_D = .2 * Pos_Dx;
-// ---- TIMER PARAMETERS ------ //
+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::dock_started,ImageConverter::dock_finished,ImageConverter::docking_duration;
double ImageConverter::zeroMax = .0000000000000000001;
double ImageConverter::zeroMin = -.0000000000000000001;
@@ -122,19 +124,15 @@ double ImageConverter::Pz_eps = .001;
double ImageConverter::Py_eps = .002;
double ImageConverter::A_eps = (CV_PI/180) * .6; // 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 = .005;
+double ImageConverter::x_thresh_undock = .01;
double ImageConverter::y_thresh_undock = .01;
-double ImageConverter::theta_thresh_undock = (CV_PI/180) * 5;
-
+double ImageConverter::theta_thresh_undock = (CV_PI/180) * 3;
-
-double ImageConverter::rand_A_esp = (CV_PI/180) * 10; // 10 deg. threshold for random theta_pose
-
-double ImageConverter::rand_X_SM = .0005; // 0.5 mm threshold for random X_pose_SM
-
-double ImageConverter::safety_margin_X = .15; // 15 cm
+// random pose initialized
+double ImageConverter::x_new,ImageConverter::y_new,ImageConverter::theta_new;
ImageConverter::ImageConverter() :
it_(nh_)
@@ -144,39 +142,16 @@ double ImageConverter::safety_margin_X = .15; // 15 cm
/* initialize random seed: */
srand (time(NULL));
+
// Ref. Values
RefPose[0] = -.0957;
RefPose[1] = -0.0194805;
-RefPose[2] = 0.308654;
+RefPose[2] = 0.306654;
RefPose[3] = 0.702366;
-x_rand_SM = RefPose[2] + .35; // 35 cm safety margin in the x-axis while moving towards the random pose
-
-Pos_Px = ((double) rand() / (RAND_MAX)) * (.16 - .09) + .09; // .09 < Kp < .15
-Pos_Ix = ((double) rand() / (RAND_MAX)) * .01; // 0 < Ki < .01
-Pos_Dx = ((double) rand() / (RAND_MAX)) * .01; // 0 < Kd < .01
-
-x_new = ((double) rand() / (RAND_MAX)) * (.98 - x_rand_SM) + x_rand_SM;
-y_new = ((double) rand() / (RAND_MAX)) * (.4 - (-.4)) -.4;
-theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) - (.5*RefPose[3]);
-
-/* ----------------- Default Gains which dock the robot successfully ----------
-Pos_Px = .1;
-Pos_Ix = 0.0028;
-Pos_Dx = 0;*/
+x_rand_SM = RefPose[2] + .45; // 45 cm spreading domain in the x-axis while moving towards the random pose
-Pos_Py = 4 * Pos_Px;
-Pos_Iy = 4 * Pos_Ix;
-Pos_Dy = 4 * Pos_Dx;
-
-S_Ang_P = .2 * Pos_Px;
-S_Ang_I = .2 * Pos_Ix;
-S_Ang_D = .2 * Pos_Dx;
-
-L_Ang_P = .8 * Pos_Px;
-L_Ang_I = .8 * Pos_Ix;
-L_Ang_D = .8 * Pos_Dx;
//marSub = nh_.subscribe("/Marker_pose",1,&ImageConverter::marCB,this);
@@ -296,7 +271,7 @@ void ImageConverter::ProgStart(int argc,char** argv)
cerr<<"Could not open video!!"<<endl;
nh_.shutdown();
}
-
+ dock_started = ros::Time::now().toSec();
// Read first image to get the dimensions
TheVideoCapturer>>TheInputImage;
@@ -318,23 +293,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
int index=0;
// Capture until press ESC or until the end of the video
while ((key != 'x') && (key!=27) && TheVideoCapturer.grab() && ros::ok() && keepMoving){
-
- /*if(msec == 10)
- {
- ++sec;
- msec = 0;
- }
- if(sec == 60)
- {
- ++min;
- sec = 0;
- }
- if(min == 60)
- {
- ++hr;
- min = 0;
- }*/
-
ros::spinOnce();
if (TheVideoCapturer.retrieve(TheInputImage))
@@ -450,8 +408,6 @@ void ImageConverter::ProgStart(int argc,char** argv)
MarPose_pub.publish(msg);
ros::spinOnce();
-
- // -------------------------Removed----------------------------- //
}
/*// Print other rectangles that contains no valid markers
for (unsigned int i=0;i<MDetector.getCandidates().size();i++)
@@ -511,8 +467,6 @@ void ImageConverter::ContStart()
void ImageConverter::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB)
{
-
-
camPose[0] = CamFB->pose.position.x;
camPose[1] = CamFB->pose.position.y;
camPose[2] = CamFB->pose.position.z;
@@ -535,36 +489,38 @@ camPose[3] = CamFB->pose.orientation.x;
- ROS_INFO_STREAM(" --------------------- GAINS ------------------ \n");
- ROS_INFO_STREAM(" Kp = " << Pos_Px << " . \n");
- ROS_INFO_STREAM(" Ki = " << Pos_Ix << " . \n");
- ROS_INFO_STREAM(" Kd = " << Pos_Dx << " . \n");
+ ROS_INFO_STREAM(" ------------------------- GAINS ---------------------- \n");
+ ROS_INFO_STREAM(" Kp = " << Pos_Px << " , Ki = " << Pos_Ix << " , Kd = " << Pos_Dx << "\n");
ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n");
- ROS_INFO_STREAM(" Zmar = " << camPose[2] << " m. \n");
- ROS_INFO_STREAM(" Zref = " << RefPose[2] << " m. \n");
+ ROS_INFO_STREAM(" Xmar = " << camPose[2] << " m. \n");
+ ROS_INFO_STREAM(" Xref = " << RefPose[2] << " m. \n");
ROS_INFO_STREAM(" Ymar = " << camPose[1] << " m. \n");
ROS_INFO_STREAM(" Yref = " << RefPose[1] << " m. \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(" theta_rob = " << 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 ");
if(Go2RandomPose == false)
{
- ROS_INFO_STREAM("---------- MOVE TOWARDS THE DOCKING STATION ... --------- \n ");
+ ROS_INFO_STREAM("---------- MOVING TOWARDS THE DOCKING STATION ... --------- \n ");
if (
(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 successfully ! -----/******---- \n ");
+ //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");
//keepMoving = false;
+ GenerateRandomVal();
Go2RandomPose = true;
-
- // to make sure that y & theta are within the threshold...
+ // to make sure that y & theta are within the threshold...
} else if (abs(RefPose[2] - camPose[2]) <= safety_margin_X)
{
if(
@@ -589,20 +545,14 @@ camPose[3] = CamFB->pose.orientation.x;
}
} else
{
- ROS_INFO_STREAM("------------------- Moving to Random POSE ---------------------- \n ");
+ ROS_INFO(" moving towards Generated Random pose which is : \n");
RandomPose(x_new,y_new,theta_new);
}
-
-/*else if (Go2RandomPose == false)
- {
- ROS_INFO_STREAM("---------- MOVE TOWARDS THE DOCKING STATION ... --------- \n ");
- }*/
-
}
void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt)
{
- ROS_INFO_STREAM("---------- Controller started --------- \n ");
+ ROS_INFO_STREAM("--------------------- Controller started ----------------------\n ");
// -----------------X--------------------- //
if(abs(RefX - MarPoseX) > Pz_eps)
@@ -661,25 +611,6 @@ 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;
-
- /*double x_offset = .25;
- if (abs(curr_errorY) >= abs(curr_errorX)) // -5 < err < +5
- {
- ROS_INFO_STREAM("robot movement in Y-direction is dominant! \n");
- control_signalX = 0;
- // scalling
- p_termY = L_yPos_P * curr_errorY;
- i_termY = L_yPos_I * int_errorY;
- d_termY = L_yPos_D * diffY;
- } else
- {
- ROS_INFO_STREAM("robot movement in X-direction is dominant! \n");
- // scalling
- p_termY = S_yPos_P * curr_errorY;
- 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;
@@ -758,7 +689,7 @@ void ImageConverter::Controller(double RefX, double MarPoseX, double RefY, doubl
//ROS_INFO_STREAM("Control signalX = " << control_signalX <<" . \n");
//ROS_INFO_STREAM("RAW Control signalY = " << control_signalY << ". \n");
//ROS_INFO_STREAM("RAW Control signalYAW = "<< control_signalYAW <<". \n");
- ROS_INFO_STREAM(" ---------------- Controller ended --------------------- \n");
+ ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n");
dock(-control_signalX, control_signalY, control_signalYAW);
@@ -810,19 +741,25 @@ void ImageConverter::move2docking(double VelX_est, double VelY_est, double omega
}
// ---- Controller part -------- END ------
-void ImageConverter::RandomPose(double X_rand, double Y_rand, double theta_rand)
+void ImageConverter::GenerateRandomVal()
{
-/*x_new = ((double) rand() / (RAND_MAX)) * (.98 - RefPose[2]) + RefPose[2];
-y_new = ((double) rand() / (RAND_MAX)) * (.52 - (-.52)) -.52;
-theta_new = ((double) rand() / (RAND_MAX)) * (RefPose[3] - (-RefPose[3])) -RefPose[3];*/
-
-
-
+ // ---------------- PID gains ------------------
+ Pos_Px = ((double) rand() / (RAND_MAX)) * (.16 - .09) + .09; // .09 < Kp < .16
+ Pos_Ix = ((double) rand() / (RAND_MAX)) * .01; // 0 < Ki < .01
+ Pos_Dx = ((double) rand() / (RAND_MAX)) * .02; // 0 < Kd < .02
+
+ // ------------------ 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);
+ theta_new = ((double) rand() / (RAND_MAX)) * (.5*RefPose[3] - (-.5*RefPose[3])) + (-.5*RefPose[3]);
+}
- ROS_INFO_STREAM(" ---------------- new random pose ---------------- \n");
+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(" -------------------------------------------------------------- \n");
double vel_x,vel_y,omega_z;
@@ -831,10 +768,13 @@ geometry_msgs::Twist msg_new;
// Leaving docking station, moving towards x-axis SM
if (X_rand - camPose[2] > x_thresh_undock)
{
- ROS_INFO_STREAM(" Adjusting X_SM pose ... \n");
- //msg_new.linear.x = -.05;
- vel_x = -.05;
- } else
+ ROS_INFO_STREAM(" Adjusting X, moving backward ... \n");
+ vel_x = -.02;
+ } else if (X_rand - camPose[2] < -x_thresh_undock)
+ {
+ ROS_INFO_STREAM(" Adjusting X, moving forward ... \n");
+ vel_x = .02;
+ }else if (abs(X_rand - camPose[2]) <= x_thresh_undock)
{
ROS_INFO(" X-axis is fixed, adjusting Y & theta - axes ... \n");
if ((camPose[1] - Y_rand > y_thresh_undock) && (theta_rand > 0))
@@ -842,32 +782,33 @@ geometry_msgs::Twist msg_new;
if(abs(abs(camPose[3]) - abs(theta_rand)) > theta_thresh_undock)
{
ROS_INFO("moving 2 left side & CW rot. \n");
- vel_y = .05;
- omega_z = -.05;
+ vel_y = .03;
+ omega_z = -.01;
} else
{
ROS_INFO("CW rot. is fixed, only moving 2 left side ...\n");
- vel_y = .05;
+ vel_y = .03;
}
} else if ((camPose[1] - Y_rand < -y_thresh_undock) && (theta_rand < 0))
{
if(abs(abs(camPose[3]) - abs(theta_rand)) > theta_thresh_undock)
{
ROS_INFO("moving 2 right side & CCW rot. \n");
- vel_y = -.05;
- omega_z = .05;
+ vel_y = -.03;
+ omega_z = .01;
}else
{
ROS_INFO("CCW rot. is fixed, only moving 2 right side ... \n");
- vel_y = -.05;
+ vel_y = -.03;
}
}else if (abs(camPose[1] - Y_rand) <= y_thresh_undock)
{
ROS_INFO(" Y-axis is fixed, adjusting theta-axis ... ! \n");
if (abs(abs(camPose[3]) - abs(theta_rand)) <= theta_thresh_undock)
{
- ROS_INFO(" theta & Y-axes adjusted! \n");
- keepMoving = false;
+ ROS_INFO(" Robot is in a new random Pose! \n");
+ //keepMoving = false;
+ Go2RandomPose = false;
} else
{
if(theta_rand > 0)
@@ -885,24 +826,24 @@ geometry_msgs::Twist msg_new;
if(abs(abs(camPose[3]) - abs(theta_rand)) > theta_thresh_undock)
{
ROS_INFO("moving 2 left side & CCW rot., chance of losing marker \n");
- vel_y = .05;
+ vel_y = .03;
omega_z = .01;
} else
{
ROS_INFO("CCW rot. is fixed, only moving 2 left side ... \n");
- vel_y = .05;
+ vel_y = .03;
}
} else if ((camPose[1] - Y_rand < -y_thresh_undock) && (theta_rand > 0))
{
if(abs(abs(camPose[3]) - abs(theta_rand)) > theta_thresh_undock)
{
ROS_INFO("moving 2 right side & CW rot., chance of losing marker \n");
- vel_y = -.05;
+ vel_y = -.03;
omega_z = -.01;
} else
{
ROS_INFO("CW rot. is fixed, only moving 2 right side ... \n");
- vel_y = .05;
+ vel_y = -.03;
}
}/* else if ((Y_rand < 0) && (theta_rand < 0) && (camPose[1] - Y_rand > y_thresh_undock))
@@ -934,22 +875,6 @@ geometry_msgs::Twist msg_new;
msg_new.linear.y = vel_y;
msg_new.angular.z = omega_z;
- /*if(X_rand - camPose[2] > Pz_eps)
- {
- msg_new.linear.x = -.05;
-
- }else if (Y_rand > camPose[1] )
- {
- msg_new.linear.y = +.05;
- }else if (Y_rand < camPose[1])
- {
- msg_new.linear.y = -.05;
- } else if (X_rand - camPose[2] <= Pz_eps && abs(Y_rand - camPose[1]) <= Py_eps )
- {
- ROS_INFO("Random pose has achieved!\n");
- keepMoving = false;
- }*/
-
commandPub.publish(msg_new);
}
--
GitLab