/* QUERY -- Query the user for the value of a parameter. Prompt with the * current value if any. Keep this up until we can push a reasonable value. * Also, store the new value in the parameter (except for list params, where, * since the values are not kept, all that may change is P_LEOF if seen). * Give prompt, or name if none, current value and range if int, real or * filename. Accept CR to leave value unchanged, else take the string * entered to be the new value. Repeat until parameter value is in range. * We mean to talk straight to the user here; thus, interact with the real * stdio, not the effective t_stdio, so that redirections do not get in * the way. In batch mode, a forced query is handled by writing a * message on the terminal of the parent cl (the original stderr), and * leaving some info describing the query in a file in uparm (if there is * no uparm, we abort). We then loop, waiting for the user to run "service" * in the interactive cl to service the query, leaving the answer in a * another file which we read and then delete. If we wait a long time and * get no response, we timeout. */ void query (struct param *pp) { static char *oormsg = "ERROR: Parameter value is out of range; try again"; register char *ip; char buf[SZ_PROMPTBUF+1]; struct operand o; int bastype, batch, arrflag, offset=0, n_ele, max_ele, fd; char *index(), *nlp, *nextstr(); char *bkg_query(), *query_status; char *abuf; bastype = pp->p_type & OT_BASIC; batch = firstask->t_flags & T_BATCH; arrflag = pp->p_type & PT_ARRAY; if (arrflag) { /* We may access the array many */ offset = getoffset (pp); /* times, so save the offset and */ /* push it when necessary. */ poffset (offset); max_ele = size_array (pp) - offset; } else max_ele = 1; forever { if (batch) { /* Query from a background job. */ query_status = bkg_query (buf, SZ_PROMPTBUF, pp); } else if (pp->p_type & (PT_GCUR|PT_IMCUR)) { /* Read a graphics cursor. */ char source[33]; int cursor; /* Determine the source of graphics cursor input, chosen from * either the graphics or image cursor or the terminal. */ if (pp->p_type & PT_GCUR) { if (c_envfind ("stdgcur", source, 32) <= 0) strcpy (source, "stdgraph"); } else { if (c_envfind ("stdimcur", source, 32) <= 0) strcpy (source, "stdimage"); } if (strcmp (source, "stdgraph") == 0) cursor = STDGRAPH; else if (strcmp (source, "stdimage") == 0) cursor = STDIMAGE; else goto text_query; /* get value from terminal */ /* Read a physical graphics cursor. */ pp->p_flags &= ~P_LEOF; if (cursor == STDIMAGE) { /* The following is a kludge used to temporarily implement * the logical image cursor read. In the future this will * be eliminated, and the c_rcursor call below (cursor * mode) will be used for stdimage as well as for stdgraph. * The present code (IMDRCUR) goes directly to the display * server to get the cursor value, bypassing cursor mode * and the (currently nonexistent) stdimage kernel. */ char str[SZ_LINE+1], keystr[10]; int wcs, key; float x, y; if (c_imdrcur ("stdimage", &x,&y,&wcs,&key,str,SZ_LINE, 1, 1) == EOF) { query_status = NULL; } else { if (isprint(key) && !isspace(key)) sprintf (keystr, "%c", key); else sprintf (keystr, "\\%03o", key); sprintf (buf, "%.3f %.3f %d %s %s\n", x, y, wcs, keystr, str); query_status = (char *) ((XINT) strlen(buf)); } } else if (c_rcursor (cursor, buf, SZ_PROMPTBUF) == EOF) { query_status = NULL; } else query_status = (char *) ((XINT) strlen(buf)); } else if (pp->p_type & PT_UKEY) { /* Read a user keystroke command from the terminal. */ pp->p_flags &= ~P_LEOF; if (c_rdukey (buf, SZ_PROMPTBUF) == EOF) query_status = NULL; else query_status = (char *) ((XINT) strlen(buf)); } else { text_query: fd = spf_open (buf, SZ_PROMPTBUF); pquery (pp, fdopen(fd,"a")); spf_close (fd); c_stgputline ((XINT)STDOUT, buf); if (c_stggetline ((XINT)STDIN, buf, SZ_PROMPTBUF) > 0) query_status = (char *) ((XINT) strlen(buf)); else query_status = NULL; } ip = buf; /* Set o to the current value of the parameter. Beware that some * of the logical branches which follow assume that struct o has * been initialized to the current value of the parameter. */ if (pp->p_type & PT_LIST) setopundef (&o); else if (arrflag) { paramget(pp, FN_VALUE); poffset (offset); o = popop(); } else o = pp->p_valo; /* Handle eof, a null-length line (lone carriage return), * and line with more than SZ_LINE chars. Ignore leading whitespace * if basic type is not string. */ if (query_status == NULL) { /* Typing eof will use current value (as will a lone * newline) but if param is a list, it is a meaningful * answer. */ if (pp->p_type & PT_LIST) { closelist (pp); /* close an existing file */ pp->p_flags |= P_LEOF; o = makeop (eofstr, OT_STRING); break; } goto testval; } /* Ignore leading whitespace if it is not significant for this * datatype. Do this before testing for empty line, so that a * return such as " \n" is equivalent to "\n". I.e., do not * penalize the user if they type the space bar by accident before * typing return to accept the default value. */ if (bastype != OT_STRING || (pp->p_type & (PT_FILNAM|PT_PSET))) while (*ip == ' ' || *ip == '\t') ip++; if (*ip == '\n') { /* Blank lines usually just accept the current value * but if the param is a string and is undefined, * it sets the string to a (defined) nullstring. */ *ip = '\0'; if (bastype == OT_STRING && opundef (&o)) o = makeop (ip, bastype); else goto testval; } if ((nlp = index (ip, '\n')) != NULL) *nlp = '\0'; /* cancel the newline */ else goto testval; /* Finally, we have handled the pathological cases... */ if ((pp->p_type & PT_LIST) && (!strcmp (ip,eofstr) || !strcmp (ip,"eof"))) { closelist (pp); pp->p_flags |= P_LEOF; o = makeop (eofstr, OT_STRING); break; } else { if (arrflag) { /* In querying for arrays we may set more than one * element of the array in a single query. However * we must set the first element. So we will pretend * to be a scalar until that first element is set * and then enter a loop where we may set other * elements. */ abuf = ip; ip = nextstr(&abuf, stdin); if (ip == NULL || ip == (char *) ERR || ip == undefval) goto testval; } o = makeop (ip, bastype); } testval: /* If parameter value is in range, we are done. If it is out of * range and we are a batch job or an interactive terminal job, * print an error message and request that the user enter a legal * value. If the CL is being run taking input from a file, abort, * else we will go into a loop reading illegal values from the * input file and printing out lots of error messages. */ if (inrange (pp, &o)) break; else if (batch) eprintf ("\n[%d] %s", bkgno, oormsg); else if (isatty (fileno (stdin))) eprintf ("%s\n", oormsg); else cl_error (E_UERR, oormsg); } if (!(pp->p_type & PT_LIST)) { /* update param with new value. */ if (cldebug) { eprintf ("changing `%s.p_val' to ", pp->p_name); fprop (stderr, &o); eprintf ("\n"); } pushop (&o); paramset (pp, FN_VALUE); pp->p_flags |= P_QUERY; } pushop (&o); if (arrflag && query_status != NULL && *ip != '\0') { /* If we have an array assign values until something * is used up or until we hit any error. */ n_ele = 1; forever { if (n_ele >= max_ele) /* End of array. */ break; ip = nextstr(&abuf, stdin); if (ip == NULL) /* End of query line. */ break; if (ip == (char *) ERR) { /* Error on query line. */ eprintf("Error loading array value.\n"); break; } if (ip != undefval) { o = makeop (ip, bastype); if ( ! inrange (pp, &o) ) { /* Not in range. */ eprintf("Array value outside range.\n"); break; } offset++; /* Next element in array. */ poffset (offset); pushop (&o); paramset (pp, FN_VALUE); } else offset++; n_ele++; } }
vector<Grid> Planner::aStarPlanning(Map &map) { // implemtation of the a star algorithm // initialize the way point list vector<Grid> wayPoint; // return an empty list if map is invalid if(map.start.x==-1||map.start.y==-1||map.goal.x==-1||map.goal.y==-1) return wayPoint; // type of the A star priority list element typedef struct {Grid pos; float f;} AStarUnit; list<AStarUnit> openlist; // open list vector<vector<float> > h(map._rows, vector<float>(map._cols, -1.0)); // heuristic value vector<vector<float> > g(map._rows, vector<float>(map._cols, -1.0)); // optimized value of the grid vector<vector<float> > f(map._rows, vector<float>(map._cols, -1.0)); // the key value of the A star algorithm (search priority) vector<vector<Grid> > b(map._rows, vector<Grid>(map._cols, Grid(-1,-1))); // back pointer to pred node vector<vector<bool> > closelist(map._rows, vector<bool>(map._cols, false)); // close list // value initialization h[map.start.y][map.start.x] = calculateDistance(map.start,map.goal); g[map.start.y][map.start.x] = 0; f[map.start.y][map.start.x] = h[map.start.y][map.start.x]; AStarUnit startP={map.start,calculateDistance(map.start,map.goal)}; openlist.push_back(startP); while (openlist.size()>0) { // iterate when the open list is not empty // pop the first item in the open list to expand Grid currentP = openlist.front().pos; closelist[currentP.y][currentP.x] = true; openlist.pop_front(); // if already reach the goal, stop iterating if (currentP==map.goal) break; // find its direct neigbour vector<Grid> neighbourList = findNeighbour(map,currentP,4); for(Grid neighbour:neighbourList) { // update the shorest distance of the cells neighbour if a shorter path is finded if (((g[neighbour.y][neighbour.x]==-1.0)||(g[neighbour.y][neighbour.x]>g[currentP.y][currentP.x]+calculateDistance(neighbour,currentP)))&&(!closelist[neighbour.y][neighbour.x])) { h[neighbour.y][neighbour.x] = calculateDistance(neighbour,map.goal); g[neighbour.y][neighbour.x] = g[currentP.y][currentP.x]+calculateDistance(neighbour,currentP); f[neighbour.y][neighbour.x] = h[map.start.y][map.start.x]+g[map.start.y][map.start.x]; b[neighbour.y][neighbour.x] = currentP; float _f =f[map.start.y][map.start.x]; // find the correct position to insert the cell into the priority list, according to f value auto insert_pos = find_if(openlist.begin(),openlist.end(),[_f](AStarUnit list_elem){return _f<list_elem.f;}); openlist.insert(insert_pos,{neighbour,_f}); } } } // form the waypoint list from the goal cell in the map deque<Grid> tempWayPoint; Grid trace = map.goal; while (trace!=Grid(-1.0,-1.0)) { tempWayPoint.push_front(trace); trace = b[trace.y][trace.x]; } wayPoint.assign(tempWayPoint.cbegin(),tempWayPoint.cend()); return wayPoint; }
/* RESTOR -- Restor all global variables for the given task and insure the * integrity of the dictionary and control stack. * Go through the dictionary and properly disgard any packages, ltasks, * pfiles, environments and params that may be above the new topd. * Write out any pfiles that are not just working copies that have been * updated before discarding them. * Don't call error() because in trying to restor to an interactive task * it might call us again and cause an inf. loop. Instead, issue fatal error * which will kill the cl for good. This seems reasonable since we might * as well die if we can't restor back to an interactive state. * N.B. we assume that a pfile's params will either all lie above or all * below tp->t_topd. If this can ever happen, must add a further check * of each pfile below topd and lob off any params above topd. * The way posargset, et al, and call/execnewtask are now, we are safe. */ void restor ( struct task *tp ) { memel *topdp; register struct ltask *ltp; register struct package *pkp; register struct param *pp; register struct pfile *pfp; struct param *last_pp; int n; if (cldebug) { eprintf ("restoring task `%s', tp: %d\n", tp->t_ltp->lt_lname,tp); eprintf (" topd %d/%d\n", topd, tp->t_topd); } topd = tp->t_topd; pc = tp->t_pc; topos = tp->t_topos; basos = tp->t_basos; topcs = tp->t_topcs; curpack = tp->t_curpack; yyin = tp->t_in; parse_state = PARSE_FREE; topdp = daddr (topd); /* Set pachead to first package below new topd. Then lob off any ltasks * all remaining packages might have above topd. It is sufficient to * stop the ltask checks for a given package once find an ltask * below topd since the dictionary always grows upward. * (Recall that since new ltasks are always added at the top of the * dictionary, and pkp->pk_ltp always points to the most recently * added ltask, then the thread moves to lower and lower addrs.) * Thus, work downward and throw out all ltasks until find one below * the new topd. */ for (pkp = reference (package, pachead); pkp; pkp = pkp->pk_npk) if ((memel)pkp < (memel)topdp) { pachead = dereference (pkp); break; } if (pkp == NULL) cl_error (E_FERR, "package list broken"); for (; pkp; pkp = pkp->pk_npk) { for (ltp = pkp->pk_ltp; ltp; ltp = ltp->lt_nlt) if ((memel)ltp < (memel)topdp) { pkp->pk_ltp = ltp; break; } if ((memel)pkp->pk_ltp >= (memel)topdp) /* All ltasks in this package were above topd */ pkp->pk_ltp = NULL; } /* Similarly for pfiles and their params; however, since new params * are always added at the top of the dictionary and linked in at the * END of the list (at pfp->pf_lastpp), the thread off pfp->pf_pp * moves to higher and higher addrs. Thus, we work our way up and * throw out all params above the new topd. Also, close off any open * list files from discarded params along the way, if any. * Also, see if any of the params were P_SET and set PF_UPDATE. * This avoids having to set PF_UPDATE for each assignment when the * is not always easily found. * N.B. hope mode param that some t_modep is using is never disgarded.. * Also, guard against writing out pfiles in background. */ for (pfp = reference (pfile, parhead); pfp; pfp = pfp->pf_npf) { /* Lob off any pfiles above new topd. Go through their * params, updating if necessary and closing any lists. */ if ((memel)pfp < (memel)topdp) { parhead = dereference (pfp); break; } for (pp = pfp->pf_pp; pp != NULL; pp = pp->p_np) { /* Close if list file and enable flushing if P_SET. */ if (pp->p_type & PT_LIST) closelist (pp); if (pp->p_flags & P_SET) pfp->pf_flags |= PF_UPDATE; } if (((pfp->pf_flags & (PF_UPDATE|PF_COPY)) == PF_UPDATE) && !(firstask->t_flags & T_BATCH)) pfileupdate (pfp); } /* Discard any recently added parameters above topd, where the pfile * itself is below topd. This happens when a new parameter is added * to an existing incore pfile, e.g., in a declaration. */ for (; pfp; pfp = pfp->pf_npf) { if ((memel)(pfp->pf_lastpp) < (memel)topdp) continue; /* quick check */ last_pp = NULL; n = 0; for (pp = pfp->pf_pp; pp != NULL; pp = pp->p_np) { if ((memel)pp >= (memel)topdp) { if (cldebug) fprintf (stderr, "chop pfile for task %s at param %s\n", pfp->pf_ltp->lt_lname, last_pp->p_name); if (last_pp) last_pp->p_np = NULL; pfp->pf_lastpp = last_pp; pfp->pf_n = n; break; } else { last_pp = pp; n++; } } } /* Delete any SET environment statements processed since this task * was spawned. If any redefs are uncovered the original values are * reset in all connected subprocesses. */ if (tp->t_envp) c_prenvfree (0, tp->t_envp); /* If the task being restored defined a package, dump all processes * in the process cache spawned since the package was loaded. */ if (tp->t_pno) pr_prunecache (tp->t_pno); }