37 #include "std_msgs/String.h" 38 #include "topic_tools/DemuxSelect.h" 39 #include "topic_tools/DemuxAdd.h" 40 #include "topic_tools/DemuxList.h" 41 #include "topic_tools/DemuxDelete.h" 66 static list<struct pub_info_t>
g_pubs;
70 topic_tools::DemuxSelect::Response &res )
77 res.prev_topic = string(
"");
82 ROS_INFO(
"demux selected to no output.");
89 ROS_INFO(
"trying to switch demux to %s", req.topic.c_str());
91 for (list<struct pub_info_t>::iterator it =
g_pubs.begin();
98 ROS_INFO(
"demux selected output: [%s]", it->topic_name.c_str());
105 ROS_WARN(
"Failed to switch to non-existing topic %s in demux.", req.topic.c_str());
121 ROS_DEBUG(
"Received an incoming msg ...");
124 bool selected_added =
false;
125 for (list<struct pub_info_t>::iterator it =
g_pubs.begin(); it !=
g_pubs.end(); ++it) {
130 selected_added =
true;
135 it->pub =
new ros::Publisher(msg->advertise(*g_node, it->topic_name, 10,
false));
139 ROS_WARN(
"failed to add topic '%s' to demux, because it's an invalid name: %s",
140 it->topic_name.c_str(), e.what());
144 ROS_INFO(
"Added publisher %s to demux!", it->topic_name.c_str());
164 topic_tools::DemuxList::Response& res)
167 for (list<struct pub_info_t>::iterator it =
g_pubs.begin();
171 res.topics.push_back(it->topic_name);
178 topic_tools::DemuxAdd::Response& res)
182 ROS_INFO(
"trying to add %s to demux", req.topic.c_str());
187 ROS_WARN(
"failed to add topic %s to demux, because it's reserved for special use",
193 for (list<struct pub_info_t>::iterator it =
g_pubs.begin();
199 ROS_WARN(
"tried to add a topic that demux was already publishing: [%s]",
200 it->topic_name.c_str());
208 g_pubs.push_back(pub_info);
210 ROS_INFO(
"PRE added %s to demux", req.topic.c_str());
216 topic_tools::DemuxDelete::Response& res)
220 ROS_INFO(
"trying to delete %s from demux", req.topic.c_str());
222 for (list<struct pub_info_t>::iterator it =
g_pubs.begin();
231 ROS_WARN(
"tried to delete currently selected topic %s from demux", req.topic.c_str());
238 ROS_INFO(
"deleted topic %s from demux", req.topic.c_str());
243 ROS_WARN(
"tried to delete non-published topic %s from demux", req.topic.c_str());
247 int main(
int argc,
char **argv)
254 printf(
"\nusage: demux IN_TOPIC OUT_TOPIC1 [OUT_TOPIC2 [...]]\n\n");
260 ros::init(argc, argv, topic_name +
string(
"_demux"),
262 vector<string> topics;
263 for (
unsigned int i = 2; i < args.size(); i++)
264 topics.push_back(args[i]);
272 g_pub_selected = demux_nh.advertise<std_msgs::String>(string(
"selected"), 1,
true);
274 for (
size_t i = 0; i < topics.size(); i++)
279 g_pubs.push_back(pub_info);
280 ROS_INFO(
"PRE added %s to demux", topics[i].c_str());
301 for (list<struct pub_info_t>::iterator it =
g_pubs.begin();
bool sel_srv_cb(topic_tools::DemuxSelect::Request &req, topic_tools::DemuxSelect::Response &res)
bool list_topic_cb(topic_tools::DemuxList::Request &req, topic_tools::DemuxList::Response &res)
bool del_topic_cb(topic_tools::DemuxDelete::Request &req, topic_tools::DemuxDelete::Response &res)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
void in_cb(const boost::shared_ptr< ShapeShifter const > &msg)
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
static ros::Subscriber g_sub
static string g_input_topic
static list< struct pub_info_t >::iterator g_selected
static ros::Publisher g_pub_selected
static const string g_none_topic
ROSCPP_DECL void removeROSArgs(int argc, const char *const *argv, V_string &args_out)
static ros::NodeHandle * g_node
static list< struct pub_info_t > g_pubs
bool add_topic_cb(topic_tools::DemuxAdd::Request &req, topic_tools::DemuxAdd::Response &res)