Fixed comms bridge seg fault.

astrobee/config/management/fault_table.config

@@ -110,9 +110,6 @@ subsystems={
     }},
   }},
   {name="communication", nodes={
-    {name="astrobee_astrobee_bridge", faults={
-      {id=51, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Atrobee to Astrobee Bridge", heartbeat={timeout_sec=1.1, misses=1.0}},
-    }},
     {name="dds_ros_bridge", faults={
       {id=33, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat form DDS ROS Bridge", heartbeat={timeout_sec=1.1, misses=5.0}},
       {id=78, warning=false, blocking=true, response=command("unloadNodelet", "dds_ros_bridge", ""), key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"},

astrobee/launch/robot/MLP.launch

@@ -286,7 +286,7 @@
     <arg name="debug" value="$(arg debug)" />
     <arg name="default" value="$(arg dds)" />
   </include>
-  <include file="$(find ff_util)/launch/ff_nodelet.launch">
+<!--  <include file="$(find ff_util)/launch/ff_nodelet.launch">
     <arg name="class" value="dds_ros_bridge/AstrobeeAstrobeeBridge" />
     <arg name="name" value="astrobee_astrobee_bridge" />
     <arg name="manager" value="mlp_multibridge" />
@@ -295,7 +295,7 @@
     <arg name="extra" value="$(arg extra)" />
     <arg name="debug" value="$(arg debug)" />
     <arg name="default" value="$(arg dds)" />
-  </include>
+  </include> -->
   <include file="$(find ff_util)/launch/ff_nodelet.launch">
     <arg name="class" value="comms_bridge/CommsBridgeNodelet" />
     <arg name="name" value="comms_bridge" />

communications/comms_bridge/src/comms_bridge_nodelet.cpp

@@ -136,6 +136,8 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
     robot_params->name = agent_name_;
     robot_params->namingContextName = robot_params->name;
 
+    ROS_ERROR("Agent name %s and participant name %s\n", agent_name_.c_str(), participant_name_.c_str());
+
     // Set values for default punlisher and subscriber
     dds_params->publishers[0].name = agent_name_;
     dds_params->publishers[0].partition = agent_name_;
@@ -161,7 +163,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
     }
 
     // Register the connections into the parameters so they can be used later
-    for (int i = 0; i <= rapid_connections_.size(); i++) {
+    for (int i = 0; i < rapid_connections_.size(); i++) {
       // This shouldn't be needed but check just in case
       if (local_subscriber != rapid_connections_[i]) {
         kn::DdsNodeParameters subscriber;
@@ -196,7 +198,6 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
     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];
-      connection[0] = tolower(connection[0]);
       advertisement_info_sub =
         std::make_shared<ff::GenericRapidSub<rapid::ext::astrobee::GenericCommsAdvertisementInfo>>(
           "AstrobeeGenericCommsAdvertisementInfoProfile", rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC,
@@ -210,6 +211,13 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
           ros_pub_.get());
       content_rapid_subs_.push_back(content_sub);
     }
+
+    std::string ns = std::string("/") + agent_name_ + "/";
+    ns[1] = std::tolower(ns[1]);  // namespaces don't start with upper case
+
+    for (size_t i = 0; i < topics_sub_.size(); i++) {
+      ros_sub_.addTopic(topics_sub_[i], (ns + topics_sub_[i]));
+    }
   }
 
   bool ReadParams() {
@@ -228,9 +236,6 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
     ros_sub_.setVerbosity(verbose);
     ros_pub_->setVerbosity(verbose);
 
-    std::string ns = std::string("/") + agent_name_ + "/";
-    ns[1] = std::tolower(ns[1]);  // namespaces don't start with upper case
-
     // Load shared topic groups
     config_reader::ConfigReader::Table links, link;
     if (!config_params_.GetTable("links", &links)) {
@@ -246,21 +251,22 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
       std::string config_agent;
       if (link.GetStr("from", &config_agent) && config_agent == agent_name_) {
         AddRapidConnections(link, "to");
-        AddTableToSubs(link, "relay_forward", ns);
-        AddTableToSubs(link, "relay_both", ns);
+        AddTableToSubs(link, "relay_forward");
+        AddTableToSubs(link, "relay_both");
       } else if (link.GetStr("to", &config_agent) && config_agent == agent_name_) {
         AddRapidConnections(link, "from");
-        AddTableToSubs(link, "relay_backward", ns);
-        AddTableToSubs(link, "relay_both", ns);
+        AddTableToSubs(link, "relay_backward");
+        AddTableToSubs(link, "relay_both");
       }
     }
+    return true;
   }
 
   void AddRapidConnections(config_reader::ConfigReader::Table &link_table,
                            std::string direction) {
     std::string connection;
     if (!link_table.GetStr(direction.c_str(), &connection)) {
-      NODELET_ERROR("Comms Bridge Nodelet: %s not specified for one link", direction);
+      NODELET_ERROR("Comms Bridge Nodelet: %s not specified for one link", direction.c_str());
       return;
     }
 
@@ -278,8 +284,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
   }
 
   void AddTableToSubs(config_reader::ConfigReader::Table &link_table,
-                      std::string table_name,
-                      std::string ns) {
+                      std::string table_name) {
     config_reader::ConfigReader::Table relay_table, relay_item;
     std::string topic_name;
     if (link_table.GetTable(table_name.c_str(), &relay_table)) {
@@ -289,7 +294,17 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
            NODELET_ERROR("Comms Bridge Nodelet: Agent topic name not specified!");
             continue;
         }
-        ros_sub_.addTopic(topic_name, (ns + topic_name));
+
+        bool found = false;
+        for (size_t i = 0; i < topics_sub_.size() && !found; i++) {
+          if (topic_name == topics_sub_[i]) {
+            found = true;
+          }
+        }
+
+        if (!found) {
+          topics_sub_.push_back(topic_name);
+        }
       }
     }
   }
@@ -302,7 +317,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet {
   std::shared_ptr<kn::DdsEntitiesFactorySvc> dds_entities_factory_;
   std::shared_ptr<ff::GenericRapidMsgRosPub> ros_pub_;
   std::string agent_name_, participant_name_;
-  std::vector<std::string> rapid_connections_;
+  std::vector<std::string> rapid_connections_, topics_sub_;
 };
 
 PLUGINLIB_EXPORT_CLASS(comms_bridge::CommsBridgeNodelet, nodelet::Nodelet)

communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp

@@ -62,7 +62,7 @@ void GenericRapidMsgRosPub::ConvertData(
   std::map<std::string, RelayTopicInfo>::iterator iter = m_relay_topics_.find(output_topic);
   if (iter == m_relay_topics_.end()) {
     ROS_ERROR("Comms Bridge Nodelet: Received content for topic %s but never received advertisement info.\n",
-              output_topic);
+              output_topic.c_str());
   } else {
     RelayTopicInfo &topic_info = iter->second;
 

communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp

@@ -57,7 +57,7 @@ void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) {
   unsigned int size;
   std::string out_topic = relay_info.out_topic;
 
-  ROS_INFO("Received ros advertise topic for topic %s\n", out_topic.c_str());
+  ROS_ERROR("Received ros advertise topic for topic %s\n", out_topic.c_str());
 
   msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now());
   msg.hdr.serial = ++advertisement_info_seq_;
@@ -74,13 +74,14 @@ void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) {
   std::strncpy(msg.dataType, info.data_type.data(), size);
   msg.dataType[size] = '\0';
 
-  // Currently the md5 sum can only be 32 characters long
-  SizeCheck(size, info.md5_sum.size(), 32, "MD5 sum", out_topic);
+  // Currently the md5 sum can only be 64 characters long
+  SizeCheck(size, info.md5_sum.size(), 64, "MD5 sum", out_topic);
   std::strncpy(msg.md5Sum, info.md5_sum.data(), size);
   msg.md5Sum[size] = '\0';
 
-  // Current the ROS message definition can only be 2048 characters long
-  SizeCheck(size, info.definition.size(), 2048, "Msg definition", out_topic);
+  ROS_ERROR("Defination is %i long\n", info.definition.size());
+  // Current the ROS message definition can only be 16384 characters long
+  SizeCheck(size, info.definition.size(), 16384, "Msg definition", out_topic);
   std::strncpy(msg.msgDefinition, info.definition.data(), size);
   msg.msgDefinition[size] = '\0';
 
@@ -95,7 +96,7 @@ void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, Conte
   unsigned int size;
   std::string out_topic = topic_info.out_topic;
 
-  ROS_INFO("Received ros content message for topic %s\n", out_topic);
+  ROS_ERROR("Received ros content message for topic %s\n", out_topic.c_str());
 
   msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now());
   msg.hdr.serial = topic_info.relay_seqnum;
@@ -105,8 +106,8 @@ void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, Conte
   std::strncpy(msg.outputTopic, out_topic.data(), size);
   msg.outputTopic[size] = '\0';
 
-  // Currently the md5 sum can only be 32 characters long
-  SizeCheck(size, content_info.type_md5_sum.size(), 32, "MD5 sum", out_topic);
+  // Currently the md5 sum can only be 64 characters long
+  SizeCheck(size, content_info.type_md5_sum.size(), 64, "MD5 sum", out_topic);
   std::strncpy(msg.md5Sum, content_info.type_md5_sum.data(), size);
   msg.md5Sum[size] = '\0';
 

communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl

@@ -7,6 +7,7 @@
 module rapid {
   module ext {
     module astrobee {
+      typedef string<16384> String16K;
 
       //@copy-c-declaration class GenericCommsAdvertisementInfoTypeSupport;
       //@copy-c-declaration class GenericCommsAdvertisementInfoDataWriter;
@@ -35,10 +36,10 @@ module rapid {
         public String128 dataType;
 
         //@copy-declaration /** ROS message md5sum of type */
-        public String32 md5Sum;
+        public String64 md5Sum;
 
         //@copy-declaration /** ROS message definition */
-        public String2K msgDefinition;
+        public String16K msgDefinition;
       };
     };
   };

communications/dds_msgs/idl/GenericCommsContent.idl

@@ -29,7 +29,7 @@ module rapid {
         public String128 outputTopic;
 
         //@copy-declaration /** The md5 sum of msg type repeated from the original advertisement */
-        public String32 md5Sum;
+        public String64 md5Sum;
 
         //@copy-declaration /** Serialized content of the message */
         public OctetSequence128K data;