Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions rmcs_ws/src/rmcs_core/include/rmcs_core/msgs.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
30 changes: 25 additions & 5 deletions rmcs_ws/src/rmcs_core/src/hardware/ec/status.cpp
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/node.hpp>
#include <rmcs_core/msgs.hpp>
#include <rmcs_description/tf_description.hpp>
#include <rmcs_executor/component.hpp>
#include <serial/serial.h>
Expand Down Expand Up @@ -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<uint8_t>(0xaf),
*serial_, package_, cache_size_, static_cast<uint8_t>(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<const uint8_t*>(&package);
return package.check_sum
Expand All @@ -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");
Expand All @@ -74,30 +78,46 @@ 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<rmcs_description::ImuLink, rmcs_description::OdomImu>(
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<uint8_t> robot_id_;
OutputInterface<rmcs_core::msgs::RoboticColor> color_;
OutputInterface<uint8_t> auto_rune_;
OutputInterface<serial::Serial> 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;

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<rmcs_description::Tf> tf_;
};

Expand Down