diff --git a/rmcs_ws/src/rmcs_core/include/rmcs_core/msgs.hpp b/rmcs_ws/src/rmcs_core/include/rmcs_core/msgs.hpp index 63275397c..2cd07d3e0 100644 --- a/rmcs_ws/src/rmcs_core/include/rmcs_core/msgs.hpp +++ b/rmcs_ws/src/rmcs_core/include/rmcs_core/msgs.hpp @@ -4,6 +4,12 @@ namespace rmcs_core::msgs { +enum class RoboticColor : uint8_t { + Blue = 0, // Blue + Red = 1, // Red + Neutral = 2, // Neutral Part +}; + enum class Switch : uint8_t { UNKNOWN = 0, UP = 1, DOWN = 2, MIDDLE = 3 }; struct __attribute__((packed)) Mouse { diff --git a/rmcs_ws/src/rmcs_core/src/hardware/ec/status.cpp b/rmcs_ws/src/rmcs_core/src/hardware/ec/status.cpp index 266d62eed..569a4747f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/ec/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/ec/status.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -30,17 +31,20 @@ class Status register_output("/serial", serial_, path, 9600, serial::Timeout::simpleTimeout(0)); register_output("/tf", tf_); + register_output("/robot_id", robot_id_); + register_output("/auto_rune", auto_rune_); + register_output("/robot_color", color_); } ~Status() = default; void update() override { while (true) { auto result = serial_util::receive_package( - *serial_, package_, cache_size_, static_cast(0xaf), + *serial_, package_, cache_size_, static_cast(header_), [](const PackageReceive& package) { - if (package.type != 0x32) + if (package.type != type_) return false; - if (package.data_size != 16) + if (package.data_size != size_) return false; auto* data = reinterpret_cast(&package); return package.check_sum @@ -52,7 +56,7 @@ class Status return; if (result == serial_util::ReceiveResult::SUCCESS) { cache_size_ = 0; - update_quaternion(); + update_data(); if (!successfully_received_) { successfully_received_ = true; RCLCPP_INFO(logger_, "Successfully received the first package"); @@ -74,23 +78,34 @@ class Status } private: - void update_quaternion() { + void update_data() { auto gimbal_imu_pose = Eigen::Quaterniond{package_.w, package_.x, package_.y, package_.z}; tf_->set_transform( gimbal_imu_pose.conjugate()); + + *auto_rune_ = package_.auto_rune; + *robot_id_ = package_.robot_id > 100 ? package_.robot_id - 100 : package_.robot_id; + *color_ = package_.robot_id > 100 ? rmcs_core::msgs::RoboticColor::Blue + : rmcs_core::msgs::RoboticColor::Red; } FpsCounter fps_counter_; rclcpp::Logger logger_; + OutputInterface robot_id_; + OutputInterface color_; + OutputInterface auto_rune_; OutputInterface serial_; + struct __attribute__((packed)) PackageReceive { uint8_t head; uint8_t type; uint8_t index; uint8_t data_size; float w, x, y, z; + uint8_t robot_id; + uint8_t auto_rune; uint8_t check_sum; } package_; size_t cache_size_ = 0; @@ -98,6 +113,11 @@ class Status bool successfully_received_ = false; int failures_count_ = 0; + // Package static data + static const inline uint8_t header_ = 0xaf; + static const inline uint8_t type_ = 0x32; + static const inline uint8_t size_ = 0x12; + OutputInterface tf_; };