void ServerPathBuildManager::update ( float timeBudget )
{
PerformanceTimer timer;
timer.start();
updateQueue( &gs_highQueue, timer, timeBudget );
updateQueue( &gs_lowQueue, timer, timeBudget );
}
/*! @brief Takes in an action and adds it to the queue.
This function parses the passed action and displays the appropriate information in the table.
It also stores the action as data in the QTableWidgetItem in the first column.
This is done using the function setData().
This function takes in an int and a QVariant.
The int represents a role - this is basically an index into an array or a key into a hashtable.
The role used here is predefined within Qt - Qt::UserRole.
This role is specifically set aside by Qt for the user to use - it should be empty unless the user puts something in it.
The QVariant is the data to be stored. QVariants can store any type of data - even structs!
In this case, an action struct is being stored using the function setValue().
After storing the action into the QVariant, the QVariant can be stored in the QTableWidgetItem using setData().
To extract the data from the QTableWidgetItem, simply call the function data().
This function takes in an int representing a role. To retrieve the data store here, pass it Qt::UserRole.
The function will return a QVariant. The QVariant can be cast into whatever data type is stored within it using the value() function.
In this case, of course, the data type is the struct Action. The form for the function should look like this:
Action act = qvariant.value<Action>();
"Action" here can be replaced with an arbitrary data type.
*/
void Queue::addActionToQueue(Action action) {
QVariant v;
v.setValue(action);
QTableWidgetItem* act = new QTableWidgetItem(0);
act->setData(Qt::UserRole, v);
act->setFlags(act->flags() ^ Qt::ItemIsEditable);
QTableWidgetItem* span = new QTableWidgetItem(0);
span->setFlags(span->flags() ^ Qt::ItemIsEditable);
span->setTextAlignment(Qt::AlignHCenter);
int index = action.queueIndex;
if (action.typ != INITIALIZE || rowCount() == 0) {
insertRow(index);
setRowHeight(index, 20);
}
setItem(index, 0, act);
setItem(index, 1, span);
for (int i=0; i < 5; i++) {
QTableWidgetItem* item = new QTableWidgetItem(0);
item->setFlags(item->flags() ^ Qt::ItemIsEditable);
item->setTextAlignment(Qt::AlignHCenter);
setItem(index, i + 2, item);
}
updateQueue(index, rowCount());
}
/*
* initialize the priority queue with the initial state
* explored <-- empty
* loop do
* if empty(queue) return failure
* node <-- queue.pop()
* if node is goal, then return solution(node)
* add node to explored
* for each action in Action(problem, node) do
* child node <-- childnode(problem, node, action)
* if child is not in queue or expolored, add node to queue
* otherwise, if the child.state is in queue and with a higher pathcost,
* then replace that node with the child
*
*/
bool UCSearch::doSearch(Problem &problem, std::vector<MapNode *> &path)
{
MapNode *init = problem.getInitState();
insertQueue(init);
while(!_queue->isEmpty())
{
MapNode *node = _queue->pop();
if(problem.isGoal(node))
{
node->getPathFromRoot(path);
return true;
}
insertExplored(node);
std::vector<MovAction_t> & action = problem.getActions();
std::vector<MovAction_t>::iterator it;
for(it=action.begin(); it<action.end(); it++)
{
if(*it == MOV_ACT_NOOP) {
continue;
}
MapNode *child = NULL;
int res = 0;
res = problem.movAction(node, *it, &child);
if(res == OP_OK)
{
if(!isInQueue(child) && !isInExplored(child))
{
insertQueue(child);
}
else
{
if(isInQueue(child))
{
//get old one
MapNode *old = findQueueByKey(child->getKey());
if(child->getPathCost() < old->getPathCost())
{
//update;
#ifdef HW1_DEBUG
std::cout <<"before update, child cost: " << child->getPathCost();
std::cout <<", old cost: " << old->getPathCost();
#endif
updateQueue(old, child);
#ifdef HW1_DEBUG
std::cout <<"after, old cost: " << old->getPathCost();
#endif
}
}
}
}
}
}
return false;
}
MagnetModel::MagnetModel(MagnetManager *magnetManager, QObject* parent)
: QAbstractTableModel(parent)
, currentRows(0)
, mman(magnetManager)
{
connect (mman, SIGNAL(updateQueue(bt::Uint32,bt::Uint32)),
this, SLOT(onUpdateQueue(bt::Uint32,bt::Uint32)));
}
// MAIN THREAD
// virtual
S32 LLQueuedThread::update(U32 max_time_ms)
{
if (!mStarted)
{
if (!mThreaded)
{
startThread();
mStarted = TRUE;
}
}
return updateQueue(max_time_ms);
}
void CpuRiscV_Functional::updatePipeline() {
IInstruction *instr;
CpuContextType *pContext = getpContext();
if (dport.valid) {
dport.valid = 0;
updateDebugPort();
}
pContext->pc = pContext->npc;
if (isRunning()) {
fetchInstruction();
}
updateState();
if (pContext->reset) {
updateQueue();
reset();
return;
}
instr = decodeInstruction(cacheline_);
if (isRunning()) {
last_hit_breakpoint_ = ~0;
if (instr) {
executeInstruction(instr, cacheline_);
} else {
pContext->npc += 4;
generateException(EXCEPTION_InstrIllegal, pContext);
RISCV_info("[%" RV_PRI64 "d] pc:%08x: %08x \t illegal instruction",
getStepCounter(),
static_cast<uint32_t>(pContext->pc), cacheline_[0]);
}
}
updateQueue();
handleTrap();
}
// MAIN THREAD
// virtual
S32 LLQueuedThread::update(U32 max_time_ms)
{
return updateQueue(max_time_ms);
}
void groupInterests(Graph * g,Graph * interGraph,Node * n,int mode)
{
int i,j, c1, c2, sum1,sum2, index;
Person *p1, *p = n->obj, * ptmp = NULL;
Node *n1, * inter = NULL;
p->distN = 1;
List * l;
Queue *q = NULL, * end = NULL, * start = NULL, * tmp = NULL; //teams list
InterObj * obj;
InterList * team,*prev1,*prev2;
Interests * inter1, * inter2, *swap;
for(i = 0; i < p->inter_num; i++) //inserting interests
{
if ((n1 = lookupNode(p->interests[0][i],interGraph)) == NULL)
{
p->interests[1][i] = 1;
obj = malloc(sizeof(InterObj));
obj->mmax = 0;
obj->wmax = 0;
obj->next_num = 2; //next team number
obj->team = malloc(sizeof(InterList)); //interest team list
obj->team->next = NULL;
obj->team->iNode = malloc(sizeof(Interests)); //team node
obj->team->iNode->tag = 1; //team number
obj->team->iNode->noe = 1; //team number of members
obj->team->iNode->q = malloc(sizeof(Queue));
obj->team->iNode->q->ID = n->ID; //member ID
obj->team->iNode->q->next = NULL;
inter = malloc(sizeof(Node));
inter->ID = p->interests[0][i];
inter->obj = obj;
insertNode(inter,interGraph);
}
else
{
obj = n1->obj;
p->interests[1][i] = 1;
team = malloc(sizeof(InterList));
team->next = NULL;
team->iNode = malloc(sizeof(Interests));
team->iNode->tag = 1;
team->iNode->noe = 1;
team->iNode->q = malloc(sizeof(Queue));
team->iNode->q->ID = n->ID;
team->iNode->q->next = NULL;
obj->next_num = 2;
obj->team = team;
}
}
end = malloc(sizeof(Queue));
end->ID = n->ID;
end->next = NULL;
q = start = end;
while(q != NULL)
{
n = lookupNode(q->ID,g);
p = n->obj;
l = p->list;
while(l != NULL)
{
n1 = lookupNode(l->neighbor->ID,g);
p1 = n1->obj;
if(!strcmp(p->nProp->prop[2],p1->nProp->prop[2])) //same gender
{
c1 = c2 = 0;
if((sum1 = p->inter_num) == 0)
break;
if((sum2 = p1->inter_num) == 0)
{
l = l->next;
continue;
}
while(c2<sum2)
{
n = lookupNode(q->ID,g);
if (p->interests[0][c1] == p1->interests[0][c2]) //common interest
{
if(p1->interests[1][c2] == 0) //node not found
{
p1->interests[1][c2] = p->interests[1][c1];
updateQueue(p1->interests[0][c2],p->interests[1][c1],n1->ID,interGraph);
}
else if(p->interests[1][c1] == p1->interests[1][c2]) //same team with parent node
{
if(c1 < sum1-1)
c1++;
c2++;
continue;
}
else //node - parent node have different interest team
{
inter = lookupNode(p1->interests[0][c2],interGraph); //common interest in interestGraph
obj = inter->obj;
team = obj->team;
swap = inter1 = inter2 = NULL;
prev1 = prev2 = NULL;
while(inter1 == NULL || inter2 == NULL)
{
if (inter1 == NULL)
{
if(team->iNode->tag == p->interests[1][c1])
inter1 = team->iNode;
else
prev1 = team;
}
if (inter2 == NULL){
if(team->iNode->tag == p1->interests[1][c2])
inter2 = team->iNode;
else
prev2 = team;
}
team = team->next;
}
if(inter1->noe < inter2->noe)
{
swap = inter1;
inter1 = inter2;
inter2 = swap;
prev2 = prev1;
}
inter1->noe += inter2->noe; //updating number of members after the 2 team union
tmp = inter2->q; //absorbed list
while(tmp != NULL)
{
n = lookupNode(tmp->ID,g);
ptmp = n->obj;
index = binary_search(ptmp->interests[0],ptmp->inter_num,p1->interests[0][c2]);
ptmp->interests[1][index] = inter1->tag; //absorbing list
if (tmp->next == NULL) // unifying teams
{
tmp->next = inter1->q;
inter1->q = inter2->q;
break;
}
else
tmp = tmp->next;
}
if (prev2 != NULL)
{
prev1 = prev2->next;
prev2->next = prev2->next->next;
}
else
{
prev1 = obj->team;
obj->team = obj->team->next;
}
free(prev1->iNode); //freeing the absorbed list
free(prev1);
}
if(c1 < sum1-1) //checking for end of interest list
c1++;
c2++;
}
else if(p->interests[0][c1] < p1->interests[0][c2] && c1 < sum1-1) //checking for common interests
c1++;
else
{
if (p1->interests[1][c2] != 0)
{
c2++;
continue;
}
inter = NULL;
obj = NULL;
if ((n = lookupNode(p1->interests[0][c2],interGraph)) == NULL)
{
p1->interests[1][c2] = 1; //marking
inter = malloc(sizeof(Node));
inter->ID = p1->interests[0][c2];
inter->obj = malloc(sizeof(InterObj));
obj = inter->obj;
obj->mmax = 0;
obj->wmax = 0;
obj->next_num = 2;
obj->team = malloc(sizeof(InterList));
obj->team->next = NULL;
obj->team->iNode = malloc(sizeof(Interests));
obj->team->iNode->tag = 1;
obj->team->iNode->noe = 1;
obj->team->iNode->q = malloc(sizeof(Queue));
obj->team->iNode->q->ID = n1->ID;
obj->team->iNode->q->next = NULL;
insertNode(inter,interGraph);
}
else
{
obj = n->obj;
p1->interests[1][c2] = obj->next_num;
team = malloc(sizeof(InterList));
team->next = obj->team;
team->iNode = malloc(sizeof(Interests));
team->iNode->tag = obj->next_num;
team->iNode->noe = 1;
team->iNode->q = malloc(sizeof(Queue));
team->iNode->q->ID = n1->ID;
team->iNode->q->next = NULL;
obj->next_num++;
obj->team = team;
}
c2++;
}
}
if(p1->distN == -1) //checking if we visited the current node in the past
{
p1->distN = 1;
end->next = malloc(sizeof(Queue));
end->next->ID = n1->ID;
end->next->next = NULL;
end = end->next;
}
}
l = l->next;
}
tmp = q;
q = q->next;
free(tmp);
}
//find max and deleting lists
for(i=0;i<interGraph->size;i++)
{
for(j=0;j<interGraph->position[i];j++)
{
obj = interGraph->table[i][j]->obj;
if (obj->next_num == 1)
continue;
while(obj->team != NULL)
{
if (mode == 0) //men's max
{
if(obj->mmax < obj->team->iNode->noe)
obj->mmax = obj->team->iNode->noe;
}
else //women's max
{
if(obj->wmax < obj->team->iNode->noe)
obj->wmax = obj->team->iNode->noe;
}
while(obj->team->iNode->q != NULL)
{
tmp = obj->team->iNode->q;
obj->team->iNode->q = obj->team->iNode->q->next;
free(tmp);
}
team = obj->team;
free(obj->team->iNode);
obj->team = obj->team->next;
free(team);
}
obj->next_num = 1;
obj->team = NULL;
}
}
}
/*! @brief Removes a row from the table (effectively removing an action from the queue).
After removal, this function updates the queue with all of the actions that follow it.
*/
void Queue::removeAction() {
removeRow(rowToRemove);
updateQueue(rowToRemove, rowCount());
}
ompl::base::PlannerStatus ompl::geometric::RRTXstatic::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
// Check if there are more starts
if (pis_.haveMoreStartStates() == true)
{
// There are, add them
while (const base::State *st = pis_.nextStart())
{
auto *motion = new Motion(si_);
si_->copyState(motion->state, st);
motion->cost = opt_->identityCost();
nn_->add(motion);
}
// And assure that, if we're using an informed sampler, it's reset
infSampler_.reset();
}
// No else
if (nn_->size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
// Allocate a sampler if necessary
if (!sampler_ && !infSampler_)
{
allocSampler();
}
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
if (!si_->getStateSpace()->isMetricSpace())
OMPL_WARN("%s: The state space (%s) is not metric and as a result the optimization objective may not satisfy "
"the triangle inequality. "
"You may need to disable rejection.",
getName().c_str(), si_->getStateSpace()->getName().c_str());
const base::ReportIntermediateSolutionFn intermediateSolutionCallback = pdef_->getIntermediateSolutionCallback();
Motion *solution = lastGoalMotion_;
Motion *approximation = nullptr;
double approximatedist = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
auto *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
base::State *dstate;
Motion *motion;
Motion *nmotion;
Motion *nb;
Motion *min;
Motion *c;
bool feas;
unsigned int rewireTest = 0;
unsigned int statesGenerated = 0;
base::Cost incCost, cost;
if (solution)
OMPL_INFORM("%s: Starting planning with existing solution of cost %.5f", getName().c_str(),
solution->cost.value());
if (useKNearest_)
OMPL_INFORM("%s: Initial k-nearest value of %u", getName().c_str(),
(unsigned int)std::ceil(k_rrt_ * log((double)(nn_->size() + 1u))));
else
OMPL_INFORM(
"%s: Initial rewiring radius of %.2f", getName().c_str(),
std::min(maxDistance_, r_rrt_ * std::pow(log((double)(nn_->size() + 1u)) / ((double)(nn_->size() + 1u)),
1 / (double)(si_->getStateDimension()))));
while (ptc == false)
{
iterations_++;
// Computes the RRG values for this iteration (number or radius of neighbors)
calculateRRG();
// sample random state (with goal biasing)
// Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal
// states.
if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ &&
goal_s->canSample())
goal_s->sampleGoal(rstate);
else
{
// Attempt to generate a sample, if we fail (e.g., too many rejection attempts), skip the remainder of this
// loop and return to try again
if (!sampleUniform(rstate))
continue;
}
// find closest state in the tree
nmotion = nn_->nearest(rmotion);
if (intermediateSolutionCallback && si_->equalStates(nmotion->state, rstate))
continue;
dstate = rstate;
// find state to add to the tree
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
// Check if the motion between the nearest state and the state to add is valid
if (si_->checkMotion(nmotion->state, dstate))
{
// create a motion
motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
incCost = opt_->motionCost(nmotion->state, motion->state);
motion->cost = opt_->combineCosts(nmotion->cost, incCost);
// Find nearby neighbors of the new motion
getNeighbors(motion);
// find which one we connect the new state to
for (auto it = motion->nbh.begin(); it != motion->nbh.end();)
{
nb = it->first;
feas = it->second;
// Compute cost using nb as a parent
incCost = opt_->motionCost(nb->state, motion->state);
cost = opt_->combineCosts(nb->cost, incCost);
if (opt_->isCostBetterThan(cost, motion->cost))
{
// Check range and feasibility
if ((!useKNearest_ || distanceFunction(motion, nb) < maxDistance_) &&
si_->checkMotion(nb->state, motion->state))
{
// mark than the motino has been checked as valid
it->second = true;
motion->cost = cost;
motion->parent = nb;
++it;
}
else
{
// Remove unfeasible neighbor from the list of neighbors
it = motion->nbh.erase(it);
}
}
else
{
++it;
}
}
// Check if the vertex should included
if (!includeVertex(motion))
{
si_->freeState(motion->state);
delete motion;
continue;
}
// Update neighbor motions neighbor datastructure
for (auto it = motion->nbh.begin(); it != motion->nbh.end(); ++it)
{
it->first->nbh.emplace_back(motion, it->second);
}
// add motion to the tree
++statesGenerated;
nn_->add(motion);
if (updateChildren_)
motion->parent->children.push_back(motion);
// add the new motion to the queue to propagate the changes
updateQueue(motion);
bool checkForSolution = false;
// Add the new motion to the goalMotion_ list, if it satisfies the goal
double distanceFromGoal;
if (goal->isSatisfied(motion->state, &distanceFromGoal))
{
goalMotions_.push_back(motion);
checkForSolution = true;
}
// Process the elements in the queue and rewire the tree until epsilon-optimality
while (!q_.empty())
{
// Get element to update
min = q_.top()->data;
// Remove element from the queue and NULL the handle so that we know it's not in the queue anymore
q_.pop();
min->handle = nullptr;
// Stop cost propagation if it is not in the relevant region
if (opt_->isCostBetterThan(bestCost_, mc_.costPlusHeuristic(min)))
break;
// Try min as a parent to optimize each neighbor
for (auto it = min->nbh.begin(); it != min->nbh.end();)
{
nb = it->first;
feas = it->second;
// Neighbor culling: removes neighbors farther than the neighbor radius
if ((!useKNearest_ || min->nbh.size() > rrg_k_) && distanceFunction(min, nb) > rrg_r_)
{
it = min->nbh.erase(it);
continue;
}
// Calculate cost of nb using min as a parent
incCost = opt_->motionCost(min->state, nb->state);
cost = opt_->combineCosts(min->cost, incCost);
// If cost improvement is better than epsilon
if (opt_->isCostBetterThan(opt_->combineCosts(cost, epsilonCost_), nb->cost))
{
if (nb->parent != min)
{
// changing parent, check feasibility
if (!feas)
{
feas = si_->checkMotion(nb->state, min->state);
if (!feas)
{
// Remove unfeasible neighbor from the list of neighbors
it = min->nbh.erase(it);
continue;
}
else
{
// mark than the motino has been checked as valid
it->second = true;
}
}
if (updateChildren_)
{
// Remove this node from its parent list
removeFromParent(nb);
// add it as a children of min
min->children.push_back(nb);
}
// Add this node to the new parent
nb->parent = min;
++rewireTest;
}
nb->cost = cost;
// Add to the queue for more improvements
updateQueue(nb);
checkForSolution = true;
}
++it;
}
if (updateChildren_)
{
// Propagatino of the cost to the children
for (auto it = min->children.begin(), end = min->children.end(); it != end; ++it)
{
c = *it;
incCost = opt_->motionCost(min->state, c->state);
cost = opt_->combineCosts(min->cost, incCost);
c->cost = cost;
// Add to the queue for more improvements
updateQueue(c);
checkForSolution = true;
}
}
}
// empty q and reset handles
while (!q_.empty())
{
q_.top()->data->handle = nullptr;
q_.pop();
}
q_.clear();
// Checking for solution or iterative improvement
if (checkForSolution)
{
bool updatedSolution = false;
for (auto &goalMotion : goalMotions_)
{
if (opt_->isCostBetterThan(goalMotion->cost, bestCost_))
{
if (opt_->isFinite(bestCost_) == false)
{
OMPL_INFORM("%s: Found an initial solution with a cost of %.2f in %u iterations (%u "
"vertices in the graph)",
getName().c_str(), goalMotion->cost.value(), iterations_, nn_->size());
}
bestCost_ = goalMotion->cost;
updatedSolution = true;
}
sufficientlyShort = opt_->isSatisfied(goalMotion->cost);
if (sufficientlyShort)
{
solution = goalMotion;
break;
}
else if (!solution || opt_->isCostBetterThan(goalMotion->cost, solution->cost))
{
solution = goalMotion;
updatedSolution = true;
}
}
if (updatedSolution)
{
if (intermediateSolutionCallback)
{
std::vector<const base::State *> spath;
Motion *intermediate_solution =
solution->parent; // Do not include goal state to simplify code.
// Push back until we find the start, but not the start itself
while (intermediate_solution->parent != nullptr)
{
spath.push_back(intermediate_solution->state);
intermediate_solution = intermediate_solution->parent;
}
intermediateSolutionCallback(this, spath, bestCost_);
}
}
}
// Checking for approximate solution (closest state found to the goal)
if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
{
approximation = motion;
approximatedist = distanceFromGoal;
}
}
// terminate if a sufficient solution is found
if (solution && sufficientlyShort)
break;
}
bool approximate = (solution == nullptr);
bool addedSolution = false;
if (approximate)
solution = approximation;
else
lastGoalMotion_ = solution;
if (solution != nullptr)
{
ptc.terminate();
// construct the solution path
std::vector<Motion *> mpath;
while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->parent;
}
// set the solution path
auto path = std::make_shared<PathGeometric>(si_);
for (int i = mpath.size() - 1; i >= 0; --i)
path->append(mpath[i]->state);
// Add the solution path.
base::PlannerSolution psol(path);
psol.setPlannerName(getName());
if (approximate)
psol.setApproximate(approximatedist);
// Does the solution satisfy the optimization objective?
psol.setOptimized(opt_, bestCost_, sufficientlyShort);
pdef_->addSolutionPath(psol);
addedSolution = true;
}
si_->freeState(xstate);
if (rmotion->state)
si_->freeState(rmotion->state);
delete rmotion;
OMPL_INFORM("%s: Created %u new states. Checked %u rewire options. %u goal states in tree. Final solution cost "
"%.3f",
getName().c_str(), statesGenerated, rewireTest, goalMotions_.size(), bestCost_.value());
return {addedSolution, approximate};
}
