cmd_vel_mux_nodelet.cpp
Go to the documentation of this file.
00001 
00009 /*****************************************************************************
00010  ** Includes
00011  *****************************************************************************/
00012 
00013 #include <fstream>
00014 #include <std_msgs/String.h>
00015 #include <pluginlib/class_list_macros.h>
00016 #include "../include/cmd_vel_mux/cmd_vel_mux_nodelet.hpp"
00017 #include "../include/cmd_vel_mux/exceptions.hpp"
00018 
00019 /*****************************************************************************
00020 ** Namespaces
00021 *****************************************************************************/
00022 
00023 namespace cmd_vel_mux {
00024 
00025 /*****************************************************************************
00026  ** Implementation
00027  *****************************************************************************/
00028 
00029 void CmdVelMuxNodelet::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg, unsigned int idx)
00030 {
00031   // Reset timer for this source
00032   cmd_vel_sub[idx].timer.stop();
00033   cmd_vel_sub[idx].timer.start();
00034 
00035   cmd_vel_sub[idx].active = true;   // obviously his source is sending commands, so active
00036 
00037   // Give permit to publish to this source if it's the only active or is
00038   // already allowed or has higher priority that the currently allowed
00039   if ((cmd_vel_sub.allowed == VACANT) ||
00040       (cmd_vel_sub.allowed == idx)    ||
00041       (cmd_vel_sub[idx].priority > cmd_vel_sub[cmd_vel_sub.allowed].priority))
00042   {
00043     if (cmd_vel_sub.allowed != idx)
00044     {
00045       cmd_vel_sub.allowed = idx;
00046 
00047       // Notify the world that a new cmd_vel source took the control
00048       std_msgs::StringPtr acv_msg(new std_msgs::String);
00049       acv_msg->data = cmd_vel_sub[idx].name;
00050       active_subscriber.publish(acv_msg);
00051     }
00052 
00053     mux_cmd_vel_pub.publish(msg);
00054   }
00055 }
00056 
00057 void CmdVelMuxNodelet::timerCallback(const ros::TimerEvent& event, unsigned int idx)
00058 {
00059   if (cmd_vel_sub.allowed == idx)
00060   {
00061     // No cmd_vel messages timeout happened to currently active source, so...
00062     cmd_vel_sub.allowed = VACANT;
00063 
00064     // ...notify the world that nobody is publishing on cmd_vel; its vacant
00065     std_msgs::StringPtr acv_msg(new std_msgs::String);
00066     acv_msg->data = "idle";
00067     active_subscriber.publish(acv_msg);
00068   }
00069 
00070   cmd_vel_sub[idx].active = false;
00071 }
00072 
00073 void CmdVelMuxNodelet::onInit()
00074 {
00075   ros::NodeHandle &nh = this->getPrivateNodeHandle();
00076 
00077   /*********************
00078   ** Dynamic Reconfigure
00079   **********************/
00080   dynamic_reconfigure_cb = boost::bind(&CmdVelMuxNodelet::reloadConfiguration, this, _1, _2);
00081   dynamic_reconfigure_server = new dynamic_reconfigure::Server<cmd_vel_mux::reloadConfig>(nh);
00082   dynamic_reconfigure_server->setCallback(dynamic_reconfigure_cb);
00083 
00084   active_subscriber = nh.advertise <std_msgs::String> ("active", 1, true); // latched topic
00085 
00086   // Notify the world that by now nobody is publishing on cmd_vel yet
00087   std_msgs::StringPtr active_msg(new std_msgs::String);
00088   active_msg->data = "idle";
00089   active_subscriber.publish(active_msg);
00090 
00091   // could use a call to reloadConfiguration here, but it seems to automatically call it once with defaults anyway.
00092   NODELET_DEBUG("CmdVelMux : successfully initialised");
00093 }
00094 
00095 void CmdVelMuxNodelet::reloadConfiguration(cmd_vel_mux::reloadConfig &config, uint32_t unused_level)
00096 {
00097   std::string yaml_cfg_file;
00098   ros::NodeHandle &nh = this->getPrivateNodeHandle();
00099   if( config.yaml_cfg_file == "" )
00100   {
00101     // typically fired on startup, so look for a parameter to set a default
00102     nh.getParam("yaml_cfg_file", yaml_cfg_file);
00103   }
00104   else
00105   {
00106     yaml_cfg_file = config.yaml_cfg_file;
00107   }
00108 
00109   /*********************
00110   ** Yaml File Parsing
00111   **********************/
00112   std::ifstream ifs(yaml_cfg_file.c_str(), std::ifstream::in);
00113   if (ifs.good() == false)
00114   {
00115     NODELET_ERROR_STREAM("CmdVelMux : configuration file not found [" << yaml_cfg_file << "]");
00116     return;
00117   }
00118   // probably need to bring the try catches back here
00119   YAML::Parser parser(ifs);
00120   YAML::Node doc;
00121   parser.GetNextDocument(doc);
00122 
00123   /*********************
00124   ** Output Publisher
00125   **********************/
00126   std::string output_name("output");
00127   const YAML::Node *node = doc.FindValue("publisher");
00128   if ( node != NULL ) {
00129     *node >> output_name;
00130   }
00131   mux_cmd_vel_pub = nh.advertise <geometry_msgs::Twist> (output_name, 10);
00132 
00133   /*********************
00134   ** Input Subscribers
00135   **********************/
00136   try {
00137     cmd_vel_sub.configure(doc["subscribers"]);
00138   }
00139   catch(EmptyCfgException& e) {
00140     NODELET_WARN("CmdVelMux : yaml configured zero subscribers, check yaml content.");
00141   }
00142   catch(YamlException& e) {
00143     NODELET_ERROR_STREAM("CmdVelMux : yaml parsing problem [" << std::string(e.what()) + "]");
00144   }
00145 
00146   // Publishers and subscribers
00147   for (unsigned int i = 0; i < cmd_vel_sub.size(); i++)
00148   {
00149     cmd_vel_sub[i].subs =
00150         nh.subscribe<geometry_msgs::Twist>(cmd_vel_sub[i].topic, 10, CmdVelFunctor(i, this));
00151 
00152     // Create (stopped by now) a one-shot timer for every subscriber
00153     cmd_vel_sub[i].timer =
00154         nh.createTimer(ros::Duration(cmd_vel_sub[i].timeout), TimerFunctor(i, this), true, false);
00155 
00156     NODELET_DEBUG("CmdVelMux : subscribed to '%s' on topic '%s'. pr: %d, to: %.2f",
00157               cmd_vel_sub[i].name.c_str(), cmd_vel_sub[i].topic.c_str(),
00158               cmd_vel_sub[i].priority, cmd_vel_sub[i].timeout);
00159   }
00160 
00161   NODELET_INFO_STREAM("CmdVelMux : (re)configured [" << yaml_cfg_file << "]");
00162   ifs.close();
00163 }
00164 
00165 } // namespace cmd_vel_mux
00166 
00167 PLUGINLIB_EXPORT_CLASS(cmd_vel_mux::CmdVelMuxNodelet, nodelet::Nodelet);


cmd_vel_mux
Author(s): Jorge Santos Simon
autogenerated on Mon Oct 6 2014 09:11:22