Duplicate messages in bidirectional_bridge fix (#113)

* Pass ros2_publisher to ros2_subscriber callback

* Pass message_info to ros2_subscriber callback

* Add gid compare to ignore messages from bridge itself

* Fix style issues to pass tests

* Edit comment

* Add else statement to raise an exception and reset rmw error

* Fix cpplint and uncrustify failures

include/ros1_bridge/bridge.hpp

@@ -99,14 +99,15 @@ create_bridge_from_2_to_1(
   size_t subscriber_queue_size,
   const std::string & ros1_type_name,
   const std::string & ros1_topic_name,
-  size_t publisher_queue_size)
+  size_t publisher_queue_size,
+  rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr)
 {
   auto factory = get_factory(ros1_type_name, ros2_type_name);
   auto ros1_pub = factory->create_ros1_publisher(
     ros1_node, ros1_topic_name, publisher_queue_size);
 
   auto ros2_sub = factory->create_ros2_subscriber(
-    ros2_node, ros2_topic_name, subscriber_queue_size, ros1_pub);
+    ros2_node, ros2_topic_name, subscriber_queue_size, ros1_pub, ros2_pub);
 
   Bridge2to1Handles handles;
   handles.ros2_subscriber = ros2_sub;
@@ -130,7 +131,8 @@ create_bidirectional_bridge(
     ros1_type_name, topic_name, queue_size, ros2_type_name, topic_name, queue_size);
   handles.bridge2to1 = create_bridge_from_2_to_1(
     ros2_node, ros1_node,
-    ros2_type_name, topic_name, queue_size, ros1_type_name, topic_name, queue_size);
+    ros2_type_name, topic_name, queue_size, ros1_type_name, topic_name, queue_size,
+    handles.bridge1to2.ros2_publisher);
   return handles;
 }
 

include/ros1_bridge/factory.hpp

@@ -19,6 +19,8 @@
 #include <memory>
 #include <string>
 
+#include "rmw/rmw.h"
+
 // include ROS 1 message event
 #include "ros/message.h"
 
@@ -85,17 +87,19 @@ class Factory : public FactoryInterface
     rclcpp::Node::SharedPtr node,
     const std::string & topic_name,
     size_t queue_size,
-    ros::Publisher ros1_pub)
+    ros::Publisher ros1_pub,
+    rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr)
   {
     rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data;
     custom_qos_profile.depth = queue_size;
     const std::string & ros1_type_name = ros1_type_name_;
     const std::string & ros2_type_name = ros2_type_name_;
     // TODO(wjwwood): use a lambda until create_subscription supports std/boost::bind.
     auto callback =
-      [this, ros1_pub, ros1_type_name, ros2_type_name](const typename ROS2_T::SharedPtr msg) {
+      [this, ros1_pub, ros1_type_name, ros2_type_name,
+      ros2_pub](const typename ROS2_T::SharedPtr msg, const rmw_message_info_t & msg_info) {
         return this->ros2_callback(
-          msg, ros1_pub, ros1_type_name, ros2_type_name);
+          msg, msg_info, ros1_pub, ros1_type_name, ros2_type_name, ros2_pub);
       };
     return node->create_subscription<ROS2_T>(
       topic_name, callback, custom_qos_profile, nullptr, true);
@@ -145,10 +149,26 @@ class Factory : public FactoryInterface
   static
   void ros2_callback(
     typename ROS2_T::SharedPtr ros2_msg,
+    const rmw_message_info_t & msg_info,
     ros::Publisher ros1_pub,
     const std::string & ros1_type_name,
-    const std::string & ros2_type_name)
+    const std::string & ros2_type_name,
+    rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr)
   {
+    if (ros2_pub) {
+      bool result = false;
+      auto ret = rmw_compare_gids_equal(&msg_info.publisher_gid, &ros2_pub->get_gid(), &result);
+      if (ret == RMW_RET_OK) {
+        if (result) {  // message GID equals to bridge's ROS2 publisher GID
+          return;  // do not publish messages from bridge itself
+        }
+      } else {
+        auto msg = std::string("Failed to compare gids: ") + rmw_get_error_string_safe();
+        rmw_reset_error();
+        throw std::runtime_error(msg);
+      }
+    }
+
     ROS1_T ros1_msg;
     convert_2_to_1(*ros2_msg, ros1_msg);
     RCUTILS_LOG_INFO_ONCE_NAMED(

include/ros1_bridge/factory_interface.hpp

@@ -73,7 +73,8 @@ class FactoryInterface
     rclcpp::Node::SharedPtr node,
     const std::string & topic_name,
     size_t queue_size,
-    ros::Publisher ros1_pub) = 0;
+    ros::Publisher ros1_pub,
+    rclcpp::PublisherBase::SharedPtr ros2_pub) = 0;
 };
 
 class ServiceFactoryInterface