Robotics StackExchange | Archived questions

My restaurant.cpp does not work?? Is there any cpp file which i can use for a p3at robot using p2os?

include "ros/ros.h"

include "std_msgs/String.h"

include

include

include <../soundplay/soundplay.h>

include

include

include

include

include "std_msgs/Float64.h"

include

typedef actionlib::SimpleActionClient MoveBaseClient;

using namespace std; string speech; string order; int state = 0; double tableX = 3.7313, tableY = 0.8048, tableW = 0.99999, tableZ = 0.00534; double homeX = 0.7707, homeY = 0.426, homeW = 0.0562, homeZ = 0.9984;

ros::Publisher rightshoulder; ros::Publisher rightelbow; ros::Publisher rightarm; ros::Publisher leftshoulder; ros::Publisher leftelbow; ros::Publisher leftarm; ros::Publisher panjoint; ros::Publisher tiltjoint;

void chatterCallback(const stdmsgs::String::ConstPtr& msg) { ROSINFO("I heard: [%s]", msg->data.cstr()); speech = msg->data.cstr(); }

bool query() { ROSINFO("Waiting for query"); while(ros::ok()) { if(speech.find("yes")!=string::npos) { ROSINFO("Yes!"); return true; break; } else if(speech.find("no")!=string::npos) { ROS_INFO("NO!"); return false; break; } ros::spinOnce();

}

}

bool sfound(string command) { if(speech.find(command)!=string::npos) return true; else return false; }

void getOrder() { ROS_INFO("Waiting for order"); while(ros::ok()) {

    if((speech.find("coffee")!=string::npos)||(speech.find("cappuccino")!=string::npos)||(speech.find("mocca")!=string::npos)||(speech.find("milk")!=string::npos)||(speech.find("teh")!=string::npos)||(speech.find("expresso")!=string::npos)||(speech.find("juice")!=string::npos)||(speech.find("rice")!=string::npos)||(speech.find("tea")!=string::npos))
    {
                order = speech ;
        ROS_INFO("Order received");
                break;
    }
ros::spinOnce();


}

}

void sleepok(int t, ros::NodeHandle &nh) { if(nh.ok()) sleep(t); }

