bool checkFeasibility(){
//ROS_INFO("checking feasibility");
if(!planned)
return true;
//remove first waypoint if nescessary
while(sqrt((pose.point.x-waypoint.point.x)*(pose.point.x-waypoint.point.x)+(pose.point.y-waypoint.point.y)*(pose.point.y-waypoint.point.y))<waypoint_check_distance){
if(current_path.size()>1){
if(segmentFeasibility(startState,current_path[1])){
current_path.pop_front();
setWaypoint(current_path.front());
}else{
//create artificial waypoint so that the tracker keeps sending commands to avoid lock situation
State<2> s = startState + (current_path.front()-startState)*((float)(waypoint_check_distance+0.2)*(1/(current_path.front()-startState).norm()));
setWaypoint(s);
break;
}
}else{
return true;
}
}
//check feasibility
//test feasibility from pose to first waypoint
if(!segmentFeasibility(startState,current_path.front()))
return false;
//test feasibility of rest of path
for(int i=0;i<current_path.size()-1;++i){
if(!segmentFeasibility(current_path[i],current_path[i+1]))
return false;
}
return true;
}
void plan(){
ROS_INFO("planning");
//if((ros::Time::now()-local_map->header.stamp)>ros::Duration(5.0))
// return;
if(keepMoving){
if(planned && current_path.size()>0){
//look for farther free space straighline of previous trajectory
int i=0;
while(i<current_path.size() && segmentFeasibility(startState,current_path[i])){
++i;
}
--i;
State<2> s=startState+(current_path[i]-startState)*std::min((current_path[i]-startState).norm(),1.0f)*(1/(current_path[i]-startState).norm());
//set and send the waypoint
setWaypoint(s);
//plan from that waypoint
startState=s;
}
}else{
stop();
}
planned=false;
planning=true;
//Run the desired planner
//RRTstar_planning();
//MSPP_planning();
Astar_planning();
if(planned){
current_path=std::deque<State<2>>();
current_path.assign(current_path_raw.begin(),current_path_raw.end());
//*
std::cout << "smoothed solution" <<std::endl;
//for(int i=0;i<current_path_raw.size();++i)
for(int i=0;i<1;++i)
smoothTraj();
if(waypointMaxDistance>0)
densifyWaypoints();
/*std::cout << "Path length: " << current_path.size() << std::endl;
for(std::deque<State<2>>::iterator it=current_path.begin(),end=current_path.end();it!=end;++it){
std::cout << (*it) << " -- ";}
std::cout << std::endl << "raw:" <<std::endl;
for(std::deque<State<2>>::iterator it=current_path_raw.begin(),end=current_path_raw.end();it!=end;++it){
std::cout << (*it) << " -- ";
}
std::cout << std::endl;*/
//*/
setWaypoint(current_path.front());
}
planning=false;
}
void CClient :: updateCurrentWaypoint ()
{
setWaypoint(CWaypointLocations::NearestWaypoint(getOrigin(),50,-1,false,true,false,NULL,false,0,false,false,Vector(0,0,0),m_iWaypointShowFlags));
}
void loop() {
api.getMyZRState(myState);
time = api.getTime();
picNum = game.getMemoryFilled();
DEBUG(("%d picture(s) have been taken\n", picNum));
DEBUG(("STATE = %d\n",state));
DEBUG(("ARRAY = %d,%d,%d\n",goodPOI[0],goodPOI[1],goodPOI[2]));
DEBUG(("TARGET = %f,%f,%f\n",target[0],target[1],target[2]));
if(takePic > -1){ //takePic is used to make sure you take an accurate picture.
takePic--;
}
if(time%60 == 0){
for(int i = 0; i < 3; i++){
goodPOI[i] = 1;
}
state = ST_GET_POI;
}
if((time > solarFlareBegin - 25)&&(time < solarFlareBegin)){
if((state < ST_SHADOW)||(state > ST_SHADOW + 9)){
state = ST_SHADOW; //if not in shadow state go there.
}
}
else if(time == solarFlareBegin + 3){
state = ST_GET_POI + 1;
}
else if (game.getNextFlare() != -1) {
solarFlareBegin = api.getTime() + game.getNextFlare();
DEBUG(("Next solar flare will occur at %ds.\n", solarFlareBegin));
}
switch(state){
case ST_GET_POI: //POI selection
closestPOI(goodPOI,POI,23);
getPOILoc(POIproj, POIID, 23);
takePic = 23; //25 probably needs to be changed. only good for the first cycle.
DEBUG(("POI Coors = %f,%f,%f\n",POI[0],POI[1],POI[2]));
state = ST_OUTER;
break;
case ST_GET_POI + 1: //if coming from shadow zone
closestPOI(goodPOI,POI,30);
getPOILoc(POIproj, POIID, 30);
takePic = 30; //25 probably needs to be changed. only good for the first cycle.
DEBUG(("POI Coors = %f,%f,%f\n",POI[0],POI[1],POI[2]));
state = ST_OUTER;
break;
case ST_OUTER: //outer picture
wp = false;
//find closest place to take picture
memcpy(target, POIproj, 3*sizeof(float)); //copy the projected POI location to target
for(int i = 0; i < 3; i++){
target[i] *= 0.465/mathVecMagnitude(POI,3); //dilate target to outer zone
}
mathVecSubtract(facing,origin,target,3);
mathVecNormalize(facing,3);
api.setAttitudeTarget(facing);
if(pathToTarget(myState,target,waypoint)){
setWaypoint(waypoint,originalVecBetween);
state = ST_OUTER + 1;
for (int i = 0; i < 3; i++) tempTarget[i] = target[i];
}
else{
state = ST_OUTER + 2;
}
break;
case ST_OUTER + 1: //something is in the way so go to waypoint
wp = true;
mathVecSubtract(facing,POIproj,target,3);
mathVecNormalize(facing,3);
api.setAttitudeTarget(facing);
if(goToWaypoint(target,waypoint,tempTarget,originalVecBetween)){
goToWaypoint(target,waypoint,tempTarget,originalVecBetween);
}
else{
state = ST_OUTER + 2;
}
break;
case ST_OUTER + 2://go to target
mathVecSubtract(facing,origin,myState,3);
mathVecNormalize(facing,3);
api.setAttitudeTarget(facing);
toTarget();
if(takePic == 0){
game.takePic(POIID);
}
if(game.getMemoryFilled() > checkNum){
checkNum++;
getPOILoc(POIproj, POIID, 5);
state = ST_INNER;
}
break;
case ST_INNER: //inner picture
wp = false;
memcpy(target, POIproj, 3*sizeof(float));
for(int i = 0; i < 3; i++){
target[i] *= 0.34/mathVecMagnitude(POI,3);
}
mathVecSubtract(facing,POIproj,target,3);
mathVecNormalize(facing,3);
api.setAttitudeTarget(facing);
haulAssTowardTarget(target,8);
state = ST_INNER + 1;
break;
case ST_INNER + 1: //after outer picture is taken, rush to inner zone.
mathVecSubtract(facing,POIproj,myState,3);
mathVecNormalize(facing,3);
api.setAttitudeTarget(facing);
if(distance(myState,target)<0.02){
haulAssTowardTarget(target,10);
}
else{
haulAssTowardTarget(target,2);
}
if(game.alignLine(POIID)){
game.takePic(POIID);
}
if(game.getMemoryFilled() > checkNum){
checkNum++;
state = ST_UPLOAD;
}
break;
case ST_SHADOW: //shadow zone
wp = false;
target[0] = 0.39; //arbitrary point in the shadow zone
target[1] = 0.00;
target[2] = 0.00;
if(pathToTarget(myState,target,waypoint)){
setWaypoint(waypoint,originalVecBetween);
goToWaypoint(target,waypoint,tempTarget,originalVecBetween);
state = 31;
}
else{
state = 32;
}
break;
case ST_SHADOW + 1:
wp = true;
if(goToWaypoint(target,waypoint,tempTarget,originalVecBetween)){
goToWaypoint(target,waypoint,tempTarget,originalVecBetween);
}
else{
toTarget();
state = 32;
}
break;
case ST_SHADOW + 2:
mathVecSubtract(facing,earth,myState,3);
mathVecNormalize(facing,3);
api.setAttitudeTarget(facing);
toTarget();
game.uploadPic();
break;
case ST_UPLOAD: //upload, needs to be fixed
for(int i = 0; i < 3; i++){
uploadPos[i] = myState[i] / mathVecMagnitude(myState, 3) * 0.65;
}
mathVecSubtract(facing,earth,uploadPos,3);
mathVecNormalize(facing,3);
api.setAttitudeTarget(facing);
haulAssTowardTarget(uploadPos,1.8);
state = ST_UPLOAD + 1;
break;
case ST_UPLOAD + 1:
//get to the closest place, keep trying to upload
api.setAttitudeTarget(facing);
if(distance(myState,origin)>0.54){
api.setPositionTarget(myState);
game.uploadPic();
}
else{
if(distance(myState,origin)>0.51){
api.setPositionTarget(uploadPos);
}
else{
haulAssTowardTarget(uploadPos,1.8);
}
}
if(picNum == 0){
DEBUG(("LOOKING FOR POI"));
state = ST_GET_POI;
}
break;
}
}
