/*
* Loop through and publish planned actions
*/
bool SimplePlanDispatcher::dispatchPlan(const std::vector<rosplan_dispatch_msgs::ActionDispatch> &actionList, double missionStart, double planStart) {
ros::NodeHandle nh("~");
ros::Rate loop_rate(10);
ROS_INFO("KCL: (PS) Dispatching plan");
replan_requested = false;
bool repeatAction = false;
while (ros::ok() && actionList.size() > current_action) {
// loop while dispatch is paused
while (ros::ok() && dispatch_paused) {
ros::spinOnce();
loop_rate.sleep();
}
// cancel plan
if(plan_cancelled) {
break;
}
// get next action
rosplan_dispatch_msgs::ActionDispatch currentMessage = actionList[current_action];
if((unsigned int)currentMessage.action_id != current_action)
ROS_ERROR("KCL: (PS) Message action_id [%d] does not meet expected [%zu]", currentMessage.action_id, current_action);
// loop while waiting for dispatch time
if(!dispatch_on_completion) {
double wait_period = 10.0;
int wait_print = (int)(currentMessage.dispatch_time + planStart - ros::WallTime::now().toSec()) / wait_period;
while (ros::ok() && ros::WallTime::now().toSec() < currentMessage.dispatch_time + planStart) {
ros::spinOnce();
loop_rate.sleep();
double remaining = planStart + currentMessage.dispatch_time - ros::WallTime::now().toSec();
if(wait_print > (int)remaining / wait_period) {
ROS_INFO("KCL: (PS) Waiting %f before dispatching action: [%i, %s, %f, %f]",
remaining,currentMessage.action_id, currentMessage.name.c_str(),
(currentMessage.dispatch_time+planStart-missionStart), currentMessage.duration);
wait_print--;
}
}
}
if(!checkPreconditions(currentMessage)) {
ROS_INFO("KCL: (PS) Preconditions not achieved [%i, %s]", currentMessage.action_id, currentMessage.name.c_str());
// publish feedback (precondition false)
rosplan_dispatch_msgs::ActionFeedback fb;
fb.action_id = currentMessage.action_id;
fb.status = "precondition false";
action_feedback_pub.publish(fb);
replan_requested = true;
} else {
// dispatch action
ROS_INFO("KCL: (PS) Dispatching action [%i, %s, %f, %f]", currentMessage.action_id, currentMessage.name.c_str(), (currentMessage.dispatch_time+planStart-missionStart), currentMessage.duration);
action_publisher.publish(currentMessage);
double late_print = (ros::WallTime::now().toSec() - (currentMessage.dispatch_time + planStart));
if(late_print>0.1) ROS_INFO("KCL: (PS) Action [%i] is %f second(s) late", currentMessage.action_id, late_print);
// wait for action to complete
if(!dispatch_concurrent) {
int counter = 0;
while (ros::ok() && !action_completed[current_action]) {
ros::spinOnce();
loop_rate.sleep();
counter++;
if (counter == 2000) {
ROS_INFO("KCL: (PS) Action %i timed out now. Cancelling...", currentMessage.action_id);
rosplan_dispatch_msgs::ActionDispatch cancelMessage;
cancelMessage.action_id = currentMessage.action_id;
cancelMessage.name = "cancel_action";
action_publisher.publish(cancelMessage);
}
}
}
}
// get ready for next action
if(!repeatAction) current_action++;
repeatAction = false;
action_received[current_action] = false;
action_completed[current_action] = false;
// finish dispatch and replan
if(replan_requested) return false;
}
return true;
}
/*
* 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;
}
