Skip to content

referee_serial fix #12

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
nice-mee merged 1 commit into from
Mar 10, 2025
Merged

referee_serial fix #12

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
23 changes: 7 additions & 16 deletions interfaces/operation_interface/msg/RobotState.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,19 +1,10 @@
uint8 robot_id
uint8 robot_level
uint16 remain_hp
uint16 max_hp
uint16 shooter_id1_17mm_cooling_rate
uint16 shooter_id1_17mm_cooling_limit
uint16 shooter_id1_17mm_speed_limit

uint16 shooter_id2_17mm_cooling_rate
uint16 shooter_id2_17mm_cooling_limit
uint16 shooter_id2_17mm_speed_limit
uint16 shooter_id1_42mm_cooling_rate
uint16 shooter_id1_42mm_cooling_limit
uint16 shooter_id1_42mm_speed_limit

uint16 current_hp
uint16 maximum_hp
uint16 shooter_barrel_cooling_value
uint16 shooter_barrel_heat_limit
uint16 chassis_power_limit
bool mains_power_gimbal_output
bool mains_power_chassis_output
bool mains_power_shooter_output
bool power_management_gimbal_output
bool power_management_chassis_output
bool power_management_shooter_output
29 changes: 10 additions & 19 deletions perception/referee_serial/include/referee_serial/robot_state.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,25 +27,16 @@ class RobotState

struct [[gnu::packed]] Data
{
uint8_t robot_id;
uint8_t robot_level;
uint16_t remain_HP;
uint16_t max_HP;
uint16_t shooter_id1_17mm_cooling_rate;
uint16_t shooter_id1_17mm_cooling_limit;
uint16_t shooter_id1_17mm_speed_limit;

uint16_t shooter_id2_17mm_cooling_rate;
uint16_t shooter_id2_17mm_cooling_limit;
uint16_t shooter_id2_17mm_speed_limit;
uint16_t shooter_id1_42mm_cooling_rate;
uint16_t shooter_id1_42mm_cooling_limit;
uint16_t shooter_id1_42mm_speed_limit;

uint16_t chassis_power_limit;
uint8_t mains_power_gimbal_output : 1;
uint8_t mains_power_chassis_output : 1;
uint8_t mains_power_shooter_output : 1;
uint8_t robot_id;
uint8_t robot_level;
uint16_t current_HP;
uint16_t maximum_HP;
uint16_t shooter_barrel_cooling_value;
uint16_t shooter_barrel_heat_limit;
uint16_t chassis_power_limit;
uint8_t power_management_gimbal_output : 1;
uint8_t power_management_chassis_output : 1;
uint8_t power_management_shooter_output : 1;
};

Header header;
Expand Down
12 changes: 9 additions & 3 deletions perception/referee_serial/src/referee_serial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include <rclcpp/utilities.hpp>
#include <string>
#include <vector>
#include <queue>
#include "referee_serial/crc.h"

#define DEBUG false
Expand Down Expand Up @@ -74,14 +75,19 @@ rclcpp::node_interfaces::NodeBaseInterface::SharedPtr RefereeSerial::get_node_ba

void RefereeSerial::receive()
{
std::vector<uint8_t> prefix(7); // header + cmd_id
std::vector<uint8_t> prefix; // header + cmd_id
std::vector<uint8_t> receive_bit(1);
RCLCPP_INFO(node_->get_logger(), "Receiving serial frames");

while (rclcpp::ok())
{
try {
serial_driver_->port()->receive(prefix);

serial_driver_->port()->receive(receive_bit);
prefix.push_back(receive_bit[0]);
if (prefix.size() < 7)
continue;
while (prefix.size() > 7)
prefix.erase(prefix.begin());
if (KeyMouse::is_wanted_pre(prefix)) // key mouse
{
handle_frame<operation_interface::msg::KeyMouse, KeyMouse>(
Expand Down
33 changes: 13 additions & 20 deletions perception/referee_serial/src/robot_state.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,46 +2,39 @@
#include <bitset>
#include <cstdint>
#include <vector>
#include <iostream>

bool RobotState::is_wanted_pre(const std::vector<uint8_t> &prefix)
{
if (prefix[0] != 0xA5) return false;
if (prefix[0] != 0xA5)
return false;

uint16_t length = static_cast<uint16_t>(prefix[1]) | (static_cast<uint16_t>(prefix[2]) << 8);
uint16_t cmd_id = static_cast<uint16_t>(prefix[5]) | (static_cast<uint16_t>(prefix[6]) << 8);

if (cmd_id != 0x0201 || length != 27) return false;
if (cmd_id != 0x0201 || length != 13)
return false;
return true;
}

RobotState::RobotState(const std::vector<uint8_t> &frame)
{
// copy the uint8_t vector to the struct
std::copy(frame.begin(), frame.end(), reinterpret_cast<uint8_t*>(&interpreted));
std::copy(frame.begin(), frame.end(), reinterpret_cast<uint8_t *>(&interpreted));
}

operation_interface::msg::RobotState RobotState::msg()
{
operation_interface::msg::RobotState msg;
msg.robot_id = interpreted.data.robot_id;
msg.robot_level = interpreted.data.robot_level;
msg.remain_hp = interpreted.data.remain_HP;
msg.max_hp = interpreted.data.max_HP;
msg.shooter_id1_17mm_cooling_rate = interpreted.data.shooter_id1_17mm_cooling_rate;
msg.shooter_id1_17mm_cooling_limit = interpreted.data.shooter_id1_17mm_cooling_limit;
msg.shooter_id1_17mm_speed_limit = interpreted.data.shooter_id1_17mm_speed_limit;

msg.shooter_id2_17mm_cooling_rate = interpreted.data.shooter_id2_17mm_cooling_rate;
msg.shooter_id2_17mm_cooling_limit = interpreted.data.shooter_id2_17mm_cooling_limit;
msg.shooter_id2_17mm_speed_limit = interpreted.data.shooter_id2_17mm_speed_limit;

msg.shooter_id1_42mm_cooling_rate = interpreted.data.shooter_id1_42mm_cooling_rate;
msg.shooter_id1_42mm_cooling_limit = interpreted.data.shooter_id1_42mm_cooling_limit;
msg.shooter_id1_42mm_speed_limit = interpreted.data.shooter_id1_42mm_speed_limit;

msg.current_hp = interpreted.data.current_HP;
msg.maximum_hp = interpreted.data.maximum_HP;
msg.shooter_barrel_cooling_value = interpreted.data.shooter_barrel_cooling_value;
msg.shooter_barrel_heat_limit = interpreted.data.shooter_barrel_heat_limit;
msg.chassis_power_limit = interpreted.data.chassis_power_limit;
msg.mains_power_gimbal_output = interpreted.data.mains_power_gimbal_output;
msg.mains_power_chassis_output = interpreted.data.mains_power_chassis_output;
msg.mains_power_shooter_output = interpreted.data.mains_power_shooter_output;
msg.power_management_gimbal_output = interpreted.data.power_management_gimbal_output;
msg.power_management_chassis_output = interpreted.data.power_management_chassis_output;
msg.power_management_shooter_output = interpreted.data.power_management_shooter_output;
return msg;
}