PlannerHNS::VehicleState SimulatedTrajectoryFollower::DoOneStep(const double& dt, const PlannerHNS::BehaviorState& behavior,
const std::vector<PlannerHNS::WayPoint>& path, const PlannerHNS::WayPoint& currPose,
const PlannerHNS::VehicleState& vehicleState, const bool& bNewTrajectory)
{
if(bNewTrajectory && path.size() > 0)
{
m_iPrevWayPoint = -1;
UpdateCurrentPath(path);
}
PlannerHNS::VehicleState currState;
if(behavior.state == PlannerHNS::FORWARD_STATE)
{
if(m_Path.size()>0)
{
PrepareNextWaypoint(currPose, vehicleState.speed, vehicleState.steer);
VeclocityControllerUpdate(dt, currState,behavior, currState.speed);
SteerControllerUpdate(currState, behavior, currState.steer);
//currState.speed = 5;
//cout << currState.speed << endl;
currState.shift = PlannerHNS::SHIFT_POS_DD;
}
}
else if(behavior.state == PlannerHNS::STOPPING_STATE)
{
currState.speed = 0;
currState.shift = PlannerHNS::SHIFT_POS_DD;
}
else
{
currState.speed = 0;
currState.shift = PlannerHNS::SHIFT_POS_NN;
}
return currState;
}
// Depth-first traversal of sub-tree (below root_elem)
xmlNodePtr ProtoXml::IterFinder::GetNext()
{
//TRACE("enter ProtoXml::IterFinder::GetNext() prev_elem:%s\n", prev_elem ? (const char*)prev_elem->name : "(null)");
if(NULL == prev_elem)
{
return NULL;
}
else if (prev_elem != root_elem)
{
// We need to skip the subtree of the prev_elem
// This involves moving right or up and right
xmlNodePtr nextElem = prev_elem->next;
while (NULL == nextElem)
{
prev_elem = prev_elem->parent; // move up
iter_depth--;
if (prev_elem != root_elem)
nextElem = prev_elem->next; // move right
else
break; // we're done (returned to root_elem)
}
prev_elem = nextElem;
// Check for match before descending
if (NULL != nextElem)
{
if (!UpdateCurrentPath(iter_depth, (const char*)nextElem->name))
PLOG(PL_WARN, "ProtoXml::IterFinder::GetNext() error: unable to update current path\n");
else if (IsMatch())
return nextElem; // found match, done for now
}
}
// Descend down or across tree
while (NULL != prev_elem)
{
// First, try to move down or the right of the tree
xmlNodePtr nextElem = prev_elem->xmlChildrenNode;
if (NULL == nextElem)
{
if (prev_elem != root_elem)
{
nextElem = prev_elem->next; // no children, try next sibling
}
else
{
prev_elem = NULL; // done, root_elem had no children
return NULL;
}
}
else if (prev_elem != root_elem)
{
iter_depth++;
}
// Second, if needed, move back up the tree and to the right
while (NULL == nextElem)
{
// No child or sibling, so move up a level
// and right to parent's next sibling
prev_elem = prev_elem->parent; // move up
iter_depth--;
if (prev_elem != root_elem)
nextElem = prev_elem->next; // move right
else
break; // we're done (returned to root_elem)
}
prev_elem = nextElem;
if (NULL != nextElem)
{
if (!UpdateCurrentPath(iter_depth, (const char*)nextElem->name))
{
PLOG(PL_WARN, "ProtoXml::IterFinder::GetNext() error: unable to update current path\n");
continue;
}
if (IsMatch()) break; // found match, done for now
}
}
return prev_elem;
} // end ProtoXml::IterFinder::GetNext()
