string IKCommandInterface::MouseInputEvent(int mx,int my,bool drag)
{
if(drag) {
if(currentLink >= 0) {
//alter current desired configuration
Config q;
GetEndConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
Vector3 wp = currentDestination;
Vector3 ofs;
Vector3 vv;
viewport->getViewVector(vv);
Real d = (wp-viewport->position()).dot(vv);
viewport->getMovementVectorAtDistance(mx,-my,d,ofs);
currentDestination = wp+ofs;
IKGoal goal;
goal.link = currentLink;
goal.localPosition = currentPoint;
goal.SetFixedPosition(currentDestination);
vector<IKGoal> problem(1,goal);
int iters=100;
bool res=SolveIK(*robot,problem,1e-3,iters,0);
command = robot->q;
sendCommand = true;
}
stringstream ss;
ss<<"Drag "<<currentLink<<" "<<currentPoint<<" "<<mx<<" "<<my<<endl;
return ss.str();
}
else {
Ray3D ray;
GetClickRay(mx,my,ray);
Config q;
GetCurConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
int link;
Vector3 localPos;
RobotInfo* rob=world->ClickRobot(ray,link,localPos);
if(rob) {
currentLink = link;
currentPoint = localPos;
currentDestination = robot->links[link].T_World*localPos;
world->robots[0].view.SetGrey();
world->robots[0].view.colors[currentLink].set(1,1,0);
}
else {
world->robots[0].view.SetGrey();
currentLink = -1;
}
return "";
}
}
string IKCommandInterface::SpaceballEvent(const RigidTransform& T)
{
if(currentLink >= 0) {
RigidTransform Tlink;
Config q;
GetEndConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
Tlink = robot->links[currentLink].T_World;
IKGoal goal;
goal.link = currentLink;
goal.localPosition.setZero();
RigidTransform Tgoal = Tlink*T;
goal.SetFixedRotation(Tgoal.R);
goal.SetFixedPosition(Tgoal.t);
vector<IKGoal> problem(1,goal);
int iters=100;
bool res=SolveIK(*robot,problem,1e-3,iters,0);
command = robot->q;
sendCommand = true;
}
return "";
}
string JointCommandInterface::MouseInputEvent(int mx,int my,bool drag)
{
if(drag) {
if(currentLink >= 0) {
//alter current desired configuration
Config q;
GetEndConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
for(size_t i=0;i<robot->drivers.size();i++) {
if(robot->DoesDriverAffect(i,currentLink)) {
Real val = robot->GetDriverValue(i);
val = Clamp(val+my*0.02,robot->drivers[i].qmin,robot->drivers[i].qmax);
robot->SetDriverValue(i,val);
}
}
command = robot->q;
sendCommand = true;
}
stringstream ss;
ss<<"Drag "<<currentLink<<" "<<mx<<" "<<my<<endl;
return ss.str();
}
else {
Ray3D ray;
GetClickRay(mx,my,ray);
Config q;
GetCurConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
int link;
Vector3 localPos;
RobotInfo* rob=world->ClickRobot(ray,link,localPos);
if(rob) {
currentLink = link;
world->robots[0].view.SetGrey();
world->robots[0].view.colors[currentLink].set(1,1,0);
}
else {
world->robots[0].view.SetGrey();
currentLink = -1;
}
return "";
}
}
void StandardInputProcessor::Spaceball(const RigidTransform& T)
{
if(currentLink >= 0) {
RigidTransform Tlink;
Tlink = GetRobot()->links[currentLink].T_World;
currentDesiredTransform = Tlink*T;
useSpaceball = true;
}
}
void IKCommandInterface::DrawGL()
{
if(currentLink >= 0) {
glPointSize(5.0);
glEnable(GL_POINT_SMOOTH);
glDisable(GL_LIGHTING);
glBegin(GL_POINTS);
glColor3f(0,1,1);
glVertex3v(GetRobot()->links[currentLink].T_World*currentPoint);
glColor3f(0,1,0.5);
glVertex3v(currentDestination);
glEnd();
}
}
void StandardInputProcessor::DrawGL()
{
if(currentLink < 0) return;
Robot* robot=GetRobot();
glPointSize(5.0);
glEnable( GL_POINT_SMOOTH);
glDisable( GL_LIGHTING);
glBegin( GL_POINTS);
glColor3f(0, 1, 1);
glVertex3v(robot->links[currentLink].T_World
* currentPoint);
glColor3f(0, 1, 0.5);
glVertex3v(currentDestination);
glEnd();
}
void PlannerCommandInterface::DrawGL()
{
if(planner && planner->goal) {
CartesianObjective* ikgoal=(CartesianObjective*)(planner->goal);
const IKGoal& goal=ikgoal->ikGoal;
glPointSize(5.0);
glEnable(GL_POINT_SMOOTH);
glDisable(GL_LIGHTING);
glBegin(GL_POINTS);
glColor3f(0,1,1);
glVertex3v(GetRobot()->links[goal.link].T_World*goal.localPosition);
glColor3f(0,1,0.5);
glVertex3v(goal.endPosition);
glEnd();
}
}
string IKPlannerCommandInterface::ActivateEvent(bool enabled)
{
if(!planner) {
printf("IK planner activated\n");
cspace = new SingleRobotCSpace(*world,0,settings);
planner = new RealTimeIKPlanner;
planner->protocol = RealTimePlannerBase::Constant;
planner->currentSplitTime=0.1;
planner->currentPadding=0.05;
planner->robot = GetRobot();
planner->settings = settings;
planner->cspace = cspace;
}
return "";
}
string RRTCommandInterface::ActivateEvent(bool enabled)
{
if(!planner) {
cspace = new SingleRobotCSpace(*world,0,settings);
RealTimeRRTPlanner* p=new RealTimeRRTPlanner;
p->delta = 0.5;
planner = p;
//planner->protocol = RealTimePlannerBase::Constant;
planner->protocol = RealTimePlannerBase::ExponentialBackoff;
planner->currentSplitTime=0.1;
planner->currentPadding=0.05;
//test insufficient padding
//planner->currentPadding=0.01;
planner->robot = GetRobot();
planner->settings = settings;
planner->cspace = cspace;
}
return "";
}
void CFootBotForagingNR::Init(TConfigurationNode& t_node) {
try {
//
// Set Id
// /
Id = s_unIdCounter++;
//
// Initialize sensors/actuators
// /
Wheels = dynamic_cast<CCI_FootBotWheelsActuator* > (GetRobot().GetActuator("footbot_wheels" ));
LEDs = dynamic_cast<CCI_FootBotLedsActuator* > (GetRobot().GetActuator("footbot_leds" ));
RABA = dynamic_cast<CCI_RangeAndBearingActuator*> (GetRobot().GetActuator("range_and_bearing" ));
RABS = dynamic_cast<CCI_RangeAndBearingSensor*> (GetRobot().GetSensor ("range_and_bearing" ));
Proximity = dynamic_cast<CCI_FootBotProximitySensor*> (GetRobot().GetSensor ("footbot_proximity" ));
Light = dynamic_cast<CCI_FootBotLightSensor*> (GetRobot().GetSensor ("footbot_light" ));
Ground = dynamic_cast<CCI_FootBotMotorGroundSensor*>(GetRobot().GetSensor ("footbot_motor_ground"));
//
// Parse XML parameters
// /
// Diffusion algorithm
DiffusionParams.Init(GetNode(t_node, "diffusion"));
// Wheel turning
WheelTurningParams.Init(GetNode(t_node, "wheel_turning"));
// Controller state
StateData.Init(GetNode(t_node, "state"));
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing the foot-bot foraging controller for robot \""
<< GetRobot().GetRobotId() << "\"", ex);
}
// Initialize other stuff
// Create a random number generator. We use the 'argos' category so
// that creation, reset, seeding and cleanup are managed by ARGoS.
RNG = CARGoSRandom::CreateRNG("argos");
Reset();
}
void PredictiveExtrapolationInputProcessor::DrawGL() {
if(!lastObjective) return;
Robot* robot = GetRobot();
if(tracking) {
CartesianTrackingObjective* ptrack = dynamic_cast<CartesianTrackingObjective*>(lastObjective);
assert(ptrack != NULL);
glPointSize(5.0);
glEnable( GL_POINT_SMOOTH);
glDisable( GL_LIGHTING);
glBegin( GL_POINTS);
glColor3f(0, 1, 1);
glVertex3v(robot->links[ptrack->link].T_World
* ptrack->localPosition);
glEnd();
glLineWidth(3.0);
glColor3f(0, 1, 0.5);
glBegin(GL_LINE_STRIP);
for(size_t i=0;i<ptrack->positions.size();i++)
glVertex3v(ptrack->positions[i]);
glEnd();
glLineWidth(1.0);
}
else {
CartesianObjective* pgoal = dynamic_cast<CartesianObjective*>(lastObjective);
assert(pgoal != NULL);
const IKGoal& goal = pgoal->ikGoal;
glPointSize(5.0);
glEnable( GL_POINT_SMOOTH);
glDisable( GL_LIGHTING);
glBegin( GL_POINTS);
glColor3f(0, 1, 1);
glVertex3v(robot->links[goal.link].T_World
* goal.localPosition);
glColor3f(0, 1, 0.5);
glVertex3v(goal.endPosition);
glEnd();
}
}
void StandardInputProcessor::Hover(int mx,int my)
{
if(move) changed = true;
useSpaceball = false;
move = false;
Ray3D ray;
GetClickRay(mx, my, ray);
int link;
Vector3 localPos;
RobotInfo* rob = world->ClickRobot( ray, link, localPos);
Robot* robot = GetRobot();
if (rob) {
currentLink = link;
currentPoint = localPos;
currentDestination = robot->links[currentLink].T_World*localPos;
rob->view.SetGrey();
rob->view.SetColor(currentLink,GLColor(1, 1, 0));
} else {
world->robots[0].view.SetGrey();
currentLink = -1;
}
}
string PlannerCommandInterface::SpaceballEvent(const RigidTransform& T)
{
if(currentLink >= 0) {
RigidTransform Tlink;
Config q;
GetEndConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
Tlink = robot->links[currentLink].T_World;
IKGoal goal;
goal.link = currentLink;
goal.localPosition.setZero();
goal.SetFixedPosition(Tlink.t+T.t);
goal.SetFixedRotation(Tlink.R*T.R);
if(planner) {
CartesianObjective* obj=new CartesianObjective(robot);
obj->ikGoal = goal;
if(planner) planner->Reset(obj);
}
}
return "";
}
const char* ORCRobotGetName(void* robot)
{
return GetRobot(robot)->GetName().c_str();
}
void CFootBotUN::Init(TConfigurationNode& t_node) {
/*
* Get sensor/actuator handles
*
* The passed string (ex. "footbot_wheels") corresponds to the XML tag of the device
* whose handle we want to have. For a list of allowed values, type at the command
* prompt:
*
* $ launch_argos -q actuators
*
* to have a list of all the possible actuators, or
*
* $ launch_argos -q sensors
*
* to have a list of all the possible sensors.
*
* NOTE: ARGoS creates and initializes actuators and sensors internally, on the basis of
* the lists provided the XML file at the <controllers><footbot_diffusion><actuators>
* and <controllers><footbot_diffusion><sensors> sections. If you forgot to
* list a device in the XML and then you request it here, an error occurs.
*/
m_pcWheels = dynamic_cast<CCI_FootBotWheelsActuator*>(GetRobot().GetActuator("footbot_wheels"));
m_pcLEDs = dynamic_cast<CCI_FootBotLedsActuator*>(GetRobot().GetActuator("footbot_leds"));
m_pcProximity = dynamic_cast<CCI_FootBotProximitySensor*>(GetRobot().GetSensor("footbot_proximity"));
//Min distance to the current position to the target point to determine it this is reached (meters)
//GetNodeAttributeOrDefault(t_node, "targetMinPointDistance", m_targetMinPointDistance, m_targetMinPointDistance);
std::string text;
if (NodeExists(t_node, "targetMinPointDistance")) {
GetNodeText(GetNode(t_node, "targetMinPointDistance"), text);
sscanf(text.c_str(), "%f", &m_targetMinPointDistance);
//m_targetMinPointDistance = fmin(MAX_RESOLUTION, m_targetMinPointDistance);
}
printf("Min distance to target point %f in robot %s\n", m_targetMinPointDistance, GetRobot().GetRobotId().c_str());
//////////////////////////////////////////////////////////////
// Initialize things required by the communications
//////////////////////////////////////////////////////////////
m_pcWifiSensor = dynamic_cast<CCI_WiFiSensor*>(GetRobot().GetSensor("wifi"));
m_pcWifiActuator = dynamic_cast<CCI_WiFiActuator*>(GetRobot().GetActuator("wifi"));
str_Me = GetRobot().GetRobotId();
m_iSendOrNotSend = 0;
// How many robots are there ?
CSpace::TAnyEntityMap& tEntityMap = m_cSpace.GetEntitiesByType("wifi_equipped_entity");
numberOfRobots = tEntityMap.size();
printf("%s: There are %d robots [RMAGAN]\n", str_Me.c_str(), numberOfRobots);
// Am I a generator?
amIaGenerator = false;
skipFirstSend = true;
int numGenerators = m_numberOfGenerators * numberOfRobots;
int myID = atoi(str_Me.c_str() + 3);
if (myID < numGenerators)
amIaGenerator = true;
if (amIaGenerator)
printf("%s: There are %d generators, I am on of them\n", str_Me.c_str(), numGenerators);
else
printf("%s: There are %d generators, but not me :-(\n", str_Me.c_str(), numGenerators);
if (amIaGenerator) {
UInt32 id = CSimulator::GetInstance().GetRNG()->Uniform(CRange<UInt32>(numGenerators, numberOfRobots));
//UInt32 id = myID + numGenerators;
std::ostringstream str_tmp(ostringstream::out);
str_tmp << "fb_" << id;
str_Dest = str_tmp.str();
printf(" (dest=%s).\n", str_Dest.c_str());
}
//////////////////////////////////////////////////////////////
// end of communications things here
//////////////////////////////////////////////////////////////
/** LCM engine */
// LCM Thread
lcmThreadCommand.setLCMEngine("udpm://239.255.76.67:7667?ttl=1", "TARGET");
lcmThreadCommand.startInternalThread();
lcmThread.setLCMEngine("udpm://239.255.76.67:7667?ttl=1", "TRACK");
lcmThread.startInternalThread();
/** NAVIGATION AND AVOIDING COLLISION */
/* Additional sensors */
encoderSensor = dynamic_cast<CCI_FootBotEncoderSensor*>(GetRobot().GetSensor("footbot_encoder"));
/* Init navigation methods */
initOdometry();
initLocalNavigation(t_node);
}
void CFootBotUN::ControlStep() {
printf("\n --------- UN %s - Simulation step %d -----------\n", GetRobot().GetRobotId().c_str(), CSimulator::GetInstance().GetSpace().GetSimulationClock());
m_pcLEDs->SetAllColors(CColor::BLUE);
/** Wifi communications section */
std::ostringstream str(ostringstream::out);
if (amIaGenerator) {
if (m_iSendOrNotSend % m_generatePacketInterval == 0) {
if (skipFirstSend) {
skipFirstSend = false;
} else {
sendPackets++;
str << "Hi I'm " << str_Me << " and I say \"Hello " << str_Dest << "\"";
str << ",Hi I'm " << str_Me << " and I say \"Hello " << str_Dest << "\"";
m_pcWifiActuator->SendMessageTo(str_Dest, str.str());
//std::cout << str_Me << " sending\n";
}
}
}
m_iSendOrNotSend++;
//searching for the received msgs
TMessageList t_incomingMsgs;
m_pcWifiSensor->GetReceivedMessages(t_incomingMsgs);
for (TMessageList::iterator it = t_incomingMsgs.begin(); it != t_incomingMsgs.end(); it++) {
receivedPackets++;
std::cout << str_Me << " received: " << it->Payload << " from " << it->Sender << " (total received=)" << receivedPackets << std::endl;
}
/** End of wifi*/
/** LCM for getting current positions and obtaining the next target point */
UInt8 robotID = atoi(GetRobot().GetRobotId().substr(3).c_str());
/* New target point from the COMMAND engine*/
if (lcmThreadCommand.getLcmHandler()->existNode(robotID)) {
Node nodeCommand = lcmThreadCommand.getLcmHandler()->getNodeById(robotID);
targetPosition.Set(nodeCommand.getPosition().GetX(), nodeCommand.getPosition().GetY());
printf("ID %d - TARGET: (%f,%f)\n", robotID, nodeCommand.getPosition().GetX(), nodeCommand.getPosition().GetY());
} else {
if (lcmThread.getLcmHandler()->existNode(robotID)) {
Node nodeCommand = lcmThread.getLcmHandler()->getNodeById(robotID);
targetPosition.Set(nodeCommand.getPosition().GetX(), nodeCommand.getPosition().GetY());
printf("ID %d - TARGET: (%f,%f)\n", robotID, nodeCommand.getPosition().GetX(), nodeCommand.getPosition().GetY());
}
}
if (lcmThread.getLcmHandler()->existNode(robotID)) {
//lcmThread.getLcmHandler()->printNodeListElements();
/** LCM related Node information */
//To get the other nodes locations through LCM
listNodeObstacles = lcmThread.getLcmHandler()->retrieveNodeList();
//Get myself
mySelf = lcmThread.getLcmHandler()->getNodeById(robotID);
printf("ID %d\n", mySelf.getId());
printf("POS (%f,%f)\n", mySelf.getPosition().GetX(), mySelf.getPosition().GetY());
printf("QUAT (%f,%f,%f,%f)\n", mySelf.getOrientation().GetW(), mySelf.getOrientation().GetX(), mySelf.getOrientation().GetY(), mySelf.getOrientation().GetZ());
printf("VEL %d\n", mySelf.getVelocity());
/** From the simulator */
CVector2 oldPosition = position;
CSpace& space = CSimulator::GetInstance().GetSpace();
const std::string stringIdA = GetRobot().GetRobotId();
CFootBotEntity *robotEntityA = static_cast<CFootBotEntity*>(&space.GetEntity(stringIdA));
CVector3 cFootBotPositionA = robotEntityA->GetEmbodiedEntity().GetPosition();
CQuaternion cFootBotOrientationA = robotEntityA->GetEmbodiedEntity().GetOrientation();
CVector3 axis;
CRadians oA;
cFootBotOrientationA.ToAngleAxis(oA, axis);
if (axis.GetZ() < 0)
oA = -oA;
// Position
position = CVector2(cFootBotPositionA.GetX(), cFootBotPositionA.GetY());
// Angle
angle = oA;
// Velocity
//velocity = CVector2(speed, 0);
velocity = CVector2(mySelf.getVelocity(), 0);
/** NAVIGATION AND AVOIDING COLLISION */
updateAgent(agent);
agent->clearObstacles();
addNodesAsObstacles(listNodeObstacles);
if ((targetPosition - position).Length() < m_targetMinPointDistance) { // 2 cm
state = ARRIVED_AT_TARGET;
printf("State: STOPPED\n");
} else {
state = MOVING;
printf("State: MOVING\n");
}
updateNavigation();
}
}
string PlannerCommandInterface::MouseInputEvent(int mx,int my,bool drag)
{
if(drag) {
printf("Dragging event, dragging status %d\n",(int)dragging);
if(!dragging) {
//first click
Ray3D ray;
GetClickRay(mousex,mousey,ray);
Config q;
GetCurConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
int link;
Vector3 localPos;
RobotInfo* rob=world->ClickRobot(ray,link,localPos);
if(rob) {
currentLink = link;
currentPoint = localPos;
currentDestination = robot->links[link].T_World*localPos;
world->robots[0].view.SetGrey();
world->robots[0].view.colors[currentLink].set(1,1,0);
}
else {
world->robots[0].view.SetGrey();
currentLink = -1;
}
dragging = true;
}
if(currentLink >= 0) {
//alter current desired configuration
Config q;
GetEndConfig(q);
Robot* robot=GetRobot();
robot->UpdateConfig(q);
Vector3 wp = currentDestination;
Vector3 ofs;
Vector3 vv;
viewport->getViewVector(vv);
Real d = (wp-viewport->position()).dot(vv);
viewport->getMovementVectorAtDistance(mx,-my,d,ofs);
currentDestination = wp + ofs;
IKGoal goal;
goal.link = currentLink;
goal.localPosition = currentPoint;
goal.SetFixedPosition(currentDestination);
if(planner) {
CartesianObjective* obj=new CartesianObjective(robot);
obj->ikGoal = goal;
planner->Reset(obj);
}
}
stringstream ss;
ss<<"Drag "<<currentLink<<" "<<currentPoint<<" "<<mx<<" "<<my<<endl;
return ss.str();
}
else {
printf("Stop dragging event, dragging status %d\n",(int)dragging);
dragging = false;
mousex=mx;
mousey=my;
return "";
}
}
string PlannerCommandInterface::UpdateEvent()
{
if(!planner) return "";
if(planner->currentPath.ramps.empty()) {
if(GetEndTime() <= GetCurTime()) { //done moving
printf("Planner initialized\n");
Config q;
GetCurConfig(q);
planner->currentPath.Init(GetRobot()->velMax,GetRobot()->accMax);
planner->currentPath.ramps.resize(1);
planner->currentPath.ramps[0].SetConstant(q);
}
else
//wait until done moving
return "";
}
//wait until next planning step, otherwise try planning
if(GetCurTime() < nextPlanTime) return "";
stringstream ss;
//advance modeled trajectory
double t = GetCurTime();
double startPlanTime = t;
ParabolicRamp::DynamicPath before,updatedCurrent;
planner->currentPath.Split(startPlanTime-lastPlanTime,before,updatedCurrent);
assert(updatedCurrent.IsValid());
planner->currentPath = updatedCurrent;
//cout<<"Path advance "<<startPlanTime-lastPlanTime<<endl;
/*
//TODO: debug with mirror config at time startPlanTime, not the queried
//config from the server
Config qcur;
GetCurConfig(qcur);
//double check the assumptions of the planner
if(MaxAbsError(planner->currentPath.ramps.front().x0,qcur)>1e-2) {
cout<<"Major discrepancy between predicted and actual path"<<endl;
cout<<"\tCur predicted config "; PrintConfig(cout,planner->currentPath.ramps.front().x0); cout<<endl;
cout<<"\tCur actual config "; PrintConfig(cout,qcur); cout<<endl;
getchar();
GetCurConfig(qcur);
if(GetEndTime() <= GetCurTime()) {
planner->currentPath.ramps.resize(1);
planner->currentPath.ramps[0].SetConstant(qcur);
updatedCurrent = planner->currentPath;
startPlanTime = GetCurTime();
}
}
*/
//cout<<"End time "<<GetEndTime()<<", predicted end time "<<t+updatedCurrent.GetTotalTime()<<endl;
Timer timer;
Real splitTime,planTime;
bool res=false;
if(currentLink >= 0) {
res=planner->PlanUpdate(splitTime,planTime);
printf("Plan update: time %g, time elapsed %g, result %d\n",t,planTime,(int)res);
ss<<"Plan "<<planTime<<", padding "<<planner->currentPadding<<", split "<<splitTime<<", res "<<res<<endl;
assert(planner->currentPath.IsValid());
}
lastPlanTime = startPlanTime;
//some time elapsed
t = GetCurTime();
//getchar();
if(res){
nextPlanTime = t + splitTime;
//impose hard real time constraint
if(t < startPlanTime + splitTime) {
printf("Plan time elapsed = %g, split time ... %g\n",t-startPlanTime,splitTime);
ParabolicRamp::DynamicPath temp2;
planner->currentPath.Split(splitTime,before,temp2);
assert(planner->currentPath.IsValid());
assert(temp2.IsValid());
SafeTrajClient::MotionResult res=SendPathImmediate(startPlanTime+splitTime,temp2);
if(res == SafeTrajClient::TransmitError) {
//revert path
printf("Warning, unable to send path to controller\n");
planner->currentPath = updatedCurrent;
assert(updatedCurrent.IsValid());
//getchar();
ss<<", failed to send path";
planner->MarkLastFailure();
}
else if(res != SafeTrajClient::Success) {
printf("Hmmm.. failed path check. Should debug!\n");
//revert path
planner->currentPath = updatedCurrent;
assert(updatedCurrent.IsValid());
}
//cout<<"End time "<<GetEndTime()<<", predicted end time "<<lastPlanTime+splitTime+updatedCurrent.GetTotalTime()<<endl;
}
else {
printf("Hard real-time constraint was violated by %gs\n",t-(startPlanTime+splitTime));
//revert path
planner->currentPath = updatedCurrent;
assert(updatedCurrent.IsValid());
planner->MarkLastFailure();
//getchar();
ss<<", violated real-time constraint "<<t-(startPlanTime+splitTime);
}
}
else
nextPlanTime = t;
return ss.str();
}