Allow bidirectional traffic

nimbro_topic_transport/launch/bidirectional_topics.launch

@@ -0,0 +1,22 @@
+<launch>
+    <arg name="allow_bidirectional" default="true" />
+
+    <!--
+    If you set up transports for a topic in both directions, don't forget to
+    provide the `ignored_publishers` parameter to both senders. Otherwise,
+    nimbro would enter an infinite republishing loop. You can test it by
+    playing with the `allow_bidirectional` argument.
+
+    The `ignored_publishers` sender parameter is a list of node-names of
+    all receivers receiving any of the topics this publisher publishes.
+     -->
+    <include file="$(find nimbro_topic_transport)/launch/tcp_receiver.launch" />
+    <include file="$(find nimbro_topic_transport)/launch/tcp_sender.launch">
+        <arg name="allow_bidirectional" value="$(arg allow_bidirectional)" />
+    </include>
+
+    <include file="$(find nimbro_topic_transport)/launch/tcp_receiver_back.launch" />
+    <include file="$(find nimbro_topic_transport)/launch/tcp_sender_back.launch">
+        <arg name="allow_bidirectional" value="$(arg allow_bidirectional)" />
+    </include>
+</launch>

nimbro_topic_transport/launch/tcp_receiver_back.launch

@@ -0,0 +1,11 @@
+<launch>
+	<node name="tcp_receiver_back" pkg="nimbro_topic_transport" type="tcp_receiver" output="screen">
+		<param name="port" value="17002" />
+
+		<!-- Remap topics so that sender & receiver do not clash if run on the
+		  same machine. This is not necessary in a typical setup :-)
+		 -->
+		<remap from="/recv/my_first_topic" to="/my_first_topic" />
+		<remap from="/recv/my_second_topic" to="/my_second_topic" />
+	</node>
+</launch>

nimbro_topic_transport/launch/tcp_sender.launch

@@ -1,5 +1,6 @@
 <launch>
 	<arg name="target" default="localhost" />
+	<arg name="allow_bidirectional" default="true" />
 
 	<!-- The UDP sender node -->
 	<node name="tcp_sender" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
@@ -10,5 +11,10 @@
 
 		<!-- Load the list of topics from a YAML file -->
 		<rosparam command="load" file="$(find nimbro_topic_transport)/launch/topics.yaml" />
+
+		<!-- If bidirectional traffic over a topic is expected, fill this parameter.
+		     See details in bidirectional_topics.launch -->
+		<rosparam param="ignored_publishers" if="$(arg allow_bidirectional)">["/tcp_receiver_back"]</rosparam>
+		<rosparam param="ignored_publishers" unless="$(arg allow_bidirectional)">[]</rosparam>
 	</node>
 </launch>

nimbro_topic_transport/launch/tcp_sender_back.launch

@@ -0,0 +1,20 @@
+<launch>
+	<arg name="target" default="localhost" />
+	<arg name="allow_bidirectional" default="true" />
+
+	<!-- The UDP sender node -->
+	<node name="tcp_sender_back" pkg="nimbro_topic_transport" type="tcp_sender" output="screen">
+
+		<!-- The destination host name or IP address -->
+		<param name="destination_addr" value="$(arg target)" />
+		<param name="destination_port" value="17002" />
+
+		<!-- Load the list of topics from a YAML file -->
+		<rosparam command="load" file="$(find nimbro_topic_transport)/launch/topics_back.yaml" />
+
+		<!-- If bidirectional traffic over a topic is expected, fill this parameter.
+		     See details in bidirectional_topics.launch -->
+		<rosparam param="ignored_publishers" if="$(arg allow_bidirectional)">["/tcp_receiver"]</rosparam>
+		<rosparam param="ignored_publishers" unless="$(arg allow_bidirectional)">[]</rosparam>
+	</node>
+</launch>

nimbro_topic_transport/launch/topics.yaml

@@ -3,3 +3,4 @@ topics:
    compress: true # enable bz2 compression
    rate: 1.0 # rate limit at 1Hz
  - name: "/my_second_topic"
+   latch: True

nimbro_topic_transport/launch/topics_back.yaml

