21 #include <cob_mimic/SetMimicAction.h> 22 #include <cob_mimic/SetMimicGoal.h> 23 #include <cob_mimic/SetMimicFeedback.h> 24 #include <cob_mimic/SetMimicResult.h> 26 #include <cob_mimic/SetMimic.h> 27 #include <cob_mimic/SetMimicRequest.h> 28 #include <cob_mimic/SetMimicResponse.h> 33 #include <boost/thread.hpp> 34 #include <boost/filesystem.hpp> 35 #include <boost/format.hpp> 36 #include <boost/random/mersenne_twister.hpp> 37 #include <boost/random/uniform_real_distribution.hpp> 38 #include <boost/random/uniform_int_distribution.hpp> 86 "--mouse-hide-timeout=0",
92 "--no-video-title-show",
97 int argc =
sizeof( argv ) /
sizeof( *argv );
106 set_mimic(default_mimic_, 1, 1.0,
false);
144 if((lgn = getlogin()) == NULL)
146 lgn = getenv(
"USER");
147 if(lgn == NULL || std::string(lgn) ==
"")
153 std::string username(lgn);
154 mimic_folder_ =
"/tmp/mimic_" + username;
155 ROS_INFO(
"copying all mimic files to %s...", mimic_folder_.c_str());
157 std::string mimics_path = pkg_path +
"/common";
160 if(boost::filesystem::exists(mimic_folder_))
162 boost::filesystem::remove_all(mimic_folder_);
165 catch(boost::filesystem::filesystem_error
const & e)
171 if(
copy_dir(boost::filesystem::path(mimics_path), boost::filesystem::path(mimic_folder_)) )
173 ROS_INFO(
"...copied all mimic files to %s", mimic_folder_.c_str());
178 ROS_ERROR(
"...could not copy mimic files to %s", mimic_folder_.c_str());
185 random_timer_.
stop();
186 action_active_ =
true;
188 if(
set_mimic(goal->mimic, goal->repeat, goal->speed, blocking_))
195 action_active_ =
false;
197 if(goal->mimic !=
"falling_asleep" && goal->mimic !=
"sleeping")
202 cob_mimic::SetMimic::Response &res )
204 service_active_ =
true;
205 random_timer_.
stop();
207 res.success =
set_mimic(req.mimic, req.repeat, req.speed, blocking_);
210 if(req.mimic !=
"falling_asleep" && req.mimic !=
"sleeping")
212 service_active_ =
false;
216 bool set_mimic(std::string mimic,
int repeat,
float speed,
bool blocking)
218 new_mimic_request_=
true;
219 ROS_INFO(
"New mimic request with: %s", mimic.c_str());
221 active_mimic_= (boost::format(
"Mimic: %1%, repeat: %2%, speed: %3%, blocking: %4%")% mimic % repeat % speed % blocking).str();
222 new_mimic_request_=
false;
223 ROS_INFO(
"Mimic: %s (speed: %f, repeat: %d)", mimic.c_str(), speed, repeat);
225 std::string
filename = mimic_folder_ +
"/" + mimic +
".mp4";
228 if ( !boost::filesystem::exists(filename) )
230 if ( !boost::filesystem::exists(mimic) )
232 ROS_ERROR(
"File not found: %s", filename.c_str());
233 active_mimic_ =
"None";
239 ROS_INFO(
"Playing mimic from non-default file: %s", mimic.c_str());
245 repeat = std::max(1, repeat);
246 if(repeat > 1 && !blocking)
248 ROS_WARN_STREAM(
"Mimic repeat ("<<repeat<<
") is overwritten by blocking ("<<blocking<<
"). Will only play once.");
255 ROS_WARN_STREAM(
"Mimic speed ("<<speed<<
") cannot be 0 or negative. Setting Speed to 1.0");
260 if(libvlc_media_player_set_rate(vlc_player_, speed)!=0){
ROS_ERROR(
"failed to set movie play rate");}
266 ROS_WARN(
"mimic %s preempted between repetitions", mimic.c_str());
267 active_mimic_ =
"None";
272 vlc_media_ = libvlc_media_new_path(vlc_inst_, filename.c_str());
275 ROS_ERROR(
"failed to create media for filepath %s", filename.c_str());
276 active_mimic_ =
"None";
281 libvlc_media_player_set_media(vlc_player_, vlc_media_);
282 libvlc_media_release(vlc_media_);
285 if(libvlc_media_player_play(vlc_player_)!=0)
288 active_mimic_ =
"None";
294 while(blocking && (libvlc_media_player_is_playing(vlc_player_) == 1))
297 ROS_DEBUG(
"still playing %s", mimic.c_str());
298 if(new_mimic_request_)
300 ROS_WARN(
"mimic %s preempted while playing", mimic.c_str());
301 active_mimic_ =
"None";
308 active_mimic_ =
"None";
316 set_mimic(random_mimics_[rand], 1, 1.5, blocking_);
320 bool copy_dir( boost::filesystem::path
const & source,
321 boost::filesystem::path
const & mimic_folder )
323 namespace fs = boost::filesystem;
327 if(!fs::exists(source) || !fs::is_directory(source))
329 ROS_ERROR_STREAM(
"Source directory " << source.string() <<
" does not exist or is not a directory.");
332 if(fs::exists(mimic_folder))
334 ROS_INFO_STREAM(
"Destination directory " << mimic_folder.string() <<
" already exists.");
338 if(!fs::create_directory(mimic_folder))
340 ROS_ERROR_STREAM(
"Unable to create mimic_folder directory" << mimic_folder.string());
344 catch(fs::filesystem_error
const & e)
350 for(fs::directory_iterator file(source); file != fs::directory_iterator(); ++file)
354 fs::path current(file->path());
355 if(fs::is_directory(current))
358 if( !
copy_dir(current, mimic_folder / current.filename()) )
364 fs::copy_file(current, mimic_folder / current.filename() );
367 catch(fs::filesystem_error
const & e)
380 diagnostic_updater_.
update();
386 stat.
summary(diagnostic_msgs::DiagnosticStatus::OK,
"Mimic running");
387 stat.
add(
"Action is active", action_active_);
388 stat.
add(
"Service is active", service_active_);
389 stat.
add(
"Active mimic", active_mimic_);
394 int main(
int argc,
char** argv)
boost::thread diagnostic_thread_
void diagnostics_timer_thread()
void as_cb_mimic_(const cob_mimic::SetMimicGoalConstPtr &goal)
bool set_mimic(std::string mimic, int repeat, float speed, bool blocking)
void summary(unsigned char lvl, const std::string s)
std::string default_mimic_
boost::random::uniform_int_distribution int_dist_
void setHardwareID(const std::string &hwid)
diagnostic_updater::Updater diagnostic_updater_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::string getName(void *handle)
void add(const std::string &name, TaskFunction f)
std::string mimic_folder_
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(""))
libvlc_media_t * vlc_media_
void produce_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
ros::ServiceServer srvServer_mimic_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
libvlc_media_player_t * vlc_player_
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
int main(int argc, char **argv)
boost::random::mt19937 gen_
std::string active_mimic_
bool isPreemptRequested()
#define ROS_WARN_STREAM(args)
ROSLIB_DECL std::string getPath(const std::string &package_name)
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
bool service_cb_mimic(cob_mimic::SetMimic::Request &req, cob_mimic::SetMimic::Response &res)
#define ROS_INFO_STREAM(args)
std::vector< std::string > random_mimics_
void random_cb(const ros::TimerEvent &)
void add(const std::string &key, const T &val)
#define ROS_ERROR_STREAM(args)
boost::random::uniform_real_distribution real_dist_
bool copy_dir(boost::filesystem::path const &source, boost::filesystem::path const &mimic_folder)
libvlc_instance_t * vlc_inst_
actionlib::SimpleActionServer< cob_mimic::SetMimicAction > as_mimic_