Added the request advertisement info code.

astrobee/config/communications/comms_bridge.config

@@ -21,6 +21,17 @@ require "context"
 -- If false, a trigger call will be needed to start communications --
 initialize_dds_on_start = false
 
+-- Request an advertisement info message if a content message was received
+-- before the advertisement info message
+-- DDS has been setup such that we shouldn't need this and it can cause more
+-- information to be sent then needed. Thus this is setting. By default, it
+-- will be false but if communications on the ISS are not working as
+-- expected, it can be enabled.
+enable_advertisement_info_request = false
+
+verbose = 2
+ad2pub_delay = 3.0
+
 links = {
   -- Logically, there could be up to three two-way links between the three robots. In practice, we
   -- will probably only have one link.
@@ -48,6 +59,3 @@ links = {
     },
   }
 }
-
-verbose = 2
-ad2pub_delay = 3.0

astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml

@@ -660,7 +660,7 @@
     <!-- ASTROBEE Extension: Generic Comms ============================ -->
     <qos_profile name="AstrobeeGenericCommsAdvertisementInfoProfile" base_name="RapidConfigQos" />
     <qos_profile name="AstrobeeGenericCommsContentProfile" base_name="RapidStateQos" />
-    <qos_profile name="AstrobeeGenericCommsResetProfile" base_name="RapidConfigQos" />
+    <qos_profile name="AstrobeeGenericCommsRequestProfile" base_name="RapidConfigQos" />
 
     <!-- ASTROBEE Extension: GNC Fam Cmd ============================== -->
     <qos_profile name="AstrobeeGncFamCmdStateProfile" base_name="RapidStateQos" />

communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h

@@ -20,6 +20,7 @@
 #define COMMS_BRIDGE_GENERIC_RAPID_MSG_ROS_PUB_H_
 
 #include <comms_bridge/bridge_publisher.h>
+#include <comms_bridge/generic_rapid_pub.h>
 #include <comms_bridge/util.h>
 
 #include <string>
@@ -38,10 +39,19 @@ class GenericRapidMsgRosPub : public BridgePublisher {
   explicit GenericRapidMsgRosPub(double ad2pub_delay = DEFAULT_ADVERTISE_TO_PUB_DELAY);
   virtual ~GenericRapidMsgRosPub();
 
-  void ConvertData(rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data);
-  void ConvertData(rapid::ext::astrobee::GenericCommsContent const* data);
-};
+  void InitializeDDS(std::map<std::string, GenericRapidPubPtr>* robot_pubs,
+                     bool enable_advertisement_info_request);
+  void HandleAdvertisementInfo(rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data);
+  void HandleContent(rapid::ext::astrobee::GenericCommsContent const* data,
+                     std::string const& connecting_robot);
+  void RequestAdvertisementInfo(std::string const& output_topic,
+                                std::string const& connecting_robot);
+
+ private:
+  bool dds_initialized_, enable_advertisement_info_request_;
 
+  std::map<std::string, GenericRapidPubPtr>* robot_rapid_pubs_;
+};
 }  // end namespace ff
 
 #endif  // COMMS_BRIDGE_GENERIC_RAPID_MSG_ROS_PUB_H_

communications/comms_bridge/include/comms_bridge/generic_rapid_pub.h

@@ -31,6 +31,7 @@
 #include "dds_msgs/AstrobeeConstantsSupport.h"
 #include "dds_msgs/GenericCommsAdvertisementInfoSupport.h"
 #include "dds_msgs/GenericCommsContentSupport.h"