void clearpausespeech() { sleep(3); //sleepok(6,n); speech.clear(); speech = ""; ROS_INFO("SPEECH CLEARED"); }

void resetmotors() { stdmsgs::Float64 zero; stdmsgs::Float64 zero2; zero.data = 0 ; zero2.data = -0.5; rightshoulder.publish(zero); leftshoulder.publish(zero); panjoint.publish(zero); tiltjoint.publish(zero); rightelbow.publish(zero); leftelbow.publish(zero); rightarm.publish(zero2); leftarm.publish(zero);

}

void wave() { stdmsgs::Float64 hello; stdmsgs::Float64 hello2; stdmsgs::Float64 hello4; stdmsgs::Float64 hello6; int i = 0;

hello.data = 2.0;
right_shoulder.publish(hello);
hello6.data = -2.0;
left_shoulder.publish(hello6);
sleep (1);

for(i=0;i<=3;i++){
hello2.data = 0;
right_elbow.publish(hello2);
left_elbow.publish(hello2);
usleep(500000);
hello2.data = -1.0;
right_elbow.publish(hello2);
hello4.data = 1.0;
left_elbow.publish(hello4);
usleep(500000);}

}

int main(int argc, char **argv) { ros::init(argc, argv, "listener"); ros::NodeHandle n; ros::Subscriber sub = n.subscribe("recognizer/output", 1000, chatterCallback); ros::ServiceClient stopspeech = n.serviceClient("recognizer/stop"); ros::ServiceClient startspeech = n.serviceClient("recognizer/start"); rightshoulder= n.advertise<stdmsgs::Float64>("/rightshoulder/command", 1.0); rightelbow = n.advertise("/rightelbow/command", 1.0); rightarm = n.advertise("/rightarm/command", 1.0); leftshoulder= n.advertise("/leftshoulder/command", 1.0); leftelbow = n.advertise("/leftelbow/command", 1.0); leftarm = n.advertise("/leftarm/command", 1.0); panjoint = n.advertise("/panjoint/command", 1.0); tiltjoint = n.advertise("/tilt_joint/command", 1.0);

std_srvs::Empty empty; ros::Rate r(10);

//tell the action client that we want to spin a thread by default MoveBaseClient ac("move_base", true);

//wait for the action server to come up //while(!ac.waitForServer(ros::Duration(5.0))) //ROSINFO("Waiting for the movebase action server to come up"); movebasemsgs::MoveBaseGoal goal; goal.targetpose.header.frameid = "map";

sound_play::SoundClient sc; sleepok(1,n); stopspeech.call(empty); sc.startWave("/home/cheeyong/Desktop/restaurantwave/readytowork.wav"); clearpausespeech(); startspeech.call(empty); resetmotors(); sleep(3); /* 1: wait for serve 2: */ while(ros::ok()) { //if(((speech.find("one")!=string::npos)||(speech.find("two")!=string::npos)||(speech.find("three")!=string::npos))&&(state==0)) {

/*if(sfound("introduce"))
{
    sleepok(1,n);
    stopspeech.call(empty);
    sc.say("Hello, my name is Ariel.","voice_cmu_us_clb_arctic_clunits");
    sleep(3);
    wave();
    sleep(4);
    sc.say("I am social service home robot developed by aricc","voice_cmu_us_clb_arctic_clunits");
    sleep(8);
    sc.say("It's nice to meet you!","voice_cmu_us_clb_arctic_clunits");
    sleep(5);
    resetmotors();
    startspeech.call(empty);
}*/
    if((sfound("table"))||(sfound("one"))||(sfound("two"))||(sfound("three")))
    {
    sleepok(1,n);
    stopspeech.call(empty);
    sc.startWave("/home/cheeyong/Desktop/restaurantwave/wantmeserveguest.wav");
    clearpausespeech();
    startspeech.call(empty);

    //sc.say("Do you want me to serve table %s",speech);
    ROS_INFO("Going to serve");


    if(query())
    {
        sleepok(1,n);
        stopspeech.call(empty);
        sc.startWave("/home/cheeyong/Desktop/restaurantwave/goingtotable.wav");
        clearpausespeech();
        state=1;

            //Navigates to table
            goal.target_pose.header.stamp = ros::Time::now();
            goal.target_pose.pose.position.x = tableX;
            goal.target_pose.pose.position.y = tableY;
            goal.target_pose.pose.orientation.w = tableW;
            goal.target_pose.pose.orientation.z = tableZ;
            ROS_INFO("Sending goal");
            ac.sendGoal(goal);
            ac.waitForResult();

        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
                ROS_INFO("Hooray, Reached the table");
        else
        {
            sleepok(1,n);
            stopspeech.call(empty);
            sc.startWave("/home/cheeyong/Desktop/restaurantwave/toomanyppl.wav");
            clearpausespeech();


                ROS_INFO("Failed to reach table, shall try to go back");
            //Navigates to table
            goal.target_pose.header.stamp = ros::Time::now();
            goal.target_pose.pose.position.x = tableX;
            goal.target_pose.pose.position.y = tableY;
            goal.target_pose.pose.orientation.w = tableW;
            goal.target_pose.pose.orientation.z = tableZ;
            ROS_INFO("Sending goal");
            ac.sendGoal(goal);
            ac.waitForResult();
        }
    }
    else
    {
        sleepok(1,n);
        sc.startWave("/home/cheeyong/Desktop/restaurantwave/ok.wav");
        clearpausespeech();
        startspeech.call(empty);
    }
}

if(state==1)
{
    sleepok(1,n);
    sc.startWave("/home/cheeyong/Desktop/restaurantwave/whatwouldyoulike.wav");
    clearpausespeech();
    startspeech.call(empty);

        getOrder();
        sleepok(1,n);
    if(order == "coffee"){
    stopspeech.call(empty);
    sc.startWave("/home/cheeyong/Desktop/restaurantwave/youwantcoffee.wav");
        clearpausespeech();
    startspeech.call(empty);
    sleep(1);
    }else if(order == "milk"){
    stopspeech.call(empty);
    sc.startWave("/home/cheeyong/Desktop/restaurantwave/youwantmilk.wav");
        clearpausespeech();
    startspeech.call(empty);
    sleep(1);
    }else if(order == "tea"){
    stopspeech.call(empty);
    sc.startWave("/home/cheeyong/Desktop/restaurantwave/youwanttea.wav");
        clearpausespeech();
    startspeech.call(empty);
    sleep(1);
    }

            if(query())
    {
                //state=2;
                sleepok(2,n);
        stopspeech.call(empty);
        sc.startWave("/home/cheeyong/Desktop/restaurantwave/willgeturorder.wav");
        clearpausespeech();
        startspeech.call(empty);

                //Navigates to home
                goal.target_pose.header.stamp = ros::Time::now();
                goal.target_pose.pose.position.x = homeX;
                goal.target_pose.pose.position.y = homeY;
                goal.target_pose.pose.orientation.w = homeW;
                goal.target_pose.pose.orientation.z = homeZ;
                ROS_INFO("Going home");
                ac.sendGoal(goal);
                ac.waitForResult();
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
                ROS_INFO("Hooray, Reached the Home");
            sleepok(1,n);
            if(order == "coffee"){
            stopspeech.call(empty);
            sc.startWave("/home/cheeyong/Desktop/restaurantwave/guestwantcoffee.wav");
            clearpausespeech();
            startspeech.call(empty);
            sleep(1);
            }else if(order == "milk"){
            stopspeech.call(empty);
            sc.startWave("/home/cheeyong/Desktop/restaurantwave/guestwantmilk.wav");
            clearpausespeech();
            startspeech.call(empty);
            sleep(1);
            }else if(order == "tea"){
            stopspeech.call(empty);
            sc.startWave("/home/cheeyong/Desktop/restaurantwave/guestwanttea.wav");
            clearpausespeech();
            startspeech.call(empty);
            sleep(1);
            }
            sleep(10);
            sc.startWave("/home/cheeyong/Desktop/restaurantwave/thankyou.wav");

            //Navigates to table
            goal.target_pose.header.stamp = ros::Time::now();
            goal.target_pose.pose.position.x = tableX;
            goal.target_pose.pose.position.y = tableY;
            goal.target_pose.pose.orientation.w = tableW;
            goal.target_pose.pose.orientation.z = tableZ;
            ROS_INFO("Sending goal");
            ac.sendGoal(goal);
            ac.waitForResult();

            sc.startWave("/home/cheeyong/Desktop/restaurantwave/heresurorder.wav");
            clearpausespeech();
            sleep(5);
                //Navigates to home
                goal.target_pose.header.stamp = ros::Time::now();
                goal.target_pose.pose.position.x = homeX;
                goal.target_pose.pose.position.y = homeY;
                goal.target_pose.pose.orientation.w = homeW;
                goal.target_pose.pose.orientation.z = homeZ;
                ROS_INFO("Going home");
                ac.sendGoal(goal);
                ac.waitForResult();
            state = 0;
        }
        else
        {
                ROS_INFO("Failed to reach table");
            state = 0;
        }


        startspeech.call(empty);
        }
}


ros::spinOnce();
r.sleep();

} }

Asked by xrobox on 2013-09-25 19:10:14 UTC

Comments

This cpp does not work on my p3at robot and i cant find the solution to it please help :D.

Asked by xrobox on 2013-09-25 19:40:44 UTC

Please clean up your question so it is readable. Maybe posting it on Gist or pastebin is in order here. Also, make your question more specific: posting source code and just asking how to make it work is too vague and doesn't help anyone but you.

Asked by georgebrindeiro on 2013-09-26 08:13:15 UTC

Answers