From 2ebf56f813d5be78436c9f6492f471e2ee9cb71b Mon Sep 17 00:00:00 2001
From: Farid Alijani <farid.alijani@student.lut.fi>
Date: Wed, 18 May 2016 21:26:55 +0200
Subject: [PATCH] RL and PID modifed
---
.../CamMark/camtomar/include/VisionControl.h | 4 +-
.../CamMark/camtomar/include/VisionControl.h~ | 1 +
.../CamMark/camtomar/src/VisionControl.cpp | 81 ++---
.../CamMark/camtomar/src/VisionControl.cpp~ | 339 ++++--------------
.../include/RL_Docking.h | 6 +-
.../include/RL_Docking.h~ | 7 +-
.../src/RL_Docking.cpp | 48 +--
.../src/RL_Docking.cpp~ | 88 +++--
8 files changed, 205 insertions(+), 369 deletions(-)
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
index 48e600a4..50b8e493 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h
@@ -117,7 +117,7 @@ public:
void myhandler(int value);
void GenerateRandomVal();
- void RandomPose(double X_rand, double Y_rand, double theta_rand);
+ void Undocking(double X_rand, double Y_rand, double theta_rand);
void ContStart();
@@ -169,7 +169,7 @@ public:
static double TT_S,TT_E;
static double zeroMin,zeroMax;
- static double Py_eps,Pz_eps,A_eps;
+ static double y_thresh_dock,x_thresh_dock,theta_thresh_dock;
static double x_thresh_undock,y_thresh_undock,theta_thresh_undock;
static double rand_X_SM,rand_A_esp,rand_Y_esp;
static double speed_reducer_X,speed_reducer_Y,speed_reducer_theta;
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
index 79669717..48e600a4 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/include/VisionControl.h~
@@ -178,3 +178,4 @@ public:
double camPose[6];
static const double RefPose[4];
+};
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
index 94c0e546..9b38fce2 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
@@ -96,16 +96,16 @@ double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::
// ---- Ref. Values for Logitech Camera ---- //
-const double PID4Docking::RefPose[4] = {-.0957, -0.0295876 /* Y_ref*/ , 0.218695 /* X_ref*/ , -0.637745 /* theta_ref*/};
+const double PID4Docking::RefPose[4] = {-.0957, -0.030321 /* Y_ref*/ , 0.219427 /* X_ref*/ , -0.635269 /* theta_ref*/};
// ---------------- PID gains---------------- //
-double PID4Docking::Kp_y = .7; //.7
-double PID4Docking::Ki_y = .001;//.005
-double PID4Docking::Kd_y = .2;
+double PID4Docking::Kp_y = .65; //.7
+double PID4Docking::Ki_y = .001;//.001
+double PID4Docking::Kd_y = .2; // .2
-double PID4Docking::Kp_theta = .2 * Kp_y;
+double PID4Docking::Kp_theta = .15 * Kp_y; // .2 * KP_y
double PID4Docking::Ki_theta = .6 * Ki_y; // .07 * Ki_y
-double PID4Docking::Kd_theta = .00005 * Kd_y;
+double PID4Docking::Kd_theta = .00005 * Kd_y; // .00005 * Kd_y
// ---------------- PID gains---------------- //
@@ -121,26 +121,23 @@ double PID4Docking::x_new,PID4Docking::y_new,PID4Docking::theta_new;
double PID4Docking::dock_started,PID4Docking::dock_finished,PID4Docking::docking_duration;
-double PID4Docking::zeroMax = .0000000000000000001;
-double PID4Docking::zeroMin = -.0000000000000000001;
double PID4Docking::speed_reducer_X = 1;
double PID4Docking::speed_reducer_Y = 1;
double PID4Docking::speed_reducer_theta = 1;
// ------ offsets X, Y, theta for Docking ---------
-double PID4Docking::Pz_eps = .001;
-double PID4Docking::Py_eps = .002;
-double PID4Docking::A_eps = (CV_PI/180) * 1; // 1 deg.
+double PID4Docking::x_thresh_dock = .001;
+double PID4Docking::y_thresh_dock = .002;
+double PID4Docking::theta_thresh_dock = (CV_PI/180) * 1; // 1 deg.
-double PID4Docking::safety_margin_X = .15; // safety margin X axis in docking process : 18 cm
+double PID4Docking::safety_margin_X = .135; // safety margin X axis in docking process : 13.5 cm
// ------ offsets X, Y, theta for Undocking ---------
double PID4Docking::x_thresh_undock = .02;
double PID4Docking::y_thresh_undock = .015;
double PID4Docking::theta_thresh_undock = (CV_PI/180) * 3;
-
double PID4Docking::docking_counter = 1;
PID4Docking::PID4Docking()
@@ -320,8 +317,6 @@ if (TheVideoCapturer.retrieve(TheInputImage))
found = false;
keepMoving = false;
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);
}
if (node_vis.ok() && found)
@@ -426,13 +421,11 @@ if (TheVideoCapturer.retrieve(TheInputImage))
key=cv::waitKey(30);
// If space is hit, don't render the image.
-if (key == ' ')
-{
- update_images = !update_images;
-}
-
+ if (key == ' ')
+ {
+ update_images = !update_images;
+ }
ros::spinOnce();
-
TT_E = ros::Time::now().toSec();
ROS_INFO_STREAM(" visualization while loop duration = " << (TT_E - TT_S) <<" \n");
@@ -470,9 +463,9 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
{
ROS_INFO_STREAM("---------- MOVING TOWARDS DOCKING PLATFORM --------- \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
+ (abs(RefPose[2] - camPose[2]) <= x_thresh_dock) //&& // Z
+ //(abs(RefPose[1] - camPose[1]) <= y_thresh_dock) && // Y
+ //(abs(RefPose[3] - camPose[3]) <= theta_thresh_dock) // Yaw
)
{
dock_finished = ros::Time::now().toSec();
@@ -489,19 +482,15 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
// 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(abs(RefPose[3]) - abs(camPose[3])) > A_eps)
- )
- {
- ROS_INFO_STREAM(" delta_X < " << safety_margin_X << " m., Fixing Y or theta. \n ");
- speed_reducer_Y = 1;
- speed_reducer_theta = 1;
+ ROS_INFO_STREAM(" delta_X < " << safety_margin_X << " m., Fixing Y or theta. \n ");
+
+ if((abs(RefPose[1] - camPose[1]) > y_thresh_dock) || (abs(abs(RefPose[3]) - abs(camPose[3])) > theta_thresh_dock))
+ {
+ speed_reducer_Y = 1;
+ speed_reducer_theta = 1;
Controller(RefPose[2], RefPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1);
- } else if(
- (abs(RefPose[1] - camPose[1]) <= Py_eps) &&
- (abs(abs(RefPose[3]) - abs(camPose[3])) <= A_eps)
- )
+
+ } else if((abs(RefPose[1] - camPose[1]) <= y_thresh_dock) && (abs(abs(RefPose[3]) - abs(camPose[3])) <= theta_thresh_dock))
{
ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n");
speed_reducer_X = .08;
@@ -514,7 +503,7 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
} else
{
ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ---------\n");
- RandomPose(x_new,y_new,theta_new);
+ Undocking(x_new,y_new,theta_new);
}
}
@@ -523,7 +512,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
ROS_INFO_STREAM("--------------------- Controller started ----------------------\n ");
// -----------------X--------------------- //
- if(abs(RefX - MarPoseX) > Pz_eps)
+ if(abs(RefX - MarPoseX) > x_thresh_dock)
{
/*// e(t) = setpoint - actual value;
curr_errorX = RefX - MarPoseX;
@@ -546,7 +535,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
}
// -----------------Y--------------------- //
- if((RefY - MarPoseY) < -Py_eps || (RefY - MarPoseY) > Py_eps)
+ if((RefY - MarPoseY) < -y_thresh_dock || (RefY - MarPoseY) > y_thresh_dock)
{
// e(t) = setpoint - actual value;
curr_errorY = RefY - MarPoseY;
@@ -577,17 +566,17 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
prev_errorY = curr_errorY;
- }else if ((RefY - MarPoseY) <= Py_eps && (RefY - MarPoseY) >= -Py_eps)
+ }else if ((RefY - MarPoseY) <= y_thresh_dock && (RefY - MarPoseY) >= -y_thresh_dock)
{
control_signalY = 0;
}
// -------------------YAW--------------------------//
- if(abs(abs(RefYAW) - abs(MarPoseYAW)) > A_eps)
+ if(abs(abs(RefYAW) - abs(MarPoseYAW)) > theta_thresh_dock)
{
// e(t) = setpoint - actual value;
curr_errorYAW = RefYAW - MarPoseYAW;
-
+
// Integrated error
int_errorYAW += curr_errorYAW * dt;
@@ -604,6 +593,7 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
// control signal
control_signalYAW = p_termYAW + i_termYAW + d_termYAW;
//control_signalYAW = - control_signalYAW;
+
if(MarPoseYAW > 0)
{
ROS_INFO("Marker current orientation > 0, CCW rotation. \n");
@@ -621,13 +611,12 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
{
control_signalYAW = 0;
}
-
//ROS_INFO_STREAM("Control signalX = " << control_signalX <<" . \n");
//ROS_INFO_STREAM("Control signalY = " << control_signalY << ". \n");
//ROS_INFO_STREAM("Control signalYAW = "<< control_signalYAW <<". \n");
ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n");
- dock(control_signalX, control_signalY, control_signalYAW);
+ //dock(control_signalX, control_signalY, control_signalYAW);
}
void PID4Docking::dock(double VelX, double VelY, double omegaZ)
@@ -641,7 +630,7 @@ void PID4Docking::dock(double VelX, double VelY, double omegaZ)
commandPub.publish(msg);
- ROS_INFO_STREAM(" Current speed of robot: \n" << msg << ".\n");
+ ROS_INFO_STREAM(" Robot speed : \n" << msg);
}
void PID4Docking::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
@@ -691,7 +680,7 @@ void PID4Docking::GenerateRandomVal()
theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - theta_dwn) + theta_dwn; // will be used for Q_Learning
}
-void PID4Docking::RandomPose(double X_rand, double Y_rand, double theta_rand)
+void PID4Docking::Undocking(double X_rand, double Y_rand, double theta_rand)
{
ROS_INFO_STREAM(" Xr = " << X_rand << ", Yr = " << Y_rand << ", Thetar = " << theta_rand << " rad ~ " << theta_rand * (180/CV_PI) << " deg\n");
ROS_INFO_STREAM(" -------------------------------------------------------------- \n");
diff --git a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
index 34650155..94c0e546 100644
--- a/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
+++ b/MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp~
@@ -42,140 +42,113 @@
#include<VisionControl.h>
+
+
using namespace cv;
using namespace aruco;
using namespace std;
-float VisCont::TheMarkerSize = .1; // Default marker size
+float PID4Docking::TheMarkerSize = .1; // Default marker size
-int VisCont::Thresh1_min = 10;
-int VisCont::Thresh2_min = 10;
+int PID4Docking::Thresh1_min = 10;
+int PID4Docking::Thresh2_min = 10;
-int VisCont::Thresh1_max = 300;
-int VisCont::Thresh2_max = 300;
+int PID4Docking::Thresh1_max = 300;
+int PID4Docking::Thresh2_max = 300;
-const string VisCont::trackbarWindowName = "Trackbars";
+const string PID4Docking::trackbarWindowName = "Trackbars";
-int VisCont::ThePyrDownLevel = 0;
+int PID4Docking::ThePyrDownLevel = 0;
-bool VisCont::update_images = true;
+bool PID4Docking::update_images = true;
-bool VisCont::found = false;
-bool VisCont::Go2RandomPose = false;
+bool PID4Docking::found = false;
+bool PID4Docking::Go2RandomPose = false;
-int VisCont::ThresParam1 = 0;
-int VisCont::ThresParam2 = 0;
+int PID4Docking::ThresParam1 = 0;
+int PID4Docking::ThresParam2 = 0;
// ---- CONTROLL PARAMETERS ------ //
-double VisCont::prev_errorX, VisCont::curr_errorX, VisCont::int_errorX, VisCont::diffX;
+double PID4Docking::prev_errorX, PID4Docking::curr_errorX, PID4Docking::int_errorX, PID4Docking::diffX;
-double VisCont::p_termX = 0;
-double VisCont::i_termX = 0;
-double VisCont::d_termX = 0;
+double PID4Docking::p_termX = 0;
+double PID4Docking::i_termX = 0;
+double PID4Docking::d_termX = 0;
-double VisCont::prev_errorY, VisCont::curr_errorY, VisCont::int_errorY, VisCont::diffY;
+double PID4Docking::prev_errorY, PID4Docking::curr_errorY, PID4Docking::int_errorY, PID4Docking::diffY;
-double VisCont::p_termY = 0;
-double VisCont::i_termY = 0;
-double VisCont::d_termY = 0;
+double PID4Docking::p_termY = 0;
+double PID4Docking::i_termY = 0;
+double PID4Docking::d_termY = 0;
-double VisCont::prev_errorYAW,VisCont::curr_errorYAW,VisCont::int_errorYAW,VisCont::diffYAW;
+double PID4Docking::prev_errorYAW,PID4Docking::curr_errorYAW,PID4Docking::int_errorYAW,PID4Docking::diffYAW;
-double VisCont::p_termYAW = 0;
-double VisCont::i_termYAW = 0;
-double VisCont::d_termYAW = 0;
+double PID4Docking::p_termYAW = 0;
+double PID4Docking::i_termYAW = 0;
+double PID4Docking::d_termYAW = 0;
-double VisCont::control_signalX, VisCont::control_signalY, VisCont::control_signalYAW;
+double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::control_signalYAW;
// ---- CONTROLL PARAMETERS ------ //
// ---- Ref. Values for Android Camera ---- //
-//const double VisCont::RefPose[4] = {-.0957, 0.00638817 /* Y_ref*/ , 0.308857 /* X_ref*/ , 0.17 /* theta_ref*/};
+//const double PID4Docking::RefPose[4] = {-.0957, 0.00638817 /* Y_ref*/ , 0.308857 /* X_ref*/ , 0.17 /* theta_ref*/};
// ---- Ref. Values for Logitech Camera ---- //
-const double VisCont::RefPose[4] = {-.0957, -0.0271482 /* Y_ref*/ , 0.266214 /* X_ref*/ , -0.535344 /* theta_ref*/};
-
+const double PID4Docking::RefPose[4] = {-.0957, -0.0295876 /* Y_ref*/ , 0.218695 /* X_ref*/ , -0.637745 /* theta_ref*/};
// ---------------- PID gains---------------- //
-double VisCont::Kp_y = .7; //.7
-double VisCont::Ki_y = .005;//.005
-double VisCont::Kd_y = .2;
+double PID4Docking::Kp_y = .7; //.7
+double PID4Docking::Ki_y = .001;//.005
+double PID4Docking::Kd_y = .2;
-double VisCont::Kp_theta = .1 * Kp_y; // .08 * Kp_y
-double VisCont::Ki_theta = .07 * Ki_y;
-double VisCont::Kd_theta = .00005 * Kd_y;
+double PID4Docking::Kp_theta = .2 * Kp_y;
+double PID4Docking::Ki_theta = .6 * Ki_y; // .07 * Ki_y
+double PID4Docking::Kd_theta = .00005 * Kd_y;
// ---------------- PID gains---------------- //
-double VisCont::TT_S,VisCont::TT_E;
+double PID4Docking::TT_S,PID4Docking::TT_E;
// random pose initialized
-const double VisCont::y_up = .3;
-const double VisCont::y_dwn = -.1;
-
-const double VisCont::theta_dwn = -.7 /*-RefPose[3]*/;
-const double VisCont::theta_up = .7 /*RefPose[3]*/;
-
-double VisCont::x_new,VisCont::y_new,VisCont::theta_new;
-
-// ---------------- Q_LEARNING PARAMETERS ---------------- //
-
-double VisCont::gamma = .8;
-double VisCont::alpha = .1;
-double VisCont::R_step = 200;
+const double PID4Docking::y_up = .3;
+const double PID4Docking::y_dwn = -.1;
-const int VisCont::row = 5/*(y_up - y_dwn) * 10000*/;
-const int VisCont::col = 5/*(theta_up - theta_dwn) * 1000*/;
+const double PID4Docking::theta_dwn = -.7 /*-RefPose[3]*/;
+const double PID4Docking::theta_up = .7 /*RefPose[3]*/;
-double VisCont::Q[7][7] = {0};
-double VisCont::R[7][7] = {0};
+double PID4Docking::x_new,PID4Docking::y_new,PID4Docking::theta_new;
-double VisCont::Q_next_state;
-int VisCont::counter = 1;
-int VisCont::Time_Reward;
-double VisCont::sample;
+double PID4Docking::dock_started,PID4Docking::dock_finished,PID4Docking::docking_duration;
+double PID4Docking::zeroMax = .0000000000000000001;
+double PID4Docking::zeroMin = -.0000000000000000001;
-int VisCont::R_indx_i = 0;
-int VisCont::R_indx_j = 2;
-
-int VisCont::P_indx_i = 1;
-int VisCont::P_indx_j = 1;
-// ---------------- Q_LEARNING PARAMETERS ---------------- //
-
-double VisCont::dock_started,VisCont::dock_finished,VisCont::docking_duration;
-double VisCont::zeroMax = .0000000000000000001;
-double VisCont::zeroMin = -.0000000000000000001;
-
-double VisCont::speed_reducer_X = 1;
-double VisCont::speed_reducer_Y = 1;
-double VisCont::speed_reducer_theta = 1;
+double PID4Docking::speed_reducer_X = 1;
+double PID4Docking::speed_reducer_Y = 1;
+double PID4Docking::speed_reducer_theta = 1;
// ------ offsets X, Y, theta for Docking ---------
-double VisCont::Pz_eps = .001;
-double VisCont::Py_eps = .005;
-double VisCont::A_eps = (CV_PI/180) * 6; // 1 deg.
+double PID4Docking::Pz_eps = .001;
+double PID4Docking::Py_eps = .002;
+double PID4Docking::A_eps = (CV_PI/180) * 1; // 1 deg.
-double VisCont::safety_margin_X = .2; // safety margin X axis in docking process : 12 cm
+double PID4Docking::safety_margin_X = .15; // safety margin X axis in docking process : 18 cm
// ------ offsets X, Y, theta for Undocking ---------
-double VisCont::x_thresh_undock = .02;
-double VisCont::y_thresh_undock = .015;
-double VisCont::theta_thresh_undock = (CV_PI/180) * 3;
+double PID4Docking::x_thresh_undock = .02;
+double PID4Docking::y_thresh_undock = .015;
+double PID4Docking::theta_thresh_undock = (CV_PI/180) * 3;
-double VisCont::docking_counter = 1;
+double PID4Docking::docking_counter = 1;
-VisCont::VisCont()
+PID4Docking::PID4Docking()
{
keepMoving = true;
/* initialize random seed: */
- //srand (time(NULL));
-
-
- R[R_indx_i][R_indx_j] = 50; // reward
- R[P_indx_i][P_indx_j] = -60; // punishment
+ srand (time(NULL));
x_rand_SM = RefPose[2] + .55; // 55 cm spreading domain in the x-axis while moving towards the random pose
@@ -183,25 +156,25 @@ VisCont::VisCont()
MarPose_pub = node_vis.advertise<geometry_msgs::PoseStamped>("/marker_pose", 100);
commandPub = node_cont.advertise<geometry_msgs::Twist>("/base_controller/command",1);
- MarPose_Sub = node_vis.subscribe("/marker_pose",1,&VisCont::camCB,this);
+ MarPose_Sub = node_vis.subscribe("/marker_pose",1,&PID4Docking::camCB,this);
}
- VisCont::~VisCont()
+ PID4Docking::~PID4Docking()
{
}
- Mat VisCont::getCurrentImage()
+ Mat PID4Docking::getCurrentImage()
{
return img;
}
-void VisCont::cvTackBarEvents(int value,void* ptr)
+void PID4Docking::cvTackBarEvents(int value,void* ptr)
{
- VisCont* ic = (VisCont*)(ptr);
+ PID4Docking* ic = (PID4Docking*)(ptr);
ic-> myhandler(value);
}
-void VisCont::myhandler(int value)
+void PID4Docking::myhandler(int value)
{
if (Thresh1_min<3) Thresh1_min=3;
@@ -227,14 +200,14 @@ void VisCont::myhandler(int value)
//imshow("INPUT IMAGE",TheInputImageCopy);
//imshow("THRESHOLD IMAGE",MDetector.getThresholdedImage());
}
-void VisCont::createTrackbars()
+void PID4Docking::createTrackbars()
{
namedWindow(trackbarWindowName, 0);
createTrackbar("ThresParam 1", trackbarWindowName, &Thresh1_min, Thresh1_max, cvTackBarEvents, this);
createTrackbar("ThresParam 2", trackbarWindowName, &Thresh2_min, Thresh2_max, cvTackBarEvents, this);
}
-void VisCont::imageCb(const sensor_msgs::ImageConstPtr& msg)
+void PID4Docking::imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
@@ -249,7 +222,7 @@ void VisCont::imageCb(const sensor_msgs::ImageConstPtr& msg)
img = cv_ptr->image;
}
-bool VisCont::readArguments ( int argc,char **argv )
+bool PID4Docking::readArguments ( int argc,char **argv )
{
if (argc<2) {
cerr<< "Invalid number of arguments!\n" <<endl;
@@ -266,7 +239,7 @@ bool VisCont::readArguments ( int argc,char **argv )
return true;
}
-void VisCont::ProgStart(int argc,char** argv)
+void PID4Docking::ProgStart(int argc,char** argv)
{
// Show images, press "SPACE" to diable image
// rendering to save CPU time
@@ -357,10 +330,10 @@ if (TheVideoCapturer.retrieve(TheInputImage))
x_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1];
z_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[2];
- cv::Mat R(3,3,cv::DataType<float>::type);
+ Mat R(3,3,cv::DataType<float>::type);
// You need to apply cv::Rodrigues() in order to obatain angles wrt to camera coords
- cv::Rodrigues(TheMarkers[0].Rvec,R);
+ Rodrigues(TheMarkers[0].Rvec,R);
// ----------- Euler angle -----------------//
float roll1 = -asin(R.at<float>(2,0));
@@ -459,15 +432,13 @@ if (key == ' ')
}
ros::spinOnce();
- //rate.sleep();
-TT_E = ros::Time::now().toSec();
-ROS_INFO_STREAM("start = "<< TT_S << " , end = " << TT_E << " , duration = " << (TT_E - TT_S) <<" \n");
-
- }
+ TT_E = ros::Time::now().toSec();
+ ROS_INFO_STREAM(" visualization while loop duration = " << (TT_E - TT_S) <<" \n");
+ }
}
-void VisCont::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber
+void PID4Docking::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber
{
camPose[0] = CamFB->pose.position.x; // not important!!!
camPose[1] = CamFB->pose.position.y; // y pose
@@ -539,9 +510,6 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
}else
{
Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[3],.1);
- //Q_Learning(camPose[1],camPose[3]); // performing Q_Learning...
- //print_R();
-
}
} else
{
@@ -550,7 +518,7 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
}
}
-void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt)
+void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW, double dt)
{
ROS_INFO_STREAM("--------------------- Controller started ----------------------\n ");
@@ -659,11 +627,10 @@ void VisCont::Controller(double RefX, double MarPoseX, double RefY, double MarPo
//ROS_INFO_STREAM("Control signalYAW = "<< control_signalYAW <<". \n");
ROS_INFO_STREAM(" ---------------------- Controller ended ----------------------- \n");
- //dock(-control_signalX, control_signalY, control_signalYAW);
dock(control_signalX, control_signalY, control_signalYAW);
}
-void VisCont::dock(double VelX, double VelY, double omegaZ)
+void PID4Docking::dock(double VelX, double VelY, double omegaZ)
{
ROS_INFO(".... REAL .... !");
geometry_msgs::Twist msg;
@@ -677,7 +644,7 @@ void VisCont::dock(double VelX, double VelY, double omegaZ)
ROS_INFO_STREAM(" Current speed of robot: \n" << msg << ".\n");
}
-void VisCont::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
+void PID4Docking::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
{
ROS_INFO_STREAM(" Zmar = " << camPose[2] << " m. \n");
@@ -709,7 +676,7 @@ void VisCont::move2docking(double VelX_est, double VelY_est, double omegaZ_est)
}
// ---- Controller part -------- END ------
-void VisCont::GenerateRandomVal()
+void PID4Docking::GenerateRandomVal()
{
// ---------------- PID gains ------------------
@@ -724,7 +691,7 @@ void VisCont::GenerateRandomVal()
theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - theta_dwn) + theta_dwn; // will be used for Q_Learning
}
-void VisCont::RandomPose(double X_rand, double Y_rand, double theta_rand)
+void PID4Docking::RandomPose(double X_rand, double Y_rand, double theta_rand)
{
ROS_INFO_STREAM(" Xr = " << X_rand << ", Yr = " << Y_rand << ", Thetar = " << theta_rand << " rad ~ " << theta_rand * (180/CV_PI) << " deg\n");
ROS_INFO_STREAM(" -------------------------------------------------------------- \n");
@@ -827,153 +794,3 @@ geometry_msgs::Twist msg_new;
commandPub.publish(msg_new);
}
-
-void VisCont::Q_Learning(double y, double theta)
-{
-
-/* while ( iterations <= it_ )
- {
- if (user_action == 1 && y_new != 0) // North
- {
- reward = R[y_new][theta_new];
- Q_next_state = Q[y_new - 1][theta_new];
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- Q_curr_state = Q_next_state;
- i--;
- counter++;
- } else if (user_action == 1 && y_new == 0) // North
- {
- cout << "You can't go further up!\n";
- } else if (user_action == 3 && i < (row - 1)) // South, i < row
- {
- reward = R[y_new][theta_new];
- Q_next_state = Q[y_new + 1][theta_new];
-
-
-
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- Q_curr_state = Q_next_state;
- i++;
- counter++;
- } else if (user_action == 3 && y_new >= (row - 1)) // South
- {
- cout << "You can't go further down!\n";
- } else if (user_action == 2 && theta_new < (col - 1)) // East
- {
- reward = R[y_new][theta_new];
- Q_next_state = Q[i][j + 1];
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- Q_curr_state = Q_next_state;
- j++;
- counter++;
- } else if (user_action == 2 && j >= (col - 1)) // East, j > col
- {
- cout << "You can't go further right!\n";
- } else if (user_action == 4 && j != 0 ) // West
- {
- reward = R[y_new][theta_new];
- Q_next_state = Q[y_new][theta_new - 1];
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- Q_curr_state = Q_next_state;
- j--;
- counter++;
- } else if (user_action == 4 && theta_new == 0) // West, j = 1
- {
- cout << "You can't go further left!\n";
- } else
- {
- cout << "\nGenerating random pose in grid for 1st. time!\n";
- generate_rand();
- }
-
- // + Reward
- if (y_new == R_indx_i && theta_new == R_indx_j)
- {
- Time_Reward = -counter;
- cout << " Time Reward = "<< Time_Reward << "\n";
-
- if(abs(Time_Reward) <= R_step)
- {
-
- cout << "\n Goal is achieved <= " << R_step << " time steps\n";
- reward = R[y_new][theta_new];
- Q_next_state = 0;
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- } else
- {
- cout << "\n Goal is achieved > " << R_step << " time steps => time_punishment\n";
- reward = -1; // ???
- Q_next_state = 0;
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- }
-
- counter = 0;
- } else if (y_new == P_indx_i && theta_new == P_indx_j) // (-) Reward => Punishment
- {
- cout << "\n Failed to achieve a goal! \n";
-
- reward = R[y_new][theta_new];
- Q_next_state = 0;
-
- sample = reward + gamma * Q_next_state;
- Q[y_new][theta_new] = ((1 - alpha) * Q[y_new][theta_new]) + (alpha * sample);
- }
-
- cout << "\n Q_value = " << Q_curr_state << " , actions N(1), E(2), S(3), W(4) : ";
- user_action = ((double) rand() / (RAND_MAX)) * (5 - 1) + 1; // Generating random user_action
- printf(" ramdom user action = %i \n",user_action);
- //cin >> user_action;
- }*/
-}
-
-
-void VisCont::print_R()
-{
- cout << " R = \n";
- for(int i = 0; i <= (row - 1); i++)
- {
- for(int j = 0; j <= (col - 1); j++)
- {
- cout << setw(col - 1) << R[i][j];
- if(j < col - 1)
- {
- cout << " , ";
- }
- } // j
- cout << "\n";
- } // i
- cout << "\n";
-}
-
-
-void VisCont::print_Q()
-{
- cout << " Q = \n";
- for(int i = 0; i <= (row - 1); i++)
- {
- for(int j = 0; j <= (col - 1); j++)
- {
- cout << setw(col - 1) << Q[i][j];
- if(j < col - 1)
- {
- cout << " , ";
- }
- } // j
- cout << "\n";
- } // i
- cout << "\n";
-}
-
-
diff --git a/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/include/RL_Docking.h b/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/include/RL_Docking.h
index b0170db7..a52ced78 100644
--- a/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/include/RL_Docking.h
+++ b/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/include/RL_Docking.h
@@ -125,7 +125,7 @@ public:
void myhandler(int value);
- void GenerateRandomVal();
+ void GenRandVal();
void Undocking(double X_rand, double Y_rand, double theta_rand);
void RL(double RefX, double MarPoseX, double RefY, double MarPoseY, double RefYAW, double MarPoseYAW);
@@ -150,8 +150,8 @@ public:
static double alpha;
static double R_step;
- static double R[23][51];
- static double Q[23][51];
+ static double R[27][51];
+ static double Q[27][51];
static double Q_curr_state,Q_next_state;
static double reward;
diff --git a/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/include/RL_Docking.h~ b/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/include/RL_Docking.h~
index 1908d8b5..b0170db7 100644
--- a/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/include/RL_Docking.h~
+++ b/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/include/RL_Docking.h~
@@ -91,7 +91,7 @@ private:
static float TheMarkerSize;
void ContStart();
- static bool update_images,found,Go2RandomPose;
+ static bool update_images,found,Go2RandomPose,inc_dock_counter;
bool readArguments ( int argc,char **argv );
@@ -142,6 +142,7 @@ public:
static const double y_up,y_dwn,theta_up,theta_dwn;
static double y_dot,theta_dot;
static int i_Goal, j_Goal;
+ static int iterations;
static int i,j,user_action;
static const int row, col;
@@ -149,8 +150,8 @@ public:
static double alpha;
static double R_step;
- static double R[27][51];
- static double Q[27][51];
+ static double R[23][51];
+ static double Q[23][51];
static double Q_curr_state,Q_next_state;
static double reward;
diff --git a/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/src/RL_Docking.cpp b/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/src/RL_Docking.cpp
index 2da20246..9a99ca7b 100644
--- a/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/src/RL_Docking.cpp
+++ b/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/src/RL_Docking.cpp
@@ -13,7 +13,6 @@
#include <aruco/cvdrawingutils.h>
#include "ros/ros.h"
-
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/tf.h>
@@ -78,11 +77,12 @@ float RL4Docking::closestRangeF;
//const double RL4Docking::RefPose[4] = {-.0957, 0.00638817 /* Y_ref*/ , 0.308857 /* X_ref*/ , 0.17 /* theta_ref*/};
// ---- Ref. Values for Logitech Camera ---- //
-const double RL4Docking::RefPose[4] = {-.0957, -0.0305812 /* Y_ref*/ , 0.26707 /* X_ref*/ , -0.527004 /* theta_ref*/};
+const double RL4Docking::RefPose[4] = {-.0957, -0.0305812 /* Y_ref*/ , 0.26814 /* X_ref*/ , -0.553234 /* theta_ref*/};
+
// random pose initialized
-const double RL4Docking::y_up = .25;
-const double RL4Docking::y_dwn = -.25;
+const double RL4Docking::y_up = .24;
+const double RL4Docking::y_dwn = -.24;
const double RL4Docking::theta_dwn = 0;
const double RL4Docking::theta_up = .72;
@@ -98,7 +98,7 @@ double RL4Docking::y_dot, RL4Docking::theta_dot; // actions
double RL4Docking::gamma = .8;
double RL4Docking::alpha = .1;
-const int RL4Docking::row = 23/*(y_up - y_dwn) * 10000*/;
+const int RL4Docking::row = 27/*(y_up - y_dwn) * 10000*/;
const int RL4Docking::col = 51/*(theta_up - theta_dwn) * 1000*/;
double RL4Docking::R_step = RL4Docking::row * RL4Docking::col;
@@ -109,10 +109,10 @@ double RL4Docking::Q_next_state;
double RL4Docking::reward;
int RL4Docking::counter = 0;
int RL4Docking::Time_Reward;
-int RL4Docking::iterations = 3;
+int RL4Docking::iterations = 20;
double RL4Docking::sample;
-int RL4Docking::i_Goal = 16; // right now it has been set manually
+int RL4Docking::i_Goal = 20; // right now it has been set manually
int RL4Docking::j_Goal = 22; // right now it has been set manually
// ---------------- Q_LEARNING PARAMETERS ---------------- //
double RL4Docking::dock_started,RL4Docking::dock_finished,RL4Docking::docking_duration;
@@ -126,7 +126,7 @@ double RL4Docking::x_thresh_dock = .001;
double RL4Docking::y_thresh_dock = .002;
double RL4Docking::theta_thresh_dock = (CV_PI/180) * 3; // 1 deg.
-double RL4Docking::safety_margin_X = .13; // safety margin X axis in docking process : 12 cm
+double RL4Docking::safety_margin_X = .145; // safety margin X axis in docking process : 14.5 cm
// ------ offsets X, Y, theta for Undocking ---------
double RL4Docking::x_thresh_undock = .02;
@@ -324,7 +324,7 @@ void RL4Docking::ProgStart(int argc,char** argv)
{
found = false;
RorP(i,j);
- GenerateRandomVal();
+ GenRandVal();
//keepMoving = false;
if(j < j_Goal)
@@ -489,7 +489,7 @@ void RL4Docking::ScanCallBackF(const sensor_msgs::LaserScan::ConstPtr& scanF){
{
ROS_INFO(" Oops!FRONTSIDE Obstacle!....! \n ");
//keepMoving = false;
- GenerateRandomVal();
+ GenRandVal();
docking_counter ++;
speed_reducer_X = 1;
speed_reducer_Y = 1;
@@ -519,7 +519,7 @@ void RL4Docking::ScanCallBackR(const sensor_msgs::LaserScan::ConstPtr& scanR)
{
ROS_INFO("Oops!BACKSIDE Obstacle!....! \n ");
//keepMoving = false;
- GenerateRandomVal();
+ GenRandVal();
docking_counter ++;
speed_reducer_X = 1;
speed_reducer_Y = 1;
@@ -533,9 +533,7 @@ void RL4Docking::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // sub
if (docking_counter > iterations )
{
- ROS_INFO("ietarations done");
- print_Q();
- keepMoving = false;
+
}
// in Marker coordinate sys.
@@ -557,7 +555,12 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
ROS_INFO_STREAM(" theta_ref = " << RefPose[3] << " rad. =~ " << (180/CV_PI) * RefPose[3] << " deg. \n");
ROS_INFO_STREAM(" --------------------------------------------------------\n ");
-
+if (docking_counter > iterations )
+{
+ ROS_INFO("ietarations done!!");
+ print_Q();
+ keepMoving = false;
+}
if(Go2RandomPose == false)
{
ROS_INFO_STREAM(" trial no. " << docking_counter << " :");
@@ -569,7 +572,7 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
ROS_INFO("REACHED X LIMIT!!");
RorP(i,j);
//keepMoving = false;
- GenerateRandomVal();
+ GenRandVal();
docking_counter ++;
speed_reducer_X = 1;
speed_reducer_Y = 1;
@@ -625,7 +628,7 @@ void RL4Docking::i_j_Generator(double y, double theta)
ROS_INFO(" Outside theta-boundary!!");
RorP(i,j);
//keepMoving = false;
- GenerateRandomVal();
+ GenRandVal();
docking_counter ++;
speed_reducer_X = 1;
speed_reducer_Y = 1;
@@ -646,7 +649,7 @@ void RL4Docking::i_j_Generator(double y, double theta)
ROS_INFO(" Outside Y-boundary!!");
RorP(i,j);
//keepMoving = false;
- GenerateRandomVal();
+ GenRandVal();
docking_counter ++;
speed_reducer_X = 1;
speed_reducer_Y = 1;
@@ -728,7 +731,6 @@ void RL4Docking::Q_Learning(int indx_i, int indx_j, int action)
double vel_y = speed_reducer_Y * .05;
double vel_x = speed_reducer_X * .06;
-
if (MoveStraight == false)
{
Action4QL(indx_i,indx_j);
@@ -937,16 +939,16 @@ void RL4Docking::dock(double VelX, double VelY, double omegaZ)
// ---- Controller part -------- END ------
-void RL4Docking::GenerateRandomVal()
+void RL4Docking::GenRandVal()
{
// ---------------- actions (Velocities) ------------------
y_dot = ((double) rand() / (RAND_MAX)) * (.1 - (-.1)) + (-.1); // -.1 < y_dot < .1
theta_dot = ((double) rand() / (RAND_MAX)) * (.1 - (-.1)) + (-.1); // -.1 < theta_dot < .1
// ------------------ Generating Random Pose ------------------
- //x_new = ((double) rand() / (RAND_MAX)) * (1.1 - x_rand_SM) + x_rand_SM;
- x_new = 1.05;
- y_new = ((double) rand() / (RAND_MAX)) * (y_up - y_dwn) + y_dwn;
+ x_new = ((double) rand() / (RAND_MAX)) * (1.1 - (4*safety_margin_X)) + (4*safety_margin_X);
+ //x_new = 1.05;
+ y_new = ((double) rand() / (RAND_MAX)) * (y_up - y_dwn) + y_dwn;
//theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - theta_dwn) + theta_dwn;
theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - (-theta_up)) + (-theta_up);
}
diff --git a/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/src/RL_Docking.cpp~ b/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/src/RL_Docking.cpp~
index 29551f1f..f40fa4fd 100644
--- a/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/src/RL_Docking.cpp~
+++ b/MobileRobot/Machine_Learning/RL4Docking/docking_with_q_learning/src/RL_Docking.cpp~
@@ -63,24 +63,26 @@ bool RL4Docking::update_images = true;
bool RL4Docking::found = false;
bool RL4Docking::Go2RandomPose = false;
+bool RL4Docking::inc_dock_counter = false;
int RL4Docking::ThresParam1 = 0;
int RL4Docking::ThresParam2 = 0;
double RL4Docking::MIN_SCAN_ANGLE_RAD = -30 * (CV_PI/180);
double RL4Docking::MAX_SCAN_ANGLE_RAD = 30 * (CV_PI/180);
-float RL4Docking::MIN_PROXIMITY_RANGE_M = .05;
+float RL4Docking::MIN_PROXIMITY_RANGE_M = .09;
float RL4Docking::closestRangeR;
float RL4Docking::closestRangeF;
+
// ---- Ref. Values for Android Camera ---- //
//const double RL4Docking::RefPose[4] = {-.0957, 0.00638817 /* Y_ref*/ , 0.308857 /* X_ref*/ , 0.17 /* theta_ref*/};
// ---- Ref. Values for Logitech Camera ---- //
-const double RL4Docking::RefPose[4] = {-.0957, -0.0292778 /* Y_ref*/ , 0.26707 /* X_ref*/ , -0.517004 /* theta_ref*/};
+const double RL4Docking::RefPose[4] = {-.0957, -0.0305812 /* Y_ref*/ , 0.265274 /* X_ref*/ , -0.535004 /* theta_ref*/};
// random pose initialized
-const double RL4Docking::y_up = .25;
-const double RL4Docking::y_dwn = -.25;
+const double RL4Docking::y_up = .24;
+const double RL4Docking::y_dwn = -.24;
const double RL4Docking::theta_dwn = 0;
const double RL4Docking::theta_up = .72;
@@ -107,6 +109,7 @@ double RL4Docking::Q_next_state;
double RL4Docking::reward;
int RL4Docking::counter = 0;
int RL4Docking::Time_Reward;
+int RL4Docking::iterations = 30;
double RL4Docking::sample;
int RL4Docking::i_Goal = 20; // right now it has been set manually
@@ -123,7 +126,7 @@ double RL4Docking::x_thresh_dock = .001;
double RL4Docking::y_thresh_dock = .002;
double RL4Docking::theta_thresh_dock = (CV_PI/180) * 3; // 1 deg.
-double RL4Docking::safety_margin_X = .13; // safety margin X axis in docking process : 12 cm
+double RL4Docking::safety_margin_X = .13; // safety margin X axis in docking process : 14.5 cm
// ------ offsets X, Y, theta for Undocking ---------
double RL4Docking::x_thresh_undock = .02;
@@ -246,7 +249,7 @@ void RL4Docking::ProgStart(int argc,char** argv)
// Show images, press "SPACE" to diable image
// rendering to save CPU time
- if (readArguments(argc,argv)==false)
+ if (readArguments(argc,argv) == false)
{
cerr<<"check the authenticity of your file again!"<<endl;
nh_.shutdown();
@@ -305,14 +308,23 @@ void RL4Docking::ProgStart(int argc,char** argv)
float roll,yaw,pitch;
float rollE,yawE,pitchE;
- if (TheMarkers.size()>0)
+ if (TheMarkers.size() > 0 && inc_dock_counter == false)
{
found = true;
- }else
+ } else if (TheMarkers.size() > 0 && inc_dock_counter == true)
+ {
+ //found = true;
+ docking_counter ++;
+ speed_reducer_X = 1;
+ speed_reducer_Y = 1;
+ speed_reducer_theta = 1;
+ Go2RandomPose = true;
+ inc_dock_counter = false;
+ } else
{
found = false;
RorP(i,j);
- //ROS_INFO_STREAM("Marker is lost, successful docking trials : " << (docking_counter - 1) << "\n");
+ GenRandVal();
//keepMoving = false;
if(j < j_Goal)
@@ -322,12 +334,8 @@ void RL4Docking::ProgStart(int argc,char** argv)
{
dock(0,0,.05);
}
- //docking_counter ++;
- GenerateRandomVal();
- speed_reducer_X = 1;
- speed_reducer_Y = 1;
- speed_reducer_theta = 1;
- Go2RandomPose = true;
+
+ inc_dock_counter = true;
}
if (ros::ok() && found)
@@ -479,8 +487,14 @@ void RL4Docking::ScanCallBackF(const sensor_msgs::LaserScan::ConstPtr& scanF){
if (closestRangeF <= MIN_PROXIMITY_RANGE_M)
{
- ROS_INFO(" Oops!FRONTSIDE Obstacle!... Stop...! \n ");
- keepMoving = false;
+ ROS_INFO(" Oops!FRONTSIDE Obstacle!....! \n ");
+ //keepMoving = false;
+ GenRandVal();
+ docking_counter ++;
+ speed_reducer_X = 1;
+ speed_reducer_Y = 1;
+ speed_reducer_theta = 1;
+ Go2RandomPose = true;
}
}
@@ -503,14 +517,24 @@ void RL4Docking::ScanCallBackR(const sensor_msgs::LaserScan::ConstPtr& scanR)
//ROS_INFO_STREAM(" Closest range for the rear laser sensor: " << closestRangeR);
if (closestRangeR <= MIN_PROXIMITY_RANGE_M)
{
- ROS_INFO("Oops!BACKSIDE Obstacle!... Stop...! \n ");
- keepMoving = false;
+ ROS_INFO("Oops!BACKSIDE Obstacle!....! \n ");
+ //keepMoving = false;
+ GenRandVal();
+ docking_counter ++;
+ speed_reducer_X = 1;
+ speed_reducer_Y = 1;
+ speed_reducer_theta = 1;
+ Go2RandomPose = true;
}
}
void RL4Docking::camCB(const geometry_msgs::PoseStamped::ConstPtr& CamFB) // subscriber
{
+ if (docking_counter > iterations )
+ {
+
+ }
// in Marker coordinate sys.
// z => X robot (thrust)
@@ -531,7 +555,12 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
ROS_INFO_STREAM(" theta_ref = " << RefPose[3] << " rad. =~ " << (180/CV_PI) * RefPose[3] << " deg. \n");
ROS_INFO_STREAM(" --------------------------------------------------------\n ");
-
+if (docking_counter > iterations )
+{
+ ROS_INFO("ietarations done!!");
+ print_Q();
+ keepMoving = false;
+}
if(Go2RandomPose == false)
{
ROS_INFO_STREAM(" trial no. " << docking_counter << " :");
@@ -543,7 +572,7 @@ camPose[3] = CamFB->pose.orientation.x; // theta orientation
ROS_INFO("REACHED X LIMIT!!");
RorP(i,j);
//keepMoving = false;
- GenerateRandomVal();
+ GenRandVal();
docking_counter ++;
speed_reducer_X = 1;
speed_reducer_Y = 1;
@@ -599,7 +628,7 @@ void RL4Docking::i_j_Generator(double y, double theta)
ROS_INFO(" Outside theta-boundary!!");
RorP(i,j);
//keepMoving = false;
- GenerateRandomVal();
+ GenRandVal();
docking_counter ++;
speed_reducer_X = 1;
speed_reducer_Y = 1;
@@ -620,7 +649,7 @@ void RL4Docking::i_j_Generator(double y, double theta)
ROS_INFO(" Outside Y-boundary!!");
RorP(i,j);
//keepMoving = false;
- GenerateRandomVal();
+ GenRandVal();
docking_counter ++;
speed_reducer_X = 1;
speed_reducer_Y = 1;
@@ -693,9 +722,7 @@ void RL4Docking::Action4QL(int inp_i, int inp_j)
ROS_INFO(" current pose : South \n ");
user_action = 1;
}
-
ROS_INFO_STREAM(" Q_value = " << Q_curr_state << " , Actions CCW(1), R(2), CW(3), L(4) : " << user_action << "\n");
-
}
void RL4Docking::Q_Learning(int indx_i, int indx_j, int action)
@@ -704,7 +731,6 @@ void RL4Docking::Q_Learning(int indx_i, int indx_j, int action)
double vel_y = speed_reducer_Y * .05;
double vel_x = speed_reducer_X * .06;
-
if (MoveStraight == false)
{
Action4QL(indx_i,indx_j);
@@ -819,7 +845,7 @@ void RL4Docking::RorP(int indx_i, int indx_j)
//keepMoving = false;
}
- //print_Q();
+ print_Q();
/*// punishment
for (int p = (i_Goal - 1); p <= (i_Goal + 1); p++)
@@ -913,16 +939,16 @@ void RL4Docking::dock(double VelX, double VelY, double omegaZ)
// ---- Controller part -------- END ------
-void RL4Docking::GenerateRandomVal()
+void RL4Docking::GenRandVal()
{
// ---------------- actions (Velocities) ------------------
y_dot = ((double) rand() / (RAND_MAX)) * (.1 - (-.1)) + (-.1); // -.1 < y_dot < .1
theta_dot = ((double) rand() / (RAND_MAX)) * (.1 - (-.1)) + (-.1); // -.1 < theta_dot < .1
// ------------------ Generating Random Pose ------------------
- //x_new = ((double) rand() / (RAND_MAX)) * (1.1 - x_rand_SM) + x_rand_SM;
- x_new = .9;
- y_new = ((double) rand() / (RAND_MAX)) * (y_up - y_dwn) + y_dwn;
+ x_new = ((double) rand() / (RAND_MAX)) * (1.1 - (2*safety_margin_X)) + (2*safety_margin_X);
+ //x_new = 1.05;
+ y_new = ((double) rand() / (RAND_MAX)) * (y_up - y_dwn) + y_dwn;
//theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - theta_dwn) + theta_dwn;
theta_new = ((double) rand() / (RAND_MAX)) * (theta_up - (-theta_up)) + (-theta_up);
}
--
GitLab