Integrating DBC file for interfacing

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()

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

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

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"         
+    # Topics to subscribe
+    subscribe_topics:
+      - "/armCMD"
+      - "/handCMD"
+      - "/gripperCMD"
+      - "/motorCMD"
+
+    # Publisher topics
+    publish_topics:
+      - "/arm_encoder"
+      - "/hand_encoder"
+      - "/gripper_encoder"

autonomy/interfacing/can/include/can_node.hpp

@@ -2,37 +2,63 @@
 #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 <memory>
 #include <string>
 #include <vector>
+#include <fstream>
+#include <unordered_map>
+#include <yaml-cpp/yaml.h>
+#include <dbcppp/Network.h>
+#include <ament_index_cpp/get_package_share_directory.hpp>
 
-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:
   CanNode();
 
 private:
   autonomy::CanCore can_;
-  std::vector<TopicConfig> topic_configs_;
-  std::vector<std::shared_ptr<rclcpp::GenericSubscription>> subscribers_;
+  YAML::Node hardware_config;
+
+  // Map of CAN message ID to its DBC definition for decoding
+  std::unordered_map<uint64_t, const dbcppp::IMessage*> can_messages;
+  std::unique_ptr<dbcppp::INetwork> 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<std::string, rclcpp::SubscriptionBase::SharedPtr> 
+      _subscribers;
+
+  std::unordered_map<std::string, rclcpp::PublisherBase::SharedPtr>
+      _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<rclcpp::SerializedMessage> msg,
-                     const std::string &topic_name,
-                     const std::string &topic_type);
+  void createSubscribersPublishers();
+
   void receiveCanMessages(); // Method to be called by the timer
 
   // Helper methods

autonomy/interfacing/can/launch/can.launch.py

@@ -14,28 +14,18 @@ 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
     can_node = Node(
         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
     ])