Finished initial comms bridge code.

nasa · Nov 20, 2023 · db2035d · db2035d
1 parent daaaa89
commit db2035d
Show file tree

Hide file tree

Showing 16 changed files with 93 additions and 610 deletions.
diff --git a/communications/comms_bridge/CMakeLists.txt b/communications/comms_bridge/CMakeLists.txt
@@ -92,18 +92,13 @@ include_directories(
   ${Boost_INCLUDE_DIRS}
 )
 
+file(GLOB cc_files
+     "src/*.cpp"
+)
 
 # Declare C++ libraries
 add_library(comms_bridge
-  src/bridge_publisher.cpp
-  src/bridge_subscriber.cpp
-  src/generic_rapid_msg_ros_pub.cpp
-  src/generic_ros_sub_rapid_pub.cpp
-  src/comms_bridge_nodelet.cpp
-  src/generic_rapid_sub.cpp
-  src/rapid_advertisement_info.cpp
-  #src/rapid_content.cpp
-  src/util.cpp
+  ${cc_files}
 )
 target_compile_definitions(comms_bridge PUBLIC ${RTIDDS_DEFINE_FLAGS})
 add_dependencies(comms_bridge ${catkin_EXPORTED_TARGETS})

diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h
@@ -38,8 +38,8 @@ class GenericRapidMsgRosPub : public BridgePublisher {
   explicit GenericRapidMsgRosPub(double ad2pub_delay = DEFAULT_ADVERTISE_TO_PUB_DELAY);
   virtual ~GenericRapidMsgRosPub();
 
-  void ConvertAdvertisementInfo(rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data);
-  void ConvertContent(rapid::ext::astrobee::GenericCommsContent const* data);
+  void ConvertData(rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data);
+  void ConvertData(rapid::ext::astrobee::GenericCommsContent const* data);
 };
 
 }  // end namespace ff

diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h
@@ -32,43 +32,75 @@
 
 #include "knShare/Time.h"
 
+#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h"
+#include "dds_msgs/GenericCommsContentSupport.h"
+
 namespace ff {
 
-/**
- * @brief base class for rapid subscriber to ros publisher
- * @details base class for rapid subscriber to ros publisher.
- *          A kn::DdsEventLoop is run within its own thread of execution.
- *          Child classes must connect requeseted messege and callback
- *          to m_ddsEventLoop and call startThread()
- */
+template<typename T>
 class GenericRapidSub {
- protected:
+ public:
   GenericRapidSub(const std::string& entity_name,
                   const std::string& subscribe_topic,
-                  GenericRapidMsgRosPub* rapid_msg_ros_pub);
-  ~GenericRapidSub();
+                  const std::string& subscriber_partition,
+                  GenericRapidMsgRosPub* rapid_msg_ros_pub)
+      : dds_event_loop_(entity_name),
+        subscribe_topic_(subscribe_topic),
+        subscriber_partition_(subscriber_partition),
+        ros_pub_(rapid_msg_ros_pub) {
+    // connect to ddsEventLoop
+    try {
+      dds_event_loop_.connect<T>(this,
+                                 subscribe_topic,       // topic
+                                 subscriber_partition,  // name
+                                 entity_name,           // profile
+                                 "");                   // library
+    } catch (std::exception& e) {
+      ROS_ERROR_STREAM("Rapid exception: " << e.what());
+      throw;
+    } catch (...) {
+      ROS_ERROR("Rapid exception unknown");
+      throw;
+    }
 
-  /**
-   * Will start thread execution by calling threadExec()
-   */
-  virtual void StartThread();
+    // start joinable thread
+    thread_ = std::thread(&GenericRapidSub::ThreadExec, this);
+  }
+
+  ~GenericRapidSub() {
+    alive_ = false;  // Notify thread to exit
+    thread_.join();
+  }
 
+  void operator() (T const* data) {
+    ros_pub_->ConvertData(data);
+  }
+
+ private:
   GenericRapidMsgRosPub* ros_pub_;
   std::string subscribe_topic_;
+  std::string subscriber_partition_;
 
   std::atomic<bool> alive_;
   std::thread thread_;
   kn::DdsEventLoop dds_event_loop_;
 
- private:
   /**
   * Function to execute within seperate thread
   *   process DdsEventLoop at 10Hz
   */
-  virtual void ThreadExec();
+  void ThreadExec() {
+    while (alive_) {
+      // process events at 10hz
+      dds_event_loop_.processEvents(kn::milliseconds(100));
+    }
+  }
 };
 
-typedef std::shared_ptr<GenericRapidSub> GenericRapidSubPtr;
+typedef std::shared_ptr<GenericRapidSub<rapid::ext::astrobee::GenericCommsAdvertisementInfo>>
+    AdvertisementInfoRapidSubPtr;
+typedef std::shared_ptr<GenericRapidSub<rapid::ext::astrobee::GenericCommsContent>>
+    ContentRapidSubPtr;
 
 }  // end namespace ff
 

diff --git a/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h
@@ -46,7 +46,8 @@ class GenericROSSubRapidPub : public BridgeSubscriber {
   GenericROSSubRapidPub();
   ~GenericROSSubRapidPub();
 
-  void InitializeDDS(std::string agent_name);
+  void InitializeDDS();
+
   void SizeCheck(unsigned int &size,
                  const int size_in,
                  const int max_size,

diff --git a/communications/comms_bridge/include/comms_bridge/rapid_advertisement_info.h b/communications/comms_bridge/include/comms_bridge/rapid_advertisement_info.h
diff --git a/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h b/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h
diff --git a/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h b/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h