From 3d2769d8134a9599980f6bd231dec843d23deea8 Mon Sep 17 00:00:00 2001 From: Sencer Yazici Date: Thu, 19 Dec 2024 14:30:49 +0300 Subject: [PATCH] add more modules --- auv_hardware/auv_canbus_bridge/README.md | 11 +++ .../auv_canbus_bridge_ros.hpp | 62 +++++++++++++++ .../auv_canbus_bridge/canbus/endpoint.hpp | 1 + .../auv_canbus_bridge/canbus_socket.hpp | 53 ++++++++++++- .../include/auv_canbus_bridge/modules.hpp | 7 +- .../modules/drive_pulse_module.hpp | 4 +- .../modules/killswitch_report_module.hpp | 52 ++++++++++++ .../modules/launch_torpedo_module.hpp | 79 +++++++++++++++++++ .../modules/power_report_module.hpp | 66 ++++++++++++++++ .../src/auv_canbus_bridge_node.cpp | 40 ++-------- 10 files changed, 333 insertions(+), 42 deletions(-) create mode 100644 auv_hardware/auv_canbus_bridge/README.md create mode 100644 auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/auv_canbus_bridge_ros.hpp create mode 100644 auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/killswitch_report_module.hpp create mode 100644 auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/launch_torpedo_module.hpp create mode 100644 auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/power_report_module.hpp diff --git a/auv_hardware/auv_canbus_bridge/README.md b/auv_hardware/auv_canbus_bridge/README.md new file mode 100644 index 00000000..8127462f --- /dev/null +++ b/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 \ No newline at end of file diff --git a/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/auv_canbus_bridge_ros.hpp b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/auv_canbus_bridge_ros.hpp new file mode 100644 index 00000000..f5c51776 --- /dev/null +++ b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/auv_canbus_bridge_ros.hpp @@ -0,0 +1,62 @@ +#include +#include +#include +#include + +#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("interface", interface_name_, + "vcan0"); + + socket_.initialize(interface_name_); + + modules_.reserve(4); + + modules_.emplace_back( + std::make_unique(node_handle_, socket_)); + modules_.emplace_back( + std::make_unique(node_handle_, socket_)); + modules_.emplace_back( + std::make_unique(node_handle_, socket_)); + modules_.emplace_back( + std::make_unique(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& 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> modules_; +}; +} // namespace auv_hardware diff --git a/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/canbus/endpoint.hpp b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/canbus/endpoint.hpp index 13131148..2ba1f820 100644 --- a/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/canbus/endpoint.hpp +++ b/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, diff --git a/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/canbus_socket.hpp b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/canbus_socket.hpp index 4af9f533..ba649805 100644 --- a/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/canbus_socket.hpp +++ b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/canbus_socket.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -12,30 +13,35 @@ #include #include #include +#include #include #include #include namespace auv_hardware { +struct CanMessage { + auv_hardware::canbus::ExtendedIdType id; + std::vector 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 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; } diff --git a/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules.hpp b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules.hpp index 9b83924b..78d0a4b2 100644 --- a/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules.hpp +++ b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules.hpp @@ -3,11 +3,12 @@ #include #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 diff --git a/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/drive_pulse_module.hpp b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/drive_pulse_module.hpp index 643e70d4..847d8ccf 100644 --- a/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/drive_pulse_module.hpp +++ b/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; } }; diff --git a/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/killswitch_report_module.hpp b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/killswitch_report_module.hpp new file mode 100644 index 00000000..a1764bbd --- /dev/null +++ b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/killswitch_report_module.hpp @@ -0,0 +1,52 @@ +#pragma once +#include + +#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( + "propulsion_board/status", 10); + ROS_INFO_STREAM("Initialized KillswitchReportModule for CANBUS"); + } + + virtual void on_received_message(auv_hardware::canbus::ExtendedIdType id, + const std::vector &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 \ No newline at end of file diff --git a/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/launch_torpedo_module.hpp b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/launch_torpedo_module.hpp new file mode 100644 index 00000000..6ed2d8cf --- /dev/null +++ b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/launch_torpedo_module.hpp @@ -0,0 +1,79 @@ +#pragma once +#include + +#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 &data) override { + // + }; + + protected: + bool send_hss_pulse(auv_hardware::canbus::ExtendedIdType id) { + bool success = true; + std::array 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 \ No newline at end of file diff --git a/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/power_report_module.hpp b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/power_report_module.hpp new file mode 100644 index 00000000..5d2d456c --- /dev/null +++ b/auv_hardware/auv_canbus_bridge/include/auv_canbus_bridge/modules/power_report_module.hpp @@ -0,0 +1,66 @@ +#pragma once +#include + +#include "auv_canbus_bridge/modules/module_base.hpp" +#include "auv_msgs/Power.h" +#include "ros/ros.h" + +namespace auv_hardware { +namespace canbus { +namespace modules { + +class PowerReportModule : public ModuleBase { + static constexpr auto kPowerReportIdentifier = + auv_hardware::canbus::make_extended_id( + 0x00, auv_hardware::canbus::NodeID::ExpansionBoard, + auv_hardware::canbus::Function::Write, + auv_hardware::canbus::Endpoint::MainboardPowerReport); + + public: + PowerReportModule(const ros::NodeHandle &node_handle, CanbusSocket &socket) + : ModuleBase(node_handle, socket) { + power_report_publisher_ = + ModuleBase::node_handle().advertise( + "mainboard/power_sensor/power", 10); + ROS_INFO_STREAM("Initialized PowerReportModule for CANBUS"); + } + + virtual void on_received_message(auv_hardware::canbus::ExtendedIdType id, + const std::vector &data) override { + if (id != kPowerReportIdentifier) { + return; + } + + if (data.size() != 8) { + ROS_WARN_STREAM("Received invalid power report message"); + return; + } + + struct PowerReport { + float voltage; + float current; + }; + + union { + PowerReport report; + std::array data; + } u; + + std::copy(data.begin(), data.end(), u.data.begin()); + + auto power_msg = auv_msgs::Power{}; + + power_msg.voltage = u.report.voltage; + power_msg.current = u.report.current; + power_msg.power = std::abs(u.report.voltage * u.report.current); + + power_report_publisher_.publish(power_msg); + }; + + private: + ros::Publisher power_report_publisher_; +}; + +} // namespace modules +} // namespace canbus +} // namespace auv_hardware \ No newline at end of file diff --git a/auv_hardware/auv_canbus_bridge/src/auv_canbus_bridge_node.cpp b/auv_hardware/auv_canbus_bridge/src/auv_canbus_bridge_node.cpp index 3c66ca24..5e516671 100644 --- a/auv_hardware/auv_canbus_bridge/src/auv_canbus_bridge_node.cpp +++ b/auv_hardware/auv_canbus_bridge/src/auv_canbus_bridge_node.cpp @@ -1,42 +1,16 @@ -#include - -#include "auv_canbus_bridge/canbus/helper.hpp" -#include "auv_canbus_bridge/canbus_socket.hpp" -#include "auv_canbus_bridge/modules.hpp" - -class CANBridgeTest { - public: - using ModuleBase = auv_hardware::canbus::modules::ModuleBase; - using DrivePulseModule = auv_hardware::canbus::modules::DrivePulseModule; - - explicit CANBridgeTest(const ros::NodeHandle &node_handle) - : node_handle_{node_handle}, - socket_{}, - interface_name_{""}, - drive_pulse_module_{node_handle_, socket_} { - auto node_handle_private = ros::NodeHandle{"~"}; - node_handle_private.param("interface", interface_name_, - "vcan0"); - - socket_.initialize(interface_name_); - } - - private: - ros::NodeHandle node_handle_; - auv_hardware::CanbusSocket socket_; - std::string interface_name_; - DrivePulseModule drive_pulse_module_; -}; +#include "auv_canbus_bridge/auv_canbus_bridge_ros.hpp" int main(int argc, char **argv) { - ros::init(argc, argv, "can_bridge_test"); - ros::NodeHandle nh; + ros::init(argc, argv, "auv_canbus_bridge_node"); + auto node_handle = ros::NodeHandle{}; + try { - CANBridgeTest can_bridge(nh); - ros::spin(); + auto canbus_ros_bridge = auv_hardware::CanbusBridgeROS{node_handle}; + canbus_ros_bridge.spin(); } catch (const std::exception &e) { ROS_FATAL("Exception: %s", e.what()); return EXIT_FAILURE; } + return EXIT_SUCCESS; }