+#include "dds_msgs/GenericCommsRequestSupport.h"
 
 namespace ff {
 
@@ -46,17 +47,19 @@ class GenericRapidPub {
                   std::string const& data_name,
                   std::string const& topic);
 
-  void ProcessAdvertisementInfo(std::string const& output_topic,
-                                bool latching,
-                                std::string const& data_type,
-                                std::string const& md5_sum,
-                                std::string definition);
+  void SendAdvertisementInfo(std::string const& output_topic,
+                             bool latching,
+                             std::string const& data_type,
+                             std::string const& md5_sum,
+                             std::string definition);
 
-  void ProcessContent(std::string const& output_topic,
-                      std::string const& md5_sum,
-                      uint8_t const* data,
-                      const size_t data_size,
-                      const int seq_num);
+  void SendContent(std::string const& output_topic,
+                   std::string const& md5_sum,
+                   uint8_t const* data,
+                   size_t const data_size,
+                   int const seq_num);
+
+  void SendRequest(std::string const& output_topic);
 
  private:
   using AdvertisementInfoSupplier =
@@ -70,7 +73,12 @@ class GenericRapidPub {
   using ContentSupplierPtr = std::unique_ptr<ContentSupplier>;
   ContentSupplierPtr content_supplier_;
 
-  unsigned int advertisement_info_seq_;
+  using RequestSupplier =
+                kn::DdsTypedSupplier<rapid::ext::astrobee::GenericCommsRequest>;
+  using RequestSupplierPtr = std::unique_ptr<RequestSupplier>;
+  RequestSupplierPtr request_supplier_;
+
+  unsigned int advertisement_info_seq_, request_seq_;
 };
 
 typedef std::shared_ptr<ff::GenericRapidPub> GenericRapidPubPtr;

communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h

@@ -20,6 +20,7 @@
 #define COMMS_BRIDGE_GENERIC_RAPID_SUB_H_
 
 #include <comms_bridge/generic_rapid_msg_ros_pub.h>
+#include <comms_bridge/generic_ros_sub_rapid_pub.h>
 
 #include <memory>
 #include <string>
@@ -34,6 +35,7 @@
 
 #include "dds_msgs/GenericCommsAdvertisementInfoSupport.h"
 #include "dds_msgs/GenericCommsContentSupport.h"
+#include "dds_msgs/GenericCommsRequestSupport.h"
 
 namespace ff {
 
@@ -43,11 +45,13 @@ class GenericRapidSub {
   GenericRapidSub(const std::string& entity_name,
                   const std::string& subscribe_topic,
                   const std::string& subscriber_partition,
-                  GenericRapidMsgRosPub* rapid_msg_ros_pub)
+                  GenericRapidMsgRosPub* rapid_msg_ros_pub,
+                  GenericROSSubRapidPub* ros_sub_rapid_pub)
       : dds_event_loop_(entity_name),
         subscribe_topic_(subscribe_topic),
         subscriber_partition_(subscriber_partition),
-        ros_pub_(rapid_msg_ros_pub) {
+        ros_pub_(rapid_msg_ros_pub),
+        ros_sub_(ros_sub_rapid_pub) {
     // connect to ddsEventLoop
     try {
       dds_event_loop_.connect<T>(this,
@@ -74,11 +78,24 @@ class GenericRapidSub {
 
   void operator() (T const* data) {
     ROS_DEBUG("Received data for topic %s\n", subscribe_topic_.c_str());
-    ros_pub_->ConvertData(data);
+    DirectData(data);
+  }
+
+  void DirectData(rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data) {
+    ros_pub_->HandleAdvertisementInfo(data);
+  }
+
+  void DirectData(rapid::ext::astrobee::GenericCommsContent const* data) {
+    ros_pub_->HandleContent(data, subscriber_partition_);
+  }
+
+  void DirectData(rapid::ext::astrobee::GenericCommsRequest const* data) {
+    ros_sub_->HandleRequest(data, subscriber_partition_);
   }
 
  private:
   GenericRapidMsgRosPub* ros_pub_;
+  GenericROSSubRapidPub* ros_sub_;
   std::string subscribe_topic_;
   std::string subscriber_partition_;
 
@@ -102,6 +119,8 @@ typedef std::shared_ptr<GenericRapidSub<rapid::ext::astrobee::GenericCommsAdvert
     AdvertisementInfoRapidSubPtr;
 typedef std::shared_ptr<GenericRapidSub<rapid::ext::astrobee::GenericCommsContent>>
     ContentRapidSubPtr;
+typedef std::shared_ptr<GenericRapidSub<rapid::ext::astrobee::GenericCommsRequest>>
+    RequestRapidSubPtr;
 
 }  // end namespace ff
 

communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h

@@ -34,6 +34,8 @@
 #include <utility>
 #include <vector>
 
+#include "dds_msgs/GenericCommsRequestSupport.h"
+
 namespace ff {
 
 class GenericROSSubRapidPub : public BridgeSubscriber {
@@ -44,7 +46,7 @@ class GenericROSSubRapidPub : public BridgeSubscriber {
   void AddTopics(std::map<std::string,
        std::vector<std::pair<std::string, std::string>>> const& link_entries);
 
-  void InitializeDDS(std::vector<std::string> const& connections);
+  void InitializeDDS(std::map<std::string, GenericRapidPubPtr>* robot_pubs);
 
   // Called with the mutex held
   virtual void subscribeTopic(std::string const& in_topic,
@@ -57,11 +59,14 @@ class GenericROSSubRapidPub : public BridgeSubscriber {
   virtual void relayMessage(const RelayTopicInfo& topic_info,
                             ContentInfo const& content_info);
 
+  void HandleRequest(rapid::ext::astrobee::GenericCommsRequest const* data,
+                     std::string const& connecting_robot);
+
  private:
   bool dds_initialized_;
 
   std::map<std::string, std::vector<std::pair<std::string, std::string>>> topic_mapping_;
-  std::map<std::string, GenericRapidPubPtr> robot_connections_;
+  std::map<std::string, GenericRapidPubPtr>* robot_rapid_pubs_;
 };
 
 }  // end namespace ff

communications/comms_bridge/src/comms_bridge_nodelet.cpp

@@ -37,6 +37,7 @@
 #include <vector>
 
 #include "comms_bridge/generic_rapid_msg_ros_pub.h"
+#include "comms_bridge/generic_rapid_pub.h"
 #include "comms_bridge/generic_rapid_sub.h"
 #include "comms_bridge/generic_ros_sub_rapid_pub.h"
 
@@ -65,7 +66,8 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
  public:
   CommsBridgeNodelet() : ff_util::FreeFlyerNodelet("comms_bridge"),
                          dds_initialized_(false),
-                         initialize_dds_on_start_(false) {}
+                         initialize_dds_on_start_(false),
+                         enable_advertisement_info_request_(false) {}
 
   virtual ~CommsBridgeNodelet() {}
 
@@ -108,6 +110,8 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
     }
     ROS_INFO_STREAM("Comms Bridge Nodelet: agent name " << agent_name_);
 
+    ros_sub_ = std::make_shared<ff::GenericROSSubRapidPub>();
+
     int fake_argc = 1;
 
     // Make path to QOS and NDDS files
@@ -217,9 +221,19 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
     std::string connection, dds_topic_name;
     ff::AdvertisementInfoRapidSubPtr advertisement_info_sub;
     ff::ContentRapidSubPtr content_sub;
-    ros_sub_.InitializeDDS(rapid_connections_);
+    ff::RequestRapidSubPtr request_sub;
+    robot_rapid_pubs_ =
+            std::make_shared<std::map<std::string, ff::GenericRapidPubPtr>>();
+    for (size_t i = 0; i < rapid_connections_.size(); i++) {
+      ff::GenericRapidPubPtr rapid_pub =
+                  std::make_shared<ff::GenericRapidPub>(rapid_connections_[i]);
+      robot_rapid_pubs_->emplace(rapid_connections_[i], rapid_pub);
+    }
+
+    ros_sub_->InitializeDDS(robot_rapid_pubs_.get());
+    ros_pub_->InitializeDDS(robot_rapid_pubs_.get(),
+                            enable_advertisement_info_request_);
     for (size_t i = 0; i < rapid_connections_.size(); i++) {
-      // Lower case the external agent name to use it like a namespace
       connection = rapid_connections_[i];
       dds_topic_name = agent_name_ + "-" +
           rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC;
@@ -230,7 +244,8 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
           "AstrobeeGenericCommsAdvertisementInfoProfile",
           dds_topic_name,
           connection,
-          ros_pub_.get());
+          ros_pub_.get(),
+          ros_sub_.get());
       advertisement_info_rapid_subs_.push_back(advertisement_info_sub);
 
       dds_topic_name = agent_name_ + "-" +
@@ -241,10 +256,25 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
           "AstrobeeGenericCommsContentProfile",
           dds_topic_name,
           connection,
-          ros_pub_.get());
+          ros_pub_.get(),
+          ros_sub_.get());
       content_rapid_subs_.push_back(content_sub);
+
+      if (enable_advertisement_info_request_) {
+        dds_topic_name = agent_name_ + "-" +
+                         rapid::ext::astrobee::GENERIC_COMMS_REQUEST_TOPIC;
+        ROS_DEBUG("Comms Bridge: DDS Sub DDS request topic name: %s\n",
+                  dds_topic_name.c_str());
+        request_sub = std::make_shared<ff::GenericRapidSub<rapid::ext::astrobee::GenericCommsRequest>>(
+            "AstrobeeGenericCommsRequestProfile",
+            dds_topic_name,
+            connection,
+            ros_pub_.get(),
+            ros_sub_.get());
+        request_rapid_subs_.push_back(request_sub);
+      }
     }
-    ros_sub_.AddTopics(link_entries_);
+    ros_sub_->AddTopics(link_entries_);
     dds_initialized_ = true;
   }
 
@@ -261,7 +291,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
     if (!config_params_.GetUInt("verbose", &verbose)) {
       NODELET_ERROR("Comms Bridge Nodelet: Could not read verbosity level. Setting to 2 (info?).");
     }
-    ros_sub_.setVerbosity(verbose);
+    ros_sub_->setVerbosity(verbose);
     ros_pub_->setVerbosity(verbose);
 
     initialize_dds_on_start_ = false;
@@ -270,6 +300,12 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
       NODELET_ERROR("Comms Bridge Nodelet: Could not read initialize dds on start. Setting to false.");
     }
 
+    enable_advertisement_info_request_ = false;
+    if (!config_params_.GetBool("enable_advertisement_info_request",
+                                &enable_advertisement_info_request_)) {
+      NODELET_ERROR("Comms Bridge Nodelet: Could not read enable advertisement info request. Setting to false.");
+    }
+
     // Load shared topic groups
     config_reader::ConfigReader::Table links, link;
     if (!config_params_.GetTable("links", &links)) {
@@ -353,14 +389,20 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
 
  private:
   bool initialize_dds_on_start_, dds_initialized_;
+  bool enable_advertisement_info_request_;
   config_reader::ConfigReader config_params_;
-  ff::GenericROSSubRapidPub ros_sub_;
+
   std::vector<ff::ContentRapidSubPtr> content_rapid_subs_;
   std::vector<ff::AdvertisementInfoRapidSubPtr> advertisement_info_rapid_subs_;
+  std::vector<ff::RequestRapidSubPtr> request_rapid_subs_;
+  std::vector<std::string> rapid_connections_;
+
   std::shared_ptr<kn::DdsEntitiesFactorySvc> dds_entities_factory_;
   std::shared_ptr<ff::GenericRapidMsgRosPub> ros_pub_;
+  std::shared_ptr<ff::GenericROSSubRapidPub> ros_sub_;
+  std::shared_ptr<std::map<std::string, ff::GenericRapidPubPtr>> robot_rapid_pubs_;
+
   std::string agent_name_, participant_name_;
-  std::vector<std::string> rapid_connections_;
   std::map<std::string, std::vector<std::pair<std::string, std::string>>> link_entries_;
   ros::ServiceServer dds_initialize_srv_;
 };