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


yocs_cmd_vel_mux
Author(s): Jorge Santos Simon
autogenerated on Thu Jun 6 2019 21:47:16