36 #include "std_msgs/String.h" 37 #include "topic_tools/MuxSelect.h" 38 #include "topic_tools/MuxAdd.h" 39 #include "topic_tools/MuxList.h" 40 #include "topic_tools/MuxDelete.h" 67 static list<struct sub_info_t>
g_subs;
82 topic_tools::MuxSelect::Response &res )
90 ROS_DEBUG(
"Unsubscribing to %s, lazy", res.prev_topic.c_str());
98 res.prev_topic = string(
"");
103 ROS_INFO(
"mux selected to no input.");
110 ROS_INFO(
"trying to switch mux to %s", req.topic.c_str());
112 for (list<struct sub_info_t>::iterator it =
g_subs.begin();
119 ROS_INFO(
"mux selected input: [%s]", it->topic_name.c_str());
140 topic_tools::MuxSelect::Response &res )
142 ROS_WARN(
"the <topic>_select service is deprecated; use mux/select instead");
158 for (
static list<struct sub_info_t>::iterator it =
g_subs.begin(); it !=
g_subs.end(); ++it) {
160 ROS_INFO(
"Unregistering from %s", it->topic_name.c_str());
175 ROS_INFO(
"lazy mode; unsubscribing");
185 topic_tools::MuxList::Response& res)
188 for (list<struct sub_info_t>::iterator it =
g_subs.begin();
192 res.topics.push_back(it->topic_name);
199 topic_tools::MuxAdd::Response& res)
203 ROS_INFO(
"trying to add %s to mux", req.topic.c_str());
208 ROS_WARN(
"failed to add topic %s to mux, because it's reserved for special use",
214 for (list<struct sub_info_t>::iterator it =
g_subs.begin();
220 ROS_WARN(
"tried to add a topic that mux was already listening to: [%s]",
221 it->topic_name.c_str());
238 ROS_WARN(
"failed to add topic %s to mux, because it's an invalid name: %s",
239 req.topic.c_str(), e.what());
243 g_subs.push_back(sub_info);
245 ROS_INFO(
"added %s to mux", req.topic.c_str());
251 topic_tools::MuxDelete::Response& res)
255 ROS_INFO(
"trying to delete %s from mux", req.topic.c_str());
257 for (list<struct sub_info_t>::iterator it =
g_subs.begin();
266 ROS_WARN(
"tried to delete currently selected topic %s from mux", req.topic.c_str());
274 ROS_INFO(
"deleted topic %s from mux", req.topic.c_str());
279 ROS_WARN(
"tried to delete non-subscribed topic %s from mux", req.topic.c_str());
283 int main(
int argc,
char **argv)
290 printf(
"\nusage: mux OUT_TOPIC IN_TOPIC1 [IN_TOPIC2 [...]]\n\n");
296 ros::init(argc, argv, topic_name +
string(
"_mux"),
298 vector<string> topics;
299 for (
unsigned int i = 2; i < args.size(); i++)
300 topics.push_back(args[i]);
309 g_pub_selected = mux_nh.advertise<std_msgs::String>(string(
"selected"), 1,
true);
311 for (
size_t i = 0; i < topics.size(); i++)
318 g_subs.push_back(sub_info);
322 std::string initial_topic;
323 pnh.
getParam(
"initial_topic", initial_topic);
325 if (initial_topic.empty())
332 ROS_INFO(
"mux selected to no input.");
338 ROS_INFO(
"trying to switch mux to %s", initial_topic.c_str());
340 for (list<struct sub_info_t>::iterator it =
g_subs.begin();
347 t.data = initial_topic;
348 ROS_INFO(
"mux selected input: [%s]", it->topic_name.c_str());
368 for (list<struct sub_info_t>::iterator it =
g_subs.begin();
void in_cb(const boost::shared_ptr< ShapeShifter const > &msg, ShapeShifter *s)
int main(int argc, char **argv)
bool add_topic_cb(topic_tools::MuxAdd::Request &req, topic_tools::MuxAdd::Response &res)
void publish(const boost::shared_ptr< M > &message) const
static list< struct sub_info_t > g_subs
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool del_topic_cb(topic_tools::MuxDelete::Request &req, topic_tools::MuxDelete::Response &res)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool list_topic_cb(topic_tools::MuxList::Request &req, topic_tools::MuxList::Response &res)
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
static ros::Publisher g_pub_selected
static ros::NodeHandle * g_node
static list< struct sub_info_t >::iterator g_selected
bool sel_srv_cb_dep(topic_tools::MuxSelect::Request &req, topic_tools::MuxSelect::Response &res)
static const string g_none_topic
uint32_t getNumSubscribers() const
bool getParam(const std::string &key, std::string &s) const
void conn_cb(const ros::SingleSubscriberPublisher &)
ROSCPP_DECL void removeROSArgs(int argc, const char *const *argv, V_string &args_out)
bool sel_srv_cb(topic_tools::MuxSelect::Request &req, topic_tools::MuxSelect::Response &res)
static string g_output_topic
static ros::Publisher g_pub