@@ -0,0 +1,6 @@
+topics:
+ - name: "/recv/my_first_topic"
+   compress: true # enable bz2 compression
+   rate: 1.0 # rate limit at 1Hz
+ - name: "/recv/my_second_topic"
+   latch: True

nimbro_topic_transport/src/tcp/tcp_packet.h

@@ -11,7 +11,8 @@ namespace nimbro_topic_transport
 
 enum TCPFlag
 {
-	TCP_FLAG_COMPRESSED = (1 << 0)
+	TCP_FLAG_COMPRESSED = (1 << 0),
+	TCP_FLAG_LATCHED = (1 << 1)
 };
 
 struct TCPHeader

nimbro_topic_transport/src/tcp/tcp_receiver.cpp

@@ -345,6 +345,10 @@ void TCPReceiver::ClientHandler::run()
 					topic_info::getMsgDef(type)
 				);
 
+				if (header.flags() & TCP_FLAG_LATCHED) {
+					options.latch = true;
+				}
+
 				// It will take subscribers some time to connect to our publisher.
 				// Therefore, latch messages so they will not be lost.
 				// No, this is often unexpected. Instead, wait before publishing.

nimbro_topic_transport/src/tcp/tcp_sender.cpp

@@ -9,6 +9,8 @@
 #include <netinet/tcp.h>
 #include <boost/algorithm/string/replace.hpp>
 
+#include "ros/message_traits.h"
+
 #include <netdb.h>
 
 namespace nimbro_topic_transport
@@ -77,11 +79,14 @@ TCPSender::TCPSender()
 		if(entry.hasMember("compress") && ((bool)entry["compress"]) == true)
 			flags |= TCP_FLAG_COMPRESSED;
 
-		boost::function<void(const topic_tools::ShapeShifter::ConstPtr&)> func;
-		func = boost::bind(&TCPSender::send, this, topic, flags, _1);
+		if(entry.hasMember("latch") && ((bool)entry["latch"]) == true)
+			flags |= TCP_FLAG_LATCHED;
+
+		boost::function<void(const ros::MessageEvent<topic_tools::ShapeShifter const>&)> func;
+		func = boost::bind(&TCPSender::messageCallback, this, topic, flags, _1);
 
 		ros::SubscribeOptions options;
-		options.initByFullCallbackType<topic_tools::ShapeShifter::ConstPtr>(topic, 20, func);
+		options.initByFullCallbackType<const ros::MessageEvent<topic_tools::ShapeShifter const>&>(topic, 20, func);
 
 		if(entry.hasMember("type"))
 		{
@@ -114,6 +119,9 @@ TCPSender::TCPSender()
 #endif
 	}
 
+    if (m_nh.hasParam("ignored_publishers"))
+        m_nh.getParam("ignored_publishers", m_ignoredPubs);
+
 	char hostnameBuf[256];
 	gethostname(hostnameBuf, sizeof(hostnameBuf));
 	hostnameBuf[sizeof(hostnameBuf)-1] = 0;
@@ -138,7 +146,7 @@ TCPSender::TCPSender()
 		boost::bind(&TCPSender::updateStats, this)
 	);
 	m_statsTimer.start();
-}	
+}
 
 TCPSender::~TCPSender()
 {
@@ -218,6 +226,8 @@ bool TCPSender::connect()
 	ROS_WARN("Not setting TCP_USER_TIMEOUT");
 #endif
 
+	this->sendLatched();
+
 	return true;
 }
 
@@ -234,7 +244,31 @@ class PtrStream
 	uint8_t* m_ptr;
 };
 
