CORE-36182: [puma_motor_driver] Port to can_hardware socketcan driver

Author: nnarain-cpr

clearpath_motor_drivers/puma_motor_driver/CMakeLists.txt

@@ -7,9 +7,8 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(can_msgs REQUIRED)
+find_package(can_hardware REQUIRED)
 find_package(clearpath_motor_msgs REQUIRED)
-find_package(clearpath_ros2_socketcan_interface REQUIRED)
 find_package(diagnostic_updater REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
@@ -18,9 +17,8 @@ find_package(sensor_msgs REQUIRED)
 
 set(DEPENDENCIES
   ament_cmake
-  can_msgs
+  can_hardware
   clearpath_motor_msgs
-  clearpath_ros2_socketcan_interface
   diagnostic_updater
   rclcpp
   std_msgs

clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/driver.hpp

@@ -27,8 +27,10 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 #include <stdint.h>
 #include <string>
 
-#include "can_msgs/msg/frame.hpp"
-#include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp"
+#include <rclcpp/rclcpp.hpp>
+
+#include "can_hardware/common/types.hpp"
+#include "can_hardware/drivers/socketcan_driver.hpp"
 
 #include "clearpath_motor_msgs/msg/puma_status.hpp"
 
@@ -43,12 +45,12 @@ class Driver
 {
 public:
   Driver(
-    const std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface,
+    const std::shared_ptr<can_hardware::drivers::SocketCanDriver> interface,
     std::shared_ptr<rclcpp::Node> nh,
     const uint8_t & device_number,
     const std::string & device_name);
 
-  void processMessage(const can_msgs::msg::Frame::SharedPtr received_msg);
+  void processMessage(const can_hardware::Frame & frame);
 
   double radPerSecToRpm() const;
 
@@ -467,7 +469,7 @@ class Driver
   void driverUpdateDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat, bool updating);
 
 private:
-  std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface_;
+  std::shared_ptr<can_hardware::drivers::SocketCanDriver> interface_;
   std::shared_ptr<rclcpp::Node> nh_;
   uint8_t device_number_;
   std::string device_name_;
@@ -486,15 +488,14 @@ class Driver
   /**
    * Helpers to generate data for CAN messages.
    */
-  can_msgs::msg::Frame::SharedPtr can_msg_;
   void sendId(const uint32_t id);
   void sendUint8(const uint32_t id, const uint8_t value);
   void sendUint16(const uint32_t id, const uint16_t value);
   void sendFixed8x8(const uint32_t id, const float value);
   void sendFixed16x16(const uint32_t id, const double value);
-  can_msgs::msg::Frame getMsg(const uint32_t id);
-  uint32_t getApi(const can_msgs::msg::Frame msg);
-  uint32_t getDeviceNumber(const can_msgs::msg::Frame msg);
+  can_hardware::Frame getMsg(const uint32_t id);
+  uint32_t getApi(const can_hardware::Frame & msg);
+  uint32_t getDeviceNumber(const can_hardware::Frame & msg);
 
   /**
    * Comparing the raw bytes of the 16x16 fixed-point numbers

clearpath_motor_drivers/puma_motor_driver/include/puma_motor_driver/multi_puma_node.hpp

@@ -25,6 +25,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 #define PUMA_MOTOR_DRIVER_MULTI_PUMA_NODE_H
 
 #include <map>
+#include <mutex>
+#include <queue>
 
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/float64.hpp"
@@ -35,7 +37,8 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI
 #include "clearpath_motor_msgs/msg/puma_multi_feedback.hpp"
 #include "clearpath_motor_msgs/msg/puma_feedback.hpp"
 
-#include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp"
+#include "can_hardware/common/types.hpp"
+#include "can_hardware/drivers/socketcan_driver.hpp"
 
 #include "diagnostic_updater/diagnostic_updater.hpp"
 
@@ -116,6 +119,13 @@ class MultiPumaNode
   */
   bool connectIfNotConnected();
 
+  /**
+   * @brief Callback function for processing incoming CAN frames.
+   * 
+   * @param frame The received CAN frame.
+   */
+  void frameCallback(const can_hardware::Frame& frame);
+
   /**
    * Main control loop that checks and maintains the socket gateway, resets
    * and reconfigures drivers that have disconnected, verifies parameters
@@ -128,7 +138,8 @@ class MultiPumaNode
   using DiagnosticStatusWrapper = diagnostic_updater::DiagnosticStatusWrapper;
   using PumaStatus = clearpath_motor_msgs::msg::PumaStatus;
 
-  std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface_;
+  // std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface_;
+  std::shared_ptr<can_hardware::drivers::SocketCanDriver> interface_;
   std::vector<puma_motor_driver::Driver> drivers_;
 
   bool active_;
@@ -142,7 +153,9 @@ class MultiPumaNode
   std::vector<int64_t> joint_can_ids_;
   std::vector<int64_t> joint_directions_;
 
-  can_msgs::msg::Frame::SharedPtr recv_msg_;
+  std::queue<can_hardware::Frame> recv_msg_queue_;
+  std::mutex recv_msg_mutex_;
+
   clearpath_motor_msgs::msg::PumaMultiStatus status_msg_;
   clearpath_motor_msgs::msg::PumaMultiFeedback feedback_msg_;
 

clearpath_motor_drivers/puma_motor_driver/package.xml

@@ -10,9 +10,8 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>can_msgs</depend>
+  <depend>can_hardware</depend>
   <depend version_gte="2.4.0">clearpath_motor_msgs</depend>
-  <depend version_gte="2.1.1">clearpath_ros2_socketcan_interface</depend>
   <depend>diagnostic_updater</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>

clearpath_motor_drivers/puma_motor_driver/src/driver.cpp

@@ -22,7 +22,6 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCL
 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 #include "clearpath_motor_msgs/msg/puma_status.hpp"
-#include "clearpath_ros2_socketcan_interface/socketcan_interface.hpp"
 
 #include "puma_motor_driver/driver.hpp"
 
@@ -59,7 +58,7 @@ enum ConfigurationState
 typedef ConfigurationStates::ConfigurationState ConfigurationState;
 
 Driver::Driver(
-  const std::shared_ptr<clearpath_ros2_socketcan_interface::SocketCANInterface> interface,
+  const std::shared_ptr<can_hardware::drivers::SocketCanDriver> interface,
   std::shared_ptr<rclcpp::Node> nh,
   const uint8_t & device_number,
   const std::string & device_name)
@@ -88,20 +87,20 @@ Driver::Driver(
   );
 }
 
-void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg)
+void Driver::processMessage(const can_hardware::Frame & received_msg)
 {
   // If it's not our message, jump out.
-  if (getDeviceNumber(*received_msg) != device_number_) {
+  if (getDeviceNumber(received_msg) != device_number_) {
     return;
   }
 
   // If there's no data then this is a request message, jump out.
-  if (received_msg->dlc == 0) {
+  if (received_msg.dlc == 0) {
     return;
   }
 
   Field * field = nullptr;
-  uint32_t received_api = getApi(*received_msg);
+  uint32_t received_api = getApi(received_msg);
   if ((received_api & CAN_MSGID_API_M & CAN_API_MC_CFG) == CAN_API_MC_CFG) {
     field = cfgFieldForMessage(received_api);
   } else if ((received_api & CAN_MSGID_API_M & CAN_API_MC_STATUS) == CAN_API_MC_STATUS) {
@@ -128,7 +127,7 @@ void Driver::processMessage(const can_msgs::msg::Frame::SharedPtr received_msg)
   }
 
   // Copy the received data and mark that field as received.
-  std::copy_n(std::begin(received_msg->data), Field::FIELD_STRUCT_DATA_SIZE,
+  std::copy_n(std::begin(received_msg.data), Field::FIELD_STRUCT_DATA_SIZE,
     std::begin(field->data));
   field->received = true;
 }
@@ -140,75 +139,75 @@ double Driver::radPerSecToRpm() const
 
 void Driver::sendId(const uint32_t id)
 {
-  can_msgs::msg::Frame msg = getMsg(id);
-  interface_->send(msg);
+  auto msg = getMsg(id);
+  interface_->sendFrame(msg);
 }
 
 void Driver::sendUint8(const uint32_t id, const uint8_t value)
 {
-  can_msgs::msg::Frame msg = getMsg(id);
+  auto msg = getMsg(id);
   msg.dlc = sizeof(uint8_t);
   uint8_t data[8] = {0};
   std::memcpy(data, &value, sizeof(uint8_t));
   std::copy(std::begin(data), std::end(data), std::begin(msg.data));
 
-  interface_->send(msg);
+  interface_->sendFrame(msg);
 }
 
 void Driver::sendUint16(const uint32_t id, const uint16_t value)
 {
-  can_msgs::msg::Frame msg = getMsg(id);
+  auto msg = getMsg(id);
   msg.dlc = sizeof(uint16_t);
   uint8_t data[8] = {0};
   std::memcpy(data, &value, sizeof(uint16_t));
   std::copy(std::begin(data), std::end(data), std::begin(msg.data));
 
-  interface_->send(msg);
+  interface_->sendFrame(msg);
 }
 
 void Driver::sendFixed8x8(const uint32_t id, const float value)
 {
-  can_msgs::msg::Frame msg = getMsg(id);
+  auto msg = getMsg(id);
   msg.dlc = sizeof(int16_t);
   int16_t output_value = static_cast<int16_t>(static_cast<float>(1 << 8) * value);
 
   uint8_t data[8] = {0};
   std::memcpy(data, &output_value, sizeof(int16_t));
   std::copy(std::begin(data), std::end(data), std::begin(msg.data));
 
-  interface_->send(msg);
+  interface_->sendFrame(msg);
 }
 
 void Driver::sendFixed16x16(const uint32_t id, const double value)
 {
-  can_msgs::msg::Frame msg = getMsg(id);
+  auto msg = getMsg(id);
   msg.dlc = sizeof(int32_t);
   int32_t output_value = static_cast<int32_t>(static_cast<double>((1 << 16) * value));
 
   uint8_t data[8] = {0};
   std::memcpy(data, &output_value, sizeof(int32_t));
   std::copy(std::begin(data), std::end(data), std::begin(msg.data));
 
-  interface_->send(msg);
+  interface_->sendFrame(msg);
 }
 
-can_msgs::msg::Frame Driver::getMsg(const uint32_t id)
+can_hardware::Frame Driver::getMsg(const uint32_t id)
 {
-  can_msgs::msg::Frame msg;
+  can_hardware::Frame msg;
   msg.id = id;
   msg.dlc = 0;
   msg.is_extended = true;
-  msg.header.stamp = nh_->get_clock()->now();
-  msg.header.frame_id = "base_link";
+  // msg.header.stamp = nh_->get_clock()->now();
+  // msg.header.frame_id = "base_link";
   return msg;
 }
 
-uint32_t Driver::getApi(const can_msgs::msg::Frame msg)
+uint32_t Driver::getApi(const can_hardware::Frame & msg)
 {
   return msg.id & (CAN_MSGID_FULL_M ^ CAN_MSGID_DEVNO_M);
 }
 
-uint32_t Driver::getDeviceNumber(const can_msgs::msg::Frame msg)
+uint32_t Driver::getDeviceNumber(const can_hardware::Frame & msg)
 {
   return msg.id & CAN_MSGID_DEVNO_M;
 }
@@ -901,7 +900,7 @@ uint16_t Driver::encoderCounts()
 
 double Driver::getP()
 {
-  Field * field;
+  Field * field = nullptr;
   switch (control_mode_) {
     case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT:
       field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_PC)));
@@ -918,7 +917,7 @@ double Driver::getP()
 
 double Driver::getI()
 {
-  Field * field;
+  Field * field = nullptr;
   switch (control_mode_) {
     case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT:
       field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_IC)));
@@ -935,7 +934,7 @@ double Driver::getI()
 
 double Driver::getD()
 {
-  Field * field;
+  Field * field = nullptr;
   switch (control_mode_) {
     case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT:
       field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_DC)));
@@ -952,7 +951,7 @@ double Driver::getD()
 
 uint8_t * Driver::getRawP()
 {
-  Field * field;
+  Field * field = nullptr;
   switch (control_mode_) {
     case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT:
       field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_PC)));
@@ -969,7 +968,7 @@ uint8_t * Driver::getRawP()
 
 uint8_t * Driver::getRawI()
 {
-  Field * field;
+  Field * field = nullptr;
   switch (control_mode_) {
     case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT:
       field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_IC)));
@@ -986,7 +985,7 @@ uint8_t * Driver::getRawI()
 
 uint8_t * Driver::getRawD()
 {
-  Field * field;
+  Field * field = nullptr;
   switch (control_mode_) {
     case clearpath_motor_msgs::msg::PumaStatus::MODE_CURRENT:
       field = ictrlFieldForMessage(getApi(getMsg(LM_API_ICTRL_DC)));

clearpath_motor_drivers/puma_motor_driver/src/multi_puma_node.cpp

@@ -93,9 +93,10 @@ MultiPumaNode::MultiPumaNode(const std::string node_name)
 
   node_handle_ = std::shared_ptr<rclcpp::Node>(this, [](rclcpp::Node *){});
 
-  // Socket
-  interface_.reset(new clearpath_ros2_socketcan_interface::SocketCANInterface(
-    canbus_dev_, node_handle_));
+  // SocketCAN Interface
+  interface_ = std::make_shared<can_hardware::drivers::SocketCanDriver>(canbus_dev_);
+  interface_->registerFrameCallback(
+    std::bind(&MultiPumaNode::frameCallback, this,  std::placeholders::_1));
 
   for (uint8_t i = 0; i < joint_names_.size(); i++) {
     drivers_.push_back(puma_motor_driver::Driver(
@@ -106,7 +107,6 @@ MultiPumaNode::MultiPumaNode(const std::string node_name)
     ));
   }
 
-  recv_msg_.reset(new can_msgs::msg::Frame());
   feedback_msg_.drivers_feedback.resize(drivers_.size());
   status_msg_.drivers.resize(drivers_.size());
 
@@ -284,8 +284,17 @@ bool MultiPumaNode::areAllActive()
   return true;
 }
 
+void MultiPumaNode::frameCallback(const can_hardware::Frame& frame)
+{
+  std::lock_guard<std::mutex> lock(recv_msg_mutex_);
+  recv_msg_queue_.push(frame);
+}
+
 void MultiPumaNode::run()
 {
+  RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
+    "[HAL-y Code]: MultiPumaNode Run Cycle %d", status_count_);
+
   if (active_) {
     // Checks to see if power flag has been reset for each driver
     for (auto & driver : drivers_) {
@@ -311,10 +320,15 @@ void MultiPumaNode::run()
     }
   }
 
-  // Process all received messages through the connected driver instances.
-  while (interface_->recv(recv_msg_)) {
+  while (!recv_msg_queue_.empty()) {
+    can_hardware::Frame frame;
+    {
+      std::lock_guard<std::mutex> lock(recv_msg_mutex_);
+      frame = std::move(recv_msg_queue_.front());
+      recv_msg_queue_.pop();
+    }
     for (auto & driver : drivers_) {
-      driver.processMessage(recv_msg_);
+      driver.processMessage(frame);
     }
   }