wheeled_robin_guided_tour: state_machine.cpp Source File

Go to the documentation of this file.
00001 
00013 //#define DEBUG_STATES
00014 
00015 #include <ros/ros.h>
00016 #include <tf/transform_listener.h>
00017 #include <std_msgs/String.h>
00018 #include <std_msgs/Bool.h>
00019 #include <geometry_msgs/PoseStamped.h>
00020 #include <actionlib/client/simple_action_client.h>
00021 #include <actionlib_msgs/GoalStatus.h>
00022 #include <move_base_msgs/MoveBaseAction.h>
00023 #include <stdio.h>
00024 #include <string>
00025 #include <sstream> 
00026 
00027 #include "video_player/PlayVideoSrv.h"
00028 
00029 // global variables
00030 ros::Time last_button_msg_time; // last time a button has been pressed
00031 int current_goal = 0; // currently active goal
00032 
00033 // function declarations
00034 void createPoseFromParams(std::string param_name, geometry_msgs::PoseStamped* pose);
00035 void buttonCb(std_msgs::Bool msg);
00036 bool last_button_state = false;
00037 
00038 // define state names
00039 enum state {
00040         START, RETURN_START, WAIT_PERSON, APPROACH_PERSON, ASK_PERSON, WAIT_BUTTON_TOUR, APPROACH_PRESENTATION, PRESENT, ASK_REPETITION, WAIT_BUTTON_REPETITION, TALK_BYE
00041 };
00042 
00043 int main(int argc, char** argv) {
00044         // init ROS
00045         ros::init(argc, argv, "state_machine");
00046         ros::NodeHandle nh;
00047         ros::Rate loop_rate(10);
00048         
00049         // set up publishers
00050         ros::Publisher speech_pub = nh.advertise<std_msgs::String>("/speech", 1);
00051         
00052         // set up subscribers
00053         ros::Subscriber button_sub = nh.subscribe<std_msgs::Bool>("/pushed", 1, buttonCb);
00054         
00055         // client for navigation goals
00056         actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> client("move_base", true);
00057         
00058         // client for video player and service variable
00059         ros::ServiceClient srv_client = nh.serviceClient<video_player::PlayVideoSrv>("/wheeled_robin/application/play_video");
00060         video_player::PlayVideoSrv srv;
00061         
00062         // init state machine
00063         state st = START;
00064         
00065         // tf listener
00066         tf::TransformListener listener;
00067         tf::StampedTransform transform;
00068         
00069         // read parameters
00070         double base_range; // range around base to trigger question for tour
00071         ros::param::get("~base_range", base_range);
00072         base_range = base_range*base_range; // cheaper to compare (no sqrt)
00073         std::string person_frame; // name of frame to person or group of persons
00074         ros::param::get("~person_frame", person_frame);
00075         
00076         std::string person_threshold_frame; // name of frame of person position threshold
00077         ros::param::get("~person_threshold_frame", person_threshold_frame);
00078         
00079         double button_duration; // time for user to interact with robot
00080         ros::param::get("~button_duration", button_duration);
00081         std::string goal_basename; // basename for goal parameter names (within goals namespace)
00082         ros::param::get("~goal_basename", goal_basename);
00083         std::string video_path; // path to folder with presentation videos
00084         ros::param::get("~video_path", video_path);
00085         
00086         // init time
00087         ros::Time ask_time;
00088         last_button_msg_time = ros::Time(0);
00089         
00090         move_base_msgs::MoveBaseGoal goal;
00091         goal.target_pose.header.frame_id = "map";
00092         
00093         // wait for other nodes to start up
00094         ros::Duration(10).sleep();
00095         
00096         while(ros::ok()) {
00097                 switch(st) {
00098                         case START: {
00099                                 createPoseFromParams("start", &(goal.target_pose));
00100                                 client.sendGoal(goal);
00101                                 st = RETURN_START;
00102                                 ROS_INFO("Switching to state %d", st);
00103                                 break;
00104                         }
00105                         case RETURN_START: {
00106                                 if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
00107                                         st = WAIT_PERSON;
00108                                         ROS_INFO("Reached start position");
00109                                         ROS_INFO("Switching to state %d", st);
00110                                 }
00111                                 current_goal = 0;
00112                                 break;
00113                         }
00114                         case WAIT_PERSON: {
00115                                 #ifdef DEBUG_STATES
00116                                         createPoseFromParams("base", &(goal.target_pose));
00117                                         client.sendGoal(goal);
00118                                         st = APPROACH_PERSON;
00119                                         break;
00120                                 #endif
00121                                 
00122                                 // tf from robot to base footprint of person at /person
00123                                 try {
00124                                         listener.lookupTransform(person_threshold_frame, person_frame, ros::Time(0), transform);
00125                                 } catch (tf::TransformException ex) {
00126                                         ROS_ERROR("%s", ex.what());
00127                                         break;
00128                                 }
00129                                 tf::Vector3 distance = transform.getOrigin();
00130                                 double person_range = distance.length2();
00131                                 
00132                                 ROS_INFO("person_range = %f", person_range);
00133                                 ROS_INFO("base_range = %f", base_range);
00134                                 
00135                                 // check if person is within range of base to trigger question for tour
00136                                 if(person_range <= base_range) { // person within range
00137                                         createPoseFromParams("base", &(goal.target_pose));
00138                                         client.sendGoal(goal);
00139                                         ROS_INFO("Person within range of base detected");
00140                                         st = APPROACH_PERSON;
00141                                 }
00142                                 break;
00143                         }
00144                         case APPROACH_PERSON: {
00145                                 if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { // goal reached
00146                                         st = ASK_PERSON;
00147                                         ROS_INFO("Reached person");
00148                                         ROS_INFO("Switching to state %d", st);
00149                                 }
00150                                 break;
00151                         }
00152                         case ASK_PERSON: {
00153                                 std_msgs::String ask_tour;
00154                                 ask_tour.data = "Please touch me if you would like to take a tour.";
00155                                 speech_pub.publish(ask_tour);
00156                                 ask_time = ros::Time::now();
00157                                 st = WAIT_BUTTON_TOUR;
00158                                 ROS_INFO("Switching to state %d", st);
00159                                 break;
00160                         }
00161                         case WAIT_BUTTON_TOUR: {
00162                                 if(ros::Time::now() - ask_time > ros::Duration(10.0)) { // no tour requested
00163                                         std_msgs::String say_nothanks;
00164                                         say_nothanks.data = "Thanks for wasting my time. Good bye.";
00165                                         speech_pub.publish(say_nothanks);
00166                                         createPoseFromParams("start", &(goal.target_pose));
00167                                         client.sendGoal(goal);
00168                                         st = RETURN_START;
00169                                         ROS_INFO("Switching to state %d", st);
00170                                         ROS_INFO("No tour requested.");
00171                                 } else { // waiting for user
00172                                         if(last_button_msg_time > ask_time && last_button_state) { // tour requested
00173                                                 std::stringstream ss;
00174                                                 ss << goal_basename;
00175                                                 ss << current_goal;
00176                                                 ROS_INFO("Lookup goal: %s", ss.str().c_str());
00177                                                 createPoseFromParams(ss.str().c_str(), &(goal.target_pose));
00178                                                 client.sendGoal(goal);
00179                                                 ROS_INFO("Tour requested");
00180                                                 st = APPROACH_PRESENTATION;
00181                                                 ROS_INFO("Switching to state %d", st);
00182                                         }
00183                                 }
00184                                 break;
00185                         }
00186                         case APPROACH_PRESENTATION: {
00187                                 if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) { // goal reached
00188                                         st = PRESENT;
00189                                         ROS_INFO("Switching to state %d", st);
00190                                 }
00191                                 break;
00192                         case PRESENT:
00193                                 std::stringstream ss;
00194                                 ss << "/goals/";
00195                                 ss << goal_basename;
00196                                 ss << current_goal;
00197                                 ss << "/folder";
00198                                 ros::param::get(ss.str().c_str(), srv.request.videoPath);
00199                                 srv.request.videoPath = video_path + srv.request.videoPath;
00200                                 #ifdef DEBUG_STATES
00201                                         st = ASK_REPETITION;
00202                                         ROS_INFO("Switching to state %d", st);
00203                                         break;
00204                                 #endif
00205                                 if (srv_client.call(srv)){
00206                                         ROS_INFO("Presentation successful");
00207                                 } else {
00208                                         ROS_ERROR("Presentation failed");
00209                                 }
00210                                 st = ASK_REPETITION;
00211                                 ROS_INFO("Switching to state %d", st);
00212                                 break;
00213                         }
00214                         case ASK_REPETITION: {
00215                                 std_msgs::String ask_pres;
00216                                 ask_pres.data = "Please touch me if you would like to see the presentation again.";
00217                                 speech_pub.publish(ask_pres);
00218                                 ask_time = ros::Time::now();
00219                                 st = WAIT_BUTTON_REPETITION;
00220                                 ROS_INFO("Switching to state %d", st);
00221                                 break;
00222                         }
00223                         case WAIT_BUTTON_REPETITION: {
00224                                 if(ros::Time::now() - ask_time <= ros::Duration(button_duration)) { // waiting for user
00225                                         if(last_button_msg_time > ask_time && last_button_state) { // repetition requested
00226                                                 st = PRESENT;
00227                                                 ROS_INFO("Button pressed");
00228                                                 ROS_INFO("Switching to state %d", st);
00229                                         } 
00230                                 } else { // no repetition requested
00231                                         current_goal++;
00232                                         std::stringstream ss;
00233                                         ss << goal_basename;
00234                                         ss << current_goal;
00235                                         ROS_INFO("Next goal is: %s", ss.str().c_str());
00236                                         std::stringstream check;
00237                                         check << "/goals/";
00238                                         check << ss.str();
00239                                         check << "/x";
00240                                         if(ros::param::has(check.str().c_str())) { // another goal exists
00241                                                 createPoseFromParams(ss.str().c_str(), &(goal.target_pose));
00242                                                 client.sendGoal(goal);
00243                                                 st = APPROACH_PRESENTATION;
00244                                                 ROS_INFO("Switching to state %d", st);
00245                                                 std_msgs::String say_continue;
00246                                                 say_continue.data = "Okay, let us continue with the next stop.";
00247                                                 speech_pub.publish(say_continue);
00248                                         } else {
00249                                                 st = TALK_BYE;
00250                                                 ROS_INFO("Switching to state %d", st);
00251                                         }
00252                                 }
00253                                 break;
00254                         }
00255                         case TALK_BYE: {
00256                                 std_msgs::String say_bye;
00257                                 say_bye.data = "I hope you enjoyed the tour. Thanks for visiting us. Please follow me back to the start.";
00258                                 speech_pub.publish(say_bye);
00259                                 createPoseFromParams("start", &(goal.target_pose));
00260                                 client.sendGoal(goal);
00261                                 st = RETURN_START;
00262                                 ROS_INFO("Switching to state %d", st);
00263                                 break;
00264                         }
00265                         default:
00266                                 ;
00267                 }
00268                 
00269                 ros::spinOnce();
00270                 loop_rate.sleep();
00271         }
00272         
00273         return 0;
00274 }
00275 
00286 void createPoseFromParams(std::string param_name, geometry_msgs::PoseStamped* pose) {
00287         pose->header.stamp = ros::Time::now();
00288         ros::param::get("/goals/"+param_name+"/x", pose->pose.position.x);
00289         ros::param::get("/goals/"+param_name+"/y", pose->pose.position.y);
00290         ros::param::get("/goals/"+param_name+"/z", pose->pose.position.z);
00291         ros::param::get("/goals/"+param_name+"/q1", pose->pose.orientation.x);
00292         ros::param::get("/goals/"+param_name+"/q2", pose->pose.orientation.y);
00293         ros::param::get("/goals/"+param_name+"/q3", pose->pose.orientation.z);
00294         ros::param::get("/goals/"+param_name+"/q4", pose->pose.orientation.w);
00295 }
00296 
00306 void buttonCb(std_msgs::Bool msg) {
00307         last_button_msg_time = ros::Time::now();
00308         last_button_state = msg.data;
00309         //ROS_INFO("Button event, %d", last_button_state);
00310 }