sound.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #include <ros/ros.h>
20 #include <diagnostic_msgs/DiagnosticArray.h>
21 #include <visualization_msgs/Marker.h>
22 #include <std_msgs/String.h>
23 #include <std_srvs/Trigger.h>
24 #include <cob_sound/SayAction.h>
25 #include <cob_sound/PlayAction.h>
26 #include <cob_srvs/SetString.h>
27 
28 #include <vlc/vlc.h>
29 #include <boost/filesystem.hpp>
30 #include <boost/lexical_cast.hpp>
31 
33 {
34 protected:
35 
47  bool mute_;
50 
51  libvlc_instance_t* vlc_inst_;
52  libvlc_media_player_t* vlc_player_;
53  libvlc_media_t* vlc_media_;
54 
55 public:
56  diagnostic_msgs::DiagnosticArray diagnostics_;
60 
62  as_say_(nh_, ros::this_node::getName() + "/say", boost::bind(&SoundAction::as_cb_say_, this, _1), false),
63  as_play_(nh_, ros::this_node::getName() + "/play", false)
64  {
65  nh_ = ros::NodeHandle("~");
66  }
67 
69  {
70  libvlc_media_player_stop(vlc_player_);
71  libvlc_media_player_release(vlc_player_);
72  libvlc_release(vlc_inst_);
73  }
74 
75  bool init()
76  {
77  vlc_inst_ = libvlc_new(0,NULL);
78  if(!vlc_inst_){ROS_ERROR("failed to create libvlc instance"); return false;}
79  vlc_player_ = libvlc_media_player_new(vlc_inst_);
80  if(!vlc_player_){ROS_ERROR("failed to create vlc media player object"); return false;}
81 
82  as_play_.registerGoalCallback(boost::bind(&SoundAction::as_goal_cb_play_, this));
83  as_play_.registerPreemptCallback(boost::bind(&SoundAction::as_preempt_cb_play_, this));
84  srvServer_say_ = nh_.advertiseService("say", &SoundAction::service_cb_say, this);
85  srvServer_play_ = nh_.advertiseService("play", &SoundAction::service_cb_play, this);
86  srvServer_stop_ = nh_.advertiseService("stop", &SoundAction::service_cb_stop, this);
87  srvServer_mute_ = nh_.advertiseService("mute", &SoundAction::service_cb_mute, this);
88  srvServer_unmute_ = nh_.advertiseService("unmute", &SoundAction::service_cb_unmute, this);
89  sub_say_ = nh_.subscribe("say", 1, &SoundAction::topic_cb_say, this);
90  sub_play_ = nh_.subscribe("play", 1, &SoundAction::topic_cb_play, this);
91  diagnostics_pub_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("diagnostics", 1);
92  diagnostics_timer_ = nh_.createTimer(ros::Duration(1.0), &SoundAction::timer_cb, this);
93  play_feedback_timer_ = nh_.createTimer(ros::Duration(0.25), &SoundAction::timer_play_feedback_cb, this, false, false);
94  pubMarker_ = nh_.advertise<visualization_msgs::Marker>("marker",1); //Advertise visualization marker topic
95  fade_volume_ = nh_.param<bool>("fade_volume", true);
96  fade_duration_ = nh_.param<double>("fade_duration", 0.5);
97 
98  mute_ = false;
99 
100  as_say_.start();
101  as_play_.start();
102  return true;
103  }
104 
106  {
107  std::string filename = as_play_.acceptNewGoal()->filename;
108  std::string message;
109  if(play(filename, message))
110  {
111  play_feedback_timer_.start();
112  }
113  else
114  {
115  cob_sound::PlayResult result;
116  result.success = false;
117  result.message = message;
118  as_play_.setAborted(result, message);
119  }
120  }
121 
123  {
124  if(as_play_.isActive())
125  {
126  fade_out();
127  play_feedback_timer_.stop();
128  libvlc_media_player_stop(vlc_player_);
129  }
130  cob_sound::PlayResult result;
131  result.success = false;
132  result.message = "Action has been preempted";
133  as_play_.setPreempted();
134  }
135 
136  void as_cb_say_(const cob_sound::SayGoalConstPtr &goal)
137  {
138  std::string message;
139  bool ret = say(goal->text, message);
140 
141  cob_sound::SayResult result;
142  result.success = ret;
143  result.message = message;
144  if (ret)
145  {
146  as_say_.setSucceeded(result, message);
147  }
148  else
149  {
150  as_say_.setAborted(result, message);
151  }
152  }
153 
154  bool service_cb_say(cob_srvs::SetString::Request &req,
155  cob_srvs::SetString::Response &res )
156  {
157  res.success = say(req.data, res.message);
158  return true;
159  }
160 
161  bool service_cb_play(cob_srvs::SetString::Request &req,
162  cob_srvs::SetString::Response &res )
163  {
164  res.success = play(req.data, res.message);
165  ros::Duration(0.1).sleep();
166  while((libvlc_media_player_is_playing(vlc_player_) == 1))
167  {
168  ros::Duration(0.1).sleep();
169  ROS_DEBUG("still playing %s", req.data.c_str());
170  }
171  return true;
172  }
173 
174  bool service_cb_stop(std_srvs::Trigger::Request &req,
175  std_srvs::Trigger::Response &res )
176  {
177  fade_out();
178  if(as_play_.isActive())
179  {
180  play_feedback_timer_.stop();
181  cob_sound::PlayResult result;
182  result.success = false;
183  result.message = "Action has been aborted";
184  as_play_.setAborted(result, result.message);
185  libvlc_media_player_stop(vlc_player_);
186  res.success = true;
187  res.message = "aborted running action";
188  }
189  else
190  {
191  if(libvlc_media_player_is_playing(vlc_player_) == 1)
192  {
193  libvlc_media_player_stop(vlc_player_);
194  res.success = true;
195  res.message = "stopped sound play";
196  }
197  else
198  {
199  res.success = false;
200  res.message = "nothing there to stop";
201  }
202  }
203  return true;
204  }
205 
206  bool service_cb_mute(std_srvs::Trigger::Request &req,
207  std_srvs::Trigger::Response &res )
208  {
209  mute_ = true;
210  res.success = true;
211  res.message = "Sound muted successfully";
212  return true;
213  }
214 
215  bool service_cb_unmute(std_srvs::Trigger::Request &req,
216  std_srvs::Trigger::Response &res )
217  {
218  mute_ = false;
219  res.success = true;
220  res.message = "Sound un-muted successfully";
221  return true;
222  }
223 
224  void topic_cb_say(const std_msgs::String::ConstPtr& msg)
225  {
226  std::string message;
227  say(msg->data.c_str(), message);
228  }
229 
230  void topic_cb_play(const std_msgs::String::ConstPtr& msg)
231  {
232  std::string message;
233  play(msg->data.c_str(), message);
234  }
235 
236  bool say(std::string data, std::string& message)
237  {
238  if (mute_)
239  {
240  message = "Sound is set to mute. You will hear nothing.";
241  ROS_WARN_STREAM(message);
242  return true;
243  }
244 
245  ROS_INFO("Saying: %s", data.c_str());
246 
247  publish_marker(data);
248 
249  std::string mode;
250  std::string command;
251  std::string cepstral_voice;
252  std::string cepstral_conf;
253  nh_.param<std::string>("mode",mode,"festival");
254  nh_.param<std::string>("cepstral_voice",cepstral_voice,"David");
255  nh_.param<std::string>("cepstral_settings",cepstral_conf,"\"speech/rate=170\"");
256  if (mode == "cepstral")
257  {
258  command = "aoss swift -p " + cepstral_conf + " -n " + cepstral_voice + " " + data;
259  }
260  else
261  {
262  command = "echo " + data + " | text2wave | aplay -q";
263  }
264 
265  int ret = system(command.c_str());
266  if (ret != 0)
267  {
268  message = "Command say failed to say '" + data + "' using mode " + mode + " (system return value: " + boost::lexical_cast<std::string>(ret) + ")";
269  ROS_ERROR_STREAM(message);
270  publish_diagnostics(diagnostic_msgs::DiagnosticStatus::ERROR, message);
271  return false;
272  }
273 
274  message = "Say successfull";
275  return true;
276  }
277 
278  bool play(std::string filename, std::string &message)
279  {
280  if (mute_)
281  {
282  message = "Sound is set to mute. You will hear nothing.";
283  ROS_WARN_STREAM(message);
284  return false;
285  }
286 
287  if (filename.empty())
288  {
289  message = "Cannot play because filename is empty.";
290  ROS_WARN_STREAM(message);
291  return false;
292  }
293 
294  if ( !boost::filesystem::exists(filename) )
295  {
296  message = "Cannot play '" + filename +"' because file does not exist.";
297  ROS_WARN_STREAM(message);
298  return false;
299  }
300 
301  vlc_media_ = libvlc_media_new_path(vlc_inst_, filename.c_str());
302  if(!vlc_media_)
303  {
304  message = "failed to create media for filepath %s", filename.c_str();
305  ROS_WARN_STREAM(message);
306  return false;
307  }
308 
309  libvlc_media_player_set_media(vlc_player_, vlc_media_);
310  libvlc_media_release(vlc_media_);
311 
312  if(!fade_out())
313  {
314  message = "failed to fade out";
315  ROS_WARN_STREAM(message);
316  return false;
317  }
318 
319  if(!fade_in())
320  {
321  message = "failed to fade in";
322  ROS_WARN_STREAM(message);
323  return false;
324  }
325 
326  message = "Play successfull";
327  return true;
328  }
329 
331  {
332  publish_diagnostics(diagnostic_msgs::DiagnosticStatus::OK, "sound controller running");
333  }
334 
336  {
337  if(as_play_.isActive())
338  {
339  if (libvlc_media_player_is_playing(vlc_player_) == 1)
340  {
341  float perc_done = libvlc_media_player_get_position(vlc_player_);
342  int64_t t = libvlc_media_player_get_time(vlc_player_);
343  cob_sound::PlayFeedback feedback;
344  feedback.position = perc_done;
345  feedback.time = t;
346  as_play_.publishFeedback(feedback);
347  }
348  else
349  {
350  play_feedback_timer_.stop();
351  cob_sound::PlayResult result;
352  result.success = true;
353  result.message = "Action succeeded";
354  as_play_.setSucceeded();
355  }
356  }
357  }
358 
359  void publish_marker(std::string data)
360  {
361  visualization_msgs::Marker marker;
362  marker.header.frame_id = "base_link";
363  marker.header.stamp = ros::Time();
364  marker.ns = "color";
365  marker.id = 0;
366  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
367  marker.action = visualization_msgs::Marker::ADD;
368  marker.lifetime = ros::Duration(std::max(data.size()*0.15, 2.0));
369  marker.text = data;
370  marker.pose.position.x = 0.0;
371  marker.pose.position.y = 0.0;
372  marker.pose.position.z = 1.8;
373  marker.pose.orientation.x = 0.0;
374  marker.pose.orientation.y = 0.0;
375  marker.pose.orientation.z = 0.0;
376  marker.pose.orientation.w = 1.0;
377  marker.scale.x = 0.1;
378  marker.scale.y = 0.1;
379  marker.scale.z = 0.1;
380  marker.color.a = 1.0;
381  marker.color.r = 1.0;
382  marker.color.g = 1.0;
383  marker.color.b = 1.0;
384  pubMarker_.publish(marker);
385  }
386 
387  void publish_diagnostics(unsigned char level, std::string message)
388  {
389  diagnostic_msgs::DiagnosticStatus status;
390  status.level = level;
391  status.name = "sound";
392  status.hardware_id = "sound";
393  status.message = message;
394  diagnostics_.status.push_back(status);
395 
396  diagnostics_.header.stamp = ros::Time::now();
397  diagnostics_pub_.publish(diagnostics_);
398 
399  diagnostics_.status.resize(0);
400  }
401 
402  bool fade_in()
403  {
404  if(libvlc_media_player_play(vlc_player_) >= 0)
405  {
406  if(fade_volume_)
407  {
408  while(libvlc_audio_set_volume(vlc_player_,0) != 0)
409  ros::Duration(0.05).sleep();
410  for(int i = 0; i < 100; i+=5)
411  {
412  libvlc_audio_set_volume(vlc_player_,i);
413  ros::Duration(fade_duration_/20.0).sleep();
414  }
415  }
416  else
417  {
418  while(libvlc_audio_set_volume(vlc_player_,100) != 0)
419  ros::Duration(0.05).sleep();
420  }
421  }
422  else
423  {
424  return false;
425  }
426 
427  return true;
428  }
429 
430  bool fade_out()
431  {
432  int volume = libvlc_audio_get_volume(vlc_player_);
433  if(libvlc_media_player_is_playing(vlc_player_))
434  {
435  if(fade_volume_)
436  {
437  for(int i = volume - (volume%5); i>=0; i-=5)
438  {
439  libvlc_audio_set_volume(vlc_player_,i);
440  ros::Duration(fade_duration_/20.0).sleep();
441  }
442  }
443  }
444  return true;
445  }
446 };
447 
448 
449 int main(int argc, char** argv)
450 {
451  ros::init(argc, argv, "cob_sound");
452 
453  SoundAction sound;
454  if(!sound.init())
455  {
456  ROS_ERROR("sound init failed");
457  return 1;
458  }
459  else
460  {
461  ROS_INFO("sound node started");
462  ros::spin();
463  return 0;
464  }
465 }
bool mute_
Definition: sound.cpp:47
ros::Subscriber sub_say_
Definition: sound.cpp:44
boost::shared_ptr< const Goal > acceptNewGoal()
void publishFeedback(const FeedbackConstPtr &feedback)
filename
bool fade_out()
Definition: sound.cpp:430
diagnostic_msgs::DiagnosticArray diagnostics_
Definition: sound.cpp:56
ros::Subscriber sub_play_
Definition: sound.cpp:45
void timer_cb(const ros::TimerEvent &)
Definition: sound.cpp:330
void publish(const boost::shared_ptr< M > &message) const
bool service_cb_say(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res)
Definition: sound.cpp:154
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void publish_marker(std::string data)
Definition: sound.cpp:359
void as_preempt_cb_play_()
Definition: sound.cpp:122
bool sleep() const
void stop()
bool init()
Definition: sound.cpp:75
void timer_play_feedback_cb(const ros::TimerEvent &)
Definition: sound.cpp:335
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
SoundAction()
Definition: sound.cpp:61
void start()
std::string getName(void *handle)
libvlc_media_player_t * vlc_player_
Definition: sound.cpp:52
ros::Publisher pubMarker_
Definition: sound.cpp:59
double fade_duration_
Definition: sound.cpp:48
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
void publish_diagnostics(unsigned char level, std::string message)
Definition: sound.cpp:387
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
ROSLIB_DECL std::string command(const std::string &cmd)
void as_cb_say_(const cob_sound::SayGoalConstPtr &goal)
Definition: sound.cpp:136
ros::Publisher diagnostics_pub_
Definition: sound.cpp:57
~SoundAction(void)
Definition: sound.cpp:68
bool service_cb_stop(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
Definition: sound.cpp:174
#define ROS_INFO(...)
bool service_cb_play(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res)
Definition: sound.cpp:161
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ros::ServiceServer srvServer_unmute_
Definition: sound.cpp:43
libvlc_instance_t * vlc_inst_
Definition: sound.cpp:51
libvlc_media_t * vlc_media_
Definition: sound.cpp:53
void registerPreemptCallback(boost::function< void()> cb)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
bool say(std::string data, std::string &message)
Definition: sound.cpp:236
actionlib::SimpleActionServer< cob_sound::PlayAction > as_play_
Definition: sound.cpp:38
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ROSCPP_DECL void spin()
bool service_cb_unmute(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
Definition: sound.cpp:215
#define ROS_WARN_STREAM(args)
ros::ServiceServer srvServer_play_
Definition: sound.cpp:40
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
bool play(std::string filename, std::string &message)
Definition: sound.cpp:278
ros::Timer play_feedback_timer_
Definition: sound.cpp:46
bool service_cb_mute(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
Definition: sound.cpp:206
int main(int argc, char **argv)
Definition: sound.cpp:449
ros::ServiceServer srvServer_mute_
Definition: sound.cpp:42
bool fade_in()
Definition: sound.cpp:402
static Time now()
ros::ServiceServer srvServer_stop_
Definition: sound.cpp:41
void topic_cb_play(const std_msgs::String::ConstPtr &msg)
Definition: sound.cpp:230
bool fade_volume_
Definition: sound.cpp:49
#define ROS_ERROR_STREAM(args)
void registerGoalCallback(boost::function< void()> cb)
void topic_cb_say(const std_msgs::String::ConstPtr &msg)
Definition: sound.cpp:224
ros::Timer diagnostics_timer_
Definition: sound.cpp:58
#define ROS_ERROR(...)
ros::ServiceServer srvServer_say_
Definition: sound.cpp:39
void as_goal_cb_play_()
Definition: sound.cpp:105
#define ROS_DEBUG(...)
actionlib::SimpleActionServer< cob_sound::SayAction > as_say_
Definition: sound.cpp:37
ros::NodeHandle nh_
Definition: sound.cpp:36


cob_sound
Author(s): Florian Weisshardt, Benjamin Maidel
autogenerated on Wed Apr 7 2021 02:11:51