// determines whether the next interpolated line segment along the path should be used
// by checking the distance the quadcopter has traveled along the line
void checkDistanceTraveled(void)
{
double line_dist = 0;
double line_dist_trav = 0;
if(closestPointIndex != prevClosestPointIndex)
{
line_dist = distanceFormula ( pathPose.poses[closestPointIndex].position.x, pathPose.poses[prevClosestPointIndex].position.x,
pathPose.poses[closestPointIndex].position.y, pathPose.poses[prevClosestPointIndex].position.y );
line_dist_trav = distanceFormula ( closestPointOnLine.pose.position.x, pathPose.poses[prevClosestPointIndex].position.x,
closestPointOnLine.pose.position.y, pathPose.poses[prevClosestPointIndex].position.y );
if( (line_dist_trav/line_dist) < LINE_DIST_RANGE )
{
closestPointIndex = prevClosestPointIndex;
}
}
}
// Outputs a constant velocity term for the sliding mode controller
void calcConstVelTerm(void)
{
double globalAngle = 0;
double vector1[2] = {1, 0};
double vector2[2] = {nextPointClosest.pose.position.x - closestPointOnLine.pose.position.x,
nextPointClosest.pose.position.y - closestPointOnLine.pose.position.y};
double dotProduct = (vector1[0]*vector2[0]) + (vector1[1]*vector2[1]);
double absProduct = distanceFormula(nextPointClosest.pose.position.x, closestPointOnLine.pose.position.x,
nextPointClosest.pose.position.y, closestPointOnLine.pose.position.y);
if(absProduct == 0)
{
std::cout << "---------------------------------------------------------------------\n";
std::cout << "Closest Point(reference):\nIndex: "<< closestPointIndex << "\n" << closestPointOnLine << "\n\n";
std::cout << "Next Point:\n" << nextPointClosest << "\n";
std::cout << "---------------------------------------------------------------------\n\n";
}
//if(absProduct != 0)
//{
globalAngle = acos(dotProduct/absProduct);
//}
//else globalAngle = 0; // FIXME: check output
if(nextPointClosest.pose.position.y - closestPointOnLine.pose.position.y >= 0) // Check if angle is over 180 degrees
{
constVelTerm.linear.x = cos(globalAngle)*PATH_VEL;
constVelTerm.linear.y = -sin(globalAngle)*PATH_VEL;
}
else
{
constVelTerm.linear.x = cos(2*PI - globalAngle)*PATH_VEL;
constVelTerm.linear.y = -sin(2*PI - globalAngle)*PATH_VEL;
}
}
// Interpolates to find closest point on the path using the bisection method
void findClosestPointOnLine(void)
{
double point1[2] = {0,0};
double point2[2] = {0,0};
if(closestPointOnLine.pose.position.x != 0 && closestPointOnLine.pose.position.y != 0) // skip first iteration
{
checkDistanceTraveled();
}
/*if(prevClosestPointIndex > closestPointIndex)
{
closestPointIndex = prevClosestPointIndex + 1;
}*/
point1[0] = pathPose.poses[closestPointIndex].position.x;
point1[1] = pathPose.poses[closestPointIndex].position.y;
point2[0] = pathPose.poses[closestPointIndex + 1].position.x;
point2[1] = pathPose.poses[closestPointIndex + 1].position.y;
nextPointClosest.pose.position.x = point2[0];
nextPointClosest.pose.position.y = point2[1];
double distance1 = 0;
double distance2 = 0;
for(int i=0; i<NUM_ITERATIONS; i++)
{
distance1 = distanceFormula(poseEst.pose.position.x, point1[0], poseEst.pose.position.y, point1[1]);
distance2 = distanceFormula(poseEst.pose.position.x, point2[0], poseEst.pose.position.y, point2[1]);
calculateMidPoints (point2[0], point1[0], point2[1], point1[1]);
if(distance1 < distance2)
{
point2[0] = midpoints[0];
point2[1] = midpoints[1];
}
else
{
point1[0] = midpoints[0];
point1[1] = midpoints[1];
}
}
closestPointOnLine.pose.position.x = midpoints[0];
closestPointOnLine.pose.position.y = midpoints[1];
}
unsigned long verifyWeightsAndAssign(ResultList * results, Node * origin)
{
ResultList * itr = results;
while(itr->next != NULL) {
Node * node1 = findVertexByValue(itr->value, origin);
Node * node2 = findVertexByValue(itr->next->value, origin);
int dist = distanceFormula(node1, node2);
node2->weight = node1->weight + dist;
itr = itr->next;
}
printf("I found the weight: %ld\n", itr->weight);
return itr->weight;
}
void updateAdjacentWeights(Node * internal, Node * origin)
{
EdgeList * tmp = internal->edges;
while(tmp != NULL) {
Node * hld = findVertexByValue(tmp->connectedto, origin);
int dist = distanceFormula(hld, internal);
if((dist + internal->weight) < hld->weight) {
hld->weight = dist + internal->weight;
hld->prev = internal->value;
}
tmp = tmp->next;
}
internal->ingraph = 0;
}
// This function will identify the closest point on the path to the quadcopter
bool calcClosestPointOnPath (void)
{
double closestDistance = 0; // distance from pose estimation to closest point on the path
double tempClosestDistance = 0;
for(int i = 0; i < pathPose.poses.size(); i++)
{
if ( (pathPose.poses[i].position.x == 0) && (pathPose.poses[i].position.y == 0) )
{
//continue;
break;
}
tempClosestDistance = distanceFormula ( pathPose.poses[i].position.x, poseEst.pose.position.x,
pathPose.poses[i].position.y, poseEst.pose.position.y );
if ( (closestDistance == 0) || (closestDistance > tempClosestDistance) )
{
closestDistance = tempClosestDistance;
prevClosestPointIndex = closestPointIndex;
closestPointIndex = i;
}
}
}
void crank(double ***matrix, double ***matrix_np1, int nx, int ny, int nz, double alpha, double dx, double C, int border, int NT){
int i, j, k, l, f, g, h;
double d;
int b0;
int bn;
int when = 1;
double ***temp;
double *tempb;
double *mat_b = (double*) malloc(sizeof(double)* nx*ny*nz);
double *mat_newb = (double*) malloc(sizeof(double)* nx*ny*nz);
double **mat_A = (double**) malloc(sizeof(double*) *nx*ny*nz);
double **augmented = (double**) malloc(sizeof(double*) *nx*ny*nz);
double *data = (double*) malloc(sizeof(double) * nx*nx*ny*ny*nz*nz);
double *augdata = (double*) malloc(sizeof(double) * nx*nx*ny*ny*nz*nz+nx*ny*nz);/*care for last column - b*/
/*now need to make augmented matrix to pass into rref*/
for(h=0;h<nx*ny*nz;h++){
*(augmented+h) = augdata + h*nx*ny*nz+1;/*care for augmented with b*/
}
switch(border){
case 0:
b0 = 0;
bn = 0;
break;
case 1:
b0 = 3;
bn = 3;
break;
case 2:
b0 = nx;
bn = 0;
break;
default :
b0 = 0;
bn = 0;
}
print3DMatrix(matrix, nx, ny, nz, dx, dx, dx, when);
when++;
/*malloc space for A*/
for(i=0;i<nx*ny*nz;i++){
*(mat_A+i) = data + i*nx*ny*nz;
}
/*make matrix b which is 1-D from init 3-D matrix --flattened*/
for(i = 0 ; i< nx ; i++){
for(j = 0 ; j < ny ; j++){
for(k = 0 ; k < nz ; k++){
*(mat_b + i*ny*nz + j*nz + k) = *(*(*(matrix + i) + j) +k);
}
}
}
for(k=0;k<NT;k++){
for(i = 0; i< nx*ny*nz ; i++){
for(j = 0;j< nx*ny*nz ; j++){
if( i == j){
*(*(mat_A+i)+j) = /*mat_b[i]**/1+6.0*C;
}
else if (i == j-1 ){
if( i != 0%nz){
*(*(mat_A+i)+j) = /**(mat_b + i-1)**/(-1.0*C);
}
else if(i ==0%nz && border== 2){
*(*(mat_A+i)+j) = /**(mat_b + (i-1)%nz)**/(-1.0*C);
}
}
else if (i == j+1){
if( i != (nz-1)%(nz-1)){
*(*(mat_A+i)+j) =/* *(mat_b + i+1)**/(-1.0*C);
}
else if(i == (nz-1)%(nz-1) && border == 2){
*(*(mat_A+i)+j) =/* *(mat_b + (i+1)%nz)**/(-1.0*C);
}
}
else if(i == j-ny*nz){
if( i > ny*nz){
*(*(mat_A+i)+j) =/* *(mat_b + i-ny*nz)**/(-1.0*C);
}
else if(i <= ny*nz && border == 2){
*(*(mat_A+i)+j) =/* *(mat_b + (i-ny*nz)%(nx*ny*nz))**/(-1.0*C);
}
}
else if(i == j+ny*nz){
if( i < ny*nz){
*(*(mat_A+i)+j) = /**(mat_b + i+ny*nz)**/(-1.0*C);
}
else if(i >= ny*nz && border == 2){
*(*(mat_A+i)+j) = /**(mat_b + (i+ny*nz)%(nx*ny*nz))**/(-1.0*C);
}
}
else if(i == j+ny){
if( i != (ny-1)%(ny-1)){
*(*(mat_A+i)+j) =/* *(mat_b + i+ny)**/(-1.0*C);
}
else if(i ==(ny-1)%(ny-1) && border == 2){
*(*(mat_A+i)+j) =/* *(mat_b + (i+ny)%ny)**/(-1.0*C);
}
}
else if(i == j-ny){
if( i != 0%ny){
*(*(mat_A+i)+j) =/* *(mat_b + i-ny)**/(-1.0*C);
}
else if(i == 0%ny && border == 2){
*(*(mat_A+i)+j) =/* *(mat_b + (i-ny)%ny)**/(-1.0*C);
}
}
else{
*(*(mat_A+i)+j) = 0.0;
}
}
/*at this point A is created*/
for(g = 0; g< nx*ny*nz ; g++){
for(h = 0;h< nx*ny*nz ; h++){
if(h == nx*ny*nz-1){
*(*(augmented+g)+h) = *(mat_b+g);
}
else{
*(*(augmented+g)+h) = *(*(mat_A+g)+h);
}
}
}
/*at this point i have Ax=b in augmented matrix form to use rref on {augmented | b} */
rref(augmented, nx*ny*nz, nx*ny*nz+1);
/*now i need to extract the nx*ny*nz+1 column to cpy back to my old mat_b, mat_A*/
for(g = 0; g< nx*ny*nz ; g++){
for(h = 0;h< nx*ny*nz ; h++){
if(h == nx*ny*nz-1){
*(mat_newb+g) = *(*(augmented+g)+h);
}
/* else{
*(*(mat_A+g)+h) = *(*(augmented+g)+h);
}*/
}
}
d = distanceFormula(mat_newb, mat_b, nx*ny*nz);
//printf("%f \n",d);
tempb = mat_b;
mat_b = mat_newb;
mat_newb = tempb;
for(f = 0 ; f< nx ; f++){
for(g = 0 ; g < ny ; g++){
for( h = 0 ; h < nz ; h++){
*(*(*(matrix + f) + g) +h) = *(mat_b + f*ny*nz + g*nz + h);
}
}
}
}
if( k==0){
print3DMatrix(matrix, nx, ny, nz, dx, dx, dx, when);
when++;
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "path_follower"); //Ros Initialize
ros::start();
ros::Rate loop_rate(T); //Set Ros frequency to 50/s (fast)
ros::NodeHandle n;
ros::Subscriber pathSub;
ros::Subscriber poseEstSub;
ros::Subscriber centroidEstSub;
ros::Publisher goalPub;
ros::Publisher velPub;
ros::Publisher centroidPub;
ros::Publisher gaussPosition;
pathSub = n.subscribe<geometry_msgs::PoseArray>("/path", 1, pathCallback);
poseEstSub = n.subscribe<geometry_msgs::PoseStamped>("/poseEstimation", 1, poseEstCallback);
centroidEstSub = n.subscribe<tf2_msgs::TFMessage>("/poseEstimationC", 1, centroidEstCallback);
goalPub = n.advertise<geometry_msgs::PoseStamped>("/goal_pose", 1000, true);
centroidPub = n.advertise<geometry_msgs::PoseArray>("/gauss", 1000, true);
velPub = n.advertise<geometry_msgs::Twist>("/path_vel", 1000, true);
gaussPosition = n.advertise<geometry_msgs::PoseStamped>("/poseEstimation", 1000, true);
// Initialize msgs and flags
newPath = false;
closestPointOnLine.pose.position.x = 0;
closestPointOnLine.pose.position.y = 0;
constVelTerm.linear.x = 0;
constVelTerm.linear.y = 0;
constVelTerm.linear.z = 0;
constVelTerm.angular.x = 0;
constVelTerm.angular.y = 0;
constVelTerm.angular.z = 0;
gauss.poses.resize(50);
gPos.header.frame_id="gauss";
while (ros::ok())
{
ros::spinOnce();
if(newPath) // check if a new path has been set
{
findIndexOfLastPointOnPath();
if(isOpenLoop) // path given is OPEN
{
newPath = false; // reset flag
goalPose.pose = (pathPose.poses)[0]; // publish first point on path
goalPose.pose.orientation = tf::createQuaternionMsgFromYaw(0);
if (pathPose.header.frame_id.compare("SWARM")==0){
gauss.poses[0]=goalPose.pose;
gauss.poses[0].position.z=.25;
gPos.pose=goalPose.pose;
gPos.pose.position.z=.25;
centroidPub.publish(gauss);
gaussPosition.publish(gPos);
}
else {
goalPub.publish(goalPose);
}
while( distanceFormula(pathPose.poses[0].position.x, poseEst.pose.position.x,
pathPose.poses[0].position.y, poseEst.pose.position.y) >= BOUNDARY_RADIUS ) // FIXME: Implement this sleep cycle as a function
{
ros::spinOnce();
loop_rate.sleep();
if(newPath || !ros::ok())
{
break;
}
}
closestPointIndex = 0; // initialize to first point in path
while(closestPointIndex != lastPointOnPathIndex) // use interpolation
{
if(newPath || !ros::ok()) // FIXME: break out if a different pose is published
{
break;
}
//std::cout << "goalPose" << gPos <<"\n\n";
//ROS_INFO("on interpolation loop OPEN\n");
findClosestPointOnLine();
closestPointOnLine.pose.orientation = tf::createQuaternionMsgFromYaw(0);
calcConstVelTerm();
// std::cout << "Goal pose:\n" << closestPointOnLine << "\n\n";
//std::cout << "Constant vel:\n" << constVelTerm << "\n\n";
velPub.publish(constVelTerm);
if (pathPose.header.frame_id.compare("SWARM")==0){
gauss.poses[0]=goalPose.pose;
gauss.poses[0].position.z=.25;
gPos.pose=goalPose.pose;
gPos.pose.position.z=.25;
centroidPub.publish(gauss);
gaussPosition.publish(gPos);
}
else {
goalPub.publish(goalPose);
}
//pathPose.poses[closestPointIndex].position.x = 0;
//pathPose.poses[closestPointIndex].position.y = 0;
calcClosestPointOnPath();
ros::spinOnce();
loop_rate.sleep();
}
goalPose.pose = (pathPose.poses)[lastPointOnPathIndex]; // publish final point on path
goalPose.pose.orientation = tf::createQuaternionMsgFromYaw(0);
if (pathPose.header.frame_id.compare("SWARM")==0){
gauss.poses[0]=goalPose.pose;
gauss.poses[0].position.z=.25;
gPos.pose=goalPose.pose;
gPos.pose.position.z=.25;
centroidPub.publish(gauss);
gaussPosition.publish(gPos);
}
else {
goalPub.publish(goalPose);
}
//pathPose.poses[closestPointIndex].position.x = 0;
//pathPose.poses[closestPointIndex].position.y = 0;
constVelTerm.linear.x = 0;
constVelTerm.linear.y = 0;
velPub.publish(constVelTerm);
// FIXME: reset path variables
//prevClosestPointIndex = 0;
}
else // path given is CLOSED
{
newPath = false; // reset flag and set stopping condition for while loop
calcClosestPointOnPath();
sortPathArray(); // array now starts at the closest point index
closestPointIndex = 0;
while( !newPath || ros::ok() ) // while no new path has been published
{
//ROS_INFO("on interpolation loop CLOSED");
findClosestPointOnLine();
closestPointOnLine.pose.orientation = tf::createQuaternionMsgFromYaw(0);
calcConstVelTerm();
//std::cout << "Goal pose:\n" << closestPointOnLine << "\n\n";
//std::cout << "Constant vel:\n" << constVelTerm << "\n\n";
velPub.publish(constVelTerm);
goalPub.publish(closestPointOnLine);
calcClosestPointOnPath();
if (closestPointIndex == lastPointOnPathIndex)
{
closestPointIndex = 0;
}
ros::spinOnce();
loop_rate.sleep();
}
constVelTerm.linear.x = 0;
constVelTerm.linear.y = 0;
velPub.publish(constVelTerm);
//ROS_INFO("finished CLOSED loop");
}
}
loop_rate.sleep();
}
}
