$search
00001 /* 00002 * Copyright (c) 2009, U. Klank klank@in.tum.de 00003 Piotr Esden-Tempski 00004 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions are met: 00009 * 00010 * * Redistributions of source code must retain the above copyright 00011 * notice, this list of conditions and the following disclaimer. 00012 * * Redistributions in binary form must reproduce the above copyright 00013 * notice, this list of conditions and the following disclaimer in the 00014 * documentation and/or other materials provided with the distribution. 00015 * * Neither the name of Willow Garage, Inc. nor the names of its 00016 * contributors may be used to endorse or promote products derived from 00017 * this software without specific prior written permission. 00018 * 00019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00020 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00021 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00022 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00023 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00024 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00025 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00026 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00027 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00028 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00029 * POSSIBILITY OF SUCH DAMAGE. 00030 */ 00031 00032 00033 #include <ros/ros.h> 00034 #include <std_msgs/String.h> 00035 #include <vision_msgs/cop_answer.h> 00036 #include <nav_pcontroller/nav_actionActionGoal.h> 00037 #include <boost/thread.hpp> 00038 //#include <cogman_msgs/ArmHandActionGoal.h> 00039 boost::mutex mutex; 00040 00041 // Main procedure 00042 void speakCallback(const std_msgs::String::ConstPtr &st) 00043 { 00044 00045 char test[4096]; 00046 if((*st).data.length() > 4096) 00047 return; 00048 printf("Saying: %s\n", (*st).data.c_str()); 00049 sprintf(test, "say \"%s\"", (*st).data.c_str()); 00050 mutex.lock(); 00051 system(test); 00052 mutex.unlock(); 00053 } 00054 void speakCopCallback(const vision_msgs::cop_answer::ConstPtr &answer) 00055 { 00056 std::string st; 00057 if((*answer).error.length() > 0) 00058 { 00059 return; 00060 st = (*answer).error; 00061 } 00062 else 00063 { 00064 st = "I Found an object, it is a "; 00065 printf("Looking for C: %i\n", (int)(*answer).found_poses[0].classes[0].compare("Cluster")); 00066 if((*answer).found_poses[0].classes[0] == "Cluster"){ 00067 printf("[Ignoring Cluster]\n"); 00068 return; 00069 } 00070 st = st.append((*answer).found_poses[0].classes[0]).append(" "); 00071 if((*answer).found_poses[0].classes.size() > 1) 00072 { 00073 if((*answer).found_poses[0].classes[1] == "Cluster "){ 00074 printf("[Ignoring Cluster]\n"); 00075 return; 00076 } 00077 st = st.append((*answer).found_poses[0].classes[1]); 00078 } 00079 } 00080 00081 char test[4096]; 00082 if(st.length() > 4096) 00083 return; 00084 printf("Saying: '%s'\n", st.c_str()); 00085 sprintf(test, "say \"%s\"", st.c_str()); 00086 mutex.lock(); 00087 system(test); 00088 mutex.unlock(); 00089 } 00090 00091 void speakNavCallback(const nav_pcontroller::nav_actionActionGoal::ConstPtr &answer) 00092 { 00093 std::string st; 00094 if((*answer).goal.target_lo.data==101) 00095 st = " I am Going to the Table. "; 00096 else if((*answer).goal.target_lo.data==100) 00097 { 00098 st = " I am Going to the second Table. "; 00099 00100 } 00101 else 00102 { 00103 return; 00104 } 00105 00106 char test[4096]; 00107 if(st.length() > 4096) 00108 return; 00109 printf("Saying: %s\n", st.c_str()); 00110 sprintf(test, "say \"%s\"", st.c_str()); 00111 mutex.lock(); 00112 system(test); 00113 mutex.unlock(); 00114 } 00115 /* 00116 void speakArmCallback(const cogman_msgs::ArmHandActionGoal::ConstPtr &answer) 00117 { 00118 std::string st; 00119 if((*answer).goal.target_lo.data==101) 00120 st = " Going to Table."; 00121 else 00122 { 00123 st = " Going to Counter."; 00124 00125 } 00126 00127 char test[4096]; 00128 if(st.length() > 4096) 00129 return; 00130 printf("Saying: %s\n", st.c_str()); 00131 sprintf(test, "say \"%s\"", st.c_str()); 00132 mutex.lock(); 00133 system(test); 00134 mutex.unlock(); 00135 } 00136 00137 */ 00138 int main(int argc, char *argv[]) 00139 { 00140 ros::init(argc, argv, "espeak") ; 00141 ros::NodeHandle n; 00142 ros::Subscriber sub = n.subscribe("/espeak/wordstosay", 1, &speakCallback); 00143 ros::Subscriber sub2 = n.subscribe("/kipla/cop_reply", 1, &speakCopCallback); 00144 ros::Subscriber sub3 = n.subscribe("/nav_pcontroller/nav_action/goal", 1, &speakNavCallback); 00145 /*ros::Subscriber sub3 = n.subscribe "/rigth_arm/result", 1, &speakArmCallback);*/ 00146 ros::spin(); 00147 return 0; 00148 } 00149 00150 00151 00152 00153