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..4415c42 100644 --- a/autonomy/interfacing/can/config/params.yaml +++ b/autonomy/interfacing/can/config/params.yaml @@ -9,8 +9,15 @@ 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 + + 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" \ 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/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 ddccc06..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 @@ -120,27 +121,88 @@ 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->handleArmJointTopic(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->handleHandJointTopic(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->handleArmEeTopic(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->handleHandEeTopic(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 - - // Send CAN message +// 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; // 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) {} +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) { + // 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)) { @@ -156,6 +218,7 @@ void CanNode::topicCallback(std::shared_ptr msg, cons } } + void CanNode::receiveCanMessages() { autonomy::CanMessage received_msg; // Attempt to receive a message. CanCore::receiveMessage is non-blocking. @@ -177,6 +240,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 +253,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 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