Пример #1
0
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;
}