diff --git a/autonomy/interfacing/can/CMakeLists.txt b/autonomy/interfacing/can/CMakeLists.txt index 7ec9e9f..c4b5d95 100644 --- a/autonomy/interfacing/can/CMakeLists.txt +++ b/autonomy/interfacing/can/CMakeLists.txt @@ -15,18 +15,25 @@ 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(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. # 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 ) @@ -36,15 +43,22 @@ ament_target_dependencies(can_lib ament_index_cpp rclcpp std_msgs + common_msgs ) # 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 + yaml-cpp + ament_index_cpp::ament_index_cpp +) # 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) @@ -77,4 +91,14 @@ install(TARGETS test_controller_node DESTINATION lib/${PROJECT_NAME}) +install( + DIRECTORY config/ + DESTINATION share/can/config +) + +install( + FILES ../dbc/humanoid.dbc + DESTINATION share/can/config +) + ament_package() 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..d5fd648 --- /dev/null +++ b/autonomy/interfacing/can/config/hardware_mapping.yaml @@ -0,0 +1,169 @@ +left: + shoulder: + pitch: # up/down arm movement + can_id: 0x01 + lower_limit: -90 + 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 + can_id: 0x04 + lower_limit: -130 + 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 + can_id: 0x06 + lower_limit: -90 + upper_limit: 90 + direction: 1 + zero_offset: 0 + limit_range: true + + gripper: + open_close: + can_id: 0x07 + lower_limit: 0 # fully closed + upper_limit: 100 # fully open (%) + direction: 1 + zero_offset: 0 + limit_range: true + + # hand: + # pinky: + # mcp: + # can_id: 0x07 + # lower_limit: 0 + # 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 + # lower_limit: 0 + # 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 + # lower_limit: 0 + # 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 + # lower_limit: 0 + # 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 + # lower_limit: 0 + # 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 + # limit_range: true \ No newline at end of file diff --git a/autonomy/interfacing/can/config/params.yaml b/autonomy/interfacing/can/config/params.yaml index 77055fc..d4bc681 100644 --- a/autonomy/interfacing/can/config/params.yaml +++ b/autonomy/interfacing/can/config/params.yaml @@ -9,8 +9,17 @@ 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" + - "/motorCMD" + + # 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..f9b09a2 100644 --- a/autonomy/interfacing/can/include/can_node.hpp +++ b/autonomy/interfacing/can/include/can_node.hpp @@ -2,18 +2,28 @@ #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 +#include +#include +#include +#include +#include -struct TopicConfig { - // name of the topic, and the message type - std::string name; - std::string type; -}; +// 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" +#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: @@ -21,18 +31,34 @@ class CanNode : public rclcpp::Node { private: autonomy::CanCore can_; - std::vector topic_configs_; - std::vector> subscribers_; + 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 + _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 // Methods - void loadTopicConfigurations(); - void createSubscribers(); - std::string discoverTopicType(const std::string &topic_name); - void topicCallback(std::shared_ptr msg, - const std::string &topic_name, - const std::string &topic_type); + void createSubscribersPublishers(); + 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..8428db3 100644 --- a/autonomy/interfacing/can/package.xml +++ b/autonomy/interfacing/can/package.xml @@ -11,6 +11,10 @@ rclcpp 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 ccea857..56f1942 100644 --- a/autonomy/interfacing/can/src/can_node.cpp +++ b/autonomy/interfacing/can/src/can_node.cpp @@ -8,19 +8,37 @@ 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 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"); 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(); 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(); @@ -57,130 +75,53 @@ 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()); - } - } +void CanNode::createSubscribersPublishers() { + _subscribers.clear(); + _publishers.clear(); - if (topic_configs_.empty()) { - RCLCPP_WARN(this->get_logger(), - "No valid topics found in configuration - CAN node will not " - "subscribe to any topics"); - } + // Create subscribers + _subscribers["/armCMD"] = this->create_subscription( + "/armCMD", rclcpp::QoS(10), + std::bind(&CanNode::armCMDCallback, this, std::placeholders::_1)); - } catch (const std::exception &e) { - RCLCPP_ERROR(this->get_logger(), "Error loading topic configurations: %s", - e.what()); - } -} + _subscribers["/handCMD"] = this->create_subscription( + "/handCMD", rclcpp::QoS(10), + std::bind(&CanNode::handCMDCallback, this, std::placeholders::_1)); -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 + _subscribers["/gripperCMD"] = this->create_subscription( + "/gripperCMD", rclcpp::QoS(10), + std::bind(&CanNode::gripperCMDCallback, this, std::placeholders::_1)); - 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["/motorCMD"] = this->create_subscription( + "/motorCMD", rclcpp::QoS(10), + std::bind(&CanNode::motorCMDCallback, this, std::placeholders::_1)); - // 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]; - } - } - } - } + // Create publishers + _publishers["/encoder"] = this->create_publisher("/encoder", 10); +} - RCLCPP_ERROR(this->get_logger(), - "Timeout waiting for topic '%s' to become available", - topic_name.c_str()); - return ""; +// Subscriber callbacks +void CanNode::armCMDCallback(const common_msgs::msg::ArmPose::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received ArmPose command"); } -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 CanNode::handCMDCallback(common_msgs::msg::HandPose::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received HandPose command"); } -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); - } - } +void CanNode::gripperCMDCallback(const common_msgs::msg::GripperPose::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received GripperPose command: position=%.2f", + msg->position); +} - 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 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. @@ -233,93 +174,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/can/test/test_can.cpp b/autonomy/interfacing/can/test/test_can.cpp deleted file mode 100644 index e69de29..0000000 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 new file mode 100644 index 0000000..9f55550 --- /dev/null +++ b/autonomy/interfacing/dbc/humanoid.dbc @@ -0,0 +1,97 @@ +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_ + 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 + +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/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/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/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/autonomy/wato_msgs/common_msgs/msg/GripperPose.msg b/autonomy/wato_msgs/common_msgs/msg/GripperPose.msg new file mode 100644 index 0000000..18cfdaf --- /dev/null +++ b/autonomy/wato_msgs/common_msgs/msg/GripperPose.msg @@ -0,0 +1,6 @@ +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 diff --git a/docker/interfacing/interfacing.Dockerfile b/docker/interfacing/interfacing.Dockerfile index ad4d7b3..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 @@ -31,23 +31,38 @@ 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 \ + 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 -# 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/* +# # CAN dbc parser +WORKDIR /usr/local +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 -DCMAKE_BUILD_SHARED_LIBS=ON .. && \ + make -j && \ + 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