Commit f991555f authored by Farid Alijani's avatar Farid Alijani
Browse files

pose_right_without_logic

parent 68fb450d
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 3.5.1, 2016-09-12T19:02:09. -->
<!-- Written by QtCreator 3.5.1, 2016-10-10T12:37:13. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
......
This diff is collapsed.
This diff is collapsed.
camPose[0] = CamFB->pose.position.x; // not important!!!
camPose[1] = CamFB->pose.position.y; // y_cam
camPose[2] = CamFB->pose.position.z; // x_cam
camPose[3] = CamFB->pose.orientation.x; // Robot roll
camPose[4] = CamFB->pose.orientation.y; // Robot pitch
camPose[5] = CamFB->pose.orientation.z; // Camera yaw
/*ROS_INFO_STREAM("--------- PID gains in trial no. " << docking_counter << " : ---------\n");
ROS_INFO_STREAM(" Kp_y = " << Kp_y << " , Ki_y = " << Ki_y << " , Kd_y = " << Kd_y << "\n");
ROS_INFO_STREAM(" Kp_theta = " << Kp_theta << " , Ki_theta = " << Ki_theta << " , Kd_theta = " << Kd_theta << "\n");*/
ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n");
// yaw has an offset of 180 deg.
if (camPose[5] > 0)
{
theta_with_offset = camPose[5] - y_off;
} else
{
theta_with_offset = camPose[5] + y_off;
}
x_robINmar_coor = camPose[2] * cos(theta_with_offset) - camPose[1] * sin(theta_with_offset);
y_robINmar_coor = -camPose[2] * sin(theta_with_offset) + camPose[1] * cos(theta_with_offset);
ROS_INFO_STREAM("theta = " << theta_with_offset << "rad. =~ " << theta_with_offset*(180.0/CV_PI) << " deg. \n");
ROS_INFO_STREAM(" theta_ref = " << RefPose[3] << " rad. =~ " << (180/CV_PI) * RefPose[3] << " deg. \n");
ROS_INFO_STREAM(" X_mar = " << x_robINmar_coor << " vs. X_ref = " << RefPose[2] << " \n");
ROS_INFO_STREAM(" Y_mar = " << y_robINmar_coor << " vs. Y_ref = " << RefPose[1] << " \n");
ROS_INFO_STREAM(" y_t = " << camPose[1] << " , x_t = " << camPose[2] << "\n");
ROS_INFO_STREAM("------------------------------------------------------ \n ");
if(Go2RandomPose == false)
{
ROS_INFO_STREAM("---------- MOVING TOWARDS DOCKING PLATFORM --------- \n ");
if (
(abs(RefPose[2] - x_robINmar_coor) <= x_dock_thresh)
)
{
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 to new Random Pose\n");
keepMoving = false;
GenerateRandomVal();
docking_counter ++;
speed_reducer_X = speed_reducer_Y = speed_reducer_theta = 1;
//Go2RandomPose = true;
// to make sure that y & theta are within the threshold...
} else if (abs(RefPose[2] - x_robINmar_coor) <= safety_margin_X)
{
if(
(abs(RefPose[1] - y_robINmar_coor) > y_dock_thresh) ||
(abs(RefPose[3] - theta_with_offset) > theta_dock_thresh)
)
{
ROS_INFO_STREAM(" delta_X < " << safety_margin_X << " m., Fixing Y or theta. \n ");
speed_reducer_Y = speed_reducer_theta = 1;
Controller(RefPose[2], RefPose[2], RefPose[1], y_robINmar_coor, RefPose[3], theta_with_offset,.1);
} else if(
(abs(RefPose[1] - y_robINmar_coor) <= y_dock_thresh) &&
(abs(RefPose[3] - theta_with_offset) <= theta_dock_thresh)
)
{
ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n");
/*speed_reducer_X = .06; // for x_dot = .1 =>.06*/
speed_reducer_X = .04; // for x_dot = .15 =>.04 ---- optimal docking time and Y-axis offset ----
/*speed_reducer_X = .0375; // for x_dot = .16 =>.0375*/
Controller(RefPose[2], x_robINmar_coor, RefPose[1], RefPose[1], RefPose[3], RefPose[3],.1);
}
}else
{
speed_reducer_X = 1;
Controller(RefPose[2], x_robINmar_coor, RefPose[1], y_robINmar_coor, RefPose[3], theta_with_offset,.1);
}
} else
{
ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ---------\n");
Undocking(x_new,y_new,theta_new);
}
camPose[0] = CamFB->pose.position.x; // not important!!!
camPose[1] = CamFB->pose.position.y; // y_cam
camPose[2] = CamFB->pose.position.z; // x_cam
camPose[3] = CamFB->pose.orientation.x; // Robot roll
camPose[4] = CamFB->pose.orientation.y; // Robot pitch
camPose[5] = CamFB->pose.orientation.z; // Camera yaw
ROS_INFO_STREAM(" --------------------- Pose estimation ------------------ \n");
ROS_INFO_STREAM(" theta = " << camPose[5] << " rad =~ " << camPose[5]*(180.0/CV_PI) << " deg\n");
ROS_INFO_STREAM(" theta_ref = " << RefPose[3] << " rad =~ " << (180/CV_PI) * RefPose[3] << " deg\n");
ROS_INFO_STREAM(" yaw = " << yaw << " rad =~ " << yaw*(180.0/CV_PI) << " deg \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(" y_t = " << y_t << " , x_t = " << z_t << "\n");
ROS_INFO_STREAM("------------------------------------------------------ \n ");
if(Go2RandomPose == false)
{
ROS_INFO_STREAM("---------- MOVING TOWARDS DOCKING PLATFORM --------- \n ");
if (
(abs(RefPose[2] - camPose[2]) <= x_dock_thresh)
)
{
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 to new Random Pose\n");
keepMoving = false;
GenerateRandomVal();
docking_counter ++;
speed_reducer_X = speed_reducer_Y = speed_reducer_theta = 1;
//Go2RandomPose = true;
// 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]) > y_dock_thresh) ||
(abs(RefPose[3] - camPose[5]) > theta_dock_thresh)
)
{
ROS_INFO_STREAM(" delta_X < " << safety_margin_X << " m., Fixing Y or theta. \n ");
speed_reducer_Y = speed_reducer_theta = 1;
Controller(RefPose[2], RefPose[2], RefPose[1], camPose[1], RefPose[3], camPose[5],.1);
} else if(
(abs(RefPose[1] - camPose[1]) <= y_dock_thresh) &&
(abs(RefPose[3] - camPose[5]) <= theta_dock_thresh)
)
{
ROS_INFO("y & theta fixed successfully, MOVING STRAIGHT AHEAD ... \n");
/*speed_reducer_X = .06; // for x_dot = .1 =>.06*/
speed_reducer_X = .04; // for x_dot = .15 =>.04 ---- optimal docking time and Y-axis offset ----
/*speed_reducer_X = .0375; // for x_dot = .16 =>.0375*/
Controller(RefPose[2], camPose[2], RefPose[1], RefPose[1], RefPose[3], RefPose[3],.1);
}
}else
{
speed_reducer_X = 1;
Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[5],.1);
}
} else
{
ROS_INFO("---------- MOVING TOWARDS RANDOM POSE ---------\n");
Undocking(x_new,y_new,theta_new);
}
This diff is collapsed.
......@@ -92,14 +92,14 @@ double PID4Docking::control_signalX, PID4Docking::control_signalY, PID4Docking::
//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 PID4Docking::RefPose[4] = {-.0957, -0.0185 /* Y_ref*/ , -0.2025 /* X_ref*/ , -0.05 /* theta_ref*/};
const double PID4Docking::RefPose[4] = {-.0957, -0.0188 /* Y_ref*/ , -0.20007 /* X_ref*/ , -0.05 /* theta_ref*/};
//const double PID4Docking::RefPose[4] = {-.0957, 0.0185 /* Y_ref*/ , 0.2025 /* X_ref*/ , -0.05 /* theta_ref*/};
// ---------------- PID gains---------------- //
double PID4Docking::Kp_y = .266; //.4
double PID4Docking::Ki_y = 0.0003 ;// 0
double PID4Docking::Kd_y = 0.014; //.1
double PID4Docking::Kp_y = .28; //.4
double PID4Docking::Ki_y = 0 ;// 0
double PID4Docking::Kd_y = 0.006; //.1
double PID4Docking::Kp_theta = .25;// .18
double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
......@@ -656,11 +656,11 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
keepMoving = false;
}
/* ---
/* ---*/
ROS_INFO_STREAM("Control signalX = " << control_signalX <<"\n");
ROS_INFO_STREAM("Control signalY = " << control_signalY << "\n");
ROS_INFO_STREAM("Control signalYAW = "<< control_signalYAW <<"\n");
// */ dock(control_signalX, control_signalY, control_signalYAW);
// dock(control_signalX, control_signalY, control_signalYAW);
}
void PID4Docking::dock(double VelX, double VelY, double omegaZ)
......
This diff is collapsed.
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment