/* * 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; }