Mapping ros::Publishers to different dynamically generated topic names
Hello all;
I am programming a tcp server to connect with another machine not using ROS. The main idea is to generate a topic for each connection, using the port value as identifier of the connection. So I use an std::map to pair ros::publisher and topic name:
std::map<std::string, ros::Publisher> comms_map;
When a new connection is received:
void addNewConnection(int socket, struct sockaddr_in addr)
{
CONNECTION aux;
aux.socket= socket;
aux.port= addr.sin_port;
aux.addr= addr.sin_addr.s_addr;
connections.push_back(aux);
//create the new publisher
std::string topic_name = getTopicName(aux.port); //Creates the topic name "/tcp_server_data_pNUMPORT"
ros::Publisher pub_aux = nh->advertise<std_msgs::String>(topic_name.c_str(), 1);
comms_map.insert(std::pair<std::string, ros::Publisher>(topic_name,pub_aux));
ROS_INFO("Added connection -> socket: %d port: %u addr: %ld",aux.socket, aux.port, aux.addr);
}
So, when I receive data, I try to publish over the correct ros::Publisher, using this code:
void publishMsgToTopic(int socket, char *buffer)
{
//publish buffer to topic tcp_server_data_pXXXXXX
std_msgs::String msg;
unsigned long addr;
unsigned short port;
findConnection(socket,addr,port);
std::string topic_name = getTopicName(port);
ROS_INFO("publishMsgToTopic -> socket %d port: %u addr: %ld. Topic: %s Message: %s",
socket, port, addr, (char *)(topic_name.c_str()), buffer);
msg.data = std::string(buffer);
ros::Publisher aux = comms_map.find(topic_name)->second;
//also tested:
// ros::Publisher aux = comms_map[topic_name];
// ros::Publisher aux = comms_map.at(topic_name);
aux.publish(msg);
}
Although the topic is created, and I do not receive any error, the topic does not publish any data ! Everything else in the process has been tested and works. IMPORTANT: each connection is attended in a child (fork process)
Could anyone give a hand about this?
Thank you very much in advance