int main(int argc, char** argv) {
ros::init(argc, argv, "coordinator"); // name this node
ros::NodeHandle nh; //standard ros node handle
ros::Subscriber action_code = nh.subscribe("/action_codes", 1, actionCB);
actionlib::SimpleActionClient<object_finder::objectFinderAction> object_finder_ac("objectFinderActionServer", true);
actionlib::SimpleActionClient<object_grabber::object_grabberAction> object_grabber_ac("objectGrabberActionServer", true);
//xxx FIX ME
//actionlib::SimpleActionClient<navigator::navigatorAction> navigator_ac("navigatorActionServer", true);
ros::ServiceServer service = nh.advertiseService("coordinator_svc", srv_callback);
TaskActionServer taskActionServer; //create a task action server
// attempt to connect to the object-finder server:
ROS_INFO("waiting for server: ");
bool server_exists = false;
while ((!server_exists)&&(ros::ok())) {
server_exists = object_finder_ac.waitForServer(ros::Duration(0.5)); //
ros::spinOnce();
ros::Duration(0.5).sleep();
ROS_INFO("retrying...");
}
ROS_INFO("connected to object_finder action server"); // if here, then we connected to the server;
//use this to visualize computed object poses
ros::Publisher pose_publisher = nh.advertise<geometry_msgs::PoseStamped>("triad_display_pose", 1, true);
g_pose_publisher = &pose_publisher;
server_exists = false;
while ((!server_exists)&&(ros::ok())) {
server_exists = object_grabber_ac.waitForServer(ros::Duration(0.5)); //
ros::spinOnce();
ros::Duration(0.5).sleep();
ROS_INFO("retrying...");
}
ROS_INFO("connected to object_grabber action server"); // if here, then we connected to the server;
//do the same with the "navigator" action server
// attempt to connect to the server:
//xxx FIX ME--add navigator
/*
ROS_INFO("waiting for server: ");
server_exists = false;
while ((!server_exists)&&(ros::ok())) {
server_exists = navigator_ac.waitForServer(ros::Duration(0.5)); //
ros::spinOnce();
ros::Duration(0.5).sleep();
ROS_INFO("retrying...");
}
ROS_INFO("connected to navigator action server"); // if here, then we connected to the server;
*/
//specifications for what we are seeking:
object_finder::objectFinderGoal object_finder_goal;
object_grabber::object_grabberGoal object_grabber_goal;
// FIX ME xxx
//navigator::navigatorGoal navigation_goal;
bool finished_before_timeout;
//ALL SET UP; WAITING FOR TRIGGER
//wait for the action trigger:
ROS_INFO("waiting for action code: rostopic pub action_codes std_msgs/Int32 100");
while (ros::ok()) {
ros::spinOnce();
switch (g_action_code) {
case DO_NOTHING:
ros::Duration(0.5).sleep();
break;
case BUSY_FINDER: // also do nothing...but maybe comment on status? set a watchdog?
if (g_found_object_code == object_finder::objectFinderResult::OBJECT_FINDER_BUSY) {
ROS_INFO("object finder is busy processing...");
ros::Duration(0.5).sleep();
break;
}
if (g_found_object_code == object_finder::objectFinderResult::OBJECT_FOUND) { //success!
ROS_INFO("switch/case happiness! found object");
g_action_code = DO_NOTHING;
break;
}
//if here, bad case:
ROS_WARN("did not find object");
g_action_code = DO_NOTHING;
break;
case BUSY_GRABBER:
if (g_object_grabber_return_code == object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY) {
ROS_INFO("object grabber is busy executing...");
ros::Duration(0.5).sleep();
break;
}
if (g_object_grabber_return_code == object_grabber::object_grabberResult::OBJECT_ACQUIRED) { //success!
ROS_INFO("switch/case happiness! acquired object");
g_action_code = DO_NOTHING;
break;
}
ROS_WARN("did not grab object");
g_action_code = DO_NOTHING;
break;
case ABORT:
ROS_INFO("aborting operations in progress: ");
if (g_found_object_code == object_finder::objectFinderResult::OBJECT_FINDER_BUSY) {
object_finder_ac.cancelGoal();
g_found_object_code == object_finder::objectFinderResult::OBJECT_FINDER_CANCELLED;
g_action_code = DO_NOTHING;
}
if (g_object_grabber_return_code == object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY) {
object_grabber_ac.cancelGoal();
g_object_grabber_return_code = object_grabber::object_grabberResult::OBJECT_GRABBER_CANCELLED;
g_action_code = DO_NOTHING;
}
//do same for navigation
break;
case FIND_BLOCK:
//set flag indicating waiting on response from object finder
// this will get reset by object-finder callback
// need to consider time-outs
ROS_INFO("received find-block command code; starting work...");
g_found_object_code = object_finder::objectFinderResult::OBJECT_FINDER_BUSY;
//populate a goal message to find TOY_BLOCK
object_finder_goal.object_id = object_finder::objectFinderGoal::TOY_BLOCK;
object_finder_goal.known_surface_ht = false; //require find table height
object_finder_goal.surface_ht = 0.05;
object_finder_ac.sendGoal(object_finder_goal, &objectFinderDoneCb);
g_action_code = BUSY_FINDER;
break;
case GRAB_BLOCK:
ROS_INFO("received grab-block command code; starting work...");
g_object_grabber_return_code = object_grabber::object_grabberResult::OBJECT_GRABBER_BUSY;
//populate a goal message to grab TOY_BLOCK
object_grabber_goal.object_code = object_grabber::object_grabberGoal::TOY_BLOCK; //specify the object to be grabbed
object_grabber_goal.desired_frame = g_perceived_object_pose;
object_grabber_ac.sendGoal(object_grabber_goal, &objectGrabberDoneCb);
g_action_code = BUSY_GRABBER;
break;
case NAVIGATE:
ROS_INFO("not implemented yet");
g_action_code = DO_NOTHING;
break;
}
/*
ROS_INFO("sending navigation goal: TABLE");
navigation_goal.location_code = navigator::navigatorGoal::TABLE; //send robot to TABLE
navigator_ac.sendGoal(navigation_goal, &navigatorDoneCb); // we could also name additional callback functions here, if desired
finished_before_timeout = navigator_ac.waitForResult(ros::Duration(30.0));
//bool finished_before_timeout = action_client.waitForResult(); // wait forever...
if (!finished_before_timeout) {
ROS_WARN("giving up waiting on result ");
return 1;
}
//SHOULD do error checking here before proceeding...
if (g_navigator_rtn_code != navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
ROS_WARN("COULD NOT REACH TABLE; QUITTING");
return 1;
}
*/
}
return 0;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "coordinator"); // name this node
ros::NodeHandle nh; //standard ros node handle
ros::Subscriber alexa_code = nh.subscribe("/Alexa_codes", 1, alexaCB);
actionlib::SimpleActionClient<object_finder_gamma::objectFinderAction> object_finder_ac("objectFinderActionServer", true);
actionlib::SimpleActionClient<object_grabber_gamma::object_grabberAction> object_grabber_ac("objectGrabberActionServer", true);
actionlib::SimpleActionClient<navigator_gamma::navigatorAction> navigator_ac("navigatorActionServer", true);
// attempt to connect to the object_finder server:
ROS_INFO("waiting for server: ");
bool server_exists = false;
while ((!server_exists)&&(ros::ok())) {
server_exists = object_finder_ac.waitForServer(ros::Duration(0.5)); //
ros::spinOnce();
ros::Duration(0.5).sleep();
ROS_INFO("retrying...");
}
ROS_INFO("connected to object_finder action server"); // if here, then we are connected to the server;
//connect to the object_grabber server
server_exists=false;
while ((!server_exists)&&(ros::ok())) {
server_exists = object_grabber_ac.waitForServer(ros::Duration(0.5)); //
ros::spinOnce();
ros::Duration(0.5).sleep();
ROS_INFO("retrying...");
}
ROS_INFO("connected to object_grabber action server"); // if here, then we are connected to the server;
// attempt to connect to the navigator server:
ROS_INFO("waiting for server: ");
server_exists = false;
while ((!server_exists)&&(ros::ok())) {
server_exists = navigator_ac.waitForServer(ros::Duration(0.5)); //
ros::spinOnce();
ros::Duration(0.5).sleep();
ROS_INFO("retrying...");
}
ROS_INFO("connected to navigator action server"); // if here, then we are connected to the server;
//specifications for what we are seeking:
object_finder_gamma::objectFinderGoal object_finder_goal;
object_grabber_gamma::object_grabberGoal object_grabber_goal;
navigator_gamma::navigatorGoal navigation_goal;
bool finished_before_timeout;
//ALL SET UP; WAITING FOR TRIGGER
//wait for the Alexa trigger:
ROS_INFO("waiting for Alexa code: rostopic pub Alexa_codes std_msgs/UInt32 100");
while(ros::ok()) {
if (!g_get_coke_trigger) {
ros::Duration(0.5).sleep();
ros::spinOnce();
} else {
g_get_coke_trigger=false; // reset the trigger
// IF HERE, START THE FETCH BEHAVIOR!!
// use navigator to move to the table
ROS_INFO("sending navigation goal: TABLE");
navigation_goal.location_code = navigator_gamma::navigatorGoal::TABLE; //send robot to TABLE
navigator_ac.sendGoal(navigation_goal,&navigatorDoneCb); // we could also name additional callback functions here, if desired
finished_before_timeout = navigator_ac.waitForResult(ros::Duration(30.0));
//bool finished_before_timeout = action_client.waitForResult(); // wait forever...
if (!finished_before_timeout) {
ROS_WARN("giving up waiting on result ");
return 1;
}
//SHOULD do error checking here before proceeding...
if (g_navigator_rtn_code != navigator_gamma::navigatorResult::DESIRED_POSE_ACHIEVED) {
ROS_WARN("COULD NOT REACH TABLE; QUITTING");
return 1;
}
// if here, we have reached the table, now use object_finder to find the coke can
object_finder_goal.object_id=object_finder_gamma::objectFinderGoal::COKE_CAN_UPRIGHT; //specify object of interest
object_finder_goal.known_surface_ht = true; //we'll say we know the table height
object_finder_goal.surface_ht = 0.05; // and specify the height, relative to torso; //TODO: TUNE THIS
//try to find the object:
object_finder_ac.sendGoal(object_finder_goal,&objectFinderDoneCb);
//decide how long we will wait
finished_before_timeout = object_finder_ac.waitForResult(ros::Duration(5.0));
//bool finished_before_timeout = action_client.waitForResult(); // wait forever...
if (!finished_before_timeout) {
ROS_WARN("giving up waiting on result ");
return 1; // halt with failure
}
//SHOULD examine the return code,
if (g_found_object_code != object_finder_gamma::objectFinderResult::OBJECT_FOUND) {
ROS_WARN("could not find object; quitting!");
return 1;
}
// if here, then presumably have a valid pose for object of interest
// use object_grabber to pick up the coke can
object_grabber_goal.object_code = object_grabber_gamma::object_grabberGoal::COKE_CAN; //specify the object to be grabbed
object_grabber_goal.object_frame = g_perceived_object_pose;
ROS_INFO("sending goal to grab object: ");
object_grabber_ac.sendGoal(object_grabber_goal,&objectGrabberDoneCb);
//decide how long to wait...
finished_before_timeout = object_grabber_ac.waitForResult(ros::Duration(40.0));
if (!finished_before_timeout) {
ROS_WARN("giving up waiting on result; quitting ");
return 1;
}
if (g_object_grabber_return_code!= object_grabber_gamma::object_grabberResult::OBJECT_ACQUIRED) {
ROS_WARN("failed to grab object; giving up!");
return 1;
}
// if here, belief is that we are holding the Coke
// use navigator to return home
ROS_INFO("sending navigation goal: HOME");
navigation_goal.location_code = navigator_gamma::navigatorGoal::HOME; //send robot to HOME
navigator_ac.sendGoal(navigation_goal,&navigatorDoneCb);
finished_before_timeout = navigator_ac.waitForResult(ros::Duration(30.0));
//bool finished_before_timeout = action_client.waitForResult(); // wait forever...
if (!finished_before_timeout) {
ROS_WARN("giving up waiting on result ");
return 1;
}
//SHOULD do error checking here before proceeding...
if (g_navigator_rtn_code!= navigator_gamma::navigatorResult::DESIRED_POSE_ACHIEVED) {
ROS_WARN("COULD NOT REACH TABLE; QUITTING");
return 1;
}
ROS_INFO("Done fetching! Run me again?");
}
}
return 0;
}