Skip to content
GitLab
Commit 735ae7ec authored by Farid Alijani's avatar Farid Alijani
Browse files

docking data

parent 4e979f0e
Expand all Hide whitespace changes
Inline Side-by-side
MobileRobot/AugReaMarker/CamMark/camtomar/src/VisionControl.cpp
View file @ 735ae7ec
......@@ -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)
......
MobileRobot/docking_data/Pose_center.txt
View file @ 735ae7ec
This diff is collapsed.
MobileRobot/docking_data/Pose_center.txt~ 0 → 100644
View file @ 735ae7ec
This diff is collapsed.
MobileRobot/docking_data/Pose_left.txt
View file @ 735ae7ec
This diff is collapsed.
MobileRobot/docking_data/Pose_left.txt~ 0 → 100644
View file @ 735ae7ec
This diff is collapsed.
MobileRobot/docking_data/Pose_right.txt
View file @ 735ae7ec
This diff is collapsed.
MobileRobot/docking_data/Pose_right.txt~ 0 → 100644
View file @ 735ae7ec
This diff is collapsed.
MobileRobot/docking_data/Velocity_center.txt~ 0 → 100644
View file @ 735ae7ec
This diff is collapsed.
MobileRobot/docking_data/Velocity_left.txt
View file @ 735ae7ec
This diff is collapsed.
MobileRobot/docking_data/Velocity_left.txt~ 0 → 100644
View file @ 735ae7ec
This diff is collapsed.
MobileRobot/docking_data/Velocity_right.txt
View file @ 735ae7ec
This diff is collapsed.
MobileRobot/docking_data/Velocity_right.txt~ 0 → 100644
View file @ 735ae7ec
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