/* 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);
}
