Miekale mit

.gitignore

@@ -1,5 +1,6 @@
 .env
 __pycache__
 .vscode/
+cursor/
 
 watod-config.local.sh

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"         
+      - "/test_controller"         
+      - "/cmd_arm_joint"
+      - "/cmd_arm_ee"
+      - "/cmd_hand_joint" 
+      - "/cmd_hand_ee"

autonomy/interfacing/can/include/can_node.hpp

@@ -22,7 +22,7 @@ class CanNode : public rclcpp::Node {
 private:
   autonomy::CanCore can_;
   std::vector<TopicConfig> topic_configs_;
-  std::vector<std::shared_ptr<rclcpp::GenericSubscription>> subscribers_;
+  std::unordered_map<std::string, std::shared_ptr<rclcpp::GenericSubscription>> 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<autonomy::CanMessage> createCanMessages(const std::string& topic_name, std::shared_ptr<rclcpp::SerializedMessage> ros_msg);
+  std::vector<autonomy::CanMessage> createCanMessages(const std::string& topic_name, std::shared_ptr<rclcpp::SerializedMessage> ros_msg, uint32_t can_id = 0);
 };
 
 #endif // CAN_NODE_HPP

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);
+

autonomy/interfacing/can/src/can_node.cpp

@@ -1,4 +1,5 @@
 #include "can_node.hpp"
+#include "mit_motor_control.hpp"
 #include <functional>
 #include <rclcpp/serialization.hpp>
 #include <thread>
@@ -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<rclcpp::SerializedMessage> 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<rclcpp::SerializedMessage> 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<rclcpp::SerializedMessage> 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<rclcpp::SerializedMessage> 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<rclcpp::SerializedMessage> 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<rclcpp::SerializedMessage> 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<rclcpp::SerializedMessage> 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<rclcpp::SerializedMessage> msg, const std::string& topic_name, [[maybe_unused]] const std::string& topic_type) {
-  std::vector<autonomy::CanMessage> 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<rclcpp::SerializedMessage> 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<autonomy::CanMessage> can_messages = createCanMessages(topic_name, msg, can_id);
+  // sendCanMessages(can_messages, topic_name);
+}
+
+void CanNode::handleHandJointTopic(std::shared_ptr<rclcpp::SerializedMessage> msg, const std::string& topic_name) {}
+void CanNode::handleHandEeTopic(std::shared_ptr<rclcpp::SerializedMessage> msg, const std::string& topic_name) {}
+void CanNode::handleArmEeTopic(std::shared_ptr<rclcpp::SerializedMessage> msg, const std::string& topic_name) {}
+
+
+void CanNode::handleGenericTopic(std::shared_ptr<rclcpp::SerializedMessage> msg, const std::string& topic_name) {
+  // Original generic handling
+  std::vector<autonomy::CanMessage> can_messages = createCanMessages(topic_name, msg);
+  sendCanMessages(can_messages, topic_name);
+}
+
+// Helper method to reduce code duplication
+void CanNode::sendCanMessages(const std::vector<autonomy::CanMessage>& 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<rclcpp::SerializedMessage> 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<autonomy::CanMessage> CanNode::createCanMessages(const std::string& topic_name, std::shared_ptr<rclcpp::SerializedMessage> ros_msg) {
+std::vector<autonomy::CanMessage> CanNode::createCanMessages(const std::string& topic_name, std::shared_ptr<rclcpp::SerializedMessage> ros_msg, uint32_t can_id) {
   std::vector<autonomy::CanMessage> 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
 

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<uint16_t>(velocity * 1000.0);  // Scale for precision
+  uint16_t force_scaled = static_cast<uint16_t>(force * 1000.0);
+  uint16_t pos_scaled = static_cast<uint16_t>(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<std::string> hasher;
+  uint32_t profile_hash = static_cast<uint32_t>(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<uint32_t>(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");
+  }
+}