Commit 735ae7ec authored by Farid Alijani's avatar Farid Alijani
Browse files

docking data

parent 4e979f0e
......@@ -92,12 +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.0199 /* Y_ref*/ , 0.201 /* X_ref*/ , -0.0335 /* theta_ref*/};
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.0185 /* Y_ref*/ , 0.2025 /* X_ref*/ , -0.05 /* theta_ref*/};
// ---------------- PID gains---------------- //
double PID4Docking::Kp_y = .38; //.4
double PID4Docking::Ki_y = 0 ;// 0
double PID4Docking::Kd_y = 0.02; //.1
double PID4Docking::Kp_y = .266; //.4
double PID4Docking::Ki_y = 0.0003 ;// 0
double PID4Docking::Kd_y = 0.014; //.1
double PID4Docking::Kp_theta = .25;// .18
double PID4Docking::Ki_theta = 0; //* Ki_y; // .15 * Ki_y
......@@ -139,6 +141,8 @@ float PID4Docking::r_off = CV_PI/2;
float PID4Docking::y_off = CV_PI;
float PID4Docking::roll,PID4Docking::yaw,PID4Docking::pitch;
float PID4Docking::yaw_1,PID4Docking::yaw_prev;
double PID4Docking::theta_with_offset;
double PID4Docking::x_robCen2cam = -.95/2; // x_cam from the center of robot
......@@ -146,6 +150,9 @@ double PID4Docking::y_robCen2cam = 0; // y_cam from the center of robot
double PID4Docking::x_robINmar_coor,PID4Docking::y_robINmar_coor;
float PID4Docking::x_t, PID4Docking::y_t, PID4Docking::z_t;
PID4Docking::PID4Docking()
{
keepMoving = true;
......@@ -157,8 +164,9 @@ PID4Docking::PID4Docking()
// Publish pose message and buffer up to 100 messages
MarPose_pub = node_vis.advertise<geometry_msgs::PoseStamped>("/marker_pose", 100);
commandPub = node_cont.advertise<geometry_msgs::Twist>("/base_controller/command",100);
Transpose_pub = node_vis.advertise<geometry_msgs::PoseStamped>("/transposed_axis", 100);
commandPub = node_cont.advertise<geometry_msgs::Twist>("/base_controller/command",100);
MarPose_Sub = node_vis.subscribe("/marker_pose",100,&PID4Docking::camCB,this);
}
......@@ -259,9 +267,9 @@ void PID4Docking::ProgStart(int argc,char** argv)
const std::string vsa = "http://192.168.0.101:8080/video?x.mjpeg";
// -- publishing video stream with Android Camera--
TheVideoCapturer.open(vsa);
//TheVideoCapturer.open(vsa);
//TheVideoCapturer.open(0);
TheVideoCapturer.open(0);
// Check video is open
if (!TheVideoCapturer.isOpened())
......@@ -286,95 +294,158 @@ void PID4Docking::ProgStart(int argc,char** argv)
}
MDetector.setCornerRefinementMethod(MarkerDetector::LINES);
char key=0;
int index=0;
//ros::Rate rate(10);
// Capture until press ESC or until the end of the video
// Detection of markers in the image passed
MDetector.detect(TheInputImage,TheMarkers,TheCameraParameters,TheMarkerSize);
TheInputImage.copyTo(TheInputImageCopy);
Mat R_init(3,3,cv::DataType<float>::type);
Rodrigues(TheMarkers[0].Rvec,R_init);
yaw_1 = atan2(R_init.at<float>(2,1), R_init.at<float>(2,2)); // yaw_init
while ((key != 'x') && (key!=27) && TheVideoCapturer.grab() && node_vis.ok() && keepMoving)
{
TT_S = ros::Time::now().toSec();
/* --- For the purpose of showing the time ---
Mat frame;
TheVideoCapturer >> frame; // get a new frame from camera
imshow("video stream",frame);
waitKey(30); // 30 ms */
if (TheVideoCapturer.retrieve(TheInputImage))
{
// Detection of markers in the image passed
MDetector.detect(TheInputImage,TheMarkers,TheCameraParameters,TheMarkerSize);
TheInputImage.copyTo(TheInputImageCopy);
geometry_msgs::PoseStamped msg;
float x_t, y_t, z_t;
if (TheMarkers.size()>0)
{
found = true;
//ROS_INFO("MARKER FOUND!!! ... \n");
}else
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);
}
// Detection of markers in the image passed
MDetector.detect(TheInputImage,TheMarkers,TheCameraParameters,TheMarkerSize);
TheInputImage.copyTo(TheInputImageCopy);
geometry_msgs::PoseStamped msg;
geometry_msgs::PoseStamped msg_t;
if (node_vis.ok() && found)
{
//y_t = -TheMarkers[0].Tvec.at<Vec3f>(0,0)[0]; // changed !!!
y_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[0];
x_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1];
z_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[2];
if (TheMarkers.size()>0)
{
found = true;
//ROS_INFO("MARKER FOUND!!! ... \n");
}else
{
found = false;
keepMoving = false;
ROS_INFO_STREAM("Marker is lost, successful docking trials : " << (docking_counter - 1) << "\n");
}
if (node_vis.ok() && found)
{
Mat R(3,3,cv::DataType<float>::type);
Rodrigues(TheMarkers[0].Rvec,R);
y_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[0];
x_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[1];
z_t = TheMarkers[0].Tvec.at<Vec3f>(0,0)[2];
roll = atan2(R.at<float>(1,0), R.at<float>(0,0));
pitch = atan2(-R.at<float>(2,0),pow((pow(R.at<float>(2,1),2)+pow(R.at<float>(2,2),2)),.5));
yaw = atan2(R.at<float>(2,1), R.at<float>(2,2)); // useful
// just to publish the transpose values
// Publish Position
msg_t.pose.position.x = x_t;
msg_t.pose.position.y = y_t;
msg_t.pose.position.z = z_t;
// Publish Orientation
msg_t.pose.orientation.x = roll;
msg_t.pose.orientation.y = pitch;
//msg_t.pose.orientation.z = yaw;
if(yaw_1 > 0)
{
if (yaw < 0)
{
msg_t.pose.orientation.z = yaw_prev;
theta_with_offset = yaw_prev - y_off;
x_robINmar_coor = z_t * cos(yaw_prev) - y_t * sin(yaw_prev);
y_robINmar_coor = -z_t * sin(yaw_prev) + y_t * cos(yaw_prev);
} else
{
yaw_prev = yaw;
msg_t.pose.orientation.z = yaw;
theta_with_offset = yaw - y_off;
x_robINmar_coor = z_t * cos(yaw) - y_t * sin(yaw);
y_robINmar_coor = -z_t * sin(yaw) + y_t * cos(yaw);
Mat R(3,3,cv::DataType<float>::type);
}
} else
{
if (yaw > 0)
{
msg_t.pose.orientation.z = yaw_prev;
theta_with_offset = yaw_prev + y_off;
x_robINmar_coor = z_t * cos(yaw_prev) - y_t * sin(yaw_prev);
y_robINmar_coor = -z_t * sin(yaw_prev) + y_t * cos(yaw_prev);
} else
{
yaw_prev = yaw;
msg_t.pose.orientation.z = yaw;
theta_with_offset = yaw + y_off;
x_robINmar_coor = z_t * cos(yaw) - y_t * sin(yaw);
y_robINmar_coor = -z_t * sin(yaw) + y_t * cos(yaw);
}
}
Transpose_pub.publish(msg_t);
/* // yaw has an offset of 180 deg.
if (yaw > 0)
{
theta_with_offset = yaw - y_off;
} else
{
theta_with_offset = yaw + y_off;
}
*/
// You need to apply cv::Rodrigues() in order to obatain angles wrt to camera coords
Rodrigues(TheMarkers[0].Rvec,R);
/*x_robINmar_coor = z_t * cos(yaw) - y_t * sin(yaw);
y_robINmar_coor = -z_t * sin(yaw) + y_t * cos(yaw);*/
/*x_robINmar_coor = z_t * cos(theta_with_offset) - y_t * sin(theta_with_offset);
y_robINmar_coor = -z_t * sin(theta_with_offset) + y_t * cos(theta_with_offset);*/
// Adding Camera frame to Robot Frame ---
tf::Quaternion quat_CAM = tf::createQuaternionFromRPY(0, 0, 0);
broadcaster.sendTransform(tf::StampedTransform(tf::Transform(quat_CAM,tf::Vector3(.25, 0, .5)),ros::Time::now(),"/base_link","/camera"));
/*// Adding Marker frame to Camera Frame ---
tf::Quaternion quat_M = tf::createQuaternionFromRPY(roll,pitch,theta_with_offset);
broadcaster.sendTransform(tf::StampedTransform(tf::Transform(quat_M,tf::Vector3(x_t, y_robINmar_coor, x_robINmar_coor)),ros::Time::now(),"/camera","/marker"));*/
// Adding Marker frame to Camera Frame ---
tf::Quaternion quat_M = tf::createQuaternionFromRPY(roll,pitch,theta_with_offset);
broadcaster.sendTransform(tf::StampedTransform(tf::Transform(quat_M,tf::Vector3(x_t, y_robINmar_coor, x_robINmar_coor)),ros::Time::now(),"/camera","/marker"));
roll = atan2(R.at<float>(1,0), R.at<float>(0,0));
pitch = atan2(-R.at<float>(2,0),pow((pow(R.at<float>(2,1),2)+pow(R.at<float>(2,2),2)),.5));
yaw = atan2(R.at<float>(2,1), R.at<float>(2,2)); // useful
// Adding Camera frame to Robot Frame ---
tf::Quaternion quat_CAM = tf::createQuaternionFromRPY(0, 0, 0);
broadcaster.sendTransform(tf::StampedTransform(tf::Transform(quat_CAM,tf::Vector3(.25, 0, .5)),ros::Time::now(),"/base_link","/camera"));
// Adding Marker frame to Camera Frame ---
tf::Quaternion quat_M = tf::createQuaternionFromRPY(roll,pitch,yaw);
broadcaster.sendTransform(tf::StampedTransform(tf::Transform(quat_M,tf::Vector3(x_t, y_t, z_t)),ros::Time::now(),"/camera","/marker"));
msg.header.frame_id = "/camera";
// Publish Position
msg.pose.position.x = x_t;
msg.pose.position.y = y_t;
msg.pose.position.z = z_t;
msg.pose.position.y = y_robINmar_coor;
msg.pose.position.z = x_robINmar_coor;
// Publish Orientation
msg.pose.orientation.x = roll;
msg.pose.orientation.y = pitch;
msg.pose.orientation.z = yaw;
msg.pose.orientation.z = theta_with_offset;
MarPose_pub.publish(msg);
}
}
/*// Print other rectangles that contains no valid markers
for (unsigned int i=0;i<MDetector.getCandidates().size();i++)
{
Marker m( MDetector.getCandidates()[i],10);
m.draw(TheInputImageCopy,Scalar(0,255,0),2);
}*/
for (unsigned int i=0;i<TheMarkers.size();i++)
for (unsigned int i=0;i<TheMarkers.size();i++)
{
int currentMarID = TheMarkers[i].id;
TheMarkers[i].draw(TheInputImageCopy,Scalar(0,255,0),2) ;
......@@ -387,16 +458,15 @@ if (TheVideoCapturer.retrieve(TheInputImage))
marker_id_string << "marker_ " << currentMarID;
}
if (update_images)
{
imshow("INPUT IMAGE",TheInputImageCopy);
//imshow("THRESHOLD IMAGE",MDetector.getThresholdedImage());
}
}else
{
printf("retrieve failed\n");
}
if (update_images)
{
imshow("INPUT IMAGE",TheInputImageCopy);
imshow("THRESHOLD IMAGE",MDetector.getThresholdedImage());
}
}else
{
printf("retrieve failed\n");
}
key=cv::waitKey(30);
// If space is hit, don't render the image.
......@@ -415,63 +485,31 @@ if (key == ' ')
}
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
camPose[2] = CamFB->pose.position.z; // x_rob
camPose[3] = CamFB->pose.orientation.x; // Robot roll
camPose[4] = CamFB->pose.orientation.y; // Robot pitch
camPose[5] = CamFB->pose.orientation.z; // Robot 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");*/
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");
// 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;
}
if (theta_with_offset > 0)
{
//x_robINmar_coor = -camPose[2] * cos(theta_with_offset) + camPose[1] * sin(theta_with_offset);
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);
} else if (theta_with_offset < 0)
{
//x_robINmar_coor = -camPose[2] * cos(theta_with_offset) - camPose[1] * sin(theta_with_offset);
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);
} else
{
ROS_INFO_STREAM(" Mew condition should be added for theta! \n");
}
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(" yaw = " << yaw << " rad =~ " << yaw*(180.0/CV_PI) << " deg \n");
ROS_INFO_STREAM(" yaw_init = " << yaw_1 << " rad =~ " << yaw_1*(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] - x_robINmar_coor) <= x_dock_thresh)
(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");
......@@ -482,19 +520,19 @@ camPose[5] = CamFB->pose.orientation.z; // Robot yaw
//Go2RandomPose = true;
// to make sure that y & theta are within the threshold...
} else if (abs(RefPose[2] - x_robINmar_coor) <= safety_margin_X)
} else if (abs(RefPose[2] - camPose[2]) <= safety_margin_X)
{
if(
(abs(RefPose[1] - y_robINmar_coor) > y_dock_thresh) ||
(abs(RefPose[3] - theta_with_offset) > theta_dock_thresh)
(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], y_robINmar_coor, RefPose[3], theta_with_offset,.1);
Controller(RefPose[2], RefPose[2], RefPose[1], camPose[1], RefPose[3], camPose[5],.1);
} else if(
(abs(RefPose[1] - y_robINmar_coor) <= y_dock_thresh) &&
(abs(RefPose[3] - theta_with_offset) <= theta_dock_thresh)
(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");
......@@ -503,12 +541,12 @@ camPose[5] = CamFB->pose.orientation.z; // Robot yaw
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);
Controller(RefPose[2], camPose[2], 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);
Controller(RefPose[2], camPose[2], RefPose[1], camPose[1], RefPose[3], camPose[5],.1);
}
} else
{
......@@ -570,11 +608,14 @@ void PID4Docking::Controller(double RefX, double MarPoseX, double RefY, double M
i_termY = Ki_y * int_errorY;
d_termY = Kd_y * diffY;
ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_termY<< " \n");
//ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_termY<< " \n");
control_signalY = p_termY + i_termY + d_termY;
control_signalY = speed_reducer_Y * control_signalY;
prev_errorY = curr_errorY;
control_signalY = -speed_reducer_Y * control_signalY;
// control_signalY = speed_reducer_Y * control_signalY;
prev_errorY = curr_errorY;
} else if ((RefY - MarPoseY) <= y_dock_thresh && (RefY - MarPoseY) >= -y_dock_thresh)
{
......@@ -611,16 +652,17 @@ ROS_INFO_STREAM("pY = " << p_termY << ", iY = " << i_termY << " dY = " << d_term
} else if (abs(RefYAW - MarPoseYAW) <= theta_dock_thresh)
{
control_signalYAW = 0;
} else
{
ROS_INFO("New condition\n");
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");
*/
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)
......
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
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