-void TCPSender::send(const std::string& topic, int flags, const topic_tools::ShapeShifter::ConstPtr& shifter)
+void TCPSender::messageCallback(const std::string& topic, int flags,
+                                const ros::MessageEvent<topic_tools::ShapeShifter const>& event)
+{
+#if WITH_CONFIG_SERVER
+    if (! (*m_enableTopic[topic])() )
+		return;
+#endif
+
+    if (m_ignoredPubs.size() > 0) // check if the message wasn't published by an ignored publisher
+    {
+        std::string messagePublisher = event.getConnectionHeader()["callerid"];
+        for (std::vector<std::string>::iterator ignoredPublisher = m_ignoredPubs.begin();
+             ignoredPublisher != m_ignoredPubs.end(); ignoredPublisher++) {
+            if (messagePublisher == *ignoredPublisher) {
+                return;
+            }
+        }
+    }
+
+    this->send(topic, flags, event.getMessage());
+}
+
+
+void TCPSender::send(const std::string& topic, int flags, topic_tools::ShapeShifter::ConstPtr shifter,
+					 const bool reconnect)
 {
 #if WITH_CONFIG_SERVER
 	if (! (*m_enableTopic[topic])() )
@@ -250,6 +284,9 @@ void TCPSender::send(const std::string& topic, int flags, const topic_tools::Sha
 	if(flags & TCP_FLAG_COMPRESSED)
 		maxDataSize = size + size / 100 + 1200; // taken from bzip2 docs
 
+	if (flags & TCP_FLAG_LATCHED)
+		this->m_latchedMessages[topic] = std::make_pair(shifter, flags);
+
 	m_packet.resize(
 		sizeof(TCPHeader) + topic.length() + type.length() + maxDataSize
 	);
@@ -312,10 +349,12 @@ void TCPSender::send(const std::string& topic, int flags, const topic_tools::Sha
 	{
 		if(m_fd == -1)
 		{
-			if(!connect())
+			if(reconnect && !connect())
 			{
 				ROS_WARN("Connection failed, trying again");
 				continue;
+			} else if (!reconnect) {
+				break;
 			}
 		}
 
@@ -363,6 +402,18 @@ void TCPSender::updateStats()
 	m_sentBytesInStatsInterval = 0;
 }
 
+void TCPSender::sendLatched() {
+
+	std::map<std::string, std::pair<topic_tools::ShapeShifter::ConstPtr, int> >::iterator it =
+			this->m_latchedMessages.begin();
+
+	// send all latched messages
+	while (it != this->m_latchedMessages.end()) {
+		this->send(it->first, it->second.second, it->second.first, false);
+		it++;
+	}
+}
+
 }
 
 

nimbro_topic_transport/src/tcp/tcp_sender.h

@@ -14,6 +14,10 @@
 #include <config_server/parameter.h>
 #endif
 
+#include <map>
+
+#include "ros/message_event.h"
+
 #include <nimbro_topic_transport/SenderStats.h>
 
 namespace nimbro_topic_transport
@@ -27,7 +31,11 @@ class TCPSender
 
 	bool connect();
 
-	void send(const std::string& topic, int flags, const topic_tools::ShapeShifter::ConstPtr& shifter);
+	void send(const std::string& topic, int flags, topic_tools::ShapeShifter::ConstPtr shifter,
+              const bool reconnect = true);
+    void messageCallback(const std::string& topic, int flags,
+                         const ros::MessageEvent<topic_tools::ShapeShifter const>& shifter);
+    void sendLatched();
 private:
 	void updateStats();
 
@@ -42,6 +50,9 @@ class TCPSender
 	std::vector<ros::Subscriber> m_subs;
 	std::vector<uint8_t> m_packet;
 	std::vector<uint8_t> m_compressionBuf;
+    std::vector<std::string> m_ignoredPubs;
+
+	std::map<std::string, std::pair<topic_tools::ShapeShifter::ConstPtr, int> > m_latchedMessages;
 
 #if WITH_CONFIG_SERVER
 	std::map<std::string, boost::shared_ptr<config_server::Parameter<bool>>> m_enableTopic;

nimbro_topic_transport/src/udp/udp_sender.cpp

@@ -140,6 +140,10 @@ UDPSender::UDPSender()
 		if(list[i].hasMember("type"))
 			type = (std::string)(list[i]["type"]);
 
+		if(list[i].hasMember("latch"))
+			ROS_WARN_STREAM("Ignoring 'latch' flag at UDP topic " << ((std::string)list[i]["name"]).c_str() <<
+							" (UDP topics can't be latched).");
+
 		TopicSender* sender = new TopicSender(this, &nh, list[i]["name"], rate, resend, flags, enabled, type);
 
 		if(m_relayMode)