Merge pull request #47 from KodlabPenn/lcm-multiple-subscriber

Lcm multiple subscriber

Author: ethanmusser

examples/leg_2DoF_example.cpp

@@ -101,14 +101,14 @@ class Hopping : public kodlab::mjbots::MjbotsControlLoop<LegLog, LegGains> {
     log_data_->hybrid_mode = mode_;
   }
 
-  void ProcessInput() override {
-    kv_ = lcm_sub_.data_.kv;
-    k_ = lcm_sub_.data_.k;
-    k_stiff_ = lcm_sub_.data_.k_stiff;
-    b_ = lcm_sub_.data_.b;
-    b_stiff_ = lcm_sub_.data_.b_stiff;
-    kp_ = lcm_sub_.data_.kp;
-    kd_ = lcm_sub_.data_.kd;
+  void ProcessInput(const LegGains &input_data) override {
+    kv_ = input_data.kv;
+    k_ = input_data.k;
+    k_stiff_ = input_data.k_stiff;
+    b_ = input_data.b;
+    b_stiff_ = input_data.b_stiff;
+    kp_ = input_data.kp;
+    kd_ = input_data.kd;
     LOG_INFO(
         "Response received: { kv = % 5.2f, k = % 5.2f, k_stiff = % 5.2f, "
         "b = % 5.2f, b_stiff = % 5.2f, kp = % 5.2f, kd = % 5.2f }",

examples/robot_example.cpp

@@ -48,9 +48,9 @@ class SimpleRobotControlLoop : public kodlab::mjbots::MjbotsControlLoop<ManyMoto
         }
     }
 
-    void ProcessInput() override
+    void ProcessInput(const ModeInput &input_data) override
     {
-        robot_->mode = lcm_sub_.data_.mode;
+        robot_->mode = input_data.mode;
         std::cout << "Switching to behavior " << robot_->mode << std::endl;
         // If the kill robot mode is detected kill robot using CTRL_C flag handler.
         if (robot_->mode == robot_->KILL_ROBOT)
@@ -73,6 +73,7 @@ int main(int argc, char **argv)
     // Define robot options
     kodlab::mjbots::ControlLoopOptions options;
     options.log_channel_name = "motor_data";
+    options.input_channel_name = "mode_input";
     options.frequency = 1000;
     options.realtime_params.main_cpu = 3;
     options.realtime_params.can_cpu = 2;

include/kodlab_mjbots_sdk/lcm_message_handler.h

@@ -0,0 +1,109 @@
+/**
+ * @file lcm_message_handler.h
+ * @author Ethan Musser ([email protected])
+ * @brief Provides LCM message handler object to be derived or composed into
+ * classes requiring LCM input via the `LcmSubscriber` object.
+ * @date 8/6/22
+ * 
+ * @copyright Copyright 2022 The Trustees of the University of Pennsylvania. All
+ * rights reserved.
+ * 
+ */
+
+#pragma once
+
+#include <string>
+#include <mutex>
+#include <optional>
+#include "lcm/lcm-cpp.hpp"
+#include "real_time_tools/thread.hpp"
+
+namespace kodlab {
+
+/**
+ * @brief Handler class for incoming LCM messages.
+ * @details This class is intended to be derived from or composed into objects
+ * requiring LCM input.  This class and its children should be compatible with
+ * the `LcmSubscriber` object, and can implemented with the
+ * `LcmSubscriber::AddSubscription` method in particular.
+ * @tparam Message LCM message type
+ */
+template<class Message>
+class LcmMessageHandler {
+
+ public:
+  /**
+   * @brief Constructs an `LcmMessageHandler` and ensures the mutex is unlocked.
+   */
+  LcmMessageHandler() {
+    mutex_.unlock();  // ensures mutex is unlocked
+  }
+
+  /**
+   * @brief Virtual destructor for clean inherited class destruction
+   */
+  virtual ~LcmMessageHandler() = default;
+
+  /**
+   * @brief Callback function for when an LCM message is received.
+   * @details This function serves as a callback for incoming LCM messages on
+   * `channel`.  It copies the decoded message data to `data`, and sets
+   * `new_message` to `true`.
+   * @param rbuf LCM recieve buffer containing raw bytes and timestamp
+   * @param channel channel name
+   * @param msg pointer to the incoming message data
+   */
+  void HandleMessage(const lcm::ReceiveBuffer *rbuf,
+                     const std::string &channel,
+                     const Message *msg) {
+    mutex_.lock();
+    data_ = *msg;
+    new_message_ = true;
+    mutex_.unlock();
+  }
+
+  /**
+   * @brief Retrieve new data, if available.
+   * @details Retrieve a `std::optional` object containing new data if the
+   * internal `new_message_` flag evaluates to `true`, or `std::nullopt_t`
+   * otherwise. If new data is retrieved, the new message flag is reset to
+   * `false`.
+   * @return `std::optional` containing message data if new message available,
+   * `std::nullopt_t` otherwise
+   */
+  std::optional<Message> GetDataIfNew() {
+    if (new_message_ && mutex_.try_lock()) {
+      Message data_out = data_;
+      new_message_ = false;
+      mutex_.unlock();
+      return data_out;
+    }
+    return {};
+  }
+
+  /**
+   * @brief Check if new message is available.
+   * @return `true` if new message available, `false` otherwise
+   */
+  [[nodiscard]] bool is_new_message() const { return new_message_; }
+
+ private:
+
+  /**
+   * @brief Flag indicating that a new message is available.
+   */
+  bool new_message_ = false;
+
+  /**
+   * @brief Data for the most-recently received message.
+   */
+  Message data_;
+
+  /**
+   * @brief Mutex for the message handler.
+   */
+  std::mutex mutex_;
+
+};
+
+} // kodlab

include/kodlab_mjbots_sdk/lcm_subscriber.h

@@ -1,85 +1,148 @@
-// BSD 3-Clause License
-// Copyright (c) 2021 The Trustees of the University of Pennsylvania. All Rights Reserved
-// Authors:
-// Shane Rozen-Levy <[email protected]>
+/**
+ * @file lcm_subscriber.h
+ * @author Ethan Musser ([email protected])
+ * @author J. Diego Caporale ([email protected])
+ * @author Shane Rozen-Levy ([email protected])
+ * @brief Provides LCM subscriber object for subscribing to one or multiple LCM
+ * channels.
+ * @date 7/7/22
+ *
+ * @copyright Copyright 2022 The Trustees of the University of Pennsylvania. All
+ * rights reserved. BSD 3-Clause License
+ *
+ */
 
 #pragma once
+
+#include <map>
+#include <mutex>
 #include <string>
 #include "lcm/lcm-cpp.hpp"
 #include "real_time_tools/thread.hpp"
 #include "kodlab_mjbots_sdk/abstract_realtime_object.h"
+#include "kodlab_mjbots_sdk/lcm_message_handler.h"
+#include "kodlab_mjbots_sdk/log.h"
 
 namespace kodlab {
-/*!
- * @brief An template class for an lcm subscriber. Subscribes to just one msg of type msg_type.
- * @tparam MessageClass the lcm msg type
+
+/**
+ * @brief LCM subscriber capable of subscribing to multiple channels
+ * @warning Running multiple `LcmSubscriber` objects on a single CPU can result
+ * in concurrency issues.  The authors do not endorse this usage, do so at your
+ * own risk.
+ * @todo Move implementation to source file once `CTRL_C_DETECTED` global
+ *       variable made available to TU.
  */
-template<class MessageClass>
 class LcmSubscriber : public AbstractRealtimeObject {
  public:
+
   /*!
-   * @brief constructor for lcm subscriber
-   * @param realtime_priority the realtime priority, max is 99
-   * @param cpu the cpu for this process
-   * @param channel_name the string for the channel name
+   * @brief Constructor an lcm subscriber
+   * @param realtime_priority realtime priority in range [1, 99]
+   * @param cpu cpu for this process
    */
-  LcmSubscriber(int realtime_priority, int cpu, std::string channel_name);
+  LcmSubscriber(int realtime_priority, int cpu);
 
-  std::mutex mutex_;         /// Mutex for if m_data
-  bool new_message_ =
-      false; /// True if there is new data, false if data is old, used to prevent user for checking too often
-  MessageClass data_;            /// A copy of the most recent lcm data
- protected:
-  /*!
-   * @brief callback function when msg is received. Copies the message to m_data
-   * @param rbuf unknown, but not used
-   * @param chan channel name
-   * @param msg ptr to the incoming message data
+  /**
+   * @brief Initialize the LCM subscriber thread.
    */
-  void HandleMsg(const lcm::ReceiveBuffer *rbuf,
-                 const std::string &chan,
-                 const MessageClass *msg);
+  void Init();
 
-  /*!
-   * @brief subscribes to the lcm channel using handle message and handles ctrl c detection
+  /**
+   * @brief Add a new LCM channel to the the LCM object's subscriptions
+   * @tparam Message LCM message type for channel
+   * @param channel_name channel name
+   * @param handler `LcmMessageHandler`-inherited message handler object
+   * @return generated `lcm::Subscription` object
    */
-  void Run() override;
+  template<class Message>
+  lcm::Subscription *AddSubscription(std::string channel_name,
+                                     LcmMessageHandler<Message> &handler) {
+    auto sub = lcm_.subscribe(channel_name,
+                              &LcmMessageHandler<Message>::HandleMessage,
+                              &handler);
+    subs_.emplace(channel_name, sub);
+    return sub;
+  }
+
+  /**
+   * @brief Remove a subscription by channel name
+   * @note Warns and continues if channel does not exist.
+   * @param channel_name channel name of subscription to be removed
+   * @return 0 if unsubscribe successful, -1 if `channel_name` is not a valid
+   * subscription
+   */
+  int RemoveSubscription(const std::string &channel_name);
 
-  std::string channel_name_;
+  /**
+   * @brief Accessor for `lcm::Subscription` objects
+   * @param channel_name channel name
+   * @return `lcm::Subscription` object on corresponding channel if subscription
+   * exists, `nullptr` otherwise
+   */
+  [[nodiscard]] const lcm::Subscription *get_subscription(const std::string &channel_name) const;
+
+ private:
+
+  /**
+   * @brief LCM object
+   */
   lcm::LCM lcm_;
+
+  /**
+   * @brief Map of channel names to corresponding `lcm::Subscription` objects
+   */
+  std::map<std::string, lcm::Subscription *> subs_;
+
+  /**
+   * @brief Waits for LCM messages and exits when ctrl+c detected
+   */
+  void Run() override;
+
 };
 
-/************************************Implementation********************************************************************/
-template<class msg_type>
-void LcmSubscriber<msg_type>::Run() {
-  lcm_.subscribe(channel_name_, &LcmSubscriber::HandleMsg, this);
-  mutex_.unlock(); // Ensures mutex is unlocked
-  std::cout << "Subscribing to " << channel_name_ << std::endl;
-  while (!CTRL_C_DETECTED) {
-    lcm_.handleTimeout(1000);
-  }
-}
 
-template<class msg_type>
-LcmSubscriber<msg_type>::LcmSubscriber(int realtime_priority, int cpu, std::string channel_name):
-    channel_name_(channel_name) {
+////////////////////////////////////////////////////////////////////////////////
+// Implementation                                                             //
+////////////////////////////////////////////////////////////////////////////////
+
+LcmSubscriber::LcmSubscriber(int realtime_priority, int cpu) {
   cpu_ = cpu;
   realtime_priority_ = realtime_priority;
-  if (!channel_name_.empty())
-    Start();
 }
 
-template<class msg_type>
-void LcmSubscriber<msg_type>::HandleMsg(const lcm::ReceiveBuffer *rbuf,
-                                        const std::string &chan,
-                                        const msg_type *msg) {
-  // Lock mutex
-  mutex_.lock();
-  // Copy data
-  data_ = *msg;
-  // Let user know new message
-  new_message_ = true;
-  // Unlock mutex
-  mutex_.unlock();
+void LcmSubscriber::Init() {
+  Start();
 }
-} // namespace kodlab
+
+int LcmSubscriber::RemoveSubscription(const std::string &channel_name) {
+  auto it = subs_.find(channel_name);
+  if (it != subs_.end()) {
+    int success = lcm_.unsubscribe(subs_[channel_name]);
+    subs_.erase(it);
+    return success;
+  } else {
+    LOG_WARN("Channel \"%s\" is not in subscription list.",
+             channel_name.c_str());
+  }
+  return -1;
+}
+
+[[nodiscard]] const lcm::Subscription *LcmSubscriber::get_subscription(const std::string &channel_name) const {
+  if (subs_.count(channel_name) == 1) {
+    return subs_.at(channel_name);
+  } else {
+    LOG_ERROR("Channel \"%s\" is not in subscription list.",
+             channel_name.c_str());
+    return nullptr;
+  }
+}
+
+void LcmSubscriber::Run() {
+  while (!CTRL_C_DETECTED) {
+    lcm_.handleTimeout(1000);
+  }
+}
+
+} // kodlab
+