void EsterelPlanDispatcher::planCallback(const rosplan_dispatch_msgs::EsterelPlan plan) {
if(finished_execution) {
ROS_INFO("KCL: (%s) Plan received.", ros::this_node::getName().c_str());
plan_received = true;
mission_start_time = ros::WallTime::now().toSec();
current_plan = plan;
printPlan();
} else {
ROS_INFO("KCL: (%s) Plan received, but current execution not yet finished.", ros::this_node::getName().c_str());
}
}
void MainWindow::on_startButton_clicked()
{
row = ui->tableProvider->rowCount();
column = ui->tableCustomer->columnCount();
fillProviders();
fillCustomers();
fillTariff();
if (isClosed())
{
createPlan();
findUV();
printUV();
while (!planIsGood()) {
findBadCell();
findCycle();
findMinAndBuildNewPlan();
findUV();
printUV();
};
};
printPlan();
}
/*
* Loop through and publish planned actions
*/
bool EsterelPlanDispatcher::dispatchPlan(double missionStartTime, double planStartTime) {
ROS_INFO("KCL: (%s) Dispatching plan.", ros::this_node::getName().c_str());
ros::Rate loop_rate(10);
replan_requested = false;
plan_cancelled = false;
// initialise machine
initialise();
// begin execution
finished_execution = false;
state_changed = false;
bool plan_started = false;
while (ros::ok() && !finished_execution) {
// loop while dispatch is paused
while (ros::ok() && dispatch_paused) {
ros::spinOnce();
loop_rate.sleep();
}
// cancel plan
if(plan_cancelled) {
ROS_INFO("KCL: (%s) Plan cancelled.", ros::this_node::getName().c_str());
break;
}
finished_execution = true;
state_changed = false;
// for each node check completion, conditions, and dispatch
for(std::vector<rosplan_dispatch_msgs::EsterelPlanNode>::const_iterator ci = current_plan.nodes.begin(); ci != current_plan.nodes.end(); ci++) {
rosplan_dispatch_msgs::EsterelPlanNode node = *ci;
// activate plan start edges
if(node.node_type == rosplan_dispatch_msgs::EsterelPlanNode::PLAN_START && !plan_started) {
// activate new edges
std::vector<int>::const_iterator ci = node.edges_in.begin();
ci = node.edges_out.begin();
for(; ci != node.edges_out.end(); ci++) {
edge_active[*ci] = true;
}
finished_execution = false;
state_changed = true;
plan_started = true;
}
// do not check actions for nodes which are not action nodes
if(node.node_type != rosplan_dispatch_msgs::EsterelPlanNode::ACTION_START && node.node_type != rosplan_dispatch_msgs::EsterelPlanNode::ACTION_END)
continue;
// If at least one node is still executing we are not done yet
if (action_dispatched[node.action.action_id] && !action_completed[node.action.action_id]) {
finished_execution = false;
}
// check action edges
bool edges_activate_action = true;
std::vector<int>::iterator eit = node.edges_in.begin();
for (; eit != node.edges_in.end(); ++eit) {
if(!edge_active[(*eit)]) edges_activate_action = false;
}
if(!edges_activate_action) continue;
// dispatch new action
if(node.node_type == rosplan_dispatch_msgs::EsterelPlanNode::ACTION_START && !action_dispatched[node.action.action_id]) {
finished_execution = false;
// query KMS for condition edges
bool condition_activate_action = false;
if(edges_activate_action) {
condition_activate_action = checkPreconditions(node.action);
}
if(condition_activate_action) {
// activate action
action_dispatched[node.action.action_id] = true;
action_received[node.action.action_id] = false;
action_completed[node.action.action_id] = false;
// dispatch action
ROS_INFO("KCL: (%s) Dispatching action [%i, %s, %f, %f]",
ros::this_node::getName().c_str(),
node.action.action_id,
node.action.name.c_str(),
(node.action.dispatch_time+planStartTime-missionStartTime),
node.action.duration);
action_dispatch_publisher.publish(node.action);
state_changed = true;
// deactivate incoming edges
std::vector<int>::const_iterator ci = node.edges_in.begin();
for(; ci != node.edges_in.end(); ci++) {
edge_active[*ci] = false;
}
// activate new edges
ci = node.edges_out.begin();
for(; ci != node.edges_out.end(); ci++) {
edge_active[*ci] = true;
}
}
}
// handle completion of an action
if(node.node_type == rosplan_dispatch_msgs::EsterelPlanNode::ACTION_END && action_completed[node.action.action_id]) {
ROS_INFO("KCL: (%s) %i: action %s completed",
ros::this_node::getName().c_str(),
node.action.action_id,
node.action.name.c_str());
finished_execution = false;
state_changed = true;
// deactivate incoming edges
std::vector<int>::const_iterator ci = node.edges_in.begin();
for(; ci != node.edges_in.end(); ci++) {
edge_active[*ci] = false;
}
// activate new edges
ci = node.edges_out.begin();
for(; ci != node.edges_out.end(); ci++) {
edge_active[*ci] = true;
}
}
} // end loop (action nodes)
ros::spinOnce();
loop_rate.sleep();
if(state_changed) printPlan();
// cancel dispatch on replan
if(replan_requested) {
ROS_INFO("KCL: (%s) Replan requested.", ros::this_node::getName().c_str());
reset();
return false;
}
}
ROS_INFO("KCL: (%s) Dispatch complete.", ros::this_node::getName().c_str());
reset();
return true;
}
