#9 update CAN Sender for speed and angle

SEA-ME-COSS · May 30, 2024 · 9a46790 · 9a46790
1 parent a3844f2
commit 9a46790
Show file tree

Hide file tree

Showing 3 changed files with 74 additions and 25 deletions.
diff --git a/control_ws/src/stanley/include/stanley/canprotocol/can_protocol.hpp b/control_ws/src/stanley/include/stanley/canprotocol/can_protocol.hpp
@@ -11,18 +11,22 @@
 #include <net/if.h>
 #include <unistd.h>
 
+#include <cstdlib> 
+
 #include "dbcppp/Network.h"
 
 class CANSender {
 public:
     CANSender(const std::string& dbc_file, const std::string& can_interface);
     ~CANSender();
-    void sendSpeedMessage(double speed);
+
+    void sendSpeedMessage(double speed, uint32_t target_message_id);
+    void sendSteerMessage(double steer, uint32_t target_message_id);
 
 private:
     int sock;
-    uint64_t target_message_id;
-    double speed_value;
     std::unique_ptr<dbcppp::INetwork> net;
-    std::unordered_map<uint64_t, const dbcppp::IMessage*> messages;
+    std::unordered_map<uint32_t, const dbcppp::IMessage*> messages;
+
+    void setupVcanInterface(const std::string& can_interface);
 };
diff --git a/control_ws/src/stanley/src/canprotocol/can_protocol.cpp b/control_ws/src/stanley/src/canprotocol/can_protocol.cpp
@@ -1,6 +1,8 @@
 #include "canprotocol/can_protocol.hpp"
 
 CANSender::CANSender(const std::string& dbc_file, const std::string& can_interface) {
+    setupVcanInterface(can_interface);
+
     std::ifstream idbc(dbc_file);
     this->net = dbcppp::INetwork::LoadDBCFromIs(idbc);
     if (!this->net) {
@@ -26,38 +28,80 @@ CANSender::CANSender(const std::string& dbc_file, const std::string& can_interfa
     addr.can_ifindex = ifr.ifr_ifindex;
     bind(this->sock, (struct sockaddr *)&addr, sizeof(addr));
 
-    this->target_message_id = 255;
+    std::cout << "CAN init Success!" << std::endl;
 }
 
-CANSender::~CANSender() {
-    close(sock);
-}
+CANSender::~CANSender() {}
 
-void CANSender::sendSpeedMessage(double speed) {
-    std::unordered_map<uint64_t, const dbcppp::IMessage*>::iterator iter = messages.find(target_message_id);
-    if (iter == messages.end()) {
-        throw std::runtime_error("Message with ID 255 not found in DBC.");
-    }
+void CANSender::setupVcanInterface(const std::string& can_interface) {
+    std::string command = "sudo modprobe vcan && sudo ip link add dev " + can_interface + " type vcan && sudo ip link set up " + can_interface;
 
-    const dbcppp::IMessage& msg = *(iter->second);
-    std::vector<uint8_t> frame_data(8, 0);
-    for (const dbcppp::ISignal& sig : msg.Signals()) {
-        if (sig.Name() == "Speed") {
-            double raw_value = sig.PhysToRaw(speed);
-            sig.Encode(raw_value, frame_data.data());
-        }
+    std::cout << command << std::endl;
+    int ret = system(command.c_str());
+    if (ret != 0) {
+        // throw std::runtime_error("Error setting up vcan interface.");
     }
+}
+
+void CANSender::sendSpeedMessage(double speed, uint32_t target_message_id) {
+    // std::unordered_map<uint64_t, const dbcppp::IMessage*>::iterator iter = messages.find(target_message_id);
+    // if (iter == messages.end()) {
+    //     throw std::runtime_error("Message with ID 255 not found in DBC.");
+    // }
+
+    // const dbcppp::IMessage& msg = *(iter->second);
+    // std::vector<uint8_t> frame_data(8, 0);
+    // for (const dbcppp::ISignal& sig : msg.Signals()) {
+    //     if (sig.Name() == "Speed") {
+    //         double raw_value = sig.PhysToRaw(speed);
+    //         sig.Encode(raw_value, frame_data.data());
+    //     }
+    // }
 
     struct can_frame frame;
     frame.can_id = target_message_id;
-    frame.can_dlc = frame_data.size();
-    std::memcpy(frame.data, frame_data.data(), frame_data.size());
+    frame.can_dlc = 8;
+
+    std::memset(frame.data, 0, sizeof(frame.data));
+    std::memcpy(frame.data, &speed, sizeof(speed));
 
     ssize_t nbytes = write(sock, &frame, sizeof(struct can_frame));
     if (nbytes != sizeof(struct can_frame)) {
-        throw std::runtime_error("Error while sending frame.");
-    } else {
+        throw std::runtime_error("Error while sending speed frame.");
+    } 
+    else {
         std::cout << "Message sent successfully: Speed = " << speed << std::endl;
     }
 }
 
+void CANSender::sendSteerMessage(double steer, uint32_t target_message_id) {
+    // std::unordered_map<uint64_t, const dbcppp::IMessage*>::iterator iter = messages.find(target_message_id);
+    // if (iter == messages.end()) {
+    //     throw std::runtime_error("Message with ID 255 not found in DBC.");
+    // }
+
+    // const dbcppp::IMessage& msg = *(iter->second);
+    // std::vector<uint8_t> frame_data(8, 0);
+    // for (const dbcppp::ISignal& sig : msg.Signals()) {
+    //     if (sig.Name() == "Speed") {
+    //         double raw_value = sig.PhysToRaw(speed);
+    //         sig.Encode(raw_value, frame_data.data());
+    //     }
+    // }
+
+    struct can_frame frame;
+    frame.can_id = target_message_id;
+    frame.can_dlc = 8;
+
+    std::memset(frame.data, 0, sizeof(frame.data));
+    std::memcpy(frame.data, &steer, sizeof(steer));
+
+    ssize_t nbytes = write(sock, &frame, sizeof(struct can_frame));
+    if (nbytes != sizeof(struct can_frame)) {
+        throw std::runtime_error("Error while sending steer frame.");
+    } 
+    else {
+        std::cout << "Message sent successfully: Steer = " << steer << std::endl;
+    }
+}
+
diff --git a/control_ws/src/stanley/src/control.cpp b/control_ws/src/stanley/src/control.cpp
@@ -84,7 +84,8 @@ void Control::publisher_timer_callback() {
 
     this->publish_drive(this->speedCommand, this->steerCommand);
 
-    this->can_sender->sendSpeedMessage(this->speedCommand);
+    this->can_sender->sendSpeedMessage(this->speedCommand, 0);
+    this->can_sender->sendSteerMessage(this->steerCommand, 1);
 }
 
 void Control::publish_drive(float speed, float steer) {