update basic can protocol structure:

control_ws/src/stanley/include/stanley/canprotocol/can_protocol.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+#include "utils/car_struct.h"
+#include "utils/msg_structs.h"
+
+#include "dbcppp/include/dbcppp/Network.h"
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
+#include <unistd.h>
+#include <iostream>
+#include <fstream>
+#include <unordered_map>
+#include <memory>
+#include <chrono>
+
+class CANReceiver {
+public:
+    CANReceiver(const std::string& dbc_file, const std::string& can_interface);
+    ~CANReceiver();
+    void receiveMessages();
+
+private:
+    void printMessage(const dbcppp::IMessage& msg, const can_frame& frame);
+
+    std::unique_ptr<dbcppp::INetwork> net;
+    std::unordered_map<uint64_t, const dbcppp::IMessage*> messages;
+    int sock;
+    uint64_t last_received;
+    std::chrono::time_point<std::chrono::steady_clock> last_received_time;
+    std::chrono::seconds message_timeout;
+};

control_ws/src/stanley/include/stanley/control.hpp

@@ -5,6 +5,8 @@
 
 #include "controller/stanley.hpp"
 
+#include "canprotocol/can_protocol.hpp"
+
 #include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "nav_msgs/msg/odometry.hpp"
@@ -25,6 +27,8 @@ class Control : public rclcpp::Node {
 private:
     Stanley controller;
 
+    std::unique_ptr<CANReceiver> can_receiver;
+
     std::vector<Path> refPoses;
     Pose currPose;
 

control_ws/src/stanley/src/canprotocol/can_protocol.cpp

@@ -0,0 +1,65 @@
+// CANReceiver.cpp
+#include "CANReceiver.hpp"
+
+CANReceiver::CANReceiver(const std::string& dbc_file, const std::string& can_interface)
+    : last_received(0), message_timeout(std::chrono::seconds(2)) {
+    std::ifstream idbc(dbc_file);
+    net = dbcppp::INetwork::LoadDBCFromIs(idbc);
+    if (!net) {
+        throw std::runtime_error("Failed to parse DBC file.");
+    }
+
+    for (const auto& msg : net->Messages()) {
+        messages[msg.Id()] = &msg;
+    }
+
+    sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+    if (sock < 0) {
+        throw std::runtime_error("Error while opening socket.");
+    }
+
+    struct sockaddr_can addr;
+    struct ifreq ifr;
+    strcpy(ifr.ifr_name, can_interface.c_str());
+    ioctl(sock, SIOCGIFINDEX, &ifr);
+
+    addr.can_family = AF_CAN;
+    addr.can_ifindex = ifr.ifr_ifindex;
+    bind(sock, (struct sockaddr *)&addr, sizeof(addr));
+}
+
+CANReceiver::~CANReceiver() {
+    close(sock);
+}
+
+void CANReceiver::receiveMessages() {
+    struct can_frame frame;
+    int nbytes = read(sock, &frame, sizeof(struct can_frame));
+    if (nbytes > 0) {
+        auto current_time = std::chrono::steady_clock::now();
+        auto iter = messages.find(frame.can_id);
+        if (iter != messages.end()) {
+            const auto& msg = *iter->second;
+            if (msg.Id() == 254 || (msg.Id() == 255 && (last_received != 254 || std::chrono::duration_cast<std::chrono::seconds>(current_time - last_received_time) >= message_timeout))) {
+                printMessage(msg, frame);
+                last_received = msg.Id();
+                last_received_time = current_time;
+            }
+        }
+    }
+
+    // Check timeout for switching back to SpeedMessage1
+    if (last_received == 254) {
+        auto current_time = std::chrono::steady_clock::now();
+        if (std::chrono::duration_cast<std::chrono::seconds>(current_time - last_received_time) >= message_timeout) {
+            last_received = 0; // Allow SpeedMessage1 to be printed again
+        }
+    }
+}
+
+void CANReceiver::printMessage(const dbcppp::IMessage& msg, const can_frame& frame) {
+    std::cout << "Received Message: " << msg.Name() << "\n";
+    for (const auto& sig : msg.Signals()) {
+        std::cout << "\t" << sig.Name() << " = " << sig.RawToPhys(sig.Decode(frame.data)) << " " << sig.Unit() << "\n";
+    }
+}

control_ws/src/stanley/src/control.cpp

@@ -6,6 +6,9 @@ Control::Control() : rclcpp::Node("vehicle_control") {
 
     this->controller = Stanley(k, ks);
 
+    // Initialize CAN receiver
+    this->can_receiver = std::make_unique<CANReceiver>("../example.dbc", "vcan0");
+
     // Subscribe
     path_subscription_ = this->create_subscription<nav_msgs::msg::Path>(
         "/pathplanner/path", 10, std::bind(&Control::path_callback, this,  std::placeholders::_1));
@@ -80,6 +83,8 @@ void Control::publisher_timer_callback() {
     }
 
     this->publish_drive(this->speedCommand, this->steerCommand);
+
+    this->can_receiver->receiveMessages();
 }
 
 void Control::publish_drive(float speed, float steer) {