00001 #include <ros/ros.h> 00002 #include <actionlib/server/simple_action_server.h> 00003 #include <std_msgs/String.h> 00004 #include <cob_sound/SayAction.h> 00005 #include <cob_sound/SayText.h> 00006 00007 class SayAction 00008 { 00009 protected: 00010 00011 ros::NodeHandle nh_; 00012 actionlib::SimpleActionServer<cob_sound::SayAction> as_; 00013 ros::ServiceServer srvServer_; 00014 ros::Subscriber sub; 00015 std::string action_name_; 00016 00017 public: 00018 00019 SayAction(std::string name) : 00020 as_(nh_, name, boost::bind(&SayAction::as_cb, this, _1), false), 00021 action_name_(name) 00022 { 00023 as_.start(); 00024 srvServer_ = nh_.advertiseService("/say", &SayAction::service_cb, this); 00025 sub = nh_.subscribe("/say", 1000, &SayAction::topic_cb, this); 00026 } 00027 00028 ~SayAction(void) 00029 { 00030 } 00031 00032 void as_cb(const cob_sound::SayGoalConstPtr &goal) 00033 { 00034 say(goal->text.data); 00035 as_.setSucceeded(); 00036 } 00037 00038 bool service_cb(cob_sound::SayText::Request &req, 00039 cob_sound::SayText::Response &res ) 00040 { 00041 say(req.text); 00042 return true; 00043 } 00044 00045 void topic_cb(const std_msgs::String::ConstPtr& msg) 00046 { 00047 say(msg->data.c_str()); 00048 } 00049 00050 00051 void say(std::string text) 00052 { 00053 ROS_INFO("Saying: %s", text.c_str()); 00054 std::string mode; 00055 std::string command; 00056 std::string cepstral_conf; 00057 nh_.param<std::string>("/sound_controller/mode",mode,"festival"); 00058 nh_.param<std::string>("/sound_controller/cepstral_settings",cepstral_conf,"\"speech/rate=170\""); 00059 if (mode == "cepstral") 00060 { 00061 command = "/opt/swift/bin/swift -p " + cepstral_conf + " " + text; 00062 } 00063 else 00064 { 00065 command = "echo " + text + " | text2wave | aplay -q"; 00066 } 00067 system(command.c_str()); 00068 } 00069 00070 00071 }; 00072 00073 00074 int main(int argc, char** argv) 00075 { 00076 ros::init(argc, argv, "cob_sound"); 00077 00078 SayAction say("say"); 00079 ros::spin(); 00080 00081 return 0; 00082 } 00083