Go to the documentation of this file.00001
00009
00010
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
00021
00022
00023 namespace cmd_vel_mux {
00024
00025
00026
00027
00028
00029 void CmdVelMuxNodelet::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg, unsigned int idx)
00030 {
00031
00032 cmd_vel_sub[idx].timer.stop();
00033 cmd_vel_sub[idx].timer.start();
00034
00035 cmd_vel_sub[idx].active = true;
00036
00037
00038
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
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
00062 cmd_vel_sub.allowed = VACANT;
00063
00064
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
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);
00085
00086
00087 std_msgs::StringPtr active_msg(new std_msgs::String);
00088 active_msg->data = "idle";
00089 active_subscriber.publish(active_msg);
00090
00091
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
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
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
00119 YAML::Parser parser(ifs);
00120 YAML::Node doc;
00121 parser.GetNextDocument(doc);
00122
00123
00124
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
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
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
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 }
00166
00167 PLUGINLIB_EXPORT_CLASS(cmd_vel_mux::CmdVelMuxNodelet, nodelet::Nodelet);