Merge pull request #3 from clearpathrobotics/lcamero/puma_queue

Update Puma Motor Driver to use Queue

Author: roni-kreinin

puma_motor_driver/src/driver.cpp

@@ -122,7 +122,7 @@ double Driver::radPerSecToRpm() const
 void Driver::sendId(const uint32_t id)
 {
   can_msgs::msg::Frame msg = getMsg(id);
-  interface_->send(msg);
+  interface_->queue(msg);
 }
 
 void Driver::sendUint8(const uint32_t id, const uint8_t value)
@@ -133,7 +133,7 @@ void Driver::sendUint8(const uint32_t id, const uint8_t value)
   std::memcpy(data, &value, sizeof(uint8_t));
   std::copy(std::begin(data), std::end(data), std::begin(msg.data));
 
-  interface_->send(msg);
+  interface_->queue(msg);
 }
 
 void Driver::sendUint16(const uint32_t id, const uint16_t value)
@@ -144,7 +144,7 @@ void Driver::sendUint16(const uint32_t id, const uint16_t value)
   std::memcpy(data, &value, sizeof(uint16_t));
   std::copy(std::begin(data), std::end(data), std::begin(msg.data));
 
-  interface_->send(msg);
+  interface_->queue(msg);
 }
 
 void Driver::sendFixed8x8(const uint32_t id, const float value)
@@ -157,7 +157,7 @@ void Driver::sendFixed8x8(const uint32_t id, const float value)
   std::memcpy(data, &output_value, sizeof(int16_t));
   std::copy(std::begin(data), std::end(data), std::begin(msg.data));
 
-  interface_->send(msg);
+  interface_->queue(msg);
 }
 
 void Driver::sendFixed16x16(const uint32_t id, const double value)
@@ -170,7 +170,7 @@ void Driver::sendFixed16x16(const uint32_t id, const double value)
   std::memcpy(data, &output_value, sizeof(int32_t));
   std::copy(std::begin(data), std::end(data), std::begin(msg.data));
 
-  interface_->send(msg);
+  interface_->queue(msg);
 }
 
 can_msgs::msg::Frame Driver::getMsg(const uint32_t id)

puma_motor_driver/src/multi_puma_node.cpp

@@ -94,6 +94,8 @@ MultiPumaNode::MultiPumaNode(const std::string node_name)
   interface_.reset(new clearpath_ros2_socketcan_interface::SocketCANInterface(
     canbus_dev_, node_handle_));
 
+  interface_->startSendTimer(1);
+
   for (uint8_t i = 0; i < joint_names_.size(); i++) {
     drivers_.push_back(puma_motor_driver::Driver(
       interface_,