From cd5ea238e03fd9f29d187c38f34aebddd4d87a6b Mon Sep 17 00:00:00 2001 From: Luis Camero Date: Fri, 15 Nov 2024 10:18:41 -0500 Subject: [PATCH] Update Puma Motor Driver to use Queue --- puma_motor_driver/src/driver.cpp | 10 +++++----- puma_motor_driver/src/multi_puma_node.cpp | 2 ++ 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/puma_motor_driver/src/driver.cpp b/puma_motor_driver/src/driver.cpp index 8245130..b43378b 100644 --- a/puma_motor_driver/src/driver.cpp +++ b/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) diff --git a/puma_motor_driver/src/multi_puma_node.cpp b/puma_motor_driver/src/multi_puma_node.cpp index 4a48c15..c5a6855 100644 --- a/puma_motor_driver/src/multi_puma_node.cpp +++ b/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_,