making sure topics are being passed in + fixing things

marinagmoreira · Nov 1, 2023 · 81008e0 · 81008e0
1 parent dbb9e01
commit 81008e0
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 27 deletions.
diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config
@@ -37,8 +37,8 @@ links = {
   {
     -- A single link entry has required fields "from" and "to" that specify the robot roles involved
     -- in the link.
-    from = {"Bumble"},  -- manager
-    to = {"Queen"},     -- actor
+    from = "Bumble",  -- manager
+    to = "Queen",     -- actor
 
     -- The link entry has three optional fields: relay_forward (messages to be relayed only in the
     -- `from` to `to` direction), relay_backward (to be relayed only in the `to` to `from` direction),
@@ -70,4 +70,4 @@ links = {
   }
 }
 
-verbose = false
+verbose = 2
diff --git a/communications/comms_bridge/src/bridge_subscriber.cpp b/communications/comms_bridge/src/bridge_subscriber.cpp
@@ -50,20 +50,20 @@ void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent<ShapeShifter
     msg_event.getConnectionHeaderPtr();
 
   if (m_verbose) {
-    printf("got data on %s:\n", topic.c_str());
+    ROS_INFO("got data on %s:\n", topic.c_str());
     if (m_verbose > 1) {
-      printf("  datatype: \"%s\"\n", ptr->getDataType().c_str());
-      printf("  md5: \"%s\"\n", ptr->getMD5Sum().c_str());
+      ROS_INFO("  datatype: \"%s\"\n", ptr->getDataType().c_str());
+      ROS_INFO("  md5: \"%s\"\n", ptr->getMD5Sum().c_str());
     }
     if (m_verbose > 2)
-      printf("  def: \"%s\"\n", ptr->getMessageDefinition().c_str());
+      ROS_INFO("  def: \"%s\"\n", ptr->getMessageDefinition().c_str());
 
     if (m_verbose > 2) {
-      printf("  conn header:\n");
+      ROS_INFO("  conn header:\n");
       for (ros::M_string::const_iterator iter = connection_header->begin();
            iter != connection_header->end();
            iter++)
-        printf("    %s: %s\n", iter->first.c_str(), iter->second.c_str());
+        ROS_INFO("    %s: %s\n", iter->first.c_str(), iter->second.c_str());
     }
   }
 
@@ -79,7 +79,7 @@ void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent<ShapeShifter
 
   if (!topic_info.advertised) {
     if (m_verbose)
-      printf("  sending advertisement\n");
+      ROS_INFO("  sending advertisement\n");
 
     bool latching = false;
     if (connection_header) {
@@ -106,7 +106,7 @@ void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent<ShapeShifter
   stream.next(*ptr);  // serializes just the message contents
   ssize_t serialized_size = ROS_BRIDGE_MAX_MSG_SIZE - stream.getLength();
   if (m_verbose > 2)
-    printf("  serialized size = %zd\n", serialized_size);
+    ROS_INFO("  serialized size = %zd\n", serialized_size);
   if (serialized_size <= 0) {
     ROS_ERROR("Serialization buffer size deficient, discarding message");
     return;
@@ -144,7 +144,7 @@ SubscriberPtr BridgeSubscriber::rosSubscribe(std::string const& topic) {
   *ptr = nh.subscribe(opts);
 
   if (m_verbose)
-    printf("Subscribed to topic %s\n", topic.c_str());
+    ROS_INFO("Subscribed to topic %s\n", topic.c_str());
 
   return ptr;
 }
@@ -155,7 +155,7 @@ bool BridgeSubscriber::addTopic(std::string const& in_topic, std::string const&
   // Enforce that all relays have a unique input topic
   if (m_relay_topics.find(in_topic) != m_relay_topics.end()) {
     if (m_verbose)
-      printf("Already subscribed to relay from topic %s\n", in_topic.c_str());
+      ROS_ERROR("Already subscribed to relay from topic %s\n", in_topic.c_str());
     return false;
   }
 
@@ -166,7 +166,7 @@ bool BridgeSubscriber::addTopic(std::string const& in_topic, std::string const&
   while (iter != m_relay_topics.end()) {
     if (iter->second.out_topic == out_topic) {
       if (m_verbose)
-        printf("Already relaying to topic %s\n", out_topic.c_str());
+        ROS_ERROR("Already relaying to topic %s\n", out_topic.c_str());
       return false;
     }
     iter++;

diff --git a/communications/comms_bridge/src/bridge_subscriber_nodelet.cpp b/communications/comms_bridge/src/bridge_subscriber_nodelet.cpp
@@ -36,11 +36,6 @@
 #include "comms_bridge/ros_dds_bridge_subscriber.h"
 
 namespace comms_bridge {
-// void usage(const char *progname)
-// {
-//   fprintf(stderr, "Usage: %s [-v<verbose>] [-a<anonymous node>] [-t meta_topic_prefix] <topic to subscribe to> <topic
-//   to republish on remote side> [<subscribe topic> <republish topic> ...]\n", progname);
-// }
 
 class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet {
  public:
@@ -86,17 +81,20 @@ class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet {
       }
     }
 
-    bool verbose;
-    if (!config_params_.GetBool("verbose", &verbose)) {
+    unsigned int  verbose;
+    if (!config_params_.GetUInt("verbose", &verbose)) {
       ROS_FATAL("BridgeSubscriberNodelet: Could not read verbosity level.");
       exit(EXIT_FAILURE);
       return;
     }
 
-    ROS_ERROR_STREAM("ROBOT NAME " << agent_name_);
     // Declare the ROS to DDS Subscriber class
     ROSDDSBridgeSubscriber sub(agent_name_);
-    std::string ns = "/" + std::tolower(agent_name_[0]) + agent_name_.substr(1) + "/";
+    if (verbose > 0)
+      sub.setVerbosity(verbose);
+
+    std::string ns = std::string("/") + agent_name_ + "/";
+    ns[1] = std::tolower(ns[1]);  // namespaces don't start with uppercase
 
     // Load shared topic groups
     config_reader::ConfigReader::Table links, link, relay_forward, relay_backward, relay_both, item_conf;
@@ -121,7 +119,7 @@ class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet {
               NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!");
               return;
             }
-            assert(sub.addTopic(topic_name, ns + topic_name));
+            sub.addTopic(topic_name, ns + topic_name);
           }
         }
         if (link.GetTable("relay_both", &relay_both)) {
@@ -132,7 +130,7 @@ class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet {
               NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!");
               return;
             }
-            assert(sub.addTopic(topic_name, ns + topic_name));
+            sub.addTopic(topic_name, ns + topic_name);
           }
         }
       } else if (link.GetStr("to", &config_agent) && config_agent == agent_name_) {
@@ -144,7 +142,7 @@ class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet {
               NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!");
               return;
             }
-            assert(sub.addTopic(topic_name, ns + topic_name));
+            sub.addTopic(topic_name, ns + topic_name);
           }
         }
         if (link.GetTable("relay_both", &relay_both)) {
@@ -155,7 +153,7 @@ class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet {
               NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!");
               return;
             }
-            assert(sub.addTopic(topic_name, ns + topic_name));
+            sub.addTopic(topic_name, ns + topic_name);
           }
         }
       }