From 009406b38ec5cb5a0b8f45cee879a8bc1ffc6d6d Mon Sep 17 00:00:00 2001 From: Miekale Smith Date: Wed, 4 Mar 2026 19:15:05 -0500 Subject: [PATCH 1/7] initial messages --- autonomy/interfacing/can/CMakeLists.txt | 2 + autonomy/interfacing/dbc/humanoid.dbc | 107 ++++++++++++++++++++++ docker/interfacing/interfacing.Dockerfile | 20 ++++ 3 files changed, 129 insertions(+) create mode 100644 autonomy/interfacing/dbc/humanoid.dbc diff --git a/autonomy/interfacing/can/CMakeLists.txt b/autonomy/interfacing/can/CMakeLists.txt index 7ec9e9f..6c58f26 100644 --- a/autonomy/interfacing/can/CMakeLists.txt +++ b/autonomy/interfacing/can/CMakeLists.txt @@ -16,6 +16,8 @@ find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package find_package(std_msgs REQUIRED) # ROS2 message package +find_package(dbcppp REQUIRED) # DBC parser + # Compiles source files into a library # A library is not executed, instead other executables can link # against it to access defined methods and classes. diff --git a/autonomy/interfacing/dbc/humanoid.dbc b/autonomy/interfacing/dbc/humanoid.dbc new file mode 100644 index 0000000..34432f4 --- /dev/null +++ b/autonomy/interfacing/dbc/humanoid.dbc @@ -0,0 +1,107 @@ +VERSION "" + + +NS_ : + NS_DESC_ + CM_ + BA_DEF_ + BA_ + VAL_ + CAT_DEF_ + CAT_ + FILTER + BA_DEF_DEF_ + EV_DATA_ + ENVVAR_DATA_ + SGTYPE_ + SGTYPE_VAL_ + BA_DEF_SGTYPE_ + BA_SGTYPE_ + SIG_TYPE_REF_ + VAL_TABLE_ + SIG_GROUP_ + SIG_VALTYPE_ + SIGTYPE_VALTYPE_ + BO_TX_BU_ + BA_DEF_REL_ + BA_REL_ + BA_DEF_DEF_REL_ + BU_SG_REL_ + BU_EV_REL_ + BU_BO_REL_ + SG_MUL_VAL_ + +BS_: +BU_: Host Motor Fingers +BO_ 1 DutyCycleCmd: 4 Host + SG_ DutyCycle : 7|32@0- (1e-05,0) [-0.95|0.95] "" Motor + +BO_ 257 CurrentLoopCmd: 4 Host + SG_ IqCurrent : 7|32@0- (0.001,0) [-60|60] "A" Motor + +BO_ 513 CurrentBrakeCmd: 4 Host + SG_ BrakeCurrent : 7|32@0- (0.001,0) [0|60] "A" Motor + +BO_ 769 VelocityLoopCmd: 4 Host + SG_ VelocityERPM : 7|32@0- (1,0) [-100000|100000] "ERPM" Motor + +BO_ 1025 PositionLoopCmd: 4 Host + SG_ PositionDeg : 7|32@0- (0.0001,0) [-36000|36000] "deg" Vector__XXX + +BO_ 1281 SetOriginCmd: 1 Host + SG_ OriginMode : 7|8@0+ (1,0) [0|1] "" Motor + +BO_ 1537 PositionVelocityCmd: 8 Host + SG_ PosVelPosition : 7|32@0- (0.0001,0) [-36000|36000] "deg" Motor + SG_ PosVelSpeed : 39|16@0- (10,0) [-327680|327670] "ERPM" Motor + SG_ PosVelAccel : 55|16@0- (10,0) [0|327670] "ERPM/s2" Motor + +BO_ 2049 MITControlCmd: 8 Host + SG_ MIT_KP : 7|12@0+ (1,0) [0|4095] "" Motor + SG_ MIT_KD : 11|12@0+ (1,0) [0|4095] "" Motor + SG_ MIT_Position : 31|16@0+ (1,0) [0|65535] "rad" Motor + SG_ MIT_Velocity : 47|12@0+ (1,0) [0|4095] "rad/s" Motor + SG_ MIT_Torque : 51|12@0+ (1,0) [0|4095] "Nm" Motor + +BO_ 3841 MotorDisableCmd: 0 Host + +BO_ 4097 FeedbackConfigCmd: 8 Host + SG_ FbkCfgReserved : 7|48@0+ (1,0) [0|0] "" Motor + SG_ FbkCfgParam : 55|16@0+ (1,0) [0|65535] "" Motor + +BO_ 10497 ServoStatusFeedback: 8 Motor + SG_ FbkPosition : 7|16@0- (0.1,0) [-3200|3200] "deg" Host + SG_ FbkSpeed : 23|16@0- (10,0) [-320000|320000] "ERPM" Host + SG_ FbkCurrent : 39|16@0- (0.01,0) [-60|60] "A" Host + SG_ FbkTemperature : 55|8@0- (1,0) [-20|127] "degC" Host + SG_ FbkErrorCode : 63|8@0+ (1,0) [0|7] "" Host + +BO_ 10753 ExtPositionFeedback: 4 Motor + SG_ ExtPosition : 7|32@0- (0.01,0) [-2.14748e+07|2.14748e+07] "deg" Host + +BO_ 11265 ServoStartFeedback: 4 Motor + SG_ StartByte0 : 7|8@0+ (1,0) [0|255] "" Host + SG_ StartByte1 : 15|8@0+ (1,0) [0|255] "" Host + SG_ StartByte2 : 23|8@0+ (1,0) [0|255] "" Host + SG_ StartByte3 : 31|8@0+ (1,0) [0|255] "" Host + +BO_ 4096 FingerPosLoopCmd: 8 Fingers + SG_ MCP_x : 7|16@0+ (0.00549325,0) [0|360] "Degrees" Vector__XXX + SG_ MCP_y : 23|16@0+ (0.00549325,0) [0|360] "Degrees" Vector__XXX + SG_ PIP_x : 39|16@0+ (0.00549325,0) [0|360] "Degrees" Vector__XXX + SG_ DIP_x : 55|16@0+ (0.00549325,0) [0|360] "Degrees" Vector__XXX + +BA_DEF_ BO_ "GenMsgBackgroundColor" STRING ; +BA_DEF_ BO_ "GenMsgForegroundColor" STRING ; +BA_DEF_ BO_ "matchingcriteria" INT 0 0; +BA_DEF_ BO_ "filterlabeling" INT 0 0; +BA_ "GenMsgForegroundColor" BO_ 4096 "#000000"; +BA_DEF_DEF_ "GenMsgBackgroundColor" "#ffffff"; +BA_DEF_DEF_ "GenMsgForegroundColor" "#000000"; +BA_DEF_DEF_ "matchingcriteria" 0; +BA_DEF_DEF_ "filterlabeling" false; +CM_ BU_ Host "CubeMars motor controller host / upper computer"; +CM_ BU_ Motor "CubeMars AK-series actuator"; +CM_ BU_ Fingers "STM devices controlling fingers"; +VAL_ 1281 OriginMode 0 "Temporary Origin" 1 "Permanent Origin"; +VAL_ 10497 FbkErrorCode 0 "No Fault" 1 "Motor Over-Temperature" 2 "Over-Current" 3 "Over-Voltage" 4 "Under-Voltage" 5 "Encoder Fault" 6 "MOSFET Over-Temperature" 7 "Motor Lock-Up"; diff --git a/docker/interfacing/interfacing.Dockerfile b/docker/interfacing/interfacing.Dockerfile index ad4d7b3..3188508 100644 --- a/docker/interfacing/interfacing.Dockerfile +++ b/docker/interfacing/interfacing.Dockerfile @@ -31,9 +31,29 @@ RUN rosdep install \ ################################ Dependencies ################################ FROM ${BASE_IMAGE} AS dependencies +# Apt dependencies +RUN apt-get update && apt-get install -y \ + git \ + build-essential \ + cmake \ + libboost-all-dev \ + libxml2-dev \ + && rm -rf /var/lib/apt/lists/* + # Copy dependency list from source stage COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list +# CAN dbc parser +WORKDIR /usr/local +RUN git clone --depth 1 --recurse-submodules https://github.com/xR3b0rn/dbcppp.git + +# Build and install dbcppp +WORKDIR /usr/local/dbcppp +RUN mkdir build && cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release .. && \ + make -j && \ + make install + # Install dependencies + tools (update must be in same layer) RUN apt-get update && \ apt-get install -y --no-install-recommends \ From c3cfe5caeb8a371cfec2745128960994548f2efd Mon Sep 17 00:00:00 2001 From: Miekale Smith Date: Thu, 2 Apr 2026 14:24:54 -0400 Subject: [PATCH 2/7] interfacing refactor, dbcppp installed --- autonomy/interfacing/can/CMakeLists.txt | 13 +- autonomy/interfacing/can/README.md | 2 +- .../can/config/hardware_mapping.yaml | 147 +++++++++ autonomy/interfacing/can/config/params.yaml | 16 +- autonomy/interfacing/can/include/can_node.hpp | 17 +- autonomy/interfacing/can/src/can_node.cpp | 278 +++++------------- autonomy/interfacing/dbc/humanoid.dbc | 7 +- .../wato_msgs/common_msgs/msg/GripperPose.msg | 5 + .../wato_msgs/common_msgs/msg/encoder.msg | 2 + docker/interfacing/interfacing.Dockerfile | 25 +- modules/docker-compose.interfacing.yaml | 5 +- 11 files changed, 285 insertions(+), 232 deletions(-) create mode 100644 autonomy/interfacing/can/config/hardware_mapping.yaml create mode 100644 autonomy/wato_msgs/common_msgs/msg/GripperPose.msg create mode 100644 autonomy/wato_msgs/common_msgs/msg/encoder.msg diff --git a/autonomy/interfacing/can/CMakeLists.txt b/autonomy/interfacing/can/CMakeLists.txt index 6c58f26..92962c7 100644 --- a/autonomy/interfacing/can/CMakeLists.txt +++ b/autonomy/interfacing/can/CMakeLists.txt @@ -15,20 +15,21 @@ find_package(ament_index_cpp REQUIRED) # ROS tool to find shared directories of find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package find_package(std_msgs REQUIRED) # ROS2 message package - -find_package(dbcppp REQUIRED) # DBC parser +find_package(Boost REQUIRED) # Boost libraries # Compiles source files into a library # A library is not executed, instead other executables can link # against it to access defined methods and classes. # We build a library so that the methods defined can be used by # both the unit test and ROS2 node executables. + add_library(can_lib src/can_core.cpp ) # Indicate to compiler where to search for header files -target_include_directories(can_lib +target_include_directories(can_lib PRIVATE + /usr/local/include PUBLIC include ) @@ -42,11 +43,15 @@ ament_target_dependencies(can_lib # Link system libraries for CAN support # Note: Using system CAN libraries instead of libsocketcan for broader compatibility -target_link_libraries(can_lib) + +target_link_libraries(can_lib + /usr/local/lib/libdbcppp.so +) # Create the can_node executable add_executable(can_node src/can_node.cpp src/can_core.cpp) ament_target_dependencies(can_node rclcpp std_msgs) + # Link to the previously built libraries target_link_libraries(can_node can_lib) diff --git a/autonomy/interfacing/can/README.md b/autonomy/interfacing/can/README.md index 0a739fc..b1569af 100644 --- a/autonomy/interfacing/can/README.md +++ b/autonomy/interfacing/can/README.md @@ -1,6 +1,6 @@ ## CAN Interfacing Package Documentation -The documentation for the CAN interfacing package follows a standard WATOnomous package level scheme. As shown below: +The documentation for the CAN interfacing package follows a standard WATonomous package level scheme. As shown below: ``` can/ ├── CMakeLists.txt diff --git a/autonomy/interfacing/can/config/hardware_mapping.yaml b/autonomy/interfacing/can/config/hardware_mapping.yaml new file mode 100644 index 0000000..1df4cdb --- /dev/null +++ b/autonomy/interfacing/can/config/hardware_mapping.yaml @@ -0,0 +1,147 @@ +left: + shoulder: + pitch: # up/down arm movement + can_id: 0x01 + lower_limit: -90 + upper_limit: 90 + direction: 1 + zero_offset: 0 + roll: # rotation along arm axis + can_id: 0x02 + lower_limit: -90 + upper_limit: 90 + direction: 1 + zero_offset: 0 + yaw: # left/right arm movement + can_id: 0x03 + lower_limit: -90 + upper_limit: 90 + direction: 1 + zero_offset: 0 + + elbow: + pitch: # forearm up/down + can_id: 0x04 + lower_limit: -130 + upper_limit: 0 + direction: 1 + zero_offset: 0 + roll: # forearm rotation + can_id: 0x05 + lower_limit: -130 + upper_limit: 0 + direction: 1 + zero_offset: 0 + + wrist: + pitch: # wrist up/down + can_id: 0x06 + lower_limit: -90 + upper_limit: 90 + direction: 1 + zero_offset: 0 + + gripper: + open_close: + can_id: 0x07 + lower_limit: 0 # fully closed + upper_limit: 100 # fully open (%) + direction: 1 + zero_offset: 0 + + # hand: + # pinky: + # mcp: + # can_id: 0x07 + # lower_limit: 0 + # upper_limit: 90 + # direction: 1 + # zero_offset: 0 + # pip: + # can_id: 0x08 + # lower_limit: 0 + # upper_limit: 90 + # direction: 1 + # zero_offset: 0 + # dip: + # can_id: 0x09 + # lower_limit: 0 + # upper_limit: 90 + # direction: 1 + # zero_offset: 0 + # ring: + # mcp: + # can_id: 0x0A + # lower_limit: 0 + # upper_limit: 90 + # direction: 1 + # zero_offset: 0 + # pip: + # can_id: 0x0B + # lower_limit: 0 + # upper_limit: 90 + # direction: 1 + # zero_offset: 0 + # dip: + # can_id: 0x0C + # lower_limit: 0 + # upper_limit: 90 + # direction: 1 + # zero_offset: 0 + # middle: + # mcp: + # can_id: 0x0D + # lower_limit: 0 + # upper_limit: 90 + # direction: 1 + # zero_offset: 0 + # pip: + # can_id: 0x0E + # lower_limit: 0 + # upper_limit: 90 + # direction: 1 + # zero_offset: 0 + # dip: + # can_id: 0x0F + # lower_limit: 0 + # upper_limit: 90 + # direction: 1 + # zero_offset: 0 + # index: + # mcp: + # can_id: 0x10 + # lower_limit: 0 + # upper_limit: 90 + # direction: 1 + # zero_offset: 0 + # pip: + # can_id: 0x11 + # lower_limit: 0 + # upper_limit: 90 + # direction: 1 + # zero_offset: 0 + # dip: + # can_id: 0x12 + # lower_limit: 0 + # upper_limit: 90 + # direction: 1 + # zero_offset: 0 + # thumb: + # cmc: + # can_id: 0x13 + # lower_limit: 0 + # upper_limit: 60 + # direction: 1 + # zero_offset: 0 + # mcp: + # can_id: 0x14 + # lower_limit: 0 + # upper_limit: 70 + # direction: 1 + # zero_offset: 0 + # ip: + # can_id: 0x15 + # lower_limit: 0 + # upper_limit: 80 + # direction: 1 + # zero_offset: 0 \ No newline at end of file diff --git a/autonomy/interfacing/can/config/params.yaml b/autonomy/interfacing/can/config/params.yaml index 77055fc..34a621d 100644 --- a/autonomy/interfacing/can/config/params.yaml +++ b/autonomy/interfacing/can/config/params.yaml @@ -9,8 +9,16 @@ 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 + # Topics to subscribe + subscribe_topics: + - "/armCMD" + - "/handCMD" + - "/gripperCMD" + + # Publisher topics + publish_topics: + - "/arm_encoder" + - "/hand_encoder" + - "/gripper_encoder" \ 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 4f2cbc6..63ae5a3 100644 --- a/autonomy/interfacing/can/include/can_node.hpp +++ b/autonomy/interfacing/can/include/can_node.hpp @@ -21,15 +21,22 @@ class CanNode : public rclcpp::Node { private: autonomy::CanCore can_; - std::vector topic_configs_; - std::vector> subscribers_; + + + // Subscribers and publishers + std::unordered_map + _subscribers; // Map of topic name to its subscriber + + std::unordered_map + _publishers; // Map of topic name to its publisher + + rclcpp::TimerBase::SharedPtr receive_timer_; // Timer to periodically check for CAN messages // Methods - void loadTopicConfigurations(); - void createSubscribers(); - std::string discoverTopicType(const std::string &topic_name); + void createSubscribersPublishers(); + void topicCallback(std::shared_ptr msg, const std::string &topic_name, const std::string &topic_type); diff --git a/autonomy/interfacing/can/src/can_node.cpp b/autonomy/interfacing/can/src/can_node.cpp index ccea857..ccf38b1 100644 --- a/autonomy/interfacing/can/src/can_node.cpp +++ b/autonomy/interfacing/can/src/can_node.cpp @@ -5,6 +5,13 @@ #include #include +// Messages +#include +#include +#include +#include +#include + CanNode::CanNode() : Node("can_node"), can_(this->get_logger()) { RCLCPP_INFO(this->get_logger(), "CAN Node has been initialized"); @@ -13,8 +20,7 @@ CanNode::CanNode() : Node("can_node"), can_(this->get_logger()) { this->declare_parameter("device_path", "/dev/canable"); this->declare_parameter("bustype", "slcan"); this->declare_parameter("bitrate", 500000); - this->declare_parameter("receive_poll_interval_ms", - 10); // Parameter for polling interval + this->declare_parameter("receive_poll_interval_ms", 10); // Get parameter values std::string can_interface = this->get_parameter("can_interface").as_string(); @@ -57,128 +63,41 @@ CanNode::CanNode() : Node("can_node"), can_(this->get_logger()) { } // Load topic configurations and create subscribers - loadTopicConfigurations(); - createSubscribers(); + createSubscribersPublishers(); } -void CanNode::loadTopicConfigurations() { - try { - // Declare and get the topics parameter as a string array - this->declare_parameter("topics", - std::vector{"/test_controller"}); - auto topic_names = this->get_parameter("topics").as_string_array(); - - for (const auto &topic_name : topic_names) { - std::string topic_type = discoverTopicType( - topic_name); // Discover the message type for a given topic - - if (!topic_type.empty()) { - TopicConfig config; - config.name = topic_name; - config.type = topic_type; - topic_configs_.push_back(config); - - RCLCPP_INFO(this->get_logger(), "Loaded topic config: %s (%s)", - topic_name.c_str(), topic_type.c_str()); - } else { - RCLCPP_WARN(this->get_logger(), - "Could not discover message type for topic: %s", - topic_name.c_str()); - } - } - - if (topic_configs_.empty()) { - RCLCPP_WARN(this->get_logger(), - "No valid topics found in configuration - CAN node will not " - "subscribe to any topics"); - } - - } catch (const std::exception &e) { - RCLCPP_ERROR(this->get_logger(), "Error loading topic configurations: %s", - e.what()); - } -} +void CanNode::createSubscribersPublishers() { + _subscribers_.clear(); + _publishers_.clear(); -std::string CanNode::discoverTopicType(const std::string &topic_name) { - auto topic_names_and_types = - this->get_topic_names_and_types(); // get topic information from ROS graph + // Create subscribers + _subscribers_["/armCMD"] = this->create_generic_subscription( + "/armCMD", rclcpp::QoS(10), + std::bind(&armCMDCallback, this)); - for (const auto &topic_info : topic_names_and_types) { - if (topic_info.first == topic_name) { - if (!topic_info.second.empty()) { - return topic_info.second[0]; - } - } - } + _subscribers_["/handCMD"] = this->create_generic_subscription( + "/handCMD", rclcpp::QoS(10), + std::bind(&handCMDCallback, this)); - // If topic is not found, wait a bit and try again (topic might not be - // published yet) - RCLCPP_INFO(this->get_logger(), - "Topic '%s' not found, waiting for it to become available...", - topic_name.c_str()); - - // Wait up to 10 seconds for the topic to appear - for (int i = 0; i < 100; ++i) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - topic_names_and_types = this->get_topic_names_and_types(); - - for (const auto &topic_info : topic_names_and_types) { - if (topic_info.first == topic_name) { - if (!topic_info.second.empty()) { - RCLCPP_INFO(this->get_logger(), "Found topic '%s' with type '%s'", - topic_name.c_str(), topic_info.second[0].c_str()); - return topic_info.second[0]; - } - } - } - } + _subscribers_["/gripperCMD"] = this->create_generic_subscription( + "/gripperCMD", rclcpp::QoS(10), + std::bind(&gripperCMDCallback, this)); - RCLCPP_ERROR(this->get_logger(), - "Timeout waiting for topic '%s' to become available", - topic_name.c_str()); - return ""; + // Create publishers + _publishers_["/encoder"] = this->create_publisher("/encoder", 10); } -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); - }); - - 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 armCMDCallback(const common_msgs::ArmPose::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received ArmPose command: x=%.2f, y=%.2f, z=%.2f", + msg->x, msg->y, msg->z); } - -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 - int successful_sends = 0; - for (const auto &can_message : can_messages) { - if (can_.sendMessage(can_message)) { - successful_sends++; - } else { - RCLCPP_ERROR(this->get_logger(), - "Failed to send CAN message for topic '%s' (ID 0x%X)", - topic_name.c_str(), can_message.id); - } - } - - if (can_messages.size() > 1) { - RCLCPP_INFO(this->get_logger(), - "Successfully sent %d/%zu CAN frames for topic '%s'", - successful_sends, can_messages.size(), topic_name.c_str()); - } +void handCMDCallback(const common_msgs::HandPose::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received HandPose command: roll=%.2f, pitch=%.2f, yaw=%.2f", + msg->roll, msg->pitch, msg->yaw); +} +void gripperCMDCallback(const common_msgs::GripperPose::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received GripperPose command: width=%.2f", + msg->width); } void CanNode::receiveCanMessages() { @@ -233,93 +152,54 @@ CanNode::createCanMessages(const std::string &topic_name, const uint8_t *ros_msg_buffer = ros_msg->get_rcl_serialized_message().buffer; size_t ros_msg_size = ros_msg->size(); - // Determine max data payload per frame - // For Classic CAN, it's 8 bytes maximum - // But reserve 1 byte for sequence number if fragmentation is needed. - const size_t max_data_bytes_per_classic_frame = 8; - - size_t max_payload_per_frame = max_data_bytes_per_classic_frame; - size_t data_chunk_size = max_payload_per_frame; - bool needs_fragmentation = ros_msg_size > max_payload_per_frame; - - if (needs_fragmentation) { - // Reserve 1 byte for sequence number if fragmenting - data_chunk_size = max_payload_per_frame - 1; - if (data_chunk_size == 0 && - max_payload_per_frame > 0) { // Should not happen with CAN_MAX_DLEN >= 1 + RCLCPP_INFO(this->get_logger(), + "Message from topic '%s' (%zu bytes) is too large for a single " + "CAN frame (max %zu bytes). Fragmenting.", + topic_name.c_str(), ros_msg_size, max_payload_per_frame); + + size_t bytes_sent = 0; + uint8_t sequence_number = 0; + + while (bytes_sent < ros_msg_size) { + autonomy::CanMessage can_fragment; + can_fragment.id = can_id; // All fragments share the same ID for now + can_fragment.is_extended_id = is_extended; + can_fragment.is_remote_frame = is_rtr; + can_fragment.timestamp_us = + timestamp; // Could also update timestamp per fragment + + size_t current_fragment_payload_size = + std::min(data_chunk_size, ros_msg_size - bytes_sent); + + can_fragment.data.resize( + 1 + current_fragment_payload_size); // 1 byte for sequence number + can_fragment.data[0] = sequence_number; + std::memcpy(can_fragment.data.data() + 1, ros_msg_buffer + bytes_sent, + current_fragment_payload_size); + + messages_to_send.push_back(can_fragment); + + // This is just logging that fragment creation was successful + // RCLCPP_INFO(this->get_logger(), "Created fragment %u for topic '%s' (ID + // 0x%X), size %zu (payload %zu)", sequence_number, topic_name.c_str(), + // can_fragment.id, can_fragment.data.size(), + // current_fragment_payload_size); + + bytes_sent += current_fragment_payload_size; + sequence_number++; + + if (sequence_number == 0 && + bytes_sent < ros_msg_size) { // Rollover, too many fragments RCLCPP_ERROR(this->get_logger(), - "Calculated data_chunk_size is 0, this should not happen. " - "Max payload: %zu", - max_payload_per_frame); - return messages_to_send; // Return empty, indicates error - } - } - - if (!needs_fragmentation) { - autonomy::CanMessage can_msg; - can_msg.id = can_id; - can_msg.is_extended_id = is_extended; - can_msg.is_remote_frame = is_rtr; - can_msg.timestamp_us = timestamp; - - can_msg.data.resize(ros_msg_size); - std::memcpy(can_msg.data.data(), ros_msg_buffer, ros_msg_size); - - RCLCPP_DEBUG(this->get_logger(), - "Packaged %zu bytes from topic '%s' into single CAN frame " - "with ID 0x%X (Classic CAN)", - ros_msg_size, topic_name.c_str(), can_msg.id); - messages_to_send.push_back(can_msg); - } else { - RCLCPP_INFO(this->get_logger(), - "Message from topic '%s' (%zu bytes) is too large for a single " - "CAN frame (max %zu bytes). Fragmenting.", - topic_name.c_str(), ros_msg_size, max_payload_per_frame); - - size_t bytes_sent = 0; - uint8_t sequence_number = 0; - - while (bytes_sent < ros_msg_size) { - autonomy::CanMessage can_fragment; - can_fragment.id = can_id; // All fragments share the same ID for now - can_fragment.is_extended_id = is_extended; - can_fragment.is_remote_frame = is_rtr; - can_fragment.timestamp_us = - timestamp; // Could also update timestamp per fragment - - size_t current_fragment_payload_size = - std::min(data_chunk_size, ros_msg_size - bytes_sent); - - can_fragment.data.resize( - 1 + current_fragment_payload_size); // 1 byte for sequence number - can_fragment.data[0] = sequence_number; - std::memcpy(can_fragment.data.data() + 1, ros_msg_buffer + bytes_sent, - current_fragment_payload_size); - - messages_to_send.push_back(can_fragment); - - // This is just logging that fragment creation was successful - // RCLCPP_INFO(this->get_logger(), "Created fragment %u for topic '%s' (ID - // 0x%X), size %zu (payload %zu)", sequence_number, topic_name.c_str(), - // can_fragment.id, can_fragment.data.size(), - // current_fragment_payload_size); - - bytes_sent += current_fragment_payload_size; - sequence_number++; - - if (sequence_number == 0 && - bytes_sent < ros_msg_size) { // Rollover, too many fragments - RCLCPP_ERROR(this->get_logger(), - "Too many fragments for message from topic '%s'. Max 256 " - "fragments supported with 1-byte sequence.", - topic_name.c_str()); - messages_to_send.clear(); // Indicate error by returning no messages - return messages_to_send; - } + "Too many fragments for message from topic '%s'. Max 256 " + "fragments supported with 1-byte sequence.", + topic_name.c_str()); + messages_to_send.clear(); // Indicate error by returning no messages + return messages_to_send; } - // RCLCPP_INFO(this->get_logger(), "Fragmented message from topic '%s' into - // %zu frames.", topic_name.c_str(), messages_to_send.size()); } + // RCLCPP_INFO(this->get_logger(), "Fragmented message from topic '%s' into + // %zu frames.", topic_name.c_str(), messages_to_send.size()); return messages_to_send; } diff --git a/autonomy/interfacing/dbc/humanoid.dbc b/autonomy/interfacing/dbc/humanoid.dbc index 34432f4..bea34e8 100644 --- a/autonomy/interfacing/dbc/humanoid.dbc +++ b/autonomy/interfacing/dbc/humanoid.dbc @@ -17,7 +17,7 @@ NS_ : SGTYPE_VAL_ BA_DEF_SGTYPE_ BA_SGTYPE_ - SIG_TYPE_REF_ + SIG_TYPE_REF_hu VAL_TABLE_ SIG_GROUP_ SIG_VALTYPE_ @@ -91,11 +91,14 @@ BO_ 4096 FingerPosLoopCmd: 8 Fingers SG_ PIP_x : 39|16@0+ (0.00549325,0) [0|360] "Degrees" Vector__XXX SG_ DIP_x : 55|16@0+ (0.00549325,0) [0|360] "Degrees" Vector__XXX +BA_DEF_ BO_ "matchingcriteria" INT 0 0; +BA_DEF_ BO_ "filterlabeling" INT 0 0; BA_DEF_ BO_ "GenMsgBackgroundColor" STRING ; BA_DEF_ BO_ "GenMsgForegroundColor" STRING ; BA_DEF_ BO_ "matchingcriteria" INT 0 0; BA_DEF_ BO_ "filterlabeling" INT 0 0; -BA_ "GenMsgForegroundColor" BO_ 4096 "#000000"; +BA_DEF_DEF_ "matchingcriteria" 0; +BA_DEF_DEF_ "filterlabeling" 0; BA_DEF_DEF_ "GenMsgBackgroundColor" "#ffffff"; BA_DEF_DEF_ "GenMsgForegroundColor" "#000000"; BA_DEF_DEF_ "matchingcriteria" 0; diff --git a/autonomy/wato_msgs/common_msgs/msg/GripperPose.msg b/autonomy/wato_msgs/common_msgs/msg/GripperPose.msg new file mode 100644 index 0000000..e971966 --- /dev/null +++ b/autonomy/wato_msgs/common_msgs/msg/GripperPose.msg @@ -0,0 +1,5 @@ +std_msgs/Header header + +string name + +float32 position \ No newline at end of file diff --git a/autonomy/wato_msgs/common_msgs/msg/encoder.msg b/autonomy/wato_msgs/common_msgs/msg/encoder.msg new file mode 100644 index 0000000..2eda0e7 --- /dev/null +++ b/autonomy/wato_msgs/common_msgs/msg/encoder.msg @@ -0,0 +1,2 @@ +int8 motor_id +float32 value \ No newline at end of file diff --git a/docker/interfacing/interfacing.Dockerfile b/docker/interfacing/interfacing.Dockerfile index 3188508..6944665 100644 --- a/docker/interfacing/interfacing.Dockerfile +++ b/docker/interfacing/interfacing.Dockerfile @@ -6,7 +6,7 @@ FROM ${BASE_IMAGE} AS source WORKDIR ${AMENT_WS}/src # Copy source code -COPY autonomy/interfacing/can can +# COPY autonomy/interfacing/can can COPY autonomy/wato_msgs/sample_msgs sample_msgs # Install rosdep if not present, update package lists @@ -38,36 +38,31 @@ RUN apt-get update && apt-get install -y \ cmake \ libboost-all-dev \ libxml2-dev \ + can-utils \ + net-tools \ + iproute2 \ + $(cat /tmp/colcon_install_list) \ && rm -rf /var/lib/apt/lists/* # Copy dependency list from source stage COPY --from=source /tmp/colcon_install_list /tmp/colcon_install_list -# CAN dbc parser +# # CAN dbc parser WORKDIR /usr/local -RUN git clone --depth 1 --recurse-submodules https://github.com/xR3b0rn/dbcppp.git +RUN git clone --recurse-submodules https://github.com/xR3b0rn/dbcppp.git # Build and install dbcppp WORKDIR /usr/local/dbcppp RUN mkdir build && cd build && \ - cmake -DCMAKE_BUILD_TYPE=Release .. && \ + cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_BUILD_SHARED_LIBS=ON .. && \ make -j && \ - make install - -# Install dependencies + tools (update must be in same layer) -RUN apt-get update && \ - apt-get install -y --no-install-recommends \ - $(cat /tmp/colcon_install_list) \ - can-utils \ - net-tools \ - iproute2 && \ - rm -rf /var/lib/apt/lists/* + make install && \ + ldconfig # Copy source code into workspace WORKDIR ${AMENT_WS} COPY --from=source ${AMENT_WS}/src src - ################################ Build ################################ FROM dependencies AS build diff --git a/modules/docker-compose.interfacing.yaml b/modules/docker-compose.interfacing.yaml index 082cf3a..85f8805 100644 --- a/modules/docker-compose.interfacing.yaml +++ b/modules/docker-compose.interfacing.yaml @@ -10,8 +10,9 @@ services: - "${INTERFACING_IMAGE:?}:main" image: "${INTERFACING_IMAGE:?}:${TAG}" profiles: [deploy, develop] - command: /bin/bash -c "ros2 launch can can.launch.py" - privileged: false + # command: /bin/bash -c "ros2 launch can can.launch.py" + command: tail -F null + privileged: true devices: - /dev/canable:/dev/canable network_mode: host # Allow the container to access the host's network interfaces From 8f7760587b62e42c93b9da0ae08ba33c24b0377c Mon Sep 17 00:00:00 2001 From: Miekale Smith Date: Thu, 2 Apr 2026 15:36:07 -0400 Subject: [PATCH 3/7] adding left flag, adding motor command callback --- autonomy/interfacing/can/CMakeLists.txt | 9 ++++++++ autonomy/interfacing/can/config/params.yaml | 1 + autonomy/interfacing/can/include/can_node.hpp | 12 ++-------- autonomy/interfacing/can/launch/can.launch.py | 23 +++++-------------- autonomy/interfacing/can/package.xml | 2 ++ autonomy/interfacing/can/src/can_node.cpp | 17 ++++++++++++-- autonomy/interfacing/can/test/test_can.cpp | 0 .../wato_msgs/common_msgs/msg/ArmPose.msg | 2 ++ .../wato_msgs/common_msgs/msg/GripperPose.msg | 1 + .../wato_msgs/common_msgs/msg/HandPose.msg | 2 ++ .../wato_msgs/common_msgs/msg/JointState.msg | 2 +- .../wato_msgs/common_msgs/msg/MotorCMD.msg | 3 +++ 12 files changed, 44 insertions(+), 30 deletions(-) delete mode 100644 autonomy/interfacing/can/test/test_can.cpp create mode 100644 autonomy/wato_msgs/common_msgs/msg/MotorCMD.msg diff --git a/autonomy/interfacing/can/CMakeLists.txt b/autonomy/interfacing/can/CMakeLists.txt index 92962c7..a5d3106 100644 --- a/autonomy/interfacing/can/CMakeLists.txt +++ b/autonomy/interfacing/can/CMakeLists.txt @@ -16,6 +16,8 @@ find_package(ament_cmake REQUIRED) # ROS2 build tool find_package(rclcpp REQUIRED) # ROS2 C++ package find_package(std_msgs REQUIRED) # ROS2 message package find_package(Boost REQUIRED) # Boost libraries +find_package(yaml-cpp REQUIRED) +find_package(ament_index_cpp REQUIRED) # Compiles source files into a library # A library is not executed, instead other executables can link @@ -46,6 +48,8 @@ ament_target_dependencies(can_lib target_link_libraries(can_lib /usr/local/lib/libdbcppp.so + yaml-cpp + ament_index_cpp::ament_index_cpp ) # Create the can_node executable @@ -84,4 +88,9 @@ install(TARGETS test_controller_node DESTINATION lib/${PROJECT_NAME}) +install( + DIRECTORY config/ + DESTINATION share/can/config +) + ament_package() diff --git a/autonomy/interfacing/can/config/params.yaml b/autonomy/interfacing/can/config/params.yaml index 34a621d..d4bc681 100644 --- a/autonomy/interfacing/can/config/params.yaml +++ b/autonomy/interfacing/can/config/params.yaml @@ -16,6 +16,7 @@ can_node: - "/armCMD" - "/handCMD" - "/gripperCMD" + - "/motorCMD" # Publisher topics publish_topics: diff --git a/autonomy/interfacing/can/include/can_node.hpp b/autonomy/interfacing/can/include/can_node.hpp index 63ae5a3..62b57b4 100644 --- a/autonomy/interfacing/can/include/can_node.hpp +++ b/autonomy/interfacing/can/include/can_node.hpp @@ -8,12 +8,7 @@ #include #include #include - -struct TopicConfig { - // name of the topic, and the message type - std::string name; - std::string type; -}; +#include class CanNode : public rclcpp::Node { public: @@ -21,7 +16,7 @@ class CanNode : public rclcpp::Node { private: autonomy::CanCore can_; - + YAML::Node hardware_config; // Subscribers and publishers std::unordered_map @@ -37,9 +32,6 @@ class CanNode : public rclcpp::Node { // Methods void createSubscribersPublishers(); - void topicCallback(std::shared_ptr msg, - const std::string &topic_name, - const std::string &topic_type); void receiveCanMessages(); // Method to be called by the timer // Helper methods diff --git a/autonomy/interfacing/can/launch/can.launch.py b/autonomy/interfacing/can/launch/can.launch.py index f567065..32a685c 100644 --- a/autonomy/interfacing/can/launch/can.launch.py +++ b/autonomy/interfacing/can/launch/can.launch.py @@ -14,17 +14,10 @@ def generate_launch_description(): 'params.yaml' ) - # Declare launch arguments - can_interface_arg = DeclareLaunchArgument( - 'can_interface', - default_value='can0', - description='Name of the CAN interface to use (e.g., can0)' - ) - - publish_rate_arg = DeclareLaunchArgument( - 'publish_rate_hz', - default_value='50', - description='Rate in Hz at which to check for CAN messages' + config_file_arg = DeclareLaunchArgument( + 'config_file', + default_value=config_file, + description='Path to the CAN node configuration YAML file' ) # Create the CAN node @@ -32,10 +25,7 @@ def generate_launch_description(): package='can', executable='can_node', name='can_node', - parameters=[config_file, { - 'can_interface': LaunchConfiguration('can_interface'), - 'publish_rate_hz': LaunchConfiguration('publish_rate_hz') - }], + parameters=[LaunchConfiguration('config_file')], output='screen' ) @@ -55,8 +45,7 @@ def generate_launch_description(): # Return the launch description return LaunchDescription([ - can_interface_arg, - publish_rate_arg, + config_file_arg, can_node, test_controller_node # Comment out this line to disable test controller ]) diff --git a/autonomy/interfacing/can/package.xml b/autonomy/interfacing/can/package.xml index 3cd227d..73c58aa 100644 --- a/autonomy/interfacing/can/package.xml +++ b/autonomy/interfacing/can/package.xml @@ -11,6 +11,8 @@ rclcpp std_msgs + yaml-cpp + ament_index_cpp ament_lint_auto ament_lint_common diff --git a/autonomy/interfacing/can/src/can_node.cpp b/autonomy/interfacing/can/src/can_node.cpp index ccf38b1..ffcfdc3 100644 --- a/autonomy/interfacing/can/src/can_node.cpp +++ b/autonomy/interfacing/can/src/can_node.cpp @@ -11,10 +11,14 @@ #include #include #include +#include CanNode::CanNode() : Node("can_node"), can_(this->get_logger()) { RCLCPP_INFO(this->get_logger(), "CAN Node has been initialized"); + hardware_config = YAML::LoadFile(ament_index_cpp::get_package_share_directory("can") + + "/config/hardware_params.yaml"); + // Load parameters from config/params.yaml this->declare_parameter("can_interface", "can0"); this->declare_parameter("device_path", "/dev/canable"); @@ -27,6 +31,7 @@ CanNode::CanNode() : Node("can_node"), can_(this->get_logger()) { std::string device_path = this->get_parameter("device_path").as_string(); std::string bustype = this->get_parameter("bustype").as_string(); int bitrate = this->get_parameter("bitrate").as_int(); + long receive_poll_interval_ms = this->get_parameter("receive_poll_interval_ms").as_int(); @@ -83,6 +88,10 @@ void CanNode::createSubscribersPublishers() { "/gripperCMD", rclcpp::QoS(10), std::bind(&gripperCMDCallback, this)); + _subscribers_["/motorCMD"] = this->create_generic_subscription( + "/motorCMD", rclcpp::QoS(10), + std::bind(&motorCMDCallback, this)); + // Create publishers _publishers_["/encoder"] = this->create_publisher("/encoder", 10); } @@ -96,8 +105,12 @@ void handCMDCallback(const common_msgs::HandPose::SharedPtr msg) { msg->roll, msg->pitch, msg->yaw); } void gripperCMDCallback(const common_msgs::GripperPose::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received GripperPose command: width=%.2f", - msg->width); + RCLCPP_INFO(this->get_logger(), "Received GripperPose command: position=%.2f", + msg->position); +} + +void motorCMDCallback(const common_msgs::MotorCMD::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received MotorCMD motor=%d", msg->id); } void CanNode::receiveCanMessages() { diff --git a/autonomy/interfacing/can/test/test_can.cpp b/autonomy/interfacing/can/test/test_can.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/autonomy/wato_msgs/common_msgs/msg/ArmPose.msg b/autonomy/wato_msgs/common_msgs/msg/ArmPose.msg index 24a9bfd..a6a115a 100644 --- a/autonomy/wato_msgs/common_msgs/msg/ArmPose.msg +++ b/autonomy/wato_msgs/common_msgs/msg/ArmPose.msg @@ -2,6 +2,8 @@ std_msgs/Header header string name +bool is_left + common_msgs/JointState shoulder common_msgs/JointState elbow common_msgs/JointState wrist diff --git a/autonomy/wato_msgs/common_msgs/msg/GripperPose.msg b/autonomy/wato_msgs/common_msgs/msg/GripperPose.msg index e971966..18cfdaf 100644 --- a/autonomy/wato_msgs/common_msgs/msg/GripperPose.msg +++ b/autonomy/wato_msgs/common_msgs/msg/GripperPose.msg @@ -2,4 +2,5 @@ std_msgs/Header header string name +bool is_left float32 position \ No newline at end of file diff --git a/autonomy/wato_msgs/common_msgs/msg/HandPose.msg b/autonomy/wato_msgs/common_msgs/msg/HandPose.msg index 72d6921..fb8c3aa 100644 --- a/autonomy/wato_msgs/common_msgs/msg/HandPose.msg +++ b/autonomy/wato_msgs/common_msgs/msg/HandPose.msg @@ -2,6 +2,8 @@ std_msgs/Header header string name +bool is_left + # Thumb common_msgs/JointState thumb_cmc common_msgs/JointState thumb_mcp diff --git a/autonomy/wato_msgs/common_msgs/msg/JointState.msg b/autonomy/wato_msgs/common_msgs/msg/JointState.msg index 3d141ff..80c6b92 100644 --- a/autonomy/wato_msgs/common_msgs/msg/JointState.msg +++ b/autonomy/wato_msgs/common_msgs/msg/JointState.msg @@ -2,7 +2,7 @@ std_msgs/Header header string name -float64[] position +float64[] position float64[] velocity float64[] effort diff --git a/autonomy/wato_msgs/common_msgs/msg/MotorCMD.msg b/autonomy/wato_msgs/common_msgs/msg/MotorCMD.msg new file mode 100644 index 0000000..d8c473a --- /dev/null +++ b/autonomy/wato_msgs/common_msgs/msg/MotorCMD.msg @@ -0,0 +1,3 @@ +int8 motor_id + +common_msgs/JointState state \ No newline at end of file From ab5dda382a67df7d6718badebada52456db76c44 Mon Sep 17 00:00:00 2001 From: Miekale Smith Date: Thu, 2 Apr 2026 17:10:36 -0400 Subject: [PATCH 4/7] adding motorcmd, adding common_msgs, application builds --- autonomy/interfacing/can/CMakeLists.txt | 3 ++ autonomy/interfacing/can/include/can_node.hpp | 19 ++++++- autonomy/interfacing/can/package.xml | 2 + autonomy/interfacing/can/src/can_node.cpp | 52 ++++++++----------- autonomy/wato_msgs/common_msgs/CMakeLists.txt | 3 ++ autonomy/wato_msgs/common_msgs/README.md | 3 +- .../msg/{encoder.msg => Encoder.msg} | 0 .../msg/{MotorCMD.msg => MotorCmd.msg} | 0 8 files changed, 49 insertions(+), 33 deletions(-) rename autonomy/wato_msgs/common_msgs/msg/{encoder.msg => Encoder.msg} (100%) rename autonomy/wato_msgs/common_msgs/msg/{MotorCMD.msg => MotorCmd.msg} (100%) diff --git a/autonomy/interfacing/can/CMakeLists.txt b/autonomy/interfacing/can/CMakeLists.txt index a5d3106..843d593 100644 --- a/autonomy/interfacing/can/CMakeLists.txt +++ b/autonomy/interfacing/can/CMakeLists.txt @@ -19,6 +19,8 @@ find_package(Boost REQUIRED) # Boost libraries find_package(yaml-cpp REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(common_msgs REQUIRED) + # Compiles source files into a library # A library is not executed, instead other executables can link # against it to access defined methods and classes. @@ -41,6 +43,7 @@ ament_target_dependencies(can_lib ament_index_cpp rclcpp std_msgs + common_msgs ) # Link system libraries for CAN support diff --git a/autonomy/interfacing/can/include/can_node.hpp b/autonomy/interfacing/can/include/can_node.hpp index 62b57b4..f7f3c25 100644 --- a/autonomy/interfacing/can/include/can_node.hpp +++ b/autonomy/interfacing/can/include/can_node.hpp @@ -9,6 +9,15 @@ #include #include #include +#include + +// Messages +#include "common_msgs/msg/arm_pose.hpp" +#include "common_msgs/msg/hand_pose.hpp" +#include "common_msgs/msg/gripper_pose.hpp" +#include "common_msgs/msg/joint_state.hpp" +#include "common_msgs/msg/encoder.hpp" +#include "common_msgs/msg/motor_cmd.hpp" class CanNode : public rclcpp::Node { public: @@ -17,14 +26,20 @@ class CanNode : public rclcpp::Node { private: autonomy::CanCore can_; YAML::Node hardware_config; + static constexpr size_t max_payload_per_frame = 8; // CAN frame max bytes + static constexpr size_t data_chunk_size = 8; // Subscribers and publishers - std::unordered_map - _subscribers; // Map of topic name to its subscriber + std::unordered_map _subscribers; std::unordered_map _publishers; // Map of topic name to its publisher + // Callbacks + void armCMDCallback(const common_msgs::msg::ArmPose::SharedPtr msg); + void handCMDCallback(const common_msgs::msg::HandPose::SharedPtr msg); + void gripperCMDCallback(const common_msgs::msg::GripperPose::SharedPtr msg); + void motorCMDCallback(const common_msgs::msg::MotorCmd::SharedPtr msg); rclcpp::TimerBase::SharedPtr receive_timer_; // Timer to periodically check for CAN messages diff --git a/autonomy/interfacing/can/package.xml b/autonomy/interfacing/can/package.xml index 73c58aa..8428db3 100644 --- a/autonomy/interfacing/can/package.xml +++ b/autonomy/interfacing/can/package.xml @@ -13,6 +13,8 @@ std_msgs yaml-cpp ament_index_cpp + + common_msgs ament_lint_auto ament_lint_common diff --git a/autonomy/interfacing/can/src/can_node.cpp b/autonomy/interfacing/can/src/can_node.cpp index ffcfdc3..12acec3 100644 --- a/autonomy/interfacing/can/src/can_node.cpp +++ b/autonomy/interfacing/can/src/can_node.cpp @@ -5,14 +5,6 @@ #include #include -// Messages -#include -#include -#include -#include -#include -#include - CanNode::CanNode() : Node("can_node"), can_(this->get_logger()) { RCLCPP_INFO(this->get_logger(), "CAN Node has been initialized"); @@ -72,47 +64,49 @@ CanNode::CanNode() : Node("can_node"), can_(this->get_logger()) { } void CanNode::createSubscribersPublishers() { - _subscribers_.clear(); - _publishers_.clear(); + _subscribers.clear(); + _publishers.clear(); // Create subscribers - _subscribers_["/armCMD"] = this->create_generic_subscription( + _subscribers["/armCMD"] = this->create_subscription( "/armCMD", rclcpp::QoS(10), - std::bind(&armCMDCallback, this)); + std::bind(&CanNode::armCMDCallback, this, std::placeholders::_1)); - _subscribers_["/handCMD"] = this->create_generic_subscription( + _subscribers["/handCMD"] = this->create_subscription( "/handCMD", rclcpp::QoS(10), - std::bind(&handCMDCallback, this)); + std::bind(&CanNode::handCMDCallback, this, std::placeholders::_1)); - _subscribers_["/gripperCMD"] = this->create_generic_subscription( + _subscribers["/gripperCMD"] = this->create_subscription( "/gripperCMD", rclcpp::QoS(10), - std::bind(&gripperCMDCallback, this)); + std::bind(&CanNode::gripperCMDCallback, this, std::placeholders::_1)); - _subscribers_["/motorCMD"] = this->create_generic_subscription( - "/motorCMD", rclcpp::QoS(10), - std::bind(&motorCMDCallback, this)); + _subscribers["/motorCMD"] = this->create_subscription( + "/motorCMD", rclcpp::QoS(10), + std::bind(&CanNode::motorCMDCallback, this, std::placeholders::_1)); // Create publishers - _publishers_["/encoder"] = this->create_publisher("/encoder", 10); + _publishers["/encoder"] = this->create_publisher("/encoder", 10); } -void armCMDCallback(const common_msgs::ArmPose::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received ArmPose command: x=%.2f, y=%.2f, z=%.2f", - msg->x, msg->y, msg->z); +// Subscriber callbacks +void CanNode::armCMDCallback(const common_msgs::msg::ArmPose::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received ArmPose command"); } -void handCMDCallback(const common_msgs::HandPose::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received HandPose command: roll=%.2f, pitch=%.2f, yaw=%.2f", - msg->roll, msg->pitch, msg->yaw); + +void CanNode::handCMDCallback(common_msgs::msg::HandPose::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received HandPose command"); } -void gripperCMDCallback(const common_msgs::GripperPose::SharedPtr msg) { + +void CanNode::gripperCMDCallback(const common_msgs::msg::GripperPose::SharedPtr msg) { RCLCPP_INFO(this->get_logger(), "Received GripperPose command: position=%.2f", msg->position); } -void motorCMDCallback(const common_msgs::MotorCMD::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received MotorCMD motor=%d", msg->id); +void CanNode::motorCMDCallback(const common_msgs::msg::MotorCmd::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received MotorCMD motor=%d", msg->motor_id); } +// Can message handling void CanNode::receiveCanMessages() { autonomy::CanMessage received_msg; // Attempt to receive a message. CanCore::receiveMessage is non-blocking. diff --git a/autonomy/wato_msgs/common_msgs/CMakeLists.txt b/autonomy/wato_msgs/common_msgs/CMakeLists.txt index de77874..9a79a07 100644 --- a/autonomy/wato_msgs/common_msgs/CMakeLists.txt +++ b/autonomy/wato_msgs/common_msgs/CMakeLists.txt @@ -18,6 +18,9 @@ set(msg_files msg/ArmPose.msg msg/HandPose.msg msg/JointState.msg + msg/GripperPose.msg + msg/MotorCmd.msg + msg/Encoder.msg ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/autonomy/wato_msgs/common_msgs/README.md b/autonomy/wato_msgs/common_msgs/README.md index 73ea65d..d0e83eb 100644 --- a/autonomy/wato_msgs/common_msgs/README.md +++ b/autonomy/wato_msgs/common_msgs/README.md @@ -9,8 +9,7 @@ common_msgs/ ├── README.md └── msg/ ├── ArmPose.msg - ├── HandPose.msg - └── JointState.msg + ├── ... ``` ## Purpose diff --git a/autonomy/wato_msgs/common_msgs/msg/encoder.msg b/autonomy/wato_msgs/common_msgs/msg/Encoder.msg similarity index 100% rename from autonomy/wato_msgs/common_msgs/msg/encoder.msg rename to autonomy/wato_msgs/common_msgs/msg/Encoder.msg diff --git a/autonomy/wato_msgs/common_msgs/msg/MotorCMD.msg b/autonomy/wato_msgs/common_msgs/msg/MotorCmd.msg similarity index 100% rename from autonomy/wato_msgs/common_msgs/msg/MotorCMD.msg rename to autonomy/wato_msgs/common_msgs/msg/MotorCmd.msg From a85b7d431e4651339cc949b873f75643c288491e Mon Sep 17 00:00:00 2001 From: Miekale Smith Date: Thu, 2 Apr 2026 17:33:40 -0400 Subject: [PATCH 5/7] adding limiting bool --- .../can/config/hardware_mapping.yaml | 24 ++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/autonomy/interfacing/can/config/hardware_mapping.yaml b/autonomy/interfacing/can/config/hardware_mapping.yaml index 1df4cdb..d5fd648 100644 --- a/autonomy/interfacing/can/config/hardware_mapping.yaml +++ b/autonomy/interfacing/can/config/hardware_mapping.yaml @@ -6,18 +6,21 @@ left: upper_limit: 90 direction: 1 zero_offset: 0 + limit_range: true roll: # rotation along arm axis can_id: 0x02 lower_limit: -90 upper_limit: 90 direction: 1 zero_offset: 0 + limit_range: true yaw: # left/right arm movement can_id: 0x03 lower_limit: -90 upper_limit: 90 direction: 1 zero_offset: 0 + limit_range: true elbow: pitch: # forearm up/down @@ -26,12 +29,14 @@ left: upper_limit: 0 direction: 1 zero_offset: 0 + limit_range: true roll: # forearm rotation can_id: 0x05 lower_limit: -130 upper_limit: 0 direction: 1 zero_offset: 0 + limit_range: true wrist: pitch: # wrist up/down @@ -40,6 +45,7 @@ left: upper_limit: 90 direction: 1 zero_offset: 0 + limit_range: true gripper: open_close: @@ -48,6 +54,7 @@ left: upper_limit: 100 # fully open (%) direction: 1 zero_offset: 0 + limit_range: true # hand: # pinky: @@ -57,18 +64,21 @@ left: # upper_limit: 90 # direction: 1 # zero_offset: 0 + # limit_range: true # pip: # can_id: 0x08 # lower_limit: 0 # upper_limit: 90 # direction: 1 # zero_offset: 0 + # limit_range: true # dip: # can_id: 0x09 # lower_limit: 0 # upper_limit: 90 # direction: 1 # zero_offset: 0 + # limit_range: true # ring: # mcp: # can_id: 0x0A @@ -76,18 +86,21 @@ left: # upper_limit: 90 # direction: 1 # zero_offset: 0 + # limit_range: true # pip: # can_id: 0x0B # lower_limit: 0 # upper_limit: 90 # direction: 1 # zero_offset: 0 + # limit_range: true # dip: # can_id: 0x0C # lower_limit: 0 # upper_limit: 90 # direction: 1 # zero_offset: 0 + # limit_range: true # middle: # mcp: # can_id: 0x0D @@ -95,18 +108,21 @@ left: # upper_limit: 90 # direction: 1 # zero_offset: 0 + # limit_range: true # pip: # can_id: 0x0E # lower_limit: 0 # upper_limit: 90 # direction: 1 # zero_offset: 0 + # limit_range: true # dip: # can_id: 0x0F # lower_limit: 0 # upper_limit: 90 # direction: 1 # zero_offset: 0 + # limit_range: true # index: # mcp: # can_id: 0x10 @@ -114,18 +130,21 @@ left: # upper_limit: 90 # direction: 1 # zero_offset: 0 + # limit_range: true # pip: # can_id: 0x11 # lower_limit: 0 # upper_limit: 90 # direction: 1 # zero_offset: 0 + # limit_range: true # dip: # can_id: 0x12 # lower_limit: 0 # upper_limit: 90 # direction: 1 # zero_offset: 0 + # limit_range: true # thumb: # cmc: # can_id: 0x13 @@ -133,15 +152,18 @@ left: # upper_limit: 60 # direction: 1 # zero_offset: 0 + # limit_range: true # mcp: # can_id: 0x14 # lower_limit: 0 # upper_limit: 70 # direction: 1 # zero_offset: 0 + # limit_range: true # ip: # can_id: 0x15 # lower_limit: 0 # upper_limit: 80 # direction: 1 - # zero_offset: 0 \ No newline at end of file + # zero_offset: 0 + # limit_range: true \ No newline at end of file From 32ba84fcc758b0594cb3de2c7736b447580d2a93 Mon Sep 17 00:00:00 2001 From: Miekale Smith Date: Thu, 2 Apr 2026 18:21:34 -0400 Subject: [PATCH 6/7] adding decode file, updating dbc to support dbcppp --- autonomy/interfacing/can/include/can_node.hpp | 9 +- autonomy/interfacing/dbc/README.md | 20 + autonomy/interfacing/dbc/decode.c | 966 ++++++++++++++++++ autonomy/interfacing/dbc/humanoid.dbc | 13 - 4 files changed, 992 insertions(+), 16 deletions(-) create mode 100644 autonomy/interfacing/dbc/README.md create mode 100644 autonomy/interfacing/dbc/decode.c diff --git a/autonomy/interfacing/can/include/can_node.hpp b/autonomy/interfacing/can/include/can_node.hpp index f7f3c25..c0a4912 100644 --- a/autonomy/interfacing/can/include/can_node.hpp +++ b/autonomy/interfacing/can/include/can_node.hpp @@ -2,9 +2,8 @@ #define CAN_NODE_HPP #include "can_core.hpp" -#include "rclcpp/generic_subscription.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" + +// Libraries #include #include #include @@ -12,6 +11,10 @@ #include // Messages +#include "rclcpp/generic_subscription.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/string.hpp" #include "common_msgs/msg/arm_pose.hpp" #include "common_msgs/msg/hand_pose.hpp" #include "common_msgs/msg/gripper_pose.hpp" diff --git a/autonomy/interfacing/dbc/README.md b/autonomy/interfacing/dbc/README.md new file mode 100644 index 0000000..cfb5a93 --- /dev/null +++ b/autonomy/interfacing/dbc/README.md @@ -0,0 +1,20 @@ +# DBC Interface + +This folder contains DBC (CAN database) file handling and integration for the humanoid autonomy system. + +## Overview + +DBC files define CAN bus messages, signals, and communication protocols used in vehicle communication systems. + +## Contents + +- `humanoid.dbc` - message definitions +- `decode.c` - generated c code for embedded system + + + +## Usage + +generate via entering the docker container and then: +`dbcparser dbc2 --dbc=dbc/humanoid.dbc --format=C >> decode.c` + diff --git a/autonomy/interfacing/dbc/decode.c b/autonomy/interfacing/dbc/decode.c new file mode 100644 index 0000000..d083f72 --- /dev/null +++ b/autonomy/interfacing/dbc/decode.c @@ -0,0 +1,966 @@ +#ifndef DBCPPP_BYTE_ORDER_LITTLE_ENDIAN +#error "Please pass -DDBCPPP_BYTE_ORDER_LITTLE_ENDIAN=<1|0>" +#endif +#include +#define bswap_16(value) ((((value) & 0xff) << 8) | ((value) >> 8)) +#define bswap_32(value) \ + (((uint32_t)bswap_16((uint16_t)((value) & 0xffff)) << 16) | \ + (uint32_t)bswap_16((uint16_t)((value) >> 16))) +#define bswap_64(value) \ + (((uint64_t)bswap_32((uint32_t)((value) & 0xffffffff)) \ + << 32) | \ + (uint64_t)bswap_32((uint32_t)((value) >> 32))) +uint64_t dbcppp_native_to_big(uint64_t& v) +{ +#if DBCPPP_BYTE_ORDER_LITTLE_ENDIAN == 1 + return bswap_64(v); +#else + return v; +#endif +} +uint64_t dbcppp_native_to_little(uint64_t& v) +{ +#if DBCPPP_BYTE_ORDER_LITTLE_ENDIAN == 1 + return v; +#else + return bswap_64(v); +#endif +} +uint64_t dbcppp_decode_DutyCycleCmd_1_DutyCycle(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_DutyCycleCmd_1_DutyCycle(uint64_t value) +{ + return value * 1e-05 + 0; +} +uint64_t dbcppp_decode_CurrentLoopCmd_257_IqCurrent(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_CurrentLoopCmd_257_IqCurrent(uint64_t value) +{ + return value * 0.001 + 0; +} +uint64_t dbcppp_decode_CurrentBrakeCmd_513_BrakeCurrent(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_CurrentBrakeCmd_513_BrakeCurrent(uint64_t value) +{ + return value * 0.001 + 0; +} +uint64_t dbcppp_decode_VelocityLoopCmd_769_VelocityERPM(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_VelocityLoopCmd_769_VelocityERPM(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_PositionLoopCmd_1025_PositionDeg(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_PositionLoopCmd_1025_PositionDeg(uint64_t value) +{ + return value * 0.0001 + 0; +} +uint64_t dbcppp_decode_SetOriginCmd_1281_OriginMode(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 56ull; + data &= 255ull; + return data; +} +double dbcppp_rawToPhys_SetOriginCmd_1281_OriginMode(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_PositionVelocityCmd_1537_PosVelPosition(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_PositionVelocityCmd_1537_PosVelPosition(uint64_t value) +{ + return value * 0.0001 + 0; +} +uint64_t dbcppp_decode_PositionVelocityCmd_1537_PosVelSpeed(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 16ull; + data &= 65535ull; + if (data & 18446744073709518848ull) + { + data |= 18446744073709518848ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_PositionVelocityCmd_1537_PosVelSpeed(uint64_t value) +{ + return value * 10 + 0; +} +uint64_t dbcppp_decode_PositionVelocityCmd_1537_PosVelAccel(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 0ull; + data &= 65535ull; + if (data & 18446744073709518848ull) + { + data |= 18446744073709518848ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_PositionVelocityCmd_1537_PosVelAccel(uint64_t value) +{ + return value * 10 + 0; +} +uint64_t dbcppp_decode_MITControlCmd_2049_MIT_KP(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 52ull; + data &= 4095ull; + return data; +} +double dbcppp_rawToPhys_MITControlCmd_2049_MIT_KP(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_MITControlCmd_2049_MIT_KD(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 40ull; + data &= 4095ull; + return data; +} +double dbcppp_rawToPhys_MITControlCmd_2049_MIT_KD(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_MITControlCmd_2049_MIT_Position(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 24ull; + data &= 65535ull; + return data; +} +double dbcppp_rawToPhys_MITControlCmd_2049_MIT_Position(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_MITControlCmd_2049_MIT_Velocity(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 12ull; + data &= 4095ull; + return data; +} +double dbcppp_rawToPhys_MITControlCmd_2049_MIT_Velocity(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_MITControlCmd_2049_MIT_Torque(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 0ull; + data &= 4095ull; + return data; +} +double dbcppp_rawToPhys_MITControlCmd_2049_MIT_Torque(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_FeedbackConfigCmd_4097_FbkCfgReserved(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 16ull; + data &= 281474976710655ull; + return data; +} +double dbcppp_rawToPhys_FeedbackConfigCmd_4097_FbkCfgReserved(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_FeedbackConfigCmd_4097_FbkCfgParam(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 0ull; + data &= 65535ull; + return data; +} +double dbcppp_rawToPhys_FeedbackConfigCmd_4097_FbkCfgParam(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_ServoStatusFeedback_10497_FbkPosition(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 48ull; + data &= 65535ull; + if (data & 18446744073709518848ull) + { + data |= 18446744073709518848ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_ServoStatusFeedback_10497_FbkPosition(uint64_t value) +{ + return value * 0.1 + 0; +} +uint64_t dbcppp_decode_ServoStatusFeedback_10497_FbkSpeed(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 65535ull; + if (data & 18446744073709518848ull) + { + data |= 18446744073709518848ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_ServoStatusFeedback_10497_FbkSpeed(uint64_t value) +{ + return value * 10 + 0; +} +uint64_t dbcppp_decode_ServoStatusFeedback_10497_FbkCurrent(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 16ull; + data &= 65535ull; + if (data & 18446744073709518848ull) + { + data |= 18446744073709518848ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_ServoStatusFeedback_10497_FbkCurrent(uint64_t value) +{ + return value * 0.01 + 0; +} +uint64_t dbcppp_decode_ServoStatusFeedback_10497_FbkTemperature(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 8ull; + data &= 255ull; + if (data & 18446744073709551488ull) + { + data |= 18446744073709551488ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_ServoStatusFeedback_10497_FbkTemperature(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_ServoStatusFeedback_10497_FbkErrorCode(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 0ull; + data &= 255ull; + return data; +} +double dbcppp_rawToPhys_ServoStatusFeedback_10497_FbkErrorCode(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_ExtPositionFeedback_10753_ExtPosition(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_ExtPositionFeedback_10753_ExtPosition(uint64_t value) +{ + return value * 0.01 + 0; +} +uint64_t dbcppp_decode_ServoStartFeedback_11265_StartByte0(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 56ull; + data &= 255ull; + return data; +} +double dbcppp_rawToPhys_ServoStartFeedback_11265_StartByte0(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_ServoStartFeedback_11265_StartByte1(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 48ull; + data &= 255ull; + return data; +} +double dbcppp_rawToPhys_ServoStartFeedback_11265_StartByte1(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_ServoStartFeedback_11265_StartByte2(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 40ull; + data &= 255ull; + return data; +} +double dbcppp_rawToPhys_ServoStartFeedback_11265_StartByte2(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_ServoStartFeedback_11265_StartByte3(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 255ull; + return data; +} +double dbcppp_rawToPhys_ServoStartFeedback_11265_StartByte3(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_FingerPosLoopCmd_4096_MCP_x(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 48ull; + data &= 65535ull; + return data; +} +double dbcppp_rawToPhys_FingerPosLoopCmd_4096_MCP_x(uint64_t value) +{ + return value * 0.00549325 + 0; +} +uint64_t dbcppp_decode_FingerPosLoopCmd_4096_MCP_y(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 65535ull; + return data; +} +double dbcppp_rawToPhys_FingerPosLoopCmd_4096_MCP_y(uint64_t value) +{ + return value * 0.00549325 + 0; +} +uint64_t dbcppp_decode_FingerPosLoopCmd_4096_PIP_x(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 16ull; + data &= 65535ull; + return data; +} +double dbcppp_rawToPhys_FingerPosLoopCmd_4096_PIP_x(uint64_t value) +{ + return value * 0.00549325 + 0; +} +uint64_t dbcppp_decode_FingerPosLoopCmd_4096_DIP_x(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 0ull; + data &= 65535ull; + return data; +} +double dbcppp_rawToPhys_FingerPosLoopCmd_4096_DIP_x(uint64_t value) +{ + return value * 0.00549325 + 0; +} +#ifndef DBCPPP_BYTE_ORDER_LITTLE_ENDIAN +#error "Please pass -DDBCPPP_BYTE_ORDER_LITTLE_ENDIAN=<1|0>" +#endif +#include +#define bswap_16(value) ((((value) & 0xff) << 8) | ((value) >> 8)) +#define bswap_32(value) \ + (((uint32_t)bswap_16((uint16_t)((value) & 0xffff)) << 16) | \ + (uint32_t)bswap_16((uint16_t)((value) >> 16))) +#define bswap_64(value) \ + (((uint64_t)bswap_32((uint32_t)((value) & 0xffffffff)) \ + << 32) | \ + (uint64_t)bswap_32((uint32_t)((value) >> 32))) +uint64_t dbcppp_native_to_big(uint64_t& v) +{ +#if DBCPPP_BYTE_ORDER_LITTLE_ENDIAN == 1 + return bswap_64(v); +#else + return v; +#endif +} +uint64_t dbcppp_native_to_little(uint64_t& v) +{ +#if DBCPPP_BYTE_ORDER_LITTLE_ENDIAN == 1 + return v; +#else + return bswap_64(v); +#endif +} +uint64_t dbcppp_decode_DutyCycleCmd_1_DutyCycle(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_DutyCycleCmd_1_DutyCycle(uint64_t value) +{ + return value * 1e-05 + 0; +} +uint64_t dbcppp_decode_CurrentLoopCmd_257_IqCurrent(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_CurrentLoopCmd_257_IqCurrent(uint64_t value) +{ + return value * 0.001 + 0; +} +uint64_t dbcppp_decode_CurrentBrakeCmd_513_BrakeCurrent(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_CurrentBrakeCmd_513_BrakeCurrent(uint64_t value) +{ + return value * 0.001 + 0; +} +uint64_t dbcppp_decode_VelocityLoopCmd_769_VelocityERPM(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_VelocityLoopCmd_769_VelocityERPM(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_PositionLoopCmd_1025_PositionDeg(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_PositionLoopCmd_1025_PositionDeg(uint64_t value) +{ + return value * 0.0001 + 0; +} +uint64_t dbcppp_decode_SetOriginCmd_1281_OriginMode(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 56ull; + data &= 255ull; + return data; +} +double dbcppp_rawToPhys_SetOriginCmd_1281_OriginMode(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_PositionVelocityCmd_1537_PosVelPosition(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_PositionVelocityCmd_1537_PosVelPosition(uint64_t value) +{ + return value * 0.0001 + 0; +} +uint64_t dbcppp_decode_PositionVelocityCmd_1537_PosVelSpeed(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 16ull; + data &= 65535ull; + if (data & 18446744073709518848ull) + { + data |= 18446744073709518848ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_PositionVelocityCmd_1537_PosVelSpeed(uint64_t value) +{ + return value * 10 + 0; +} +uint64_t dbcppp_decode_PositionVelocityCmd_1537_PosVelAccel(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 0ull; + data &= 65535ull; + if (data & 18446744073709518848ull) + { + data |= 18446744073709518848ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_PositionVelocityCmd_1537_PosVelAccel(uint64_t value) +{ + return value * 10 + 0; +} +uint64_t dbcppp_decode_MITControlCmd_2049_MIT_KP(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 52ull; + data &= 4095ull; + return data; +} +double dbcppp_rawToPhys_MITControlCmd_2049_MIT_KP(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_MITControlCmd_2049_MIT_KD(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 40ull; + data &= 4095ull; + return data; +} +double dbcppp_rawToPhys_MITControlCmd_2049_MIT_KD(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_MITControlCmd_2049_MIT_Position(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 24ull; + data &= 65535ull; + return data; +} +double dbcppp_rawToPhys_MITControlCmd_2049_MIT_Position(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_MITControlCmd_2049_MIT_Velocity(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 12ull; + data &= 4095ull; + return data; +} +double dbcppp_rawToPhys_MITControlCmd_2049_MIT_Velocity(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_MITControlCmd_2049_MIT_Torque(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 0ull; + data &= 4095ull; + return data; +} +double dbcppp_rawToPhys_MITControlCmd_2049_MIT_Torque(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_FeedbackConfigCmd_4097_FbkCfgReserved(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 16ull; + data &= 281474976710655ull; + return data; +} +double dbcppp_rawToPhys_FeedbackConfigCmd_4097_FbkCfgReserved(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_FeedbackConfigCmd_4097_FbkCfgParam(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 0ull; + data &= 65535ull; + return data; +} +double dbcppp_rawToPhys_FeedbackConfigCmd_4097_FbkCfgParam(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_ServoStatusFeedback_10497_FbkPosition(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 48ull; + data &= 65535ull; + if (data & 18446744073709518848ull) + { + data |= 18446744073709518848ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_ServoStatusFeedback_10497_FbkPosition(uint64_t value) +{ + return value * 0.1 + 0; +} +uint64_t dbcppp_decode_ServoStatusFeedback_10497_FbkSpeed(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 65535ull; + if (data & 18446744073709518848ull) + { + data |= 18446744073709518848ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_ServoStatusFeedback_10497_FbkSpeed(uint64_t value) +{ + return value * 10 + 0; +} +uint64_t dbcppp_decode_ServoStatusFeedback_10497_FbkCurrent(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 16ull; + data &= 65535ull; + if (data & 18446744073709518848ull) + { + data |= 18446744073709518848ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_ServoStatusFeedback_10497_FbkCurrent(uint64_t value) +{ + return value * 0.01 + 0; +} +uint64_t dbcppp_decode_ServoStatusFeedback_10497_FbkTemperature(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 8ull; + data &= 255ull; + if (data & 18446744073709551488ull) + { + data |= 18446744073709551488ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_ServoStatusFeedback_10497_FbkTemperature(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_ServoStatusFeedback_10497_FbkErrorCode(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 0ull; + data &= 255ull; + return data; +} +double dbcppp_rawToPhys_ServoStatusFeedback_10497_FbkErrorCode(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_ExtPositionFeedback_10753_ExtPosition(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 4294967295ull; + if (data & 18446744071562067968ull) + { + data |= 18446744071562067968ull; + } + return data; + return data; +} +double dbcppp_rawToPhys_ExtPositionFeedback_10753_ExtPosition(uint64_t value) +{ + return value * 0.01 + 0; +} +uint64_t dbcppp_decode_ServoStartFeedback_11265_StartByte0(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 56ull; + data &= 255ull; + return data; +} +double dbcppp_rawToPhys_ServoStartFeedback_11265_StartByte0(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_ServoStartFeedback_11265_StartByte1(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 48ull; + data &= 255ull; + return data; +} +double dbcppp_rawToPhys_ServoStartFeedback_11265_StartByte1(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_ServoStartFeedback_11265_StartByte2(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 40ull; + data &= 255ull; + return data; +} +double dbcppp_rawToPhys_ServoStartFeedback_11265_StartByte2(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_ServoStartFeedback_11265_StartByte3(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 255ull; + return data; +} +double dbcppp_rawToPhys_ServoStartFeedback_11265_StartByte3(uint64_t value) +{ + return value * 1 + 0; +} +uint64_t dbcppp_decode_FingerPosLoopCmd_4096_MCP_x(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 48ull; + data &= 65535ull; + return data; +} +double dbcppp_rawToPhys_FingerPosLoopCmd_4096_MCP_x(uint64_t value) +{ + return value * 0.00549325 + 0; +} +uint64_t dbcppp_decode_FingerPosLoopCmd_4096_MCP_y(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 32ull; + data &= 65535ull; + return data; +} +double dbcppp_rawToPhys_FingerPosLoopCmd_4096_MCP_y(uint64_t value) +{ + return value * 0.00549325 + 0; +} +uint64_t dbcppp_decode_FingerPosLoopCmd_4096_PIP_x(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 16ull; + data &= 65535ull; + return data; +} +double dbcppp_rawToPhys_FingerPosLoopCmd_4096_PIP_x(uint64_t value) +{ + return value * 0.00549325 + 0; +} +uint64_t dbcppp_decode_FingerPosLoopCmd_4096_DIP_x(const void* nbytes) +{ + uint64_t data; + data = *reinterpret_cast(nbytes); + data = dbcppp_native_to_big(data); + data >>= 0ull; + data &= 65535ull; + return data; +} +double dbcppp_rawToPhys_FingerPosLoopCmd_4096_DIP_x(uint64_t value) +{ + return value * 0.00549325 + 0; +} diff --git a/autonomy/interfacing/dbc/humanoid.dbc b/autonomy/interfacing/dbc/humanoid.dbc index bea34e8..9f55550 100644 --- a/autonomy/interfacing/dbc/humanoid.dbc +++ b/autonomy/interfacing/dbc/humanoid.dbc @@ -17,7 +17,6 @@ NS_ : SGTYPE_VAL_ BA_DEF_SGTYPE_ BA_SGTYPE_ - SIG_TYPE_REF_hu VAL_TABLE_ SIG_GROUP_ SIG_VALTYPE_ @@ -91,18 +90,6 @@ BO_ 4096 FingerPosLoopCmd: 8 Fingers SG_ PIP_x : 39|16@0+ (0.00549325,0) [0|360] "Degrees" Vector__XXX SG_ DIP_x : 55|16@0+ (0.00549325,0) [0|360] "Degrees" Vector__XXX -BA_DEF_ BO_ "matchingcriteria" INT 0 0; -BA_DEF_ BO_ "filterlabeling" INT 0 0; -BA_DEF_ BO_ "GenMsgBackgroundColor" STRING ; -BA_DEF_ BO_ "GenMsgForegroundColor" STRING ; -BA_DEF_ BO_ "matchingcriteria" INT 0 0; -BA_DEF_ BO_ "filterlabeling" INT 0 0; -BA_DEF_DEF_ "matchingcriteria" 0; -BA_DEF_DEF_ "filterlabeling" 0; -BA_DEF_DEF_ "GenMsgBackgroundColor" "#ffffff"; -BA_DEF_DEF_ "GenMsgForegroundColor" "#000000"; -BA_DEF_DEF_ "matchingcriteria" 0; -BA_DEF_DEF_ "filterlabeling" false; CM_ BU_ Host "CubeMars motor controller host / upper computer"; CM_ BU_ Motor "CubeMars AK-series actuator"; CM_ BU_ Fingers "STM devices controlling fingers"; From debec396c1298b553b1e4b28d66b69289f798200 Mon Sep 17 00:00:00 2001 From: Miekale Smith Date: Thu, 2 Apr 2026 18:38:57 -0400 Subject: [PATCH 7/7] installed dbc --- autonomy/interfacing/can/CMakeLists.txt | 5 +++++ autonomy/interfacing/can/include/can_node.hpp | 11 ++++++++++- autonomy/interfacing/can/src/can_node.cpp | 15 +++++++++++++++ 3 files changed, 30 insertions(+), 1 deletion(-) diff --git a/autonomy/interfacing/can/CMakeLists.txt b/autonomy/interfacing/can/CMakeLists.txt index 843d593..c4b5d95 100644 --- a/autonomy/interfacing/can/CMakeLists.txt +++ b/autonomy/interfacing/can/CMakeLists.txt @@ -96,4 +96,9 @@ install( DESTINATION share/can/config ) +install( + FILES ../dbc/humanoid.dbc + DESTINATION share/can/config +) + ament_package() diff --git a/autonomy/interfacing/can/include/can_node.hpp b/autonomy/interfacing/can/include/can_node.hpp index c0a4912..f9b09a2 100644 --- a/autonomy/interfacing/can/include/can_node.hpp +++ b/autonomy/interfacing/can/include/can_node.hpp @@ -7,7 +7,10 @@ #include #include #include +#include +#include #include +#include #include // Messages @@ -29,11 +32,17 @@ class CanNode : public rclcpp::Node { private: autonomy::CanCore can_; YAML::Node hardware_config; + + // Map of CAN message ID to its DBC definition for decoding + std::unordered_map can_messages; + std::unique_ptr dbc_net; + static constexpr size_t max_payload_per_frame = 8; // CAN frame max bytes static constexpr size_t data_chunk_size = 8; // Subscribers and publishers - std::unordered_map _subscribers; + std::unordered_map + _subscribers; std::unordered_map _publishers; // Map of topic name to its publisher diff --git a/autonomy/interfacing/can/src/can_node.cpp b/autonomy/interfacing/can/src/can_node.cpp index 12acec3..56f1942 100644 --- a/autonomy/interfacing/can/src/can_node.cpp +++ b/autonomy/interfacing/can/src/can_node.cpp @@ -11,6 +11,21 @@ CanNode::CanNode() : Node("can_node"), can_(this->get_logger()) { hardware_config = YAML::LoadFile(ament_index_cpp::get_package_share_directory("can") + "/config/hardware_params.yaml"); + // Load DBC file + { + std::ifstream idbc(ament_index_cpp::get_package_share_directory("can") + "/config/humanoid.dbc"); + dbc_net = dbcppp::INetwork::LoadDBCFromIs(idbc); + if (!dbc_net) { + RCLCPP_ERROR(this->get_logger(), "Failed to load DBC file"); + } else { + RCLCPP_INFO(this->get_logger(), "DBC file loaded successfully"); + // Build the message ID to IMessage* map for quick lookup during decoding + for (const auto& msg : dbc_net->Messages()) { + can_messages.insert(std::make_pair(msg.Id(), &msg)); + } + } + } + // Load parameters from config/params.yaml this->declare_parameter("can_interface", "can0"); this->declare_parameter("device_path", "/dev/canable");