From 459c025460f131247e1fb47767c254fd61e7ef3f Mon Sep 17 00:00:00 2001 From: miekale Date: Sat, 21 Jun 2025 17:10:56 -0400 Subject: [PATCH 1/5] adding support for topic specific handlers --- .gitignore | 1 + autonomy/interfacing/can/config/params.yaml | 9 +- autonomy/interfacing/can/include/can_node.hpp | 4 +- autonomy/interfacing/can/src/can_node.cpp | 124 +++++++++++++++--- 4 files changed, 115 insertions(+), 23 deletions(-) diff --git a/.gitignore b/.gitignore index 7411f32..64716e2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ .env __pycache__ .vscode/ +cursor/ watod-config.local.sh diff --git a/autonomy/interfacing/can/config/params.yaml b/autonomy/interfacing/can/config/params.yaml index 77055fc..d94f7fb 100644 --- a/autonomy/interfacing/can/config/params.yaml +++ b/autonomy/interfacing/can/config/params.yaml @@ -9,8 +9,11 @@ can_node: # Communication Settings publish_rate_hz: 50 # Rate for checking incoming CAN messages - receive_timeout_ms: 10000 # Timeout for receiving CAN messages - + receive_timeout_ms: 10000 # Timeout for receiving CAN messages # Topics to subscribe to (message types will be auto-detected) topics: - - "/test_controller" \ No newline at end of file + - "/test_controller" + - "/cmd_arm_joint" + - "/cmd_arm_ee" + - "/cmd_hand_joint" + - "/cmd_hand_ee" \ No newline at end of file diff --git a/autonomy/interfacing/can/include/can_node.hpp b/autonomy/interfacing/can/include/can_node.hpp index 534af57..577b121 100644 --- a/autonomy/interfacing/can/include/can_node.hpp +++ b/autonomy/interfacing/can/include/can_node.hpp @@ -22,7 +22,7 @@ class CanNode : public rclcpp::Node { private: autonomy::CanCore can_; std::vector topic_configs_; - std::vector> subscribers_; + std::unordered_map> subscribers_; rclcpp::TimerBase::SharedPtr receive_timer_; // Timer to periodically check for CAN messages // Methods @@ -34,7 +34,7 @@ class CanNode : public rclcpp::Node { // Helper methods uint32_t generateCanId(const std::string& topic_name); - std::vector createCanMessages(const std::string& topic_name, std::shared_ptr ros_msg); + std::vector createCanMessages(const std::string& topic_name, std::shared_ptr ros_msg, uint32_t can_id = 0); }; #endif // CAN_NODE_HPP diff --git a/autonomy/interfacing/can/src/can_node.cpp b/autonomy/interfacing/can/src/can_node.cpp index ddccc06..ca7b7de 100644 --- a/autonomy/interfacing/can/src/can_node.cpp +++ b/autonomy/interfacing/can/src/can_node.cpp @@ -120,27 +120,115 @@ std::string CanNode::discoverTopicType(const std::string& topic_name) { void CanNode::createSubscribers() { for (const auto& topic_config : topic_configs_) { - // Create generic subscriber that can handle any message type - auto subscriber = this->create_generic_subscription( - topic_config.name, - topic_config.type, - 10, - [this, topic_name = topic_config.name, topic_type = topic_config.type] - (std::shared_ptr msg) { - this->topicCallback(msg, topic_name, topic_type); - } - ); + if (topic_config.name == "/test_controller") { + subscribers_[topic_config.name] = this->create_generic_subscription( + topic_config.name, + topic_config.type, + 10, + [this](std::shared_ptr msg) { + this->handleControllerTopic(msg, "/test_controller"); + } + ); + } else if (topic_config.name == "/cmd_arm_joint") { + subscribers_[topic_config.name] = this->create_generic_subscription( + topic_config.name, + topic_config.type, + 10, + [this](std::shared_ptr msg) { + this->handleJointTopic(msg, "/cmd_arm_joint"); + } + ); + } else if (topic_config.name == "/cmd_hand_joint") { + subscribers_[topic_config.name] = this->create_generic_subscription( + topic_config.name, + topic_config.type, + 10, + [this](std::shared_ptr msg) { + this->handleJointTopic(msg, "/cmd_hand_joint"); + } + ); + } else if (topic_config.name == "/cmd_arm_ee") { + subscribers_[topic_config.name] = this->create_generic_subscription( + topic_config.name, + topic_config.type, + 10, + [this](std::shared_ptr msg) { + this->handleEndEffectorTopic(msg, "/cmd_arm_ee"); + } + ); + } else if (topic_config.name == "/cmd_hand_ee") { + subscribers_[topic_config.name] = this->create_generic_subscription( + topic_config.name, + topic_config.type, + 10, + [this](std::shared_ptr msg) { + this->handleEndEffectorTopic(msg, "/cmd_hand_ee"); + } + ); + } else { + subscribers_[topic_config.name] = this->create_generic_subscription( + topic_config.name, + topic_config.type, + 10, + [this, topic_name = topic_config.name](std::shared_ptr msg) { + this->handleGenericTopic(msg, topic_name); + } + ); + } - subscribers_.push_back(subscriber); RCLCPP_INFO(this->get_logger(), "Created generic subscriber for topic: %s (type: %s)", topic_config.name.c_str(), topic_config.type.c_str()); } } -void CanNode::topicCallback(std::shared_ptr msg, const std::string& topic_name, [[maybe_unused]] const std::string& topic_type) { - std::vector can_messages = createCanMessages(topic_name, msg); // Create CAN message(s) from ROS message +// Add these new handler methods +void CanNode::handleControllerTopic(std::shared_ptr msg, const std::string& topic_name) { + // Custom CAN ID range for controller messages + uint32_t can_id = 0x100; // Controller messages start at 0x100 +y std::vector can_messages = createCanMessages(topic_name, msg, base_can_id); + + // Send with high priority + for (const auto& can_message : can_messages) { + if (!can_.sendMessage(can_message)) { + RCLCPP_ERROR(this->get_logger(), "Failed to send controller CAN message (ID 0x%X)", can_message.id); + } + } +} + +void CanNode::handleJointTopic(std::shared_ptr msg, const std::string& topic_name) { + // Custom CAN ID range for joint messages + uint32_t base_can_id = 0x200; // Joint messages start at 0x200 + std::vector can_messages = createCanMessages(topic_name, msg, base_can_id); + + // Send joint messages + for (const auto& can_message : can_messages) { + if (!can_.sendMessage(can_message)) { + RCLCPP_ERROR(this->get_logger(), "Failed to send joint CAN message (ID 0x%X)", can_message.id); + } + } +} + +void CanNode::handleEndEffectorTopic(std::shared_ptr msg, const std::string& topic_name) { + // Custom CAN ID range for end effector messages + uint32_t base_can_id = 0x300; // End effector messages start at 0x300 + std::vector can_messages = createCanMessages(topic_name, msg, base_can_id); - // Send CAN message + // Send end effector messages + for (const auto& can_message : can_messages) { + if (!can_.sendMessage(can_message)) { + RCLCPP_ERROR(this->get_logger(), "Failed to send end effector CAN message (ID 0x%X)", can_message.id); + } + } + + +void CanNode::handleGenericTopic(std::shared_ptr msg, const std::string& topic_name) { + // Original generic handling + std::vector can_messages = createCanMessages(topic_name, msg); + sendCanMessages(can_messages, topic_name); +} + +// Helper method to reduce code duplication +void CanNode::sendCanMessages(const std::vector& can_messages, const std::string& topic_name) { int successful_sends = 0; for (const auto& can_message : can_messages) { if (can_.sendMessage(can_message)) { @@ -177,6 +265,8 @@ void CanNode::receiveCanMessages() { // or an error occurred (which CanCore should log). No action needed here for no-message case. } + +// Not sure if this is needed, but it's here for now uint32_t CanNode::generateCanId(const std::string& topic_name) { // Generate a CAN ID from topic name using hash // Use the first 29 bits for extended CAN ID (CAN 2.0B) @@ -188,11 +278,9 @@ uint32_t CanNode::generateCanId(const std::string& topic_name) { return hash & 0x1FFFFFFF; } -std::vector CanNode::createCanMessages(const std::string& topic_name, std::shared_ptr ros_msg) { +std::vector CanNode::createCanMessages(const std::string& topic_name, std::shared_ptr ros_msg, uint32_t can_id) { std::vector messages_to_send; - - uint32_t can_id = generateCanId(topic_name); - + bool is_extended = true; // Use extended CAN ID (29 bits) bool is_rtr = false; // Remote Transmission Request From 50c6ddfd15086981293d93cb3f5b75f7183eb891 Mon Sep 17 00:00:00 2001 From: miekale Date: Sat, 21 Jun 2025 17:15:13 -0400 Subject: [PATCH 2/5] adding dummy topics --- autonomy/interfacing/can/src/can_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autonomy/interfacing/can/src/can_node.cpp b/autonomy/interfacing/can/src/can_node.cpp index ca7b7de..2803119 100644 --- a/autonomy/interfacing/can/src/can_node.cpp +++ b/autonomy/interfacing/can/src/can_node.cpp @@ -181,7 +181,7 @@ void CanNode::createSubscribers() { } } -// Add these new handler methods +// dummy handler for testing void CanNode::handleControllerTopic(std::shared_ptr msg, const std::string& topic_name) { // Custom CAN ID range for controller messages uint32_t can_id = 0x100; // Controller messages start at 0x100 From 1e0e8091de3fafcb3dd35ccbb3f30dfb687e71a8 Mon Sep 17 00:00:00 2001 From: miekale Date: Sat, 21 Jun 2025 17:15:20 -0400 Subject: [PATCH 3/5] adding dummy topics --- autonomy/interfacing/can/src/can_node.cpp | 39 ++++++----------------- 1 file changed, 9 insertions(+), 30 deletions(-) diff --git a/autonomy/interfacing/can/src/can_node.cpp b/autonomy/interfacing/can/src/can_node.cpp index 2803119..58f1c67 100644 --- a/autonomy/interfacing/can/src/can_node.cpp +++ b/autonomy/interfacing/can/src/can_node.cpp @@ -135,7 +135,7 @@ void CanNode::createSubscribers() { topic_config.type, 10, [this](std::shared_ptr msg) { - this->handleJointTopic(msg, "/cmd_arm_joint"); + this->handleArmJointTopic(msg, "/cmd_arm_joint"); } ); } else if (topic_config.name == "/cmd_hand_joint") { @@ -144,7 +144,7 @@ void CanNode::createSubscribers() { topic_config.type, 10, [this](std::shared_ptr msg) { - this->handleJointTopic(msg, "/cmd_hand_joint"); + this->handleHandJointTopic(msg, "/cmd_hand_joint"); } ); } else if (topic_config.name == "/cmd_arm_ee") { @@ -153,7 +153,7 @@ void CanNode::createSubscribers() { topic_config.type, 10, [this](std::shared_ptr msg) { - this->handleEndEffectorTopic(msg, "/cmd_arm_ee"); + this->handleArmEeTopic(msg, "/cmd_arm_ee"); } ); } else if (topic_config.name == "/cmd_hand_ee") { @@ -162,7 +162,7 @@ void CanNode::createSubscribers() { topic_config.type, 10, [this](std::shared_ptr msg) { - this->handleEndEffectorTopic(msg, "/cmd_hand_ee"); + this->handleHandEeTopic(msg, "/cmd_hand_ee"); } ); } else { @@ -182,10 +182,10 @@ void CanNode::createSubscribers() { } // dummy handler for testing -void CanNode::handleControllerTopic(std::shared_ptr msg, const std::string& topic_name) { +void CanNode::handleArmJointTopic(std::shared_ptr msg, const std::string& topic_name) { // Custom CAN ID range for controller messages uint32_t can_id = 0x100; // Controller messages start at 0x100 -y std::vector can_messages = createCanMessages(topic_name, msg, base_can_id); + std::vector can_messages = createCanMessages(topic_name, msg, can_id); // Send with high priority for (const auto& can_message : can_messages) { @@ -195,30 +195,9 @@ y std::vector can_messages = createCanMessages(topic_name } } -void CanNode::handleJointTopic(std::shared_ptr msg, const std::string& topic_name) { - // Custom CAN ID range for joint messages - uint32_t base_can_id = 0x200; // Joint messages start at 0x200 - std::vector can_messages = createCanMessages(topic_name, msg, base_can_id); - - // Send joint messages - for (const auto& can_message : can_messages) { - if (!can_.sendMessage(can_message)) { - RCLCPP_ERROR(this->get_logger(), "Failed to send joint CAN message (ID 0x%X)", can_message.id); - } - } -} - -void CanNode::handleEndEffectorTopic(std::shared_ptr msg, const std::string& topic_name) { - // Custom CAN ID range for end effector messages - uint32_t base_can_id = 0x300; // End effector messages start at 0x300 - std::vector can_messages = createCanMessages(topic_name, msg, base_can_id); - - // Send end effector messages - for (const auto& can_message : can_messages) { - if (!can_.sendMessage(can_message)) { - RCLCPP_ERROR(this->get_logger(), "Failed to send end effector CAN message (ID 0x%X)", can_message.id); - } - } +void CanNode::handleHandJointTopic(std::shared_ptr msg, const std::string& topic_name) {} +void CanNode::handleHandEeTopic(std::shared_ptr msg, const std::string& topic_name) {} +void CanNode::handleArmEeTopic(std::shared_ptr msg, const std::string& topic_name) {} void CanNode::handleGenericTopic(std::shared_ptr msg, const std::string& topic_name) { From 64e16ba5c8ad7cfcd3696700ec36f79ed575e8ed Mon Sep 17 00:00:00 2001 From: miekale Date: Sat, 21 Jun 2025 17:34:02 -0400 Subject: [PATCH 4/5] commenting out info --- autonomy/interfacing/can/src/can_node.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/autonomy/interfacing/can/src/can_node.cpp b/autonomy/interfacing/can/src/can_node.cpp index 58f1c67..121d45e 100644 --- a/autonomy/interfacing/can/src/can_node.cpp +++ b/autonomy/interfacing/can/src/can_node.cpp @@ -184,15 +184,9 @@ void CanNode::createSubscribers() { // dummy handler for testing void CanNode::handleArmJointTopic(std::shared_ptr msg, const std::string& topic_name) { // Custom CAN ID range for controller messages - uint32_t can_id = 0x100; // Controller messages start at 0x100 - std::vector can_messages = createCanMessages(topic_name, msg, can_id); - - // Send with high priority - for (const auto& can_message : can_messages) { - if (!can_.sendMessage(can_message)) { - RCLCPP_ERROR(this->get_logger(), "Failed to send controller CAN message (ID 0x%X)", can_message.id); - } - } + // uint32_t can_id = 0x100; // set based on robot message + // std::vector can_messages = createCanMessages(topic_name, msg, can_id); + // sendCanMessages(can_messages, topic_name); } void CanNode::handleHandJointTopic(std::shared_ptr msg, const std::string& topic_name) {} From 07769f715879431bb91c328ca9051d31cb2b0d49 Mon Sep 17 00:00:00 2001 From: miekale Date: Sat, 21 Jun 2025 18:14:28 -0400 Subject: [PATCH 5/5] adding mit motors files --- autonomy/interfacing/can/config/params.yaml | 4 ++ .../can/include/mit_motor_control.hpp | 6 ++ autonomy/interfacing/can/src/can_node.cpp | 2 + .../interfacing/can/src/mit_motor_control.cpp | 70 +++++++++++++++++++ 4 files changed, 82 insertions(+) create mode 100644 autonomy/interfacing/can/include/mit_motor_control.hpp create mode 100644 autonomy/interfacing/can/src/mit_motor_control.cpp diff --git a/autonomy/interfacing/can/config/params.yaml b/autonomy/interfacing/can/config/params.yaml index d94f7fb..4415c42 100644 --- a/autonomy/interfacing/can/config/params.yaml +++ b/autonomy/interfacing/can/config/params.yaml @@ -10,6 +10,10 @@ can_node: # Communication Settings publish_rate_hz: 50 # Rate for checking incoming CAN messages receive_timeout_ms: 10000 # Timeout for receiving CAN messages + + profile_type: "trapezoidal" # "trapezoidal" or "triangular" + max_acc: 1.0 # m/s^2 + # Topics to subscribe to (message types will be auto-detected) topics: - "/test_controller" diff --git a/autonomy/interfacing/can/include/mit_motor_control.hpp b/autonomy/interfacing/can/include/mit_motor_control.hpp new file mode 100644 index 0000000..48f5193 --- /dev/null +++ b/autonomy/interfacing/can/include/mit_motor_control.hpp @@ -0,0 +1,6 @@ +#include "can_node.hpp" + +// MIT Motor Control Functions +void moveMIT(double velocity, double force, double position, int id); +void moveProfile(const std::string& profile_type, double max_acc); + diff --git a/autonomy/interfacing/can/src/can_node.cpp b/autonomy/interfacing/can/src/can_node.cpp index 121d45e..1fbf116 100644 --- a/autonomy/interfacing/can/src/can_node.cpp +++ b/autonomy/interfacing/can/src/can_node.cpp @@ -1,4 +1,5 @@ #include "can_node.hpp" +#include "mit_motor_control.hpp" #include #include #include @@ -217,6 +218,7 @@ void CanNode::sendCanMessages(const std::vector& can_messa } } + void CanNode::receiveCanMessages() { autonomy::CanMessage received_msg; // Attempt to receive a message. CanCore::receiveMessage is non-blocking. diff --git a/autonomy/interfacing/can/src/mit_motor_control.cpp b/autonomy/interfacing/can/src/mit_motor_control.cpp new file mode 100644 index 0000000..58193f1 --- /dev/null +++ b/autonomy/interfacing/can/src/mit_motor_control.cpp @@ -0,0 +1,70 @@ +void CanNode::moveMIT(double velocity, double force, double position, int id) { + // Create CAN message for MIT (Massachusetts Institute of Technology) control mode + // This typically involves velocity, force, and position parameters + + autonomy::CanMessage can_msg; + can_msg.id = id; // MIT control command ID + can_msg.is_extended_id = true; + can_msg.is_remote_frame = false; + can_msg.dlc = 8; // 8 bytes for the control parameters + + // Pack the control parameters into the CAN message data + // Assuming IEEE 754 double precision format (8 bytes total) + // We'll use 2 bytes for velocity, 2 for force, 2 for position, 2 reserved + uint16_t vel_scaled = static_cast(velocity * 1000.0); // Scale for precision + uint16_t force_scaled = static_cast(force * 1000.0); + uint16_t pos_scaled = static_cast(position * 1000.0); + + can_msg.data[0] = (vel_scaled >> 8) & 0xFF; // Velocity high byte + can_msg.data[1] = vel_scaled & 0xFF; // Velocity low byte + can_msg.data[2] = (force_scaled >> 8) & 0xFF; // Force high byte + can_msg.data[3] = force_scaled & 0xFF; // Force low byte + can_msg.data[4] = (pos_scaled >> 8) & 0xFF; // Position high byte + can_msg.data[5] = pos_scaled & 0xFF; // Position low byte + can_msg.data[6] = 0x00; // Reserved + can_msg.data[7] = 0x00; // Reserved + + // Send the CAN message + if (can_.sendMessage(can_msg)) { + RCLCPP_INFO(this->get_logger(), "MIT control message sent - V:%.3f, F:%.3f, P:%.3f", + velocity, force, position); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to send MIT control message"); + } +} + +void CanNode::moveProfile(const std::string& profile_type, double max_acc) { + // Create CAN message for profile-based motion control + // Profile types could be: "trapezoidal", "s-curve", "polynomial", etc. + + autonomy::CanMessage can_msg; + can_msg.id = 0x201; // Profile control command ID + can_msg.is_extended_id = true; + can_msg.is_remote_frame = false; + can_msg.dlc = 8; // 8 bytes for profile parameters + + // Encode profile type (first 4 bytes) + std::hash hasher; + uint32_t profile_hash = static_cast(hasher(profile_type)); + + can_msg.data[0] = (profile_hash >> 24) & 0xFF; // Profile type hash high byte + can_msg.data[1] = (profile_hash >> 16) & 0xFF; // Profile type hash mid-high byte + can_msg.data[2] = (profile_hash >> 8) & 0xFF; // Profile type hash mid-low byte + can_msg.data[3] = profile_hash & 0xFF; // Profile type hash low byte + + // Encode maximum acceleration (last 4 bytes) + uint32_t acc_scaled = static_cast(max_acc * 1000.0); // Scale for precision + + can_msg.data[4] = (acc_scaled >> 24) & 0xFF; // Max acc high byte + can_msg.data[5] = (acc_scaled >> 16) & 0xFF; // Max acc mid-high byte + can_msg.data[6] = (acc_scaled >> 8) & 0xFF; // Max acc mid-low byte + can_msg.data[7] = acc_scaled & 0xFF; // Max acc low byte + + // Send the CAN message + if (can_.sendMessage(can_msg)) { + RCLCPP_INFO(this->get_logger(), "Profile control message sent - Type:%s, MaxAcc:%.3f", + profile_type.c_str(), max_acc); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to send profile control message"); + } +} \ No newline at end of file