// Returns squared mahalanobis distance
float Reco::mahalanobis_distance (Eigen::Matrix3f cov, Eigen::Vector3f mean, pcl::PointXYZ pt)
{
Eigen::Vector3f diff (pt.x - mean[0], pt.y - mean[1], pt.z - mean[2]);
//return diff.dot(cov.inverse() * diff);
return sqrt(diff.dot(cov.inverse() * diff));
}
void fi::VPDetectionWrapper::validateVanishingPoint(const std::vector<std::vector< Eigen::Vector2f> > &computed_vp_hypothesis, const Eigen::Matrix3f &cam_calib, Eigen::Vector3f &final_robust_vp_x, Eigen::Vector3f &final_robust_vp_y)
{
Eigen::Matrix3f inv_cam_calib = cam_calib.inverse();
//trans from vps to rays through camera axis, see Z+H Chapter 8, more on single view geometry!
unsigned int num_vps = computed_vp_hypothesis.size();
std::vector< Eigen::Vector3f> computed_vp_hypothesis_x;
std::vector< Eigen::Vector3f> computed_vp_hypothesis_y;
std::vector< Eigen::Vector3f> computed_vp_hypothesis_z;
for (unsigned int i = 0; i < num_vps; i++)
{
std::vector<Eigen::Vector2f> a_vp = computed_vp_hypothesis.at(i);
Eigen::Vector2f a_x = a_vp.at(0);
Eigen::Vector3f x_h, n_x;
x_h(0) = a_x(0);
x_h(1) = a_x(1);
x_h(2) = 1;
n_x = inv_cam_calib * x_h;
n_x = n_x.normalized();
computed_vp_hypothesis_x.push_back(n_x);
Eigen::Vector2f a_y = a_vp.at(1);
Eigen::Vector3f y_h, n_y;
y_h(0) = a_y(0);
y_h(1) = a_y(1);
y_h(2) = 1;
n_y = inv_cam_calib * y_h;
n_y = n_y.normalized();
computed_vp_hypothesis_y.push_back(n_y);
Eigen::Vector2f a_z = a_vp.at(2);
Eigen::Vector3f z_h, n_z;
z_h(0) = a_z(0);
z_h(1) = a_z(1);
z_h(2) = 1;
n_z = inv_cam_calib * z_h;
n_z = n_z.normalized();
computed_vp_hypothesis_z.push_back(n_z);
}
std::vector<Eigen::Vector3f> in_liers_x;
std::vector<Eigen::Vector3f> in_liers_y;
std::vector<Eigen::Vector3f> in_liers_z;
bool found_inliers_x = getRansacInliers(computed_vp_hypothesis_x, in_liers_x);
bool found_inliers_y = getRansacInliers(computed_vp_hypothesis_y, in_liers_y);
bool found_inliers_z = getRansacInliers(computed_vp_hypothesis_z, in_liers_z);
Eigen::VectorXf optimized_vp_x;
Eigen::VectorXf optimized_vp_y;
Eigen::VectorXf optimized_vp_z;
leastQuaresVPFitting(in_liers_x, optimized_vp_x);
leastQuaresVPFitting(in_liers_y, optimized_vp_y);
leastQuaresVPFitting(in_liers_z, optimized_vp_z);
std::cout<<"Vanishing Points Validated"<<std::endl;
//test the angles and see if OK otherwise check again if truelly orthogonal
Eigen::Vector3f vp_x (optimized_vp_x[3], optimized_vp_x[4], optimized_vp_x[5]);;
Eigen::Vector3f vp_y (optimized_vp_y[3], optimized_vp_y[4], optimized_vp_y[5]);
Eigen::Vector3f vp_z (optimized_vp_z[3], optimized_vp_z[4], optimized_vp_z[5]);
Eigen::Vector3f vp_x_centroid (optimized_vp_x[0], optimized_vp_x[1], optimized_vp_x[2]);
Eigen::Vector3f vp_y_centroid (optimized_vp_y[0], optimized_vp_y[1], optimized_vp_y[2]);
Eigen::Vector3f vp_z_centroid (optimized_vp_z[0], optimized_vp_z[1], optimized_vp_z[2]);
float angle_value_radiens_cxy = angleBetweenVectors(vp_x_centroid, vp_y_centroid);
float angle_value_degrees_cxy = pcl::rad2deg(angle_value_radiens_cxy);
float angle_value_radiens_cxz = angleBetweenVectors(vp_x_centroid, vp_z_centroid);
float angle_value_degrees_cxz = pcl::rad2deg(angle_value_radiens_cxz);
float angle_value_radiens_cyz = angleBetweenVectors(vp_y_centroid, vp_z_centroid);
float angle_value_degrees_cyz = pcl::rad2deg(angle_value_radiens_cyz);
float angle_value_radiens_xy = angleBetweenVectors(vp_x, vp_y);
float angle_value_degrees_xy = pcl::rad2deg(angle_value_radiens_xy);
float angle_value_radiens_xz = angleBetweenVectors(vp_x, vp_z);
float angle_value_degrees_xz = pcl::rad2deg(angle_value_radiens_xz);
float angle_value_radiens_yz = angleBetweenVectors(vp_y, vp_z);
float angle_value_degrees_yz = pcl::rad2deg(angle_value_radiens_yz);
//collect only the mean vps
final_robust_vp_x = optimized_vp_x.tail<3> ();
final_robust_vp_y = optimized_vp_y.tail<3> ();
//final_robust_vp_x = vp_x_centroid;
//final_robust_vp_y = vp_y_centroid;
}
std::pair<float, float> worldBallPosFromImgCoords(AL::ALMotionProxy motionProxy,
std::pair<int, int> ballPosCam,
int imgWidth, int imgHeight,
int camera)
{
std::string cameraName = "CameraTop";
if (camera == 1)
{
cameraName = "CameraBottom";
}
// Image coordinates of ball
int u = ballPosCam.first;
int v = ballPosCam.second;
// Angles of observation of the ball
float phi = ((float)u-((float)imgWidth)/2)/(float)imgWidth * img_WFOV;
float theta = ((float)v-((float)imgHeight)/2)/(float)imgHeight * img_HFOV;
// Select the right coordinates for the NAO system!
// x outward from camera, y to the left and z vertically upwards
// Coefficients for line-equation going from NAO camera to the ball
float b_c = -sin(phi);
float c_c = -sin(theta);
float a_c = sqrt((cos(phi)*cos(phi)+cos(theta)*cos(theta))/2);
int space = 2; //FRAME_ROBOT
bool useSensorValues = true;
std::vector<float> transVec =
motionProxy.getTransform(cameraName, space, useSensorValues); // Transform camera -> FRAME_ROBOT
std::vector<float> cameraPos =
motionProxy.getPosition(cameraName, space, useSensorValues); // Camera position in FRAME_ROBOT
// std::cout << "Position of bottom camera: " << std::endl;
// std::cout << cameraPos.at(0) << " " << cameraPos.at(1) << " " << cameraPos.at(2) << std::endl;
// std::cout << cameraPos.at(3) << " " << cameraPos.at(4) << " " << cameraPos.at(5) << std::endl;
// Put the camera transform into an Eigen matrix for easy multiplication
Eigen::Matrix4f trans;
trans <<
transVec[0] , transVec[1] , transVec[2] , transVec[3] ,
transVec[4] , transVec[5] , transVec[6] , transVec[7] ,
transVec[8] , transVec[9] , transVec[10], transVec[11],
transVec[12], transVec[13], transVec[14], transVec[15];
Eigen::Vector4f vec(a_c, b_c, c_c, 1);
// Transform the line equation from NAO camera coordinate system into FRAME_ROBOT
Eigen::Vector4f transformedLine = trans*vec;
// std::cout << "trans*vec = " << transformedLine << std::endl;
// Solve line-plane intersection with plane at z (floor)
// Solution from Wikipedia line-plane intersection article
float z = 0.00;
Eigen::Matrix3f lineMat;
lineMat <<
cameraPos.at(0)-transformedLine[0], 1.0-0.0, 0.0-0.0,
cameraPos.at(1)-transformedLine[1], 0.0-0.0, 1.0-0.0,
cameraPos.at(2)-transformedLine[2], z - z, z - z;
Eigen::Vector3f lineVec;
lineVec << cameraPos.at(0)-0.0, cameraPos.at(1)-0.0, cameraPos.at(2)-z;
Eigen::Vector3f txy = lineMat.inverse()*lineVec;
std::cout << "Ball is at (x, y): (" << txy[1] << ", " << txy[2] << ")" << std::endl;
std::pair<float, float> return_value(txy[1], txy[2]);
return return_value; //Return ball position (x, y)
}
int main(int argc, char * argv[])
{
int width = 640;
int height = 480;
Resolution::getInstance(width, height);
Intrinsics::getInstance(528, 528, 320, 240);
cv::Mat intrinsicMatrix = cv::Mat(3,3,CV_64F);
intrinsicMatrix.at<double>(0,0) = Intrinsics::getInstance().fx();
intrinsicMatrix.at<double>(1,1) = Intrinsics::getInstance().fy();
intrinsicMatrix.at<double>(0,2) = Intrinsics::getInstance().cx();
intrinsicMatrix.at<double>(1,2) = Intrinsics::getInstance().cy();
intrinsicMatrix.at<double>(0,1) =0;
intrinsicMatrix.at<double>(1,0) =0;
intrinsicMatrix.at<double>(2,0) =0;
intrinsicMatrix.at<double>(2,1) =0;
intrinsicMatrix.at<double>(2,2) =1;
Bytef * decompressionBuffer = new Bytef[Resolution::getInstance().numPixels() * 2];
IplImage * deCompImage = 0;
std::string logFile="/home/lili/Kinect_Logs/2015-11-05.00.klg";
// assert(pcl::console::parse_argument(argc, argv, "-l", logFile) > 0 && "Please provide a log file");
RawLogReader logReader(decompressionBuffer,
deCompImage,
logFile,
true);
cv::Mat1b tmp(height, width);
cv::Mat3b depthImg(height, width);
PlaceRecognition placeRecognition(&intrinsicMatrix);
iSAMInterface iSAM;
//Keyframes
KeyframeMap map(true);
Eigen::Vector3f lastPlaceRecognitionTrans = Eigen::Vector3f::Zero();
Eigen::Matrix3f lastPlaceRecognitionRot = Eigen::Matrix3f::Identity();
int64_t lastTime = 0;
OdometryProvider * odom = 0;
//int frame_index = 0;
// uint64_t timestamp;
/*if(true)
{
odom = new FOVISOdometry;
if(logReader.hasMore())
{
logReader.getNext();
Eigen::Matrix3f Rcurr = Eigen::Matrix3f::Identity();
Eigen::Vector3f tcurr = Eigen::Vector3f::Zero();
odom->getIncrementalTransformation(tcurr,
Rcurr,
logReader.timestamp,
(unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0]);
}
}*/
//else
// {
odom = new DVOdometry;
if(logReader.hasMore())
{
logReader.getNext();
DVOdometry * dvo = static_cast<DVOdometry *>(odom);
dvo->firstRun((unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0]);
}
//}
ofstream fout1("camera_pose_DVOMarch28.txt");
ofstream fout2("camera_pose_KeyframeMotionMetric0.1March28.txt");
ofstream fout3("loop_closure_transformationMarch28.txt");
ofstream fout4("camera_pose_after_optimizationMarch28.txt");
ofstream fout5("camera_pose_after_optimizationMarch28DVOCov.txt");
ofstream fout6("camera_pose_after_optimizationMarch28DVOLoopTransCov.txt");
/*
pcl::visualization::PCLVisualizer cloudViewer;
cloudViewer.setBackgroundColor(1, 1, 1);
cloudViewer.initCameraParameters();
cloudViewer.addCoordinateSystem(0.1, 0, 0, 0);
*/
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(cloud->makeShared());
//cloudViewer.addPointCloud<pcl::PointXYZRGB>(cloud->makeShared(), color, "Cloud Viewer");
int loopClosureCount=0;
while(logReader.hasMore())
{
logReader.getNext();
cv::Mat3b rgbImg(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData);
cv::Mat1w depth(height, width, (unsigned short *)&decompressionBuffer[0]);
cv::normalize(depth, tmp, 0, 255, cv::NORM_MINMAX, 0);
cv::cvtColor(tmp, depthImg, CV_GRAY2RGB);
cv::imshow("RGB", rgbImg);
cv::imshow("Depth", depthImg);
char key = cv::waitKey(1);
if(key == 'q')
{
break;
}
else if(key == ' ')
{
key = cv::waitKey(0);
}
if(key == 'q')
{
break;
}
Eigen::Matrix3f Rcurr = Eigen::Matrix3f::Identity();
Eigen::Vector3f tcurr = Eigen::Vector3f::Zero();
// #1
odom->getIncrementalTransformation(tcurr,
Rcurr,
logReader.timestamp,
(unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0]);
fout1<<tcurr[0]<<" "<<tcurr[1]<<" "<<tcurr[2]<<" "<<Rcurr(0,0)<<" "<<Rcurr(0,1)<<" "<<Rcurr(0,2)<<" "<<Rcurr(1,0)<<" "<<Rcurr(1,1)<<" "<<Rcurr(1,2)<<" "<<Rcurr(2,0)<<" "<<Rcurr(2,1)<<" "<<Rcurr(2,2)<<endl;
Eigen::Matrix3f Rdelta = Rcurr.inverse() * lastPlaceRecognitionRot;
Eigen::Vector3f tdelta = tcurr - lastPlaceRecognitionTrans;
//Eigen::MatrixXd covariance = odom->getCovariance();
//Eigen::MatrixXd covariance=Eigen::Matrix<double, 6, 6>::Identity()* 1e-3;
if((Projection::rodrigues2(Rdelta).norm() + tdelta.norm()) >= 0.1)
{
Eigen::MatrixXd covariance = odom->getCovariance();
iSAM.addCameraCameraConstraint(lastTime,
logReader.timestamp,
lastPlaceRecognitionRot,
lastPlaceRecognitionTrans,
Rcurr,
tcurr);
//covariance);
printCovariance(fout5, covariance);
lastTime = logReader.timestamp;
lastPlaceRecognitionRot = Rcurr;
lastPlaceRecognitionTrans = tcurr;
cout<<"before add keyframe"<<endl;
// #2
map.addKeyframe((unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0],
Rcurr,
tcurr,
logReader.timestamp);
fout2<<tcurr[0]<<" "<<tcurr[1]<<" "<<tcurr[2]<<" "<<Rcurr(0,0)<<" "<<Rcurr(0,1)<<" "<<Rcurr(0,2)<<" "<<Rcurr(1,0)<<" "<<Rcurr(1,1)<<" "<<Rcurr(1,2)<<" "<<Rcurr(2,0)<<" "<<Rcurr(2,1)<<" "<<Rcurr(2,2)<<endl;
/*
//Save keyframe
{
cv::Mat3b rgbImgKeyframe(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData);
cv::Mat1w depthImgKeyframe(height, width, (unsigned short *)&decompressionBuffer[0]);
//save keyframe depth
char fileName[1024] = {NULL};
sprintf(fileName, "keyframe_depth_%06d.png", frame_index);
cv::imwrite(fileName, depthImgKeyframe);
//save keyframe rgb
sprintf(fileName, "keyframe_rgb_%06d.png", frame_index);
cv::imwrite(fileName, rgbImgKeyframe);
frame_index ++;
}
*/
int64_t matchTime;
Eigen::Matrix4d transformation;
// Eigen::MatrixXd cov(6,6);
//isam::Covariance(0.001 * Eigen::Matrix<double, 6, 6>::Identity()))
Eigen::MatrixXd cov=0.001 * Eigen::Matrix<double, 6, 6>::Identity();
cout<<"map.addKeyframe is OK"<<endl;
// #3
if(placeRecognition.detectLoop((unsigned char *)logReader.deCompImage->imageData,
(unsigned short *)&decompressionBuffer[0],
logReader.timestamp,
matchTime,
transformation,
cov,
loopClosureCount))
{
//printCovariance(fout6, cov);
cout<<"logReader.timestamp "<<logReader.timestamp<<endl;
cout<<"matchTime "<<matchTime<<endl;
/*
transformation << -0.2913457145219732, 0.228056050293173, -0.9290361201559172, 2.799184934345601,
0.6790194052589797, 0.7333821627861707, -0.03291277242681545, 1.310438143604587,
0.673832562222562, -0.6404225489719699, -0.3685222338703895, 6.988973505496276,
0, 0, 0, 0.999999999999998;
*/
/*
transformation << 0.9998996846969838, 0.003948215234314986, -0.01360265192291004, 0.05847011404293689,
-0.004032877285312574, 0.9999726343121815, -0.006202138950136233, 0.04528938486109094,
0.01357779229749574, 0.006256374606648019, 0.9998882444218992, 0.02203456132723125,
0, 0, 0, 1;
*/
iSAM.addLoopConstraint(logReader.timestamp, matchTime, transformation);//, cov);
fout3<<transformation(0,0)<<" "<<transformation(0,1)<<" "<<transformation(0,2)<<" "<<transformation(0,3)<<" "<<transformation(1,0)<<" "<<transformation(1,1)<<" "<<transformation(1,2)<<" "<<transformation(1,3)<<" "<<transformation(2,0)<<" "<<transformation(2,1)<<" "<<transformation(2,2)<<" "<<transformation(2,3)<<" "<<transformation(3,0)<<" "<<transformation(3,1)<<" "<<transformation(3,2)<<" "<<transformation(3,3)<<endl;
loopClosureCount++;
}
}
if(loopClosureCount>=1)
{
break;
}
}
/*
for(int i=0; i<loopClosureCount;i++)
{
iSAM.addLoopConstraint(placeRecognition.loopClosureConstraints.at(i)->time1,
placeRecognition.loopClosureConstraints.at(i)->time2,
placeRecognition.loopClosureConstraints.at(i)->constraint);
}*/
std::vector<std::pair<uint64_t, Eigen::Matrix4f> > posesBefore;
iSAM.getCameraPoses(posesBefore);
cout<<"It works good before optimization"<<endl;
// #4
double residual =iSAM.optimise();
cout<<"It works good after optimize and before map.applyPoses"<<endl;
// map.applyPoses(isam);
//cout<<"It works good before *cloud=map.getMap and after map.applyPoses(isam)"<<endl;
/*
pcl::PointCloud<pcl::PointXYZRGB> *cloud = map.getMap();
// Write it back to disk under a different name.
// Another possibility would be "savePCDFileBinary()".
cout<<"before storing the point cloud map"<<endl;
pcl::io::savePCDFileASCII ("outputCloudMap03DVODensity005.pcd", *cloud);
cout << "Saved data points to outputMap.pcd." << std::endl;
cout<<"copy data into octomap..."<<endl;
octomap::ColorOcTree tree( 0.05 );
for (size_t i=0; i<(*cloud).points.size(); i++)
{
// 将点云里的点插入到octomap中
tree.updateNode( octomap::point3d((*cloud).points[i].x, (*cloud).points[i].y, (*cloud).points[i].z), true );
}
for (size_t i=0; i<(*cloud).points.size(); i++)
{
tree.integrateNodeColor( (*cloud).points[i].x, (*cloud).points[i].y, (*cloud).points[i].z, (*cloud).points[i].r, (*cloud).points[i].g, (*cloud).points[i].b);
}
tree.updateInnerOccupancy();
tree.write("OctomapColorLab03DVODensity005.ot");
cout<<"please see the done."<<endl;
*/
//pcl::visualization::PCLVisualizer cloudViewer;
// cloudViewer.setBackgroundColor(1, 1, 1);
//cloudViewer.initCameraParameters();
// cloudViewer.addCoordinateSystem(0.1, 0, 0, 0);
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(cloud->makeShared());
//cloudViewer.addPointCloud<pcl::PointXYZRGB>(cloud->makeShared(), color, "Cloud Viewer");
std::vector<std::pair<uint64_t, Eigen::Matrix4f> > newPoseGraph;
iSAM.getCameraPoses(newPoseGraph);
/*
for(unsigned int i = 0; i < newPoseGraph.size(); i++)
{
// file << std::setprecision(6) << std::fixed << (double)newPoseGraph.at(i).first / 1000000.0 << " ";
Eigen::Vector3f trans = newPoseGraph.at(i).second.topRightCorner(3, 1);
Eigen::Matrix3f rot = newPoseGraph.at(i).second.topLeftCorner(3, 3);
fout4 << trans(0) << " " << trans(1) << " " << trans(2) << " ";
Eigen::Quaternionf currentCameraRotation(rot);
//file << currentCameraRotation.x() << " " << currentCameraRotation.y() << " " << currentCameraRotation.z() << " " << currentCameraRotation.w() << "\n";
}*/
for(std::vector<std::pair<uint64_t, Eigen::Matrix4f> >::iterator ite=newPoseGraph.begin(); ite!=newPoseGraph.end(); ite++)
{
Eigen::Matrix3f Roptimized;
Roptimized<<ite->second(0,0), ite->second(0,1), ite->second(0,2),
ite->second(1,0), ite->second(1,1), ite->second(1,2),
ite->second(2,0), ite->second(2,1), ite->second(2,2);
Eigen::Quaternionf quatOptimized(Roptimized);
fout4<<ite->second(0,3)<<" "<<ite->second(1,3)<<" "<<ite->second(2,3)<<" "<<quatOptimized.w()<<" "<<quatOptimized.x()<<" "<<quatOptimized.y()<<" "<<quatOptimized.z()<<endl;
}
cout<<"The number of optimized poses"<<newPoseGraph.size()<<endl;
// drawPoses(poses, cloudViewer, 1.0, 0, 0);
//drawPoses(posesBefore, cloudViewer, 0, 0, 1.0);
//cloudViewer.spin();
delete [] decompressionBuffer;
return 0;
}
void
motion_controller::running(void)
{
cycle_start = ros::Time::now();
// receive path and when received block path receiver until mission completed
if (m_path && !block_path_receiver)
{
nav_msgs::Path temp_path = *m_path;
std::vector<geometry_msgs::PoseStamped> temp_path_vector;
extracted_path.clear();
for( std::vector<geometry_msgs::PoseStamped>::iterator itr = temp_path.poses.begin() ; itr != temp_path.poses.end(); ++itr)
{
temp_path_vector.push_back(*itr);
}
if(!temp_path_vector.empty())
{
ROS_INFO("Path Received!");
}
p_count++;
while(!temp_path_vector.empty())
{
extracted_path.push_back(temp_path_vector.back());
temp_path_vector.pop_back();
}
block_path_receiver = true;
}
// switch status according to the command
switch (m_move)
{
// move forward
case FORWARD:
{
// v is linear speed and gain is angular velocity
v = sqrt(dist)*v_max/(1 + gamma*k*k)*10000/PI;
gain = 0.3265*v*k;
if (fabs(gain) > 600)
{
gain = boost::math::sign(gain) * 600;
}
if(fabs(v)>2000)
{
v = boost::math::sign(v)*2000;
}
// set motor rotation velocity
locomotor->set_vel(floor(v)+floor(gain),-floor(v)+floor(gain));
// when the euler distance is less than 100mm, achieved waypoint
if (distance2d(m_state, m_cmd) < 0.09 && indicator(m_state[2],m_cmd[2],0.5))
{
m_move = IDLE;
}
break;
}
case BACKWARD:
{
v = sqrt(dist) * 0.75/(1+0.2*k_back*k_back) *10000/3.1415926;
gain = 0.3265*v*k_back;
if (fabs(gain) > 600)
{
gain = boost::math::sign(gain) * 600;
}
if(fabs(v)>2000)
{
v = boost::math::sign(v)*2000;
}
locomotor->set_vel(-floor(v)+floor(gain),floor(v)+floor(gain));
if (distance2d(m_state, m_cmd) < 0.09 && indicator(m_state[2],m_cmd[2],0.5))
{
m_move = IDLE;
}
break;
}
case LIFTFORK:
{
// TODO: Lift height should be determined, fork stroke should be determined
printf("Lift and fork!\n");
locomotor->set_vel(0, 0);
sleep(3);
// locomotor->shut_down();
lift_motor->power_on();
if ((fork_count%2) == 1)
// change -243720 to be -223720, wants to liftfork a little bit lower
lift_motor->set_pos(-223720);
else
lift_motor->set_pos(-293720);
sleep(10);
printf("Start Test\n");
// pose correction code inserted here first make sure tag is attached vertically, second camera has no pitch angle relative to the vehicle
if ((fork_count%2) == 1)
{
printf("Start Correction!\n");
int count_detect = 0;
while(ros::ok())
{
if(abs_pose1)
{
Eigen::Quaternion<float> quat;
quat.w() = abs_pose1->pose.pose.orientation.w;
quat.x() = abs_pose1->pose.pose.orientation.x;
quat.y() = abs_pose1->pose.pose.orientation.y;
quat.z() = abs_pose1->pose.pose.orientation.z;
Eigen::Matrix3f Rotation;
Rotation = qToRotation(quat);
Eigen::Matrix3f FixTF;
FixTF << 1, 0, 0,
0, 0, -1,
0, 1, 0;
Rotation = FixTF * Rotation.inverse();
float yaw_error;
yaw_error = getYawFromMat(Rotation);
gain = -3265*(k_angle*yaw_error)/3.1415926;
if(fabs(gain)>150)
{
gain = boost::math::sign(gain) * 150;
}
locomotor->set_vel(floor(gain), floor(gain));
if (fabs(yaw_error*180/3.1415926) < 0.5)
{
locomotor->set_vel(0,0);
printf("Yaw %f corrected!\n", yaw_error*180/3.1415926);
break;
}
}
else
{
locomotor->set_vel(0, 0);
usleep(10000);
count_detect++;
}
ros::spinOnce();
if (count_detect>1000)
{
ROS_WARN("No Tag detected when stoped");
ros::shutdown();
exit(1);
}
}
count_detect = 0;
while(ros::ok())
{
if(abs_pose1)
{
Eigen::Quaternion<float> quat;
quat.w() = abs_pose1->pose.pose.orientation.w;
quat.x() = abs_pose1->pose.pose.orientation.x;
quat.y() = abs_pose1->pose.pose.orientation.y;
quat.z() = abs_pose1->pose.pose.orientation.z;
Eigen::Matrix3f Rotation;
Eigen::Vector3f translation;
translation << abs_pose1->pose.pose.position.x,
abs_pose1->pose.pose.position.y,
abs_pose1->pose.pose.position.z;
Rotation = qToRotation(quat);
translation = translation;
float x_error;
x_error = translation[0];
v = 0.25*(x_error-0.003)*10000/3.1415926;
// printf("x is %f\n", x_error);
// printf("v is %f\n\n", floor(v));
if(fabs(v)>150)
{
v = boost::math::sign(v) * 150;
}
locomotor->set_vel(floor(v), -floor(v));
if (fabs(x_error-0.003) < 0.003)
{
locomotor->set_vel(0, 0);
printf("x %f corrected!\n", (x_error-0.006));
break;
}
}
else
{
locomotor->set_vel(0, 0);
usleep(10000);
count_detect++;
}
ros::spinOnce();
if (count_detect>1000)
{
ROS_WARN("No Tag detected when stoped");
ros::shutdown();
exit(1);
}
}
}
if ((fork_count%2) == 0)
{
int count_detect = 0;
while(ros::ok())
{
if (abs_pose)
{
Eigen::Quaternion<float> quat;
quat.w() = abs_pose->pose.pose.orientation.w;
quat.x() = abs_pose->pose.pose.orientation.x;
quat.y() = abs_pose->pose.pose.orientation.y;
quat.z() = abs_pose->pose.pose.orientation.z;
Eigen::Matrix3f Rotation;
Rotation = qToRotation(quat);
Eigen::Matrix3f fixTF;
fixTF << 1, 0, 0,
0, -1, 0,
0, 0, -1;
Rotation = Rotation.inverse()*fixTF;
m_state[2] = getYawFromMat(Rotation);
gain = 3265*(k_angle*angle(m_cmd[2],m_state[2]))/PI;
if (fabs(gain) > 100)
{
gain = boost::math::sign(gain) * 100;
}
if (fabs(gain) >100)
{
ros::shutdown();
exit(1);
}
locomotor->set_vel(floor(gain),floor(gain));
if (indicator(m_cmd[2], m_state[2], 0.008))
{
locomotor->set_vel(0, 0);
printf("Corrected!\n");
break;
}
}
else
{
locomotor->set_vel(0, 0);
usleep(10000);
count_detect++;
}
ros::spinOnce();
if (count_detect>1000)
{
locomotor->set_vel(0, 0);
ROS_WARN("No Tag detected when stoped");
//ros::shutdown();
//exit(1);
}
}
}
// if we carry out hand hold barcode test, after correction, stop
// ros::shutdown();
// exit(1);
locomotor->shut_down();
sleep(0.5);
// comment following two lines can set make telefork not strech out
telefork_motor->set_pos( boost::math::sign(lift_param)*(-386000) );
sleep(15);
if ((fork_count%2) == 1)
lift_motor->set_pos(-293720);
else
lift_motor->set_pos(-223720);
sleep(3);
telefork_motor->set_pos(0);
sleep(15);
// ros::shutdown();
// exit(1);
lift_motor->shut_down();
locomotor->power_on();
sleep(0.5);
m_move = IDLE;
break;
}
case TURN:
{
v = cos(alpha) * dist* k_dist *10000/PI;
if(fabs(v)>300)
{
v = boost::math::sign(v)*300;
}
gain = 3265*(k_angle*angle(m_cmd[2],m_state[2]))/PI;
if (fabs(gain) > 400)
{
gain = boost::math::sign(gain) * 400;
}
locomotor->set_vel(floor(v)+floor(gain),-floor(v)+floor(gain));
if (indicator(m_state[2],m_cmd[2],0.01))
{
m_move = IDLE;
}
break;
}
case IDLE:
{
locomotor->set_vel(0,0);
if (extracted_path.size()!=0)
{
geometry_msgs::PoseStamped temp_pose = extracted_path.back();
float yaw_ = 2*atan2(temp_pose.pose.orientation.z,temp_pose.pose.orientation.w);
m_cmd << temp_pose.pose.position.x * m_resolution,
temp_pose.pose.position.y * m_resolution,
angle(yaw_,0);
printf("Next Commanded Pose is (%f, %f, %f)\n", m_cmd[0], m_cmd[1], m_cmd[2]);
if ( (fabs(m_cmd[0] - m_state[0])>0.5) && (fabs(m_cmd[1] - m_state[1])>0.5) )
{
printf("Invalid commanded position received!\n");
locomotor->shut_down();
ros::shutdown();
exit(1);
}
if ( (fabs(m_cmd[0] - m_state[0])>0.5) || (fabs(m_cmd[1] - m_state[1])>0.5) )
{
if (fabs(angle(m_cmd[2],m_state[2]))>0.5)
{
printf("Invalid commanded pose orientation received!\n");
locomotor->shut_down();
ros::shutdown();
exit(1);
}
if (fabs(m_cmd[0] - m_state[0])>0.5)
{
if (cos(m_state[2]) * (m_cmd[0] - m_state[0]) > 0)
m_move = FORWARD;
else
m_move = BACKWARD;
}
else
{
if (sin(m_state[2]) * (m_cmd[1] - m_state[1]) > 0)
m_move = FORWARD;
else
m_move = BACKWARD;
}
if (m_move == FORWARD)
printf("Move Forward!\n");
else
printf("Move Backward!\n");
}
else if (fabs(m_cmd[2] - m_state[2])>0.5)
{
m_move = TURN;
printf("Turn Around!\n");
}
else if (temp_pose.pose.position.z!=0)
{
m_move = LIFTFORK;
fork_count++;
lift_param = temp_pose.pose.position.z;
}
else
m_move = IDLE;
extracted_path.pop_back();
}
else
{
if (p_count == 2)
{
lift_motor->power_on();
lift_motor->set_pos(0);
sleep(12);
lift_motor->shut_down();
sleep(0.5);
}
block_path_receiver = false;
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = "world";
pose_msg.header.stamp = ros::Time::now();
pose_msg.pose.position.x = m_state[0];
pose_msg.pose.position.y = m_state[1];
if (block_path_receiver)
pose_msg.pose.position.z = 1;
else
pose_msg.pose.position.z = 0;
pose_msg.pose.orientation.w = cos(m_state[2]/2);
pose_msg.pose.orientation.x = 0;
pose_msg.pose.orientation.y = 0;
pose_msg.pose.orientation.z = sin(m_state[2]/2);
m_posePub.publish(pose_msg);
printf("IDLE!\n");
sleep(1);
m_move = IDLE;
}
break;
}
}
vel.first = (vel.first + locomotor->get_vel().first)/2;
vel.second = (vel.second + locomotor->get_vel().second)/2;
cycle_period = (ros::Time::now() - cycle_start).toSec();
m_state[0] = m_state[0] + (-vel.second+vel.first)/2 * cos(m_state[2]) * 0.018849555 * cycle_period/60;
m_state[1] = m_state[1] + (-vel.second+vel.first)/2 * sin(m_state[2]) * 0.018849555 * cycle_period/60;
m_state[2] = m_state[2] + (vel.first + vel.second)/0.653 * 0.018849555 * cycle_period/60;
if (abs_pose)
{
int id;
id = abs_pose->id - 3;
Eigen::Quaternion<float> quat;
quat.w() = abs_pose->pose.pose.orientation.w;
quat.x() = abs_pose->pose.pose.orientation.x;
quat.y() = abs_pose->pose.pose.orientation.y;
quat.z() = abs_pose->pose.pose.orientation.z;
Eigen::Matrix3f Rotation;
Eigen::Vector3f translation;
translation << abs_pose->pose.pose.position.x,
abs_pose->pose.pose.position.y,
abs_pose->pose.pose.position.z;
Rotation = qToRotation(quat);
translation = -Rotation.inverse()*translation;
Eigen::Matrix3f fixTF;
fixTF << 1, 0, 0,
0, -1, 0,
0, 0, -1;
Rotation = Rotation.inverse()*fixTF;
m_state[0] = translation[0] + m_resolution * (id%10);
m_state[1] = translation[1] + m_resolution * floor(id/10.0);
m_state[2] = getYawFromMat(Rotation) + 0.04;
}
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = "world";
pose_msg.header.stamp = ros::Time::now();
pose_msg.pose.position.x = m_state[0];
pose_msg.pose.position.y = m_state[1];
if (block_path_receiver)
pose_msg.pose.position.z = 1;
else
pose_msg.pose.position.z = 0;
pose_msg.pose.orientation.w = cos(m_state[2]/2);
pose_msg.pose.orientation.x = 0;
pose_msg.pose.orientation.y = 0;
pose_msg.pose.orientation.z = sin(m_state[2]/2);
m_posePub.publish(pose_msg);
delta_y = m_cmd[1]-m_state[1];
if(fabs(m_cmd[1]-m_state[1]) > 1.6)
{
delta_y = boost::math::sign(m_cmd[1]-m_state[1]) * 1.6;
}
delta_x = m_cmd[0]-m_state[0];
if(fabs(m_cmd[0]-m_state[0]) > 1.6)
{
delta_x = boost::math::sign(m_cmd[0]-m_state[0]) * 1.6;
}
beta = angle(m_cmd[2], atan2(delta_y, delta_x));
alpha = angle(m_state[2], atan2(delta_y, delta_x));
beta1 = angle(m_cmd[2]+PI, atan2(delta_y, delta_x));
alpha1 = angle(m_state[2]+3.1415926, atan2(delta_y, delta_x));
dist = sqrt(delta_x*delta_x + delta_y* delta_y);
k = -1/dist * (k2*(alpha-atan(-k1*beta)) + sin(alpha)*(1 + k1/(1+(k1*beta) * (k1*beta))));
k_back = -1/dist * (k2*(alpha1-atan2(-k1*beta1,1)) + sin(alpha1)*(1 + k1/(1+(k1*beta1) * (k1*beta1))));
}