add more modules

auv_hardware/auv_canbus_bridge/README.md

@@ -0,0 +1,11 @@
+# auv_canbus_bridge
+
+
+- [auv\_canbus\_bridge](#auv_canbus_bridge)
+  - [setup virtual can for testing](#setup-virtual-can-for-testing)
+
+
+## setup virtual can for testing
+sudo modprobe vcan
+sudo ip link add dev vcan0 type vcan
+sudo ip link set up vcan0

auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/auv_canbus_bridge_ros.hpp

@@ -0,0 +1,62 @@
+#include <algorithm>
+#include <array>
+#include <memory>
+#include <vector>
+
+#include "auv_canbus_bridge/canbus/helper.hpp"
+#include "auv_canbus_bridge/canbus_socket.hpp"
+#include "auv_canbus_bridge/modules.hpp"
+
+namespace auv_hardware {
+
+class CanbusBridgeROS {
+ public:
+  using ModuleBase = auv_hardware::canbus::modules::ModuleBase;
+  using DrivePulseModule = auv_hardware::canbus::modules::DrivePulseModule;
+  using LaunchTorpedoModule =
+      auv_hardware::canbus::modules::LaunchTorpedoModule;
+  using PowerReportModule = auv_hardware::canbus::modules::PowerReportModule;
+  using KillswitchReportModule =
+      auv_hardware::canbus::modules::KillswitchReportModule;
+
+  explicit CanbusBridgeROS(const ros::NodeHandle& node_handle)
+      : node_handle_{node_handle}, socket_{}, interface_name_{""} {
+    auto node_handle_private = ros::NodeHandle{"~"};
+    node_handle_private.param<std::string>("interface", interface_name_,
+                                           "vcan0");
+
+    socket_.initialize(interface_name_);
+
+    modules_.reserve(4);
+
+    modules_.emplace_back(
+        std::make_unique<DrivePulseModule>(node_handle_, socket_));
+    modules_.emplace_back(
+        std::make_unique<LaunchTorpedoModule>(node_handle_, socket_));
+    modules_.emplace_back(
+        std::make_unique<PowerReportModule>(node_handle_, socket_));
+    modules_.emplace_back(
+        std::make_unique<KillswitchReportModule>(node_handle_, socket_));
+  }
+
+  void spin() {
+    while (ros::ok()) {
+      const auto message = socket_.handle();
+      if (message.has_value()) {
+        std::for_each(modules_.begin(), modules_.end(),
+                      [&message](const std::unique_ptr<ModuleBase>& module) {
+                        module->on_received_message(message->id, message->data);
+                      });
+      }
+
+      ros::spinOnce();
+    }
+  }
+
+ private:
+  ros::NodeHandle node_handle_;
+  auv_hardware::CanbusSocket socket_;
+  std::string interface_name_;
+  std::vector<std::unique_ptr<ModuleBase>> modules_;
+};
+}  // namespace auv_hardware

auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/canbus/endpoint.hpp

@@ -11,6 +11,7 @@ enum class Endpoint : uint16_t {
   SetHSS2OutputCommand,
   SetHSS3OutputCommand,
   LatchedServoCommand,
+  SonarActivationCommand,
   MainboardPowerReport,
   BackSonarReport,
   FrontSonarReport,

auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/canbus_socket.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <fcntl.h>
 #include <linux/can.h>
 #include <linux/can/raw.h>
 #include <net/if.h>
@@ -12,30 +13,35 @@
 #include <cstring>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <stdexcept>
 #include <string>
 #include <thread>
 
 namespace auv_hardware {
+struct CanMessage {
+  auv_hardware::canbus::ExtendedIdType id;
+  std::vector<uint8_t> data;
+};
 
 class CanbusSocket {
  public:
   CanbusSocket() : socket_fd_(-1) {}
 
-  void initialize(const std::string &intarface_name) {
+  void initialize(const std::string &interface_name) {
     socket_fd_ = ::socket(PF_CAN, SOCK_RAW, CAN_RAW);
     if (socket_fd_ < 0) {
       throw std::runtime_error("Failed to open CAN socket");
     }
 
     struct ifreq ifr;
-    std::strncpy(ifr.ifr_name, intarface_name.c_str(), IFNAMSIZ - 1);
+    std::strncpy(ifr.ifr_name, interface_name.c_str(), IFNAMSIZ - 1);
     ifr.ifr_name[IFNAMSIZ - 1] = '\0';
 
     const auto ioctl_status = ::ioctl(socket_fd_, SIOCGIFINDEX, &ifr);
 
     if (ioctl_status < 0) {
-      ROS_ERROR_STREAM("Failed to get CAN interface index: " << intarface_name);
+      ROS_ERROR_STREAM("Failed to get CAN interface index: " << interface_name);
       close();
       throw std::runtime_error("Failed to get CAN interface index");
     }
@@ -50,9 +56,16 @@ class CanbusSocket {
     if (bind_status < 0) {
       close();
       ROS_ERROR_STREAM(
-          "Failed to bind CAN socket on interface: " << intarface_name);
+          "Failed to bind CAN socket on interface: " << interface_name);
       throw std::runtime_error("Failed to bind CAN socket");
     }
+
+    // Set socket to non-blocking mode
+    const auto flags = fcntl(socket_fd_, F_GETFL, 0);
+    if (flags == -1 || fcntl(socket_fd_, F_SETFL, flags | O_NONBLOCK) == -1) {
+      close();
+      throw std::runtime_error("Failed to set CAN socket to non-blocking mode");
+    }
   }
 
   ~CanbusSocket() { close(); }
@@ -110,6 +123,38 @@ class CanbusSocket {
     return true;
   }
 
+  std::optional<CanMessage> handle() {
+    if (!is_connected()) {
+      ROS_ERROR_STREAM("Cannot receive CAN frame: (not connected)");
+      return std::nullopt;
+    }
+
+    struct can_frame frame = {};
+    {
+      std::scoped_lock lock{socket_mutex_};
+      const auto bytes_read = ::read(socket_fd_, &frame, sizeof(frame));
+
+      if (bytes_read < 0) {
+        if (errno == EAGAIN || errno == EWOULDBLOCK) {
+          // No data available, this is expected in non-blocking mode
+          return std::nullopt;
+        } else {
+          throw std::runtime_error("Failed to read from CAN socket");
+        }
+      }
+
+      if (bytes_read != sizeof(frame)) {
+        throw std::runtime_error("Incomplete CAN frame received");
+      }
+    }
+
+    CanMessage message;
+    message.id = frame.can_id & CAN_EFF_MASK;
+    message.data.resize(frame.can_dlc);
+    std::memcpy(message.data.data(), frame.data, frame.can_dlc);
+    return message;
+  }
+
  protected:
   bool is_connected() const { return socket_fd_ >= 0; }
 

auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules.hpp

@@ -3,11 +3,12 @@
 #include <variant>
 
 #include "auv_canbus_bridge/modules/drive_pulse_module.hpp"
+#include "auv_canbus_bridge/modules/killswitch_report_module.hpp"
+#include "auv_canbus_bridge/modules/launch_torpedo_module.hpp"
+#include "auv_canbus_bridge/modules/power_report_module.hpp"
 
 namespace auv_hardware {
 namespace canbus {
-namespace modules {
-
-}  // namespace modules
+namespace modules {}  // namespace modules
 }  // namespace canbus
 }  // namespace auv_hardware

auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/drive_pulse_module.hpp

@@ -43,8 +43,8 @@ class DrivePulseModule : public ModuleBase {
     auto pack_pulse = [&data, &msg](const int offset) {
       for (int i = 0; i < 4; ++i) {
         const auto pulse = msg->channels[offset + i];
-        data[2 * i] = (pulse >> 8) & 0xFF;  // High byte
-        data[2 * i + 1] = pulse & 0xFF;     // Low byte
+        data[2 * i] = pulse & 0xFF;
+        data[2 * i + 1] = (pulse >> 8) & 0xFF;
       }
     };
 

auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/killswitch_report_module.hpp

@@ -0,0 +1,52 @@
+#pragma once
+#include <array>
+
+#include "auv_canbus_bridge/modules/module_base.hpp"
+#include "ros/ros.h"
+#include "std_msgs/Bool.h"
+
+namespace auv_hardware {
+namespace canbus {
+namespace modules {
+
+class KillswitchReportModule : public ModuleBase {
+  static constexpr auto kPropulsionBoardStatusReportIdentifier =
+      auv_hardware::canbus::make_extended_id(
+          0x00, auv_hardware::canbus::NodeID::ExpansionBoard,
+          auv_hardware::canbus::Function::Write,
+          auv_hardware::canbus::Endpoint::PropulsionBoardStatusReport);
+
+ public:
+  KillswitchReportModule(const ros::NodeHandle &node_handle,
+                         CanbusSocket &socket)
+      : ModuleBase(node_handle, socket) {
+    status_report_publisher_ =
+        ModuleBase::node_handle().advertise<std_msgs::Bool>(
+            "propulsion_board/status", 10);
+    ROS_INFO_STREAM("Initialized KillswitchReportModule for CANBUS");
+  }
+
+  virtual void on_received_message(auv_hardware::canbus::ExtendedIdType id,
+                                   const std::vector<uint8_t> &data) override {
+    if (id != kPropulsionBoardStatusReportIdentifier) {
+      return;
+    }
+
+    if (data.size() != 1) {
+      ROS_WARN_STREAM("Received invalid status report message");
+      return;
+    }
+
+    auto status_msg = std_msgs::Bool{};
+    status_msg.data = data[0];
+
+    status_report_publisher_.publish(status_msg);
+  };
+
+ private:
+  ros::Publisher status_report_publisher_;
+};
+
+}  // namespace modules
+}  // namespace canbus
+}  // namespace auv_hardware

auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/launch_torpedo_module.hpp

@@ -0,0 +1,79 @@
+#pragma once
+#include <array>
+
+#include "auv_canbus_bridge/modules/module_base.hpp"
+#include "ros/ros.h"
+#include "std_srvs/Trigger.h"
+
+namespace auv_hardware {
+namespace canbus {
+namespace modules {
+
+class LaunchTorpedoModule : public ModuleBase {
+  constexpr static auto kLaunchTorpedo1ExtendedId =
+      auv_hardware::canbus::make_extended_id(
+          0x00, auv_hardware::canbus::NodeID::Mainboard,
+          auv_hardware::canbus::Function::Write,
+          auv_hardware::canbus::Endpoint::SetHSS1OutputCommand);
+
+  constexpr static auto kLaunchTorpedo2ExtendedId =
+      auv_hardware::canbus::make_extended_id(
+          0x00, auv_hardware::canbus::NodeID::Mainboard,
+          auv_hardware::canbus::Function::Write,
+          auv_hardware::canbus::Endpoint::SetHSS2OutputCommand);
+
+ public:
+  LaunchTorpedoModule(const ros::NodeHandle &node_handle, CanbusSocket &socket)
+      : ModuleBase(node_handle, socket) {
+    launch_torpedo_1_service_ = ModuleBase::node_handle().advertiseService(
+        "actuators/torpedo_1/launch",
+        &LaunchTorpedoModule::launch_torpedo_1_handler, this);
+
+    launch_torpedo_2_service_ = ModuleBase::node_handle().advertiseService(
+        "actuators/torpedo_2/launch",
+        &LaunchTorpedoModule::launch_torpedo_2_handler, this);
+
+    ROS_INFO_STREAM("Initialized LaunchTorpedoModule for CANBUS");
+  }
+
+  bool launch_torpedo_1_handler(std_srvs::Trigger::Request &req,
+                                std_srvs::Trigger::Response &res) {
+    res.success = send_hss_pulse(kLaunchTorpedo1ExtendedId);
+    res.message = "Torpedo 1 launched";
+    return true;
+  }
+
+  bool launch_torpedo_2_handler(std_srvs::Trigger::Request &req,
+                                std_srvs::Trigger::Response &res) {
+    res.success = send_hss_pulse(kLaunchTorpedo2ExtendedId);
+    res.message = "Torpedo 2 launched";
+    return true;
+  }
+
+  virtual void on_received_message(auv_hardware::canbus::ExtendedIdType id,
+                                   const std::vector<uint8_t> &data) override {
+    //
+  };
+
+ protected:
+  bool send_hss_pulse(auv_hardware::canbus::ExtendedIdType id) {
+    bool success = true;
+    std::array<uint8_t, 1> data = {1};
+
+    success = dispatch_message(id, data);
+    ros::Duration(1.0).sleep();
+
+    data[0] = 0;
+    success &= dispatch_message(id, data);
+
+    return success;
+  }
+
+ private:
+  ros::ServiceServer launch_torpedo_1_service_;
+  ros::ServiceServer launch_torpedo_2_service_;
+};
+
+}  // namespace modules
+}  // namespace canbus
+}  // namespace auv_hardware