From 60406be0375bf4196ce715f379b5aef85b0129ea Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 7 Apr 2026 21:25:42 +0900 Subject: [PATCH 01/22] =?UTF-8?q?feat:=20simulator-cli=20=E3=81=ABibis?= =?UTF-8?q?=E3=83=90=E3=82=A4=E3=83=8A=E3=83=AA=E3=83=97=E3=83=AD=E3=83=88?= =?UTF-8?q?=E3=82=B3=E3=83=AB=E3=82=92=E7=A7=BB=E6=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit grSimフォーク(ibis-ssl/grSim)で実装されていた独自機能をER-Force simulator-cliに移植し、ibisブランチではデフォルトで有効化する。 ## 追加した機能 ### IbisCommandAdaptor - UDP:12345で715バイトのibisバイナリパケットを受信 - SSLビジョンデータをキャッシュしてロボット位置からチームを自動判別 - P-gainによる角度制御、加速度制限、極座標→ロボットローカル速度変換を実装 - sslsim::RobotControl (MoveLocalVelocity) に変換してシミュレータへ送信 ### IbisFeedbackAdaptor - SSLビジョンデータ（位置・向き）とRadioResponse（速度・ボール検出）を統合 - 128バイトのibisバイナリフィードバックパケットを構築 - UDP:50100+robotIdで制御系に非同期送信 ### RefereeTeamDetector - Game Controller(UDP:10003)のマルチキャストを受信 - チーム名照合でblue/yellowを自動判別し、解決後は受信を停止 ### PacketSenderThread - QThread+QMutex/QWaitConditionによるUDPバッチ送信スレッド - 送信レイテンシを物理ループから切り離す ### ibis_protocol.h - ibisバイナリプロトコルの定数・構造体・デシリアライズ関数・パケットビルダーを集約 ## CLIオプション（デフォルト値付き） - --ibis-port (12345) - --ibis-feedback-addr (127.0.0.1) - --ibis-feedback-port-base (50100) - --ibis-feedback-team-name (ibis) - --ibis-use-referee - --ibis-acc-speedup/brake (4.0/6.0 m/s^2) --- src/simulator/CMakeLists.txt | 1 + src/simulator/ibis_protocol.h | 224 +++++++++++++ src/simulator/packet_sender_thread.cpp | 57 ++++ src/simulator/packet_sender_thread.h | 36 +++ src/simulator/simulator.cpp | 417 +++++++++++++++++++++++++ 5 files changed, 735 insertions(+) create mode 100644 src/simulator/ibis_protocol.h create mode 100644 src/simulator/packet_sender_thread.cpp create mode 100644 src/simulator/packet_sender_thread.h diff --git a/src/simulator/CMakeLists.txt b/src/simulator/CMakeLists.txt index d1b44a175d..32b0d9bc32 100644 --- a/src/simulator/CMakeLists.txt +++ b/src/simulator/CMakeLists.txt @@ -20,6 +20,7 @@ add_executable(simulator-cli WIN32 MACOSX_BUNDLE simulator.cpp ssl_robocup_server.cpp + packet_sender_thread.cpp ) target_link_libraries(simulator-cli diff --git a/src/simulator/ibis_protocol.h b/src/simulator/ibis_protocol.h new file mode 100644 index 0000000000..53e7e693ba --- /dev/null +++ b/src/simulator/ibis_protocol.h @@ -0,0 +1,224 @@ +/* + * ibis-ssl binary protocol constants, data types, and packet functions. + * + * Ported from ibis-ssl/grSim fork (ibis_command_receiver.cpp / binary_feedback_sender.cpp). + * No grSim-specific dependencies -- uses only standard C++ and stdint. + */ + +#pragma once + +#include +#include +#include + +// --------------------------------------------------------------------------- +// Protocol constants +// --------------------------------------------------------------------------- + +constexpr int IBIS_ROBOT_SLOTS = 11; +constexpr int IBIS_CMD_SIZE = 64; +constexpr int IBIS_SLOT_SIZE = IBIS_CMD_SIZE + 1; +constexpr int IBIS_PACKET_SIZE = IBIS_SLOT_SIZE * IBIS_ROBOT_SLOTS; // 715 + +constexpr int IBIS_DEFAULT_PORT = 12345; +constexpr double IBIS_CHIP_ANGLE_DEG = 30.0; +constexpr double IBIS_CHIP_ANGLE_RAD = IBIS_CHIP_ANGLE_DEG * M_PI / 180.0; +constexpr double IBIS_MAX_KICK_SPEED = 8.0; // m/s +constexpr double IBIS_THETA_P_GAIN = 4.0; +constexpr double IBIS_DT = 1.0 / 30.0; +constexpr int IBIS_FEEDBACK_SIZE = 128; +constexpr int IBIS_FEEDBACK_PORT_BASE = 50100; +constexpr double IBIS_POSITION_MATCH_THRESHOLD = 0.5; // metres + +// --------------------------------------------------------------------------- +// Byte offsets in the 64-byte RobotCommandSerializedV2 (from crane's robot_packet.h) +// --------------------------------------------------------------------------- + +enum IbisAddress { + HEADER = 0, + CHECK_COUNTER = 1, + VISION_GLOBAL_X_H = 2, + VISION_GLOBAL_X_L = 3, + VISION_GLOBAL_Y_H = 4, + VISION_GLOBAL_Y_L = 5, + VISION_GLOBAL_TH_H = 6, + VISION_GLOBAL_TH_L = 7, + TARGET_GLOBAL_TH_H = 8, + TARGET_GLOBAL_TH_L = 9, + KICK_POWER = 10, + DRIBBLE_POWER = 11, + ACCEL_LIMIT_H = 12, + ACCEL_LIMIT_L = 13, + LINEAR_VEL_LIMIT_H = 14, + LINEAR_VEL_LIMIT_L = 15, + ANGULAR_VEL_LIMIT_H = 16, + ANGULAR_VEL_LIMIT_L = 17, + LATENCY_MS_H = 18, + LATENCY_MS_L = 19, + ELAPSED_VISION_H = 20, + ELAPSED_VISION_L = 21, + FLAGS = 22, + CONTROL_MODE = 23, + CONTROL_MODE_ARGS = 24, + // CONTROL_MODE_ARGS size = 8, args end at offset 31 + TARGET_POS_X_H = 32, + TARGET_POS_X_L = 33, + TARGET_POS_Y_H = 34, + TARGET_POS_Y_L = 35, + TERMINAL_VEL_H = 36, + TERMINAL_VEL_L = 37, +}; + +enum IbisFlagBit { + IS_VISION_AVAILABLE = 0, + ENABLE_CHIP = 1, + STOP_EMERGENCY = 3, +}; + +// --------------------------------------------------------------------------- +// Deserialized ibis command +// --------------------------------------------------------------------------- + +struct IbisCommand { + float vision_global_pos[2]; // metres, SSL vision coordinate system + float target_global_theta; // radians + float kick_power; // 0..1 normalised + float dribble_power; // 0..1 normalised + bool enable_chip; + bool stop_emergency; + float acceleration_limit; // m/s^2 (0 means "use default") + float linear_velocity_limit; // m/s (0 means "no limit") + float angular_velocity_limit; // rad/s + float polar_velocity_r; // m/s + float polar_velocity_theta; // radians (global direction) + uint8_t check_counter; +}; + +// Cached SSL vision state for one robot (position in mm, orientation in rad). +struct IbisVisionState { + float x_mm = 0.0f; + float y_mm = 0.0f; + float orientation_rad = 0.0f; + bool valid = false; +}; + +// Pure deserialization (same logic as grSim ibis_command_receiver.cpp) + +inline float ibisDecodeTwoByte(uint8_t high, uint8_t low, float range) +{ + uint16_t two_byte = (static_cast(high) << 8) | low; + return static_cast(two_byte - 32767.f) / 32767.f * range; +} + +inline IbisCommand ibisDeserialize(const uint8_t* d) +{ + IbisCommand cmd; + cmd.check_counter = d[CHECK_COUNTER]; + cmd.vision_global_pos[0] = ibisDecodeTwoByte(d[VISION_GLOBAL_X_H], d[VISION_GLOBAL_X_L], 32.767f); + cmd.vision_global_pos[1] = ibisDecodeTwoByte(d[VISION_GLOBAL_Y_H], d[VISION_GLOBAL_Y_L], 32.767f); + cmd.target_global_theta = ibisDecodeTwoByte(d[TARGET_GLOBAL_TH_H], d[TARGET_GLOBAL_TH_L], static_cast(M_PI)); + cmd.kick_power = d[KICK_POWER] / 20.f; + cmd.dribble_power = d[DRIBBLE_POWER] / 20.f; + cmd.acceleration_limit = ibisDecodeTwoByte(d[ACCEL_LIMIT_H], d[ACCEL_LIMIT_L], 32.767f); + cmd.linear_velocity_limit = ibisDecodeTwoByte(d[LINEAR_VEL_LIMIT_H], d[LINEAR_VEL_LIMIT_L], 32.767f); + cmd.angular_velocity_limit = ibisDecodeTwoByte(d[ANGULAR_VEL_LIMIT_H], d[ANGULAR_VEL_LIMIT_L], 32.767f); + + uint8_t flags = d[FLAGS]; + cmd.enable_chip = (flags >> ENABLE_CHIP) & 0x01; + cmd.stop_emergency = (flags >> STOP_EMERGENCY) & 0x01; + + // POLAR_VELOCITY_TARGET_MODE args at CONTROL_MODE_ARGS (offset 24) + cmd.polar_velocity_r = ibisDecodeTwoByte(d[CONTROL_MODE_ARGS + 0], d[CONTROL_MODE_ARGS + 1], 32.767f); + cmd.polar_velocity_theta = ibisDecodeTwoByte(d[CONTROL_MODE_ARGS + 2], d[CONTROL_MODE_ARGS + 3], 32.767f); + + return cmd; +} + +// --------------------------------------------------------------------------- +// 128-byte feedback packet builder +// Ported from grSim BinaryFeedbackSender::buildPacket, using scalar args +// instead of Robot* so it has no grSim/ODE dependency. +// +// Parameters: +// buffer - 128-byte output buffer (must be pre-allocated) +// robotId - robot identifier (0..15) +// counter - rolling counter (caller increments) +// yaw_rad - orientation in radians (SSL vision convention) +// ball_detected - is ball in contact with dribbler +// kick_status - 0=none, 1=flat, 2=chip +// odom_x_m - position x in metres (SSL vision coords) +// odom_y_m - position y in metres (SSL vision coords) +// vel_x_ms - global velocity x in m/s (SSL vision coords) +// vel_y_ms - global velocity y in m/s (SSL vision coords) +// --------------------------------------------------------------------------- + +inline void ibisBuildFeedbackPacket( + uint8_t* buffer, + int robotId, + uint8_t counter, + float yaw_rad, + bool ball_detected, + uint8_t kick_status, + float odom_x_m, + float odom_y_m, + float vel_x_ms, + float vel_y_ms) +{ + std::memset(buffer, 0, IBIS_FEEDBACK_SIZE); + + // Header (0-1) + buffer[0] = 0xAB; + buffer[1] = 0xEA; + + // Robot ID (2) + buffer[2] = static_cast(robotId); + + // Counter (3) + buffer[3] = counter; + + // Yaw angle in radians (4-7) + std::memcpy(&buffer[4], &yaw_rad, sizeof(float)); + + // Battery voltage: fixed 24.0 V (8-11) + float voltage = 24.0f; + std::memcpy(&buffer[8], &voltage, sizeof(float)); + + // Ball detection sensors 0-2 (12-14) + buffer[12] = ball_detected ? 1 : 0; + buffer[13] = ball_detected ? 1 : 0; + buffer[14] = ball_detected ? 1 : 0; + + // Kick status (15): 0=none, 1=flat, 2=chip + buffer[15] = kick_status; + + // Error info (16-23): 0 (no errors) + // Motor current (24-27): 0 + // Ball detection 3 (28): 0 + // Already zeroed by memset. + + // Temperature: fixed 25 C (29-35) + for (int i = 29; i <= 35; i++) { + buffer[i] = 25; + } + + // Angle diff (36-39): 0 + float angle_diff = 0.0f; + std::memcpy(&buffer[36], &angle_diff, sizeof(float)); + + // Capacitor voltage: fixed 200.0 V (40-43) + float cap_voltage = 200.0f; + std::memcpy(&buffer[40], &cap_voltage, sizeof(float)); + + // Odometry position (44-51) + std::memcpy(&buffer[44], &odom_x_m, sizeof(float)); + std::memcpy(&buffer[48], &odom_y_m, sizeof(float)); + + // Global velocity (52-59) + std::memcpy(&buffer[52], &vel_x_ms, sizeof(float)); + std::memcpy(&buffer[56], &vel_y_ms, sizeof(float)); + + // Check version byte: 0x01 = simulator (60) + buffer[60] = 0x01; + + // Extended data (61-127): 0 (already zeroed) +} diff --git a/src/simulator/packet_sender_thread.cpp b/src/simulator/packet_sender_thread.cpp new file mode 100644 index 0000000000..015865c382 --- /dev/null +++ b/src/simulator/packet_sender_thread.cpp @@ -0,0 +1,57 @@ +#include "packet_sender_thread.h" + +#include +#include +#include + +PacketSenderThread::PacketSenderThread(QObject* parent) + : QThread(parent) +{ + start(); +} + +PacketSenderThread::~PacketSenderThread() +{ + stop(); + wait(); +} + +void PacketSenderThread::enqueue(QByteArray data, const QHostAddress& addr, quint16 port) +{ + QMutexLocker locker(&mutex_); + queue_.enqueue({std::move(data), addr, port}); + cond_.wakeOne(); +} + +void PacketSenderThread::stop() +{ + QMutexLocker locker(&mutex_); + running_ = false; + cond_.wakeOne(); +} + +void PacketSenderThread::run() +{ + QUdpSocket socket; + socket.setSocketOption(QAbstractSocket::MulticastTtlOption, QVariant(1)); + + QList batch; + while (true) { + batch.clear(); + { + QMutexLocker locker(&mutex_); + while (queue_.isEmpty() && running_) { + cond_.wait(&mutex_); + } + if (!running_ && queue_.isEmpty()) { + break; + } + while (!queue_.isEmpty()) { + batch.append(queue_.dequeue()); + } + } + for (const Packet& pkt : batch) { + socket.writeDatagram(pkt.data, pkt.addr, pkt.port); + } + } +} diff --git a/src/simulator/packet_sender_thread.h b/src/simulator/packet_sender_thread.h new file mode 100644 index 0000000000..5786efb64f --- /dev/null +++ b/src/simulator/packet_sender_thread.h @@ -0,0 +1,36 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +// Sends UDP packets asynchronously from a dedicated worker thread. +// The producer (main/rcv thread) calls enqueue(); the worker thread +// batches and sends them so that UDP send latency does not block callers. +class PacketSenderThread : public QThread { + Q_OBJECT +public: + struct Packet { + QByteArray data; + QHostAddress addr; + quint16 port; + }; + + explicit PacketSenderThread(QObject* parent = nullptr); + ~PacketSenderThread() override; + + void enqueue(QByteArray data, const QHostAddress& addr, quint16 port); + void stop(); + +protected: + void run() override; + +private: + QQueue queue_; + QMutex mutex_; + QWaitCondition cond_; + bool running_ = true; +}; diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 004b9e84f0..b8a52ac019 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -46,6 +46,11 @@ #include "core/sslprotocols.h" #include "ssl_robocup_server.h" +#include "ibis_protocol.h" +#include "packet_sender_thread.h" + +#include "protobuf/ssl_vision/ssl_wrapper.pb.h" +#include "protobuf/ssl_gc/state/ssl_gc_referee_message.pb.h" /** * Stand alone Erforce simulator @@ -646,6 +651,359 @@ void SimProxy::handleCommand(const Command &command) { emit gotCommand(command); } +class IbisCommandAdaptor : public QObject { + Q_OBJECT +public: + IbisCommandAdaptor(int port, Timer* timer, double accSpeedup, double accBrake) + : m_server(this) + , m_timer(timer) + , m_accSpeedup(accSpeedup) + , m_accBrake(accBrake) + { + m_server.bind(QHostAddress::Any, static_cast(port)); + connect(&m_server, &QUdpSocket::readyRead, this, &IbisCommandAdaptor::handleDatagrams); + } + +public slots: + void handleVisionData(const QByteArray& data, qint64, QString) { + SSL_WrapperPacket pkt; + if (!pkt.ParseFromArray(data.data(), data.size()) || !pkt.has_detection()) { + return; + } + const auto& det = pkt.detection(); + for (const auto& r : det.robots_blue()) { + if (r.has_robot_id() && r.has_orientation()) { + const uint32_t id = r.robot_id(); + if (id < kMaxRobots) { + m_vision[0][id] = {r.x(), r.y(), r.orientation(), true}; + } + } + } + for (const auto& r : det.robots_yellow()) { + if (r.has_robot_id() && r.has_orientation()) { + const uint32_t id = r.robot_id(); + if (id < kMaxRobots) { + m_vision[1][id] = {r.x(), r.y(), r.orientation(), true}; + } + } + } + } + +signals: + void sendRadioCommands(const SSLSimRobotControl& commands, bool isBlue, qint64 processingDelay); + +private slots: + void handleDatagrams() { + while (m_server.hasPendingDatagrams()) { + const qint64 start = m_timer->currentTime(); + auto datagram = m_server.receiveDatagram(); + const auto& data = datagram.data(); + + if (data.size() != IBIS_PACKET_SIZE) { + continue; + } + + const uint8_t* buf = reinterpret_cast(data.constData()); + + SSLSimRobotControl blueControl{new sslsim::RobotControl}; + SSLSimRobotControl yellowControl{new sslsim::RobotControl}; + bool hasBlue = false, hasYellow = false; + + for (int slot = 0; slot < IBIS_ROBOT_SLOTS; ++slot) { + const int offset = slot * IBIS_SLOT_SIZE; + const uint8_t robot_id = buf[offset]; + if (robot_id >= IBIS_ROBOT_SLOTS) { + continue; + } + + const uint8_t* cmd_data = buf + offset + 1; + if (cmd_data[CHECK_COUNTER] == m_robotStates[robot_id].last_check_counter) { + continue; + } + + const IbisCommand cmd = ibisDeserialize(cmd_data); + m_robotStates[robot_id].last_check_counter = cmd.check_counter; + + // Team auto-detection: match vision_global_pos against cached positions. + // Also keeps the matched vision entry for orientation lookup below. + int teamIdx = -1; + const IbisVisionState* vis = nullptr; + for (int t = 0; t < 2; ++t) { + const IbisVisionState& v = m_vision[t][robot_id]; + if (!v.valid) { continue; } + const float dx = v.x_mm / 1000.0f - cmd.vision_global_pos[0]; + const float dy = v.y_mm / 1000.0f - cmd.vision_global_pos[1]; + if (std::hypot(dx, dy) < IBIS_POSITION_MATCH_THRESHOLD) { + teamIdx = t; + vis = &v; + break; + } + } + if (teamIdx < 0) { + continue; + } + const bool ibisIsBlue = (teamIdx == 0); + + auto* robotCmd = ibisIsBlue + ? blueControl->add_robot_commands() + : yellowControl->add_robot_commands(); + robotCmd->set_id(robot_id); + + if (cmd.stop_emergency) { + auto* lv = robotCmd->mutable_move_command()->mutable_local_velocity(); + lv->set_forward(0.0f); + lv->set_left(0.0f); + lv->set_angular(0.0f); + m_robotStates[robot_id].prev_vx = 0.0; + m_robotStates[robot_id].prev_vy = 0.0; + } else { + const double current_theta = vis->orientation_rad; + + double theta_error = cmd.target_global_theta - current_theta; + while (theta_error > M_PI) theta_error -= 2.0 * M_PI; + while (theta_error < -M_PI) theta_error += 2.0 * M_PI; + + double omega = IBIS_THETA_P_GAIN * theta_error; + omega = std::max(-static_cast(cmd.angular_velocity_limit), + std::min(omega, static_cast(cmd.angular_velocity_limit))); + + const double predicted_theta = current_theta + omega * IBIS_DT; + const double vel_angle = cmd.polar_velocity_theta - predicted_theta; + double target_vx = cmd.polar_velocity_r * std::cos(vel_angle); + double target_vy = cmd.polar_velocity_r * std::sin(vel_angle); + + auto& state = m_robotStates[robot_id]; + const double current_speed = std::hypot(state.prev_vx, state.prev_vy); + const double target_speed = std::hypot(target_vx, target_vy); + double acc_limit = (target_speed < current_speed) ? m_accBrake : m_accSpeedup; + if (cmd.acceleration_limit > 0.0f && cmd.acceleration_limit < static_cast(acc_limit)) { + acc_limit = cmd.acceleration_limit; + } + + const double delta_vx = target_vx - state.prev_vx; + const double delta_vy = target_vy - state.prev_vy; + const double delta_norm = std::hypot(delta_vx, delta_vy); + const double max_delta = acc_limit * IBIS_DT; + + double out_vx, out_vy; + if (delta_norm > max_delta && delta_norm > 1e-9) { + out_vx = state.prev_vx + (delta_vx / delta_norm) * max_delta; + out_vy = state.prev_vy + (delta_vy / delta_norm) * max_delta; + } else { + out_vx = target_vx; + out_vy = target_vy; + } + + const double out_speed = std::hypot(out_vx, out_vy); + if (cmd.linear_velocity_limit > 0.0f && out_speed > cmd.linear_velocity_limit) { + out_vx = out_vx / out_speed * cmd.linear_velocity_limit; + out_vy = out_vy / out_speed * cmd.linear_velocity_limit; + } + + state.prev_vx = out_vx; + state.prev_vy = out_vy; + + auto* lv = robotCmd->mutable_move_command()->mutable_local_velocity(); + lv->set_forward(static_cast(out_vx)); + lv->set_left(static_cast(out_vy)); + lv->set_angular(static_cast(omega)); + + if (cmd.kick_power > 0.001f) { + robotCmd->set_kick_speed(static_cast(IBIS_MAX_KICK_SPEED * cmd.kick_power)); + robotCmd->set_kick_angle(cmd.enable_chip ? static_cast(IBIS_CHIP_ANGLE_DEG) : 0.0f); + } + if (cmd.dribble_power > 0.001f) { + robotCmd->set_dribbler_speed(100.0f); + } + } + + if (ibisIsBlue) { hasBlue = true; } else { hasYellow = true; } + } + + if (hasBlue) { + emit sendRadioCommands(blueControl, true, start); + } + if (hasYellow) { + emit sendRadioCommands(yellowControl, false, start); + } + warnLatency(m_timer->currentTime() - start); + } + } + +private: + static constexpr uint32_t kMaxRobots = 16; + + struct PerRobotState { + double prev_vx = 0.0; + double prev_vy = 0.0; + uint8_t last_check_counter = 0xFF; + }; + + // m_vision[0] = blue, m_vision[1] = yellow + IbisVisionState m_vision[2][kMaxRobots] = {}; + PerRobotState m_robotStates[IBIS_ROBOT_SLOTS] = {}; + + QUdpSocket m_server; + Timer* m_timer; + double m_accSpeedup; + double m_accBrake; +}; + +class RefereeTeamDetector : public QObject { + Q_OBJECT +public: + RefereeTeamDetector(const QString& teamName, bool localhost) + : m_socket(this) + , m_teamName(teamName.toLower().trimmed()) + { + m_socket.bind(QHostAddress::AnyIPv4, + SSL_GAME_CONTROLLER_PORT, + QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint); + if (!localhost) { + m_socket.joinMulticastGroup(QHostAddress(SSL_GAME_CONTROLLER_ADDRESS)); + } + connect(&m_socket, &QUdpSocket::readyRead, this, &RefereeTeamDetector::handleDatagrams); + } + +signals: + void teamDetected(bool ibisIsBlue); + +private slots: + void handleDatagrams() { + while (m_socket.hasPendingDatagrams()) { + auto datagram = m_socket.receiveDatagram(); + SSL_Referee ref; + if (!ref.ParseFromArray(datagram.data().data(), datagram.data().size())) { + continue; + } + auto tryMatch = [&](const SSL_Referee::TeamInfo& info, bool isBlue) { + if (!info.has_name()) { return false; } + if (QString::fromStdString(info.name()).toLower().trimmed() != m_teamName) { return false; } + disconnect(&m_socket, &QUdpSocket::readyRead, this, &RefereeTeamDetector::handleDatagrams); + emit teamDetected(isBlue); + return true; + }; + if (ref.has_blue() && tryMatch(ref.blue(), true)) { return; } + if (ref.has_yellow() && tryMatch(ref.yellow(), false)) { return; } + } + } + +private: + QUdpSocket m_socket; + QString m_teamName; +}; + +class IbisFeedbackAdaptor : public QObject { + Q_OBJECT +public: + IbisFeedbackAdaptor(const QHostAddress& addr, quint16 portBase, bool useReferee) + : m_sender(new PacketSenderThread()) + , m_addr(addr) + , m_portBase(portBase) + , m_ibisIsBlue(true) + , m_refereeResolved(!useReferee) + { + } + + ~IbisFeedbackAdaptor() override { + m_sender->stop(); + m_sender->wait(); + delete m_sender; + } + +public slots: + void handleVisionData(const QByteArray& data, qint64, QString) { + SSL_WrapperPacket pkt; + if (!pkt.ParseFromArray(data.data(), data.size()) || !pkt.has_detection()) { + return; + } + const auto& det = pkt.detection(); + for (const auto& r : det.robots_blue()) { + if (!r.has_robot_id()) { continue; } + const uint32_t id = r.robot_id(); + if (id < kMaxRobots) { + m_vision[0][id] = {r.x(), r.y(), + r.has_orientation() ? r.orientation() : 0.0f, + true}; + } + } + for (const auto& r : det.robots_yellow()) { + if (!r.has_robot_id()) { continue; } + const uint32_t id = r.robot_id(); + if (id < kMaxRobots) { + m_vision[1][id] = {r.x(), r.y(), + r.has_orientation() ? r.orientation() : 0.0f, + true}; + } + } + } + + void handleRobotResponse(const QList& responses) { + if (!m_refereeResolved) { + if (++m_waitLogCount % 200 == 0) { + log(stdout, "ibis: waiting for Game Controller to identify team color\n"); + } + return; + } + const int teamIdx = m_ibisIsBlue ? 0 : 1; + for (const auto& resp : responses) { + if (!resp.has_is_blue() || resp.is_blue() != m_ibisIsBlue) { continue; } + if (!resp.has_estimated_speed()) { continue; } + const uint32_t id = resp.id(); + if (id >= kMaxRobots) { continue; } + + const IbisVisionState& vis = m_vision[teamIdx][id]; + if (!vis.valid) { continue; } + + // Rotate robot-local velocity (v_f=forward, v_s=left) to global SSL coords. + const float theta = vis.orientation_rad; + const float v_f = resp.estimated_speed().v_f(); + const float v_s = resp.estimated_speed().v_s(); + const float vel_x = v_f * std::cos(theta) - v_s * std::sin(theta); + const float vel_y = v_f * std::sin(theta) + v_s * std::cos(theta); + + uint8_t buffer[IBIS_FEEDBACK_SIZE]; + ibisBuildFeedbackPacket( + buffer, + static_cast(id), + m_counters[id]++, + vis.orientation_rad, + resp.has_ball_detected() && resp.ball_detected(), + 0, // kick_status not tracked in ER-Force simulator + vis.x_mm / 1000.0f, + vis.y_mm / 1000.0f, + vel_x, vel_y); + + m_sender->enqueue( + QByteArray(reinterpret_cast(buffer), IBIS_FEEDBACK_SIZE), + m_addr, m_portBase + static_cast(id)); + } + } + + void handleRefereePacket(bool ibisIsBlue) { + m_ibisIsBlue = ibisIsBlue; + if (!m_refereeResolved) { + m_refereeResolved = true; + log(stdout, "ibis: team color resolved to %s from Game Controller\n", + ibisIsBlue ? "BLUE" : "YELLOW"); + } + } + +private: + static constexpr uint32_t kMaxRobots = 16; + + // m_vision[0] = blue, m_vision[1] = yellow + IbisVisionState m_vision[2][kMaxRobots] = {}; + uint8_t m_counters[kMaxRobots] = {}; + int m_waitLogCount = 0; + + PacketSenderThread* m_sender; + QHostAddress m_addr; + quint16 m_portBase; + bool m_ibisIsBlue; + bool m_refereeResolved; +}; + #include "simulator.moc" @@ -677,6 +1035,22 @@ int main(int argc, char* argv[]) parser.addOption(realismConfig); parser.addOption(localhostConfig); + // ibis binary protocol options (always enabled on the ibis branch) + QCommandLineOption ibisPortOpt("ibis-port", "ibis command receiver UDP port", "port", QString::number(IBIS_DEFAULT_PORT)); + QCommandLineOption ibisFeedbackAddrOpt("ibis-feedback-addr", "ibis feedback destination address", "addr", "127.0.0.1"); + QCommandLineOption ibisFeedbackPortBaseOpt("ibis-feedback-port-base", "ibis feedback base port (robotId is added)", "port", QString::number(IBIS_FEEDBACK_PORT_BASE)); + QCommandLineOption ibisFeedbackTeamNameOpt("ibis-feedback-team-name", "Team name to look up in Game Controller for color detection", "name", "ibis"); + QCommandLineOption ibisUseRefereeOpt("ibis-use-referee", "Use Game Controller referee to auto-detect ibis team color"); + QCommandLineOption ibisAccSpeedupOpt("ibis-acc-speedup", "Acceleration limit for speedup [m/s^2]", "accel", "4.0"); + QCommandLineOption ibisAccBrakeOpt("ibis-acc-brake", "Acceleration limit for braking [m/s^2]", "accel", "6.0"); + parser.addOption(ibisPortOpt); + parser.addOption(ibisFeedbackAddrOpt); + parser.addOption(ibisFeedbackPortBaseOpt); + parser.addOption(ibisFeedbackTeamNameOpt); + parser.addOption(ibisUseRefereeOpt); + parser.addOption(ibisAccSpeedupOpt); + parser.addOption(ibisAccBrakeOpt); + parser.process(app); auto* desc = sslsim::RobotSpecs::descriptor(); @@ -748,6 +1122,49 @@ int main(int argc, char* argv[]) vision.moveToThread(&rcv_thread); commands.moveToThread(&rcv_thread); + // ibis binary protocol components (always enabled on the ibis branch) + { + const int cmdPort = parser.value(ibisPortOpt).toInt(); + const double accSpeedup = parser.value(ibisAccSpeedupOpt).toDouble(); + const double accBrake = parser.value(ibisAccBrakeOpt).toDouble(); + const QHostAddress fbAddr = QHostAddress(parser.value(ibisFeedbackAddrOpt)); + const quint16 fbPortBase = static_cast(parser.value(ibisFeedbackPortBaseOpt).toUInt()); + const bool useReferee = parser.isSet(ibisUseRefereeOpt); + + auto* ibisCmd = new IbisCommandAdaptor(cmdPort, &timer, accSpeedup, accBrake); + auto* ibisFb = new IbisFeedbackAdaptor(fbAddr, fbPortBase, useReferee); + + // IbisCommandAdaptor receives vision data to cache robot positions/orientations + QObject::connect(&sim, &SimProxy::gotPacket, + ibisCmd, &IbisCommandAdaptor::handleVisionData); + // IbisCommandAdaptor sends converted commands to the simulator + QObject::connect(ibisCmd, &IbisCommandAdaptor::sendRadioCommands, + &sim, &SimProxy::handleRadioCommands); + + // IbisFeedbackAdaptor receives vision data for position/orientation + QObject::connect(&sim, &SimProxy::gotPacket, + ibisFb, &IbisFeedbackAdaptor::handleVisionData); + // IbisFeedbackAdaptor receives radio responses for velocity and ball detection + QObject::connect(&sim, &SimProxy::sendRadioResponses, + ibisFb, &IbisFeedbackAdaptor::handleRobotResponse); + + if (useReferee) { + auto* referee = new RefereeTeamDetector( + parser.value(ibisFeedbackTeamNameOpt), + parser.isSet(localhostConfig)); + QObject::connect(referee, &RefereeTeamDetector::teamDetected, + ibisFb, &IbisFeedbackAdaptor::handleRefereePacket); + referee->moveToThread(&rcv_thread); + } + + ibisCmd->moveToThread(&rcv_thread); + ibisFb->moveToThread(&rcv_thread); + + log(stdout, "ibis: command receiver on UDP port %d\n", cmdPort); + log(stdout, "ibis: feedback sender to %s base port %d\n", + parser.value(ibisFeedbackAddrOpt).toStdString().c_str(), + static_cast(fbPortBase)); + } rcv_thread.start(); From 8f4579956ade21a32ae7eb5a382d00324d22a9bb Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 7 Apr 2026 21:32:27 +0900 Subject: [PATCH 02/22] =?UTF-8?q?ci:=20ghcr.io=E3=81=B8=E3=81=AEDocker=20i?= =?UTF-8?q?mage=E3=83=91=E3=83=96=E3=83=AA=E3=83=83=E3=82=B7=E3=83=A5?= =?UTF-8?q?=E3=83=AF=E3=83=BC=E3=82=AF=E3=83=95=E3=83=AD=E3=83=BC=E3=82=92?= =?UTF-8?q?=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit grSimのdocker-publish.yamlを参考に、ibis-ssl/framework-simulatorcliイメージを ghcr.io (GitHub Container Registry) へpublishするワークフローを追加。 - イメージ名: ghcr.io/ibis-ssl/framework-simulatorcli - Dockerfile: data/docker/Dockerfile.simulatorcli - プラットフォーム: linux/amd64, linux/arm64 - トリガー: masterへのpush / v*.*.*タグ / PR(ビルドのみ) / workflow_dispatch - GitHub Actions cache、SBOM、attestation対応 --- .github/workflows/docker-publish.yaml | 85 +++++++++++++++++++++++++++ 1 file changed, 85 insertions(+) create mode 100644 .github/workflows/docker-publish.yaml diff --git a/.github/workflows/docker-publish.yaml b/.github/workflows/docker-publish.yaml new file mode 100644 index 0000000000..00ecdf8251 --- /dev/null +++ b/.github/workflows/docker-publish.yaml @@ -0,0 +1,85 @@ +name: Build and Push Docker Image + +on: + push: + branches: + - master + tags: + - 'v*.*.*' + pull_request: + branches: + - master + paths: + - 'data/docker/Dockerfile.simulatorcli' + - 'data/docker/simulatorcli_entrypoint.bash' + - '.github/workflows/docker-publish.yaml' + workflow_dispatch: + +env: + REGISTRY: ghcr.io + IMAGE_NAME: ibis-ssl/framework-simulatorcli + +jobs: + build-and-push: + runs-on: ubuntu-latest + permissions: + contents: read + packages: write + attestations: write + id-token: write + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + with: + submodules: recursive + + - name: Set up QEMU + uses: docker/setup-qemu-action@v3 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + + - name: Log in to Container Registry + if: github.event_name != 'pull_request' + uses: docker/login-action@v3 + with: + registry: ${{ env.REGISTRY }} + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Extract metadata (tags, labels) + id: meta + uses: docker/metadata-action@v5 + with: + images: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} + tags: | + type=raw,value=latest,enable={{is_default_branch}} + type=ref,event=branch + type=semver,pattern={{version}} + type=semver,pattern={{major}}.{{minor}} + type=sha,prefix=,format=short + type=ref,event=pr + + - name: Build and push Docker image + id: push + uses: docker/build-push-action@v6 + with: + context: . + file: ./data/docker/Dockerfile.simulatorcli + platforms: linux/amd64,linux/arm64 + push: ${{ github.event_name != 'pull_request' }} + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + cache-from: type=gha + cache-to: type=gha,mode=max + provenance: true + sbom: true + + - name: Generate artifact attestation + if: github.event_name != 'pull_request' + uses: actions/attest-build-provenance@v2 + with: + subject-name: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }} + subject-digest: ${{ steps.push.outputs.digest }} + push-to-registry: true From fac9f1dead1cdc9dcffa176b7255b67d8891b18a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 7 Apr 2026 21:37:14 +0900 Subject: [PATCH 03/22] =?UTF-8?q?ci:=20docker-publish=E3=81=AE=E3=83=87?= =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=AB=E3=83=88=E3=83=96=E3=83=A9=E3=83=B3?= =?UTF-8?q?=E3=83=81=E3=82=92ibis=E3=81=AB=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit masterではなくibisブランチを中心に運用するため、 ワークフローのトリガーブランチをibisに更新。 --- .github/workflows/docker-publish.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/docker-publish.yaml b/.github/workflows/docker-publish.yaml index 00ecdf8251..2431a7f389 100644 --- a/.github/workflows/docker-publish.yaml +++ b/.github/workflows/docker-publish.yaml @@ -3,12 +3,12 @@ name: Build and Push Docker Image on: push: branches: - - master + - ibis tags: - 'v*.*.*' pull_request: branches: - - master + - ibis paths: - 'data/docker/Dockerfile.simulatorcli' - 'data/docker/simulatorcli_entrypoint.bash' From 16ef746e08063fcf756c2b640151612cbfdc53d7 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 7 Apr 2026 22:56:01 +0900 Subject: [PATCH 04/22] =?UTF-8?q?ci:=20docker-publish=20CI=20=E3=81=AE?= =?UTF-8?q?=E3=83=93=E3=83=AB=E3=83=89=E6=99=82=E9=96=93=E3=82=92=E5=A4=A7?= =?UTF-8?q?=E5=B9=85=E7=9F=AD=E7=B8=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit arm64クロスコンパイル（QEMUエミュレーション）を廃止し、 amd64のみのビルドに変更することでCI実行時間を削減する。 QEMUによるarm64ビルドは実ビルドの5〜10倍以上の時間を要していた。 Dockerfile.simulatorcliも合わせて最適化: - ビルドステージ: 不要なQt5パッケージを削除（simulator-cliはQt6のみ使用） - ランタイムステージ: devパッケージをランタイムライブラリのみに置換 - qt6-base-dev, libqt6opengl6-dev → libqt6core6t64, libqt6gui6t64等 - libprotobuf-dev → libprotobuf32t64 --- .github/workflows/docker-publish.yaml | 5 +---- data/docker/Dockerfile.simulatorcli | 8 +++----- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/.github/workflows/docker-publish.yaml b/.github/workflows/docker-publish.yaml index 2431a7f389..bb9aaa21ea 100644 --- a/.github/workflows/docker-publish.yaml +++ b/.github/workflows/docker-publish.yaml @@ -34,9 +34,6 @@ jobs: with: submodules: recursive - - name: Set up QEMU - uses: docker/setup-qemu-action@v3 - - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 @@ -67,7 +64,7 @@ jobs: with: context: . file: ./data/docker/Dockerfile.simulatorcli - platforms: linux/amd64,linux/arm64 + platforms: linux/amd64 push: ${{ github.event_name != 'pull_request' }} tags: ${{ steps.meta.outputs.tags }} labels: ${{ steps.meta.outputs.labels }} diff --git a/data/docker/Dockerfile.simulatorcli b/data/docker/Dockerfile.simulatorcli index a7ab54fabd..cfc0e16b02 100644 --- a/data/docker/Dockerfile.simulatorcli +++ b/data/docker/Dockerfile.simulatorcli @@ -10,8 +10,7 @@ RUN set -xe; \ apt-get install --no-install-recommends -y \ cmake make g++ libssl-dev patch \ protobuf-compiler libprotobuf-dev \ - qt6-base-dev libqt6opengl6-dev \ - qtbase5-dev libqt5opengl5-dev; \ + qt6-base-dev libqt6opengl6-dev; \ apt-get clean -y; \ rm -rf /var/lib/apt/lists/*; @@ -37,9 +36,8 @@ ARG DEBIAN_FRONTEND=noninteractive RUN set -xe; \ apt-get update; \ apt-get install --no-install-recommends -y \ - qt6-base-dev libqt6opengl6-dev \ - qtbase5-dev libqt5opengl5-dev \ - libprotobuf-dev tini; \ + libqt6core6t64 libqt6gui6t64 libqt6network6t64 libqt6opengl6t64 \ + libprotobuf32t64 tini; \ apt-get clean -y; \ rm -rf /var/lib/apt/lists/*; From 0d579824818f86e095c7b0439c75300fe2458fd2 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 8 Apr 2026 01:01:05 +0900 Subject: [PATCH 05/22] =?UTF-8?q?feat:=20ibis=E3=83=95=E3=82=A3=E3=83=BC?= =?UTF-8?q?=E3=83=89=E3=83=90=E3=83=83=E3=82=AF=E9=80=81=E4=BF=A1=E3=82=92?= =?UTF-8?q?=E3=82=BF=E3=82=A4=E3=83=9E=E3=83=BC=E9=A7=86=E5=8B=95=E5=8C=96?= =?UTF-8?q?=E3=81=97=E3=81=A6=E3=83=AC=E3=83=BC=E3=83=88=E8=A8=AD=E5=AE=9A?= =?UTF-8?q?=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ロボットレスポンス受信とフィードバック送信の結合を分離し、 QTimerを使った定期送信に変更。これにより送信タイミングが コマンドレスポンスの受信タイミングに依存しなくなる。 変更内容: - IbisFeedbackAdaptorにfeedbackHzパラメータを追加（デフォルト125Hz） - QTimer（PreciseTimer）を使って指定レートでフィードバックを定期送信 - RobotCacheを追加し、ロボットレスポンス受信時に速度・ボール検知をキャッシュ - タイマー発火時にキャッシュデータを参照して送信する方式に変更 - --ibis-feedback-hz CLIオプションを追加（最小値1Hz） - ログ出力にフィードバック送信レートのHz表示を追加 --- src/simulator/simulator.cpp | 66 +++++++++++++++++++++++++++---------- 1 file changed, 48 insertions(+), 18 deletions(-) diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index b8a52ac019..9f4fbe6651 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -896,13 +897,19 @@ private slots: class IbisFeedbackAdaptor : public QObject { Q_OBJECT public: - IbisFeedbackAdaptor(const QHostAddress& addr, quint16 portBase, bool useReferee) + IbisFeedbackAdaptor(const QHostAddress& addr, quint16 portBase, bool useReferee, int feedbackHz = 125) : m_sender(new PacketSenderThread()) , m_addr(addr) , m_portBase(portBase) , m_ibisIsBlue(true) , m_refereeResolved(!useReferee) { + // タイマー駆動のフィードバック送信（指定レートで定期送信） + m_feedbackTimer = new QTimer(this); + m_feedbackTimer->setTimerType(Qt::PreciseTimer); + m_feedbackTimer->setInterval(1000 / feedbackHz); + connect(m_feedbackTimer, &QTimer::timeout, this, &IbisFeedbackAdaptor::onFeedbackTimer); + m_feedbackTimer->start(); } ~IbisFeedbackAdaptor() override { @@ -955,12 +962,33 @@ public slots: const IbisVisionState& vis = m_vision[teamIdx][id]; if (!vis.valid) { continue; } - // Rotate robot-local velocity (v_f=forward, v_s=left) to global SSL coords. + // ロボットローカル速度（v_f=前方, v_s=左）をグローバルSSL座標に変換してキャッシュ const float theta = vis.orientation_rad; const float v_f = resp.estimated_speed().v_f(); const float v_s = resp.estimated_speed().v_s(); - const float vel_x = v_f * std::cos(theta) - v_s * std::sin(theta); - const float vel_y = v_f * std::sin(theta) + v_s * std::cos(theta); + m_robotCache[id].vel_x = v_f * std::cos(theta) - v_s * std::sin(theta); + m_robotCache[id].vel_y = v_f * std::sin(theta) + v_s * std::cos(theta); + m_robotCache[id].ball_detected = resp.has_ball_detected() && resp.ball_detected(); + } + } + + void handleRefereePacket(bool ibisIsBlue) { + m_ibisIsBlue = ibisIsBlue; + if (!m_refereeResolved) { + m_refereeResolved = true; + log(stdout, "ibis: team color resolved to %s from Game Controller\n", + ibisIsBlue ? "BLUE" : "YELLOW"); + } + } + +private slots: + // タイマー発火時にキャッシュデータを使ってフィードバックを送信 + void onFeedbackTimer() { + if (!m_refereeResolved) { return; } + const int teamIdx = m_ibisIsBlue ? 0 : 1; + for (uint32_t id = 0; id < kMaxRobots; ++id) { + const IbisVisionState& vis = m_vision[teamIdx][id]; + if (!vis.valid) { continue; } uint8_t buffer[IBIS_FEEDBACK_SIZE]; ibisBuildFeedbackPacket( @@ -968,11 +996,11 @@ public slots: static_cast(id), m_counters[id]++, vis.orientation_rad, - resp.has_ball_detected() && resp.ball_detected(), + m_robotCache[id].ball_detected, 0, // kick_status not tracked in ER-Force simulator vis.x_mm / 1000.0f, vis.y_mm / 1000.0f, - vel_x, vel_y); + m_robotCache[id].vel_x, m_robotCache[id].vel_y); m_sender->enqueue( QByteArray(reinterpret_cast(buffer), IBIS_FEEDBACK_SIZE), @@ -980,23 +1008,22 @@ public slots: } } - void handleRefereePacket(bool ibisIsBlue) { - m_ibisIsBlue = ibisIsBlue; - if (!m_refereeResolved) { - m_refereeResolved = true; - log(stdout, "ibis: team color resolved to %s from Game Controller\n", - ibisIsBlue ? "BLUE" : "YELLOW"); - } - } - private: static constexpr uint32_t kMaxRobots = 16; + struct RobotCache { + float vel_x = 0.0f; + float vel_y = 0.0f; + bool ball_detected = false; + }; + // m_vision[0] = blue, m_vision[1] = yellow IbisVisionState m_vision[2][kMaxRobots] = {}; + RobotCache m_robotCache[kMaxRobots] = {}; uint8_t m_counters[kMaxRobots] = {}; int m_waitLogCount = 0; + QTimer* m_feedbackTimer; PacketSenderThread* m_sender; QHostAddress m_addr; quint16 m_portBase; @@ -1043,6 +1070,7 @@ int main(int argc, char* argv[]) QCommandLineOption ibisUseRefereeOpt("ibis-use-referee", "Use Game Controller referee to auto-detect ibis team color"); QCommandLineOption ibisAccSpeedupOpt("ibis-acc-speedup", "Acceleration limit for speedup [m/s^2]", "accel", "4.0"); QCommandLineOption ibisAccBrakeOpt("ibis-acc-brake", "Acceleration limit for braking [m/s^2]", "accel", "6.0"); + QCommandLineOption ibisFeedbackHzOpt("ibis-feedback-hz", "ibis feedback send rate in Hz (default: 125)", "hz", "125"); parser.addOption(ibisPortOpt); parser.addOption(ibisFeedbackAddrOpt); parser.addOption(ibisFeedbackPortBaseOpt); @@ -1050,6 +1078,7 @@ int main(int argc, char* argv[]) parser.addOption(ibisUseRefereeOpt); parser.addOption(ibisAccSpeedupOpt); parser.addOption(ibisAccBrakeOpt); + parser.addOption(ibisFeedbackHzOpt); parser.process(app); @@ -1130,9 +1159,10 @@ int main(int argc, char* argv[]) const QHostAddress fbAddr = QHostAddress(parser.value(ibisFeedbackAddrOpt)); const quint16 fbPortBase = static_cast(parser.value(ibisFeedbackPortBaseOpt).toUInt()); const bool useReferee = parser.isSet(ibisUseRefereeOpt); + const int feedbackHz = qMax(1, parser.value(ibisFeedbackHzOpt).toInt()); auto* ibisCmd = new IbisCommandAdaptor(cmdPort, &timer, accSpeedup, accBrake); - auto* ibisFb = new IbisFeedbackAdaptor(fbAddr, fbPortBase, useReferee); + auto* ibisFb = new IbisFeedbackAdaptor(fbAddr, fbPortBase, useReferee, feedbackHz); // IbisCommandAdaptor receives vision data to cache robot positions/orientations QObject::connect(&sim, &SimProxy::gotPacket, @@ -1161,9 +1191,9 @@ int main(int argc, char* argv[]) ibisFb->moveToThread(&rcv_thread); log(stdout, "ibis: command receiver on UDP port %d\n", cmdPort); - log(stdout, "ibis: feedback sender to %s base port %d\n", + log(stdout, "ibis: feedback sender to %s base port %d at %d Hz\n", parser.value(ibisFeedbackAddrOpt).toStdString().c_str(), - static_cast(fbPortBase)); + static_cast(fbPortBase), feedbackHz); } rcv_thread.start(); From ffdefe2e93b7e3415136f3bc25d8adcc36eb677d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 10 Apr 2026 08:05:32 +0900 Subject: [PATCH 06/22] =?UTF-8?q?feat:=20ibis=E3=83=95=E3=82=A3=E3=83=BC?= =?UTF-8?q?=E3=83=89=E3=83=90=E3=83=83=E3=82=AF=E3=81=AE=E4=BD=8D=E7=BD=AE?= =?UTF-8?q?=E6=83=85=E5=A0=B1=E6=9B=B4=E6=96=B0=E3=82=92200Hz=E3=81=AE?= =?UTF-8?q?=E3=82=B7=E3=83=9F=E3=83=A5=E3=83=AC=E3=83=BC=E3=82=BF=E7=9B=B4?= =?UTF-8?q?=E7=B5=90=E3=81=AB=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ## 変更内容 ### sendGroundTruth シグナルの追加（Simulator → SimProxy） - `Simulator::sendGroundTruth(QByteArray)` シグナルを新設 - `Simulator::process()`（200Hz）内で `stepSimulation` 直後にノイズなし位置データを emit - `world::SimulatorState` 形式でシリアライズして伝送 - `SimProxy` でフォワードするシグナルと接続を追加 ### IbisFeedbackAdaptor の位置更新ソース変更 - `handleVisionData`（SSL Visionパース、66.67Hz）を廃止 - `handleGroundTruth`（world::SimulatorState パース、200Hz）に置き換え - Bullet座標系 → SSL座標系の変換: x_mm = p_y×1000, y_mm = -p_x×1000 - クォータニオン回転行列第1列から yaw 角を算出 ### ibis-referee-port CLIオプションの追加 - `RefereeTeamDetector` コンストラクタにポート引数を追加 - `--ibis-referee-port` オプションで Game Controller ポートを変更可能に ## 背景 フィードバック送信レート（125Hz）はVisionパケット更新レート（66.67Hz）を上回っており、 位置キャッシュが古いデータのまま複数回送信される問題があった。 シミュレータの物理ステップ（200Hz）から直接取得することで完全に解消する。 --- .../simulator/include/simulator/simulator.h | 1 + src/amun/simulator/simulator.cpp | 15 +++++ src/simulator/simulator.cpp | 66 +++++++++++-------- 3 files changed, 55 insertions(+), 27 deletions(-) diff --git a/src/amun/simulator/include/simulator/simulator.h b/src/amun/simulator/include/simulator/simulator.h index e41c8040ea..a094f8061b 100644 --- a/src/amun/simulator/include/simulator/simulator.h +++ b/src/amun/simulator/include/simulator/simulator.h @@ -78,6 +78,7 @@ class camun::simulator::Simulator : public QObject void sendStatus(const Status &status); void sendRadioResponses(const QList &responses); void sendRealData(const QByteArray& data); // sends amun::SimulatorState + void sendGroundTruth(const QByteArray& data); // sends world::SimulatorState at 200Hz, no noise void sendSSLSimError(const QList& errors, ErrorSource source); public slots: diff --git a/src/amun/simulator/simulator.cpp b/src/amun/simulator/simulator.cpp index e8f5132aec..37337c81a0 100644 --- a/src/amun/simulator/simulator.cpp +++ b/src/amun/simulator/simulator.cpp @@ -274,6 +274,21 @@ void Simulator::process() m_data->dynamicsWorld->stepSimulation(timeDelta, 10, SUB_TIMESTEP); m_time = current_time; + // Emit ground truth robot positions at full simulation rate (200Hz), without noise + { + world::SimulatorState gt; + gt.set_time(m_time); + for (auto it = m_data->robotsBlue.cbegin(); it != m_data->robotsBlue.cend(); ++it) { + it.value().first->update(gt.add_blue_robots(), m_data->ball); + } + for (auto it = m_data->robotsYellow.cbegin(); it != m_data->robotsYellow.cend(); ++it) { + it.value().first->update(gt.add_yellow_robots(), m_data->ball); + } + QByteArray gtData(static_cast(gt.ByteSizeLong()), 0); + gt.SerializeToArray(gtData.data(), gtData.size()); + emit sendGroundTruth(gtData); + } + // only send a vision packet every third frame = 15 ms - epsilon (=half frame) // gives a vision frequency of 66.67Hz if (m_lastSentStatusTime + 12500000 <= m_time) { diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 9f4fbe6651..ee40252f1f 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -52,6 +52,7 @@ #include "protobuf/ssl_vision/ssl_wrapper.pb.h" #include "protobuf/ssl_gc/state/ssl_gc_referee_message.pb.h" +#include "protobuf/world.pb.h" /** * Stand alone Erforce simulator @@ -598,6 +599,7 @@ class SimProxy: public QObject { void sendSSLSimError(const QList& errors, ErrorSource source); // out void sendRadioResponses(const QList &responses); // out void gotPacket(const QByteArray &data, qint64 time, QString sender); // out + void sendGroundTruth(const QByteArray& data); // out - world::SimulatorState at 200Hz void gotCommand(const Command &command); // internal void handleRadioCommands(const SSLSimRobotControl& control, bool isBlue, qint64 processingStart); // in public slots: @@ -643,6 +645,7 @@ void SimProxy::handleCommand(const Command &command) { connect(this, &SimProxy::handleRadioCommands, m_sim, &Simulator::handleRadioCommands); connect(m_sim, &Simulator::sendSSLSimError, this, &SimProxy::sendSSLSimError); connect(m_sim, &Simulator::sendRadioResponses, this, &SimProxy::sendRadioResponses); + connect(m_sim, &Simulator::sendGroundTruth, this, &SimProxy::sendGroundTruth); auto* simCommand = m_teamCommand->mutable_simulator(); simCommand->set_enable(true); auto* trCommand = m_teamCommand->mutable_transceiver(); @@ -853,12 +856,12 @@ private slots: class RefereeTeamDetector : public QObject { Q_OBJECT public: - RefereeTeamDetector(const QString& teamName, bool localhost) + RefereeTeamDetector(const QString& teamName, bool localhost, quint16 port = SSL_GAME_CONTROLLER_PORT) : m_socket(this) , m_teamName(teamName.toLower().trimmed()) { m_socket.bind(QHostAddress::AnyIPv4, - SSL_GAME_CONTROLLER_PORT, + port, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint); if (!localhost) { m_socket.joinMulticastGroup(QHostAddress(SSL_GAME_CONTROLLER_ADDRESS)); @@ -919,30 +922,35 @@ class IbisFeedbackAdaptor : public QObject { } public slots: - void handleVisionData(const QByteArray& data, qint64, QString) { - SSL_WrapperPacket pkt; - if (!pkt.ParseFromArray(data.data(), data.size()) || !pkt.has_detection()) { + // world::SimulatorState から IbisVisionState を更新する（200Hz） + // world::SimRobot の座標系: p_x/p_y はゲーム座標系のメートル単位（Bullet座標 / SIMULATOR_SCALE） + // SSL座標系への変換: x_mm = p_y * 1000, y_mm = -p_x * 1000 + // 方向角: クォータニオン (i,j,k,real) の回転行列第1列から yaw を算出 + void handleGroundTruth(const QByteArray& data) { + world::SimulatorState state; + if (!state.ParseFromArray(data.data(), data.size())) { return; } - const auto& det = pkt.detection(); - for (const auto& r : det.robots_blue()) { - if (!r.has_robot_id()) { continue; } - const uint32_t id = r.robot_id(); - if (id < kMaxRobots) { - m_vision[0][id] = {r.x(), r.y(), - r.has_orientation() ? r.orientation() : 0.0f, - true}; - } - } - for (const auto& r : det.robots_yellow()) { - if (!r.has_robot_id()) { continue; } - const uint32_t id = r.robot_id(); - if (id < kMaxRobots) { - m_vision[1][id] = {r.x(), r.y(), - r.has_orientation() ? r.orientation() : 0.0f, - true}; + auto updateTeam = [this](const auto& robots, int teamIdx) { + for (const auto& r : robots) { + const uint32_t id = r.id(); + if (id >= kMaxRobots) { continue; } + const float qx = r.rotation().i(); + const float qy = r.rotation().j(); + const float qz = r.rotation().k(); + const float qw = r.rotation().real(); + const float dir_x = 1.0f - 2.0f * (qy*qy + qz*qz); + const float dir_y = 2.0f * (qx*qy + qw*qz); + m_vision[teamIdx][id] = { + r.p_y() * 1000.0f, + -r.p_x() * 1000.0f, + std::atan2(dir_y, dir_x), + true + }; } - } + }; + updateTeam(state.blue_robots(), 0); + updateTeam(state.yellow_robots(), 1); } void handleRobotResponse(const QList& responses) { @@ -1071,6 +1079,7 @@ int main(int argc, char* argv[]) QCommandLineOption ibisAccSpeedupOpt("ibis-acc-speedup", "Acceleration limit for speedup [m/s^2]", "accel", "4.0"); QCommandLineOption ibisAccBrakeOpt("ibis-acc-brake", "Acceleration limit for braking [m/s^2]", "accel", "6.0"); QCommandLineOption ibisFeedbackHzOpt("ibis-feedback-hz", "ibis feedback send rate in Hz (default: 125)", "hz", "125"); + QCommandLineOption ibisRefereePortOpt("ibis-referee-port", "Game Controller multicast port for team color detection", "port", QString::number(SSL_GAME_CONTROLLER_PORT)); parser.addOption(ibisPortOpt); parser.addOption(ibisFeedbackAddrOpt); parser.addOption(ibisFeedbackPortBaseOpt); @@ -1079,6 +1088,7 @@ int main(int argc, char* argv[]) parser.addOption(ibisAccSpeedupOpt); parser.addOption(ibisAccBrakeOpt); parser.addOption(ibisFeedbackHzOpt); + parser.addOption(ibisRefereePortOpt); parser.process(app); @@ -1160,6 +1170,7 @@ int main(int argc, char* argv[]) const quint16 fbPortBase = static_cast(parser.value(ibisFeedbackPortBaseOpt).toUInt()); const bool useReferee = parser.isSet(ibisUseRefereeOpt); const int feedbackHz = qMax(1, parser.value(ibisFeedbackHzOpt).toInt()); + const quint16 refereePort = static_cast(parser.value(ibisRefereePortOpt).toUInt()); auto* ibisCmd = new IbisCommandAdaptor(cmdPort, &timer, accSpeedup, accBrake); auto* ibisFb = new IbisFeedbackAdaptor(fbAddr, fbPortBase, useReferee, feedbackHz); @@ -1171,9 +1182,9 @@ int main(int argc, char* argv[]) QObject::connect(ibisCmd, &IbisCommandAdaptor::sendRadioCommands, &sim, &SimProxy::handleRadioCommands); - // IbisFeedbackAdaptor receives vision data for position/orientation - QObject::connect(&sim, &SimProxy::gotPacket, - ibisFb, &IbisFeedbackAdaptor::handleVisionData); + // IbisFeedbackAdaptor receives ground truth positions at 200Hz for position/orientation + QObject::connect(&sim, &SimProxy::sendGroundTruth, + ibisFb, &IbisFeedbackAdaptor::handleGroundTruth); // IbisFeedbackAdaptor receives radio responses for velocity and ball detection QObject::connect(&sim, &SimProxy::sendRadioResponses, ibisFb, &IbisFeedbackAdaptor::handleRobotResponse); @@ -1181,7 +1192,8 @@ int main(int argc, char* argv[]) if (useReferee) { auto* referee = new RefereeTeamDetector( parser.value(ibisFeedbackTeamNameOpt), - parser.isSet(localhostConfig)); + parser.isSet(localhostConfig), + refereePort); QObject::connect(referee, &RefereeTeamDetector::teamDetected, ibisFb, &IbisFeedbackAdaptor::handleRefereePacket); referee->moveToThread(&rcv_thread); From 6fee79a9fcc6aa5ae8956a9bb4540cd567136f40 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 10 Apr 2026 08:10:38 +0900 Subject: [PATCH 07/22] =?UTF-8?q?refactor:=20simulator-cli=20Docker?= =?UTF-8?q?=E3=82=A4=E3=83=A1=E3=83=BC=E3=82=B8=E3=82=92cmake=20install?= =?UTF-8?q?=E3=81=A7=E8=BB=BD=E9=87=8F=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ランタイムイメージにbuildディレクトリ全体をコピーしていた問題を解消。 cmake installを使ってバイナリとconfigファイルのみを抽出し、 必要最小限のファイルのみランタイムステージへコピーするよう変更。 - RELATIVE_DATA_DIRS=ONでビルドし、実行時のconfig参照を./config/に固定 - cmake --install でbin/とconfig/のみをインストールプレフィックスへ配置 - ランタイムステージはinstall済みのbin/simulator-cliとconfig/のみをコピー - 不要になったsimulator-cli_entrypoint.bashを削除 - ENTRYPOINTをtiniに、CMDをsimulator-cliに整理 - CMakeLists.txtにinstallターゲットを追加（バイナリ・configファイル） --- CMakeLists.txt | 2 ++ data/docker/Dockerfile.simulatorcli | 16 +++++++--------- data/docker/Dockerfile.simulatorcli.dockerignore | 1 - data/docker/simulatorcli_entrypoint.bash | 15 --------------- src/simulator/CMakeLists.txt | 2 ++ 5 files changed, 11 insertions(+), 25 deletions(-) delete mode 100755 data/docker/simulatorcli_entrypoint.bash diff --git a/CMakeLists.txt b/CMakeLists.txt index 144366ffdf..695a6584d8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -222,6 +222,8 @@ find_package(V8 10.5.7) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) add_subdirectory(src) +install(DIRECTORY config/ DESTINATION config FILES_MATCHING PATTERN "*.txt") + if(UNIX AND NOT APPLE) configure_file(data/pkg/ra.desktop.in ra.desktop) configure_file(data/pkg/ra-logplayer.desktop.in ra-logplayer.desktop) diff --git a/data/docker/Dockerfile.simulatorcli b/data/docker/Dockerfile.simulatorcli index cfc0e16b02..e225360b87 100644 --- a/data/docker/Dockerfile.simulatorcli +++ b/data/docker/Dockerfile.simulatorcli @@ -21,9 +21,9 @@ COPY . . RUN set -xe; \ mkdir build; \ cd build; \ - cmake -DCMAKE_BUILD_TYPE=RelWithDebInfo ..; \ + cmake -DCMAKE_BUILD_TYPE=RelWithDebInfo -DRELATIVE_DATA_DIRS=ON ..; \ make simulator-cli -j $(nproc); \ - find . -maxdepth 1 ! -name 'bin' -exec rm -r {} \; ; + cmake --install . --prefix /home/default/install; # # Run stage @@ -45,15 +45,13 @@ RUN useradd --create-home --shell /bin/bash default USER default WORKDIR /home/default -COPY --chown=default:default /data/docker/simulatorcli_entrypoint.bash . -ENTRYPOINT ["tini", "--", "./simulatorcli_entrypoint.bash"] - -COPY --chown=default:default --from=build-stage /home/default/COPYING . -COPY --chown=default:default --from=build-stage /home/default/COPYING.GPL . -COPY --chown=default:default --from=build-stage /home/default/config config -COPY --chown=default:default --from=build-stage /home/default/build build +COPY --chown=default:default --from=build-stage /home/default/install/bin/simulator-cli bin/simulator-cli +COPY --chown=default:default --from=build-stage /home/default/install/config config # 10300: Control - Accepts simulator configuration commands # 10301: Blue - Accepts robot commands by the blue team # 10302: Yellow - Accepts robot commands by the yellow team EXPOSE 10300 10301 10302 + +ENTRYPOINT ["tini", "--"] +CMD ["./bin/simulator-cli"] diff --git a/data/docker/Dockerfile.simulatorcli.dockerignore b/data/docker/Dockerfile.simulatorcli.dockerignore index f7f9b4b3ae..4996568519 100644 --- a/data/docker/Dockerfile.simulatorcli.dockerignore +++ b/data/docker/Dockerfile.simulatorcli.dockerignore @@ -16,6 +16,5 @@ /strategy /data !/data/pkg -!/data/docker/simulatorcli_entrypoint.bash # vim: filetype=gitignore diff --git a/data/docker/simulatorcli_entrypoint.bash b/data/docker/simulatorcli_entrypoint.bash deleted file mode 100755 index 0088c422d2..0000000000 --- a/data/docker/simulatorcli_entrypoint.bash +++ /dev/null @@ -1,15 +0,0 @@ -#!/usr/bin/env bash -GEOMETRY_OPTION="" -if [[ -n "$GEOMETRY" ]]; then - GEOMETRY_OPTION="--geometry=$GEOMETRY" - echo "Passing '${GEOMETRY_OPTION}'" -fi - -REALISM_OPTION="" -if [[ -n "$REALISM" ]]; then - REALISM_OPTION="--realism=$REALISM" - echo "Passing '${REALISM_OPTION}'" -fi - -echo "Starting the ER-Force simulator-cli" -exec ./build/bin/simulator-cli "$GEOMETRY_OPTION" "$REALISM_OPTION" diff --git a/src/simulator/CMakeLists.txt b/src/simulator/CMakeLists.txt index 32b0d9bc32..5cd05e1810 100644 --- a/src/simulator/CMakeLists.txt +++ b/src/simulator/CMakeLists.txt @@ -30,3 +30,5 @@ target_link_libraries(simulator-cli Qt6::Widgets amun::simulator ) + +install(TARGETS simulator-cli RUNTIME DESTINATION bin) From 8b481b247e7f11678237dc0d173d67bfdf63bf4f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 10 Apr 2026 08:55:28 +0900 Subject: [PATCH 08/22] =?UTF-8?q?perf:=20=E3=82=B7=E3=83=9F=E3=83=A5?= =?UTF-8?q?=E3=83=AC=E3=83=BC=E3=82=BF=E3=82=B9=E3=83=86=E3=83=83=E3=83=97?= =?UTF-8?q?=E3=82=92200Hz=E2=86=92250Hz=E3=81=AB=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ibisフィードバック送信レート（125Hz = 8ms）の整数倍（×2）に揃えることで、 毎回のfeedback送信時に必ず2ステップ分の新鮮な位置データが得られる状態にする。 ## 変更内容 - SUB_TIMESTEP: 1/200.f（5ms）→ 1/250.f（4ms） - process() トリガー間隔: 5ms → 4ms（scaling=1.0 時） ## 周期の関係 - sendGroundTruth: 250Hz（4ms） - feedbackTimer: 125Hz（8ms）= 2ステップに1回 - 位置データの最大鮮度: 4ms以内 ## 副作用 - Vision パケット送信レートが 66.67Hz → 62.5Hz に変化 （閾値 12.5ms に対して4msステップが当たるのが4ステップ目=16ms） ibis用途では影響なし --- src/amun/simulator/include/simulator/simulator.h | 2 +- src/amun/simulator/simulator.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/amun/simulator/include/simulator/simulator.h b/src/amun/simulator/include/simulator/simulator.h index a094f8061b..2b72281f3f 100644 --- a/src/amun/simulator/include/simulator/simulator.h +++ b/src/amun/simulator/include/simulator/simulator.h @@ -35,7 +35,7 @@ // higher values break the rolling friction of the ball const float SIMULATOR_SCALE = 10.0f; -const float SUB_TIMESTEP = 1/200.f; +const float SUB_TIMESTEP = 1/250.f; const float COLLISION_MARGIN = 0.04f; const unsigned FOCAL_LENGTH = 390; diff --git a/src/amun/simulator/simulator.cpp b/src/amun/simulator/simulator.cpp index 37337c81a0..0ad59cd478 100644 --- a/src/amun/simulator/simulator.cpp +++ b/src/amun/simulator/simulator.cpp @@ -965,8 +965,8 @@ void Simulator::setScaling(double scaling) // clear pending vision packets resetVisionPackets(); } else { - // scale default timing of 5 milliseconds - const int t = 5 / scaling; + // scale default timing of 4 milliseconds (250Hz, multiple of ibis feedback 125Hz) + const int t = 4 / scaling; m_trigger->start(qMax(1, t)); // The vision packet timings are wrong after a scaling change From 1f9115988506163b6526e9e7216667e59a2cddd1 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 13 Apr 2026 22:34:00 +0900 Subject: [PATCH 09/22] Set simulator to 125Hz and sync ibis feedback --- .../simulator/include/simulator/simulator.h | 4 +- src/amun/simulator/simulator.cpp | 19 ++-- src/simulator/simulator.cpp | 92 +++++++++---------- 3 files changed, 53 insertions(+), 62 deletions(-) diff --git a/src/amun/simulator/include/simulator/simulator.h b/src/amun/simulator/include/simulator/simulator.h index 2b72281f3f..4b76d0977a 100644 --- a/src/amun/simulator/include/simulator/simulator.h +++ b/src/amun/simulator/include/simulator/simulator.h @@ -78,7 +78,7 @@ class camun::simulator::Simulator : public QObject void sendStatus(const Status &status); void sendRadioResponses(const QList &responses); void sendRealData(const QByteArray& data); // sends amun::SimulatorState - void sendGroundTruth(const QByteArray& data); // sends world::SimulatorState at 200Hz, no noise + void sendGroundTruth(const QByteArray& data); // sends world::SimulatorState at 125Hz, no noise void sendSSLSimError(const QList& errors, ErrorSource source); public slots: @@ -118,7 +118,7 @@ private slots: const Timer *m_timer; QTimer *m_trigger; qint64 m_time; - qint64 m_lastSentStatusTime; + unsigned int m_simulationFrameCounter; double m_timeScaling; bool m_enabled; bool m_charge; diff --git a/src/amun/simulator/simulator.cpp b/src/amun/simulator/simulator.cpp index 0ad59cd478..6cd47fd6aa 100644 --- a/src/amun/simulator/simulator.cpp +++ b/src/amun/simulator/simulator.cpp @@ -118,7 +118,7 @@ Simulator::Simulator(const Timer *timer, const amun::SimulatorSetup &setup, bool m_isPartial(useManualTrigger), m_timer(timer), m_time(0), - m_lastSentStatusTime(0), + m_simulationFrameCounter(0), m_timeScaling(1.), m_enabled(false), m_charge(false), @@ -126,7 +126,7 @@ Simulator::Simulator(const Timer *timer, const amun::SimulatorSetup &setup, bool m_visionProcessingTime(5 * 1000 * 1000), m_aggregator(new ErrorAggregator(this)) { - // triggers by default every 5 milliseconds if simulator is enabled + // triggers by default every 8 milliseconds if simulator is enabled // timing may change if time is scaled m_trigger = new QTimer(this); m_trigger->setTimerType(Qt::PreciseTimer); @@ -274,7 +274,7 @@ void Simulator::process() m_data->dynamicsWorld->stepSimulation(timeDelta, 10, SUB_TIMESTEP); m_time = current_time; - // Emit ground truth robot positions at full simulation rate (200Hz), without noise + // Emit ground truth robot positions at full simulation rate (125Hz), without noise { world::SimulatorState gt; gt.set_time(m_time); @@ -289,9 +289,9 @@ void Simulator::process() emit sendGroundTruth(gtData); } - // only send a vision packet every third frame = 15 ms - epsilon (=half frame) - // gives a vision frequency of 66.67Hz - if (m_lastSentStatusTime + 12500000 <= m_time) { + // send a vision packet every second simulation frame + // with the 125 Hz base loop this results in an effective vision frequency of 62.5 Hz + if ((m_simulationFrameCounter++ % 2) == 0) { auto data = createVisionPacket(); @@ -312,8 +312,6 @@ void Simulator::process() timer->start(timeout); m_visionTimers.enqueue(timer); } - - m_lastSentStatusTime = m_time; } // send timing information @@ -800,6 +798,7 @@ void Simulator::handleCommand(const Command &command) if (sim.has_enable()) { m_enabled = sim.enable(); m_time = m_timer->currentTime(); + m_simulationFrameCounter = 0; // update timer when simulator status is changed setScaling(m_timeScaling); } @@ -965,8 +964,8 @@ void Simulator::setScaling(double scaling) // clear pending vision packets resetVisionPackets(); } else { - // scale default timing of 4 milliseconds (250Hz, multiple of ibis feedback 125Hz) - const int t = 4 / scaling; + // scale default timing of 8 milliseconds (125Hz) + const int t = 8 / scaling; m_trigger->start(qMax(1, t)); // The vision packet timings are wrong after a scaling change diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index ee40252f1f..85356a596a 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -599,7 +599,7 @@ class SimProxy: public QObject { void sendSSLSimError(const QList& errors, ErrorSource source); // out void sendRadioResponses(const QList &responses); // out void gotPacket(const QByteArray &data, qint64 time, QString sender); // out - void sendGroundTruth(const QByteArray& data); // out - world::SimulatorState at 200Hz + void sendGroundTruth(const QByteArray& data); // out - world::SimulatorState at 125Hz void gotCommand(const Command &command); // internal void handleRadioCommands(const SSLSimRobotControl& control, bool isBlue, qint64 processingStart); // in public slots: @@ -817,7 +817,9 @@ private slots: robotCmd->set_kick_angle(cmd.enable_chip ? static_cast(IBIS_CHIP_ANGLE_DEG) : 0.0f); } if (cmd.dribble_power > 0.001f) { - robotCmd->set_dribbler_speed(100.0f); + // Match Amun's normalized 0..1 dribbler command conversion. + constexpr float kMaxDribblerSpeedRpm = static_cast(150.0 * 60.0 * 0.5 / M_PI); + robotCmd->set_dribbler_speed(kMaxDribblerSpeedRpm * cmd.dribble_power); } } @@ -900,20 +902,13 @@ private slots: class IbisFeedbackAdaptor : public QObject { Q_OBJECT public: - IbisFeedbackAdaptor(const QHostAddress& addr, quint16 portBase, bool useReferee, int feedbackHz = 125) + IbisFeedbackAdaptor(const QHostAddress& addr, quint16 portBase, bool useReferee) : m_sender(new PacketSenderThread()) , m_addr(addr) , m_portBase(portBase) , m_ibisIsBlue(true) , m_refereeResolved(!useReferee) - { - // タイマー駆動のフィードバック送信（指定レートで定期送信） - m_feedbackTimer = new QTimer(this); - m_feedbackTimer->setTimerType(Qt::PreciseTimer); - m_feedbackTimer->setInterval(1000 / feedbackHz); - connect(m_feedbackTimer, &QTimer::timeout, this, &IbisFeedbackAdaptor::onFeedbackTimer); - m_feedbackTimer->start(); - } + {} ~IbisFeedbackAdaptor() override { m_sender->stop(); @@ -922,7 +917,7 @@ class IbisFeedbackAdaptor : public QObject { } public slots: - // world::SimulatorState から IbisVisionState を更新する（200Hz） + // world::SimulatorState を受け取るたびに IbisVisionState を更新し、そのループの feedback を送信する（125Hz） // world::SimRobot の座標系: p_x/p_y はゲーム座標系のメートル単位（Bullet座標 / SIMULATOR_SCALE） // SSL座標系への変換: x_mm = p_y * 1000, y_mm = -p_x * 1000 // 方向角: クォータニオン (i,j,k,real) の回転行列第1列から yaw を算出 @@ -951,15 +946,43 @@ public slots: }; updateTeam(state.blue_robots(), 0); updateTeam(state.yellow_robots(), 1); - } - - void handleRobotResponse(const QList& responses) { if (!m_refereeResolved) { if (++m_waitLogCount % 200 == 0) { log(stdout, "ibis: waiting for Game Controller to identify team color\n"); } return; } + + const int teamIdx = m_ibisIsBlue ? 0 : 1; + for (uint32_t id = 0; id < kMaxRobots; ++id) { + const IbisVisionState& vis = m_vision[teamIdx][id]; + if (!vis.valid) { continue; } + + uint8_t buffer[IBIS_FEEDBACK_SIZE]; + ibisBuildFeedbackPacket( + buffer, + static_cast(id), + m_counters[id]++, + vis.orientation_rad, + m_robotCache[id].ball_detected, + 0, // kick_status not tracked in ER-Force simulator + vis.x_mm / 1000.0f, + vis.y_mm / 1000.0f, + m_robotCache[id].vel_x, + m_robotCache[id].vel_y); + + m_sender->enqueue( + QByteArray(reinterpret_cast(buffer), IBIS_FEEDBACK_SIZE), + m_addr, + m_portBase + static_cast(id)); + } + } + + void handleRobotResponse(const QList& responses) { + if (!m_refereeResolved) { + return; + } + const int teamIdx = m_ibisIsBlue ? 0 : 1; for (const auto& resp : responses) { if (!resp.has_is_blue() || resp.is_blue() != m_ibisIsBlue) { continue; } @@ -989,33 +1012,6 @@ public slots: } } -private slots: - // タイマー発火時にキャッシュデータを使ってフィードバックを送信 - void onFeedbackTimer() { - if (!m_refereeResolved) { return; } - const int teamIdx = m_ibisIsBlue ? 0 : 1; - for (uint32_t id = 0; id < kMaxRobots; ++id) { - const IbisVisionState& vis = m_vision[teamIdx][id]; - if (!vis.valid) { continue; } - - uint8_t buffer[IBIS_FEEDBACK_SIZE]; - ibisBuildFeedbackPacket( - buffer, - static_cast(id), - m_counters[id]++, - vis.orientation_rad, - m_robotCache[id].ball_detected, - 0, // kick_status not tracked in ER-Force simulator - vis.x_mm / 1000.0f, - vis.y_mm / 1000.0f, - m_robotCache[id].vel_x, m_robotCache[id].vel_y); - - m_sender->enqueue( - QByteArray(reinterpret_cast(buffer), IBIS_FEEDBACK_SIZE), - m_addr, m_portBase + static_cast(id)); - } - } - private: static constexpr uint32_t kMaxRobots = 16; @@ -1031,7 +1027,6 @@ private slots: uint8_t m_counters[kMaxRobots] = {}; int m_waitLogCount = 0; - QTimer* m_feedbackTimer; PacketSenderThread* m_sender; QHostAddress m_addr; quint16 m_portBase; @@ -1078,7 +1073,6 @@ int main(int argc, char* argv[]) QCommandLineOption ibisUseRefereeOpt("ibis-use-referee", "Use Game Controller referee to auto-detect ibis team color"); QCommandLineOption ibisAccSpeedupOpt("ibis-acc-speedup", "Acceleration limit for speedup [m/s^2]", "accel", "4.0"); QCommandLineOption ibisAccBrakeOpt("ibis-acc-brake", "Acceleration limit for braking [m/s^2]", "accel", "6.0"); - QCommandLineOption ibisFeedbackHzOpt("ibis-feedback-hz", "ibis feedback send rate in Hz (default: 125)", "hz", "125"); QCommandLineOption ibisRefereePortOpt("ibis-referee-port", "Game Controller multicast port for team color detection", "port", QString::number(SSL_GAME_CONTROLLER_PORT)); parser.addOption(ibisPortOpt); parser.addOption(ibisFeedbackAddrOpt); @@ -1087,7 +1081,6 @@ int main(int argc, char* argv[]) parser.addOption(ibisUseRefereeOpt); parser.addOption(ibisAccSpeedupOpt); parser.addOption(ibisAccBrakeOpt); - parser.addOption(ibisFeedbackHzOpt); parser.addOption(ibisRefereePortOpt); parser.process(app); @@ -1169,11 +1162,10 @@ int main(int argc, char* argv[]) const QHostAddress fbAddr = QHostAddress(parser.value(ibisFeedbackAddrOpt)); const quint16 fbPortBase = static_cast(parser.value(ibisFeedbackPortBaseOpt).toUInt()); const bool useReferee = parser.isSet(ibisUseRefereeOpt); - const int feedbackHz = qMax(1, parser.value(ibisFeedbackHzOpt).toInt()); const quint16 refereePort = static_cast(parser.value(ibisRefereePortOpt).toUInt()); auto* ibisCmd = new IbisCommandAdaptor(cmdPort, &timer, accSpeedup, accBrake); - auto* ibisFb = new IbisFeedbackAdaptor(fbAddr, fbPortBase, useReferee, feedbackHz); + auto* ibisFb = new IbisFeedbackAdaptor(fbAddr, fbPortBase, useReferee); // IbisCommandAdaptor receives vision data to cache robot positions/orientations QObject::connect(&sim, &SimProxy::gotPacket, @@ -1182,7 +1174,7 @@ int main(int argc, char* argv[]) QObject::connect(ibisCmd, &IbisCommandAdaptor::sendRadioCommands, &sim, &SimProxy::handleRadioCommands); - // IbisFeedbackAdaptor receives ground truth positions at 200Hz for position/orientation + // IbisFeedbackAdaptor receives ground truth positions at 125Hz and emits one feedback packet per loop QObject::connect(&sim, &SimProxy::sendGroundTruth, ibisFb, &IbisFeedbackAdaptor::handleGroundTruth); // IbisFeedbackAdaptor receives radio responses for velocity and ball detection @@ -1203,9 +1195,9 @@ int main(int argc, char* argv[]) ibisFb->moveToThread(&rcv_thread); log(stdout, "ibis: command receiver on UDP port %d\n", cmdPort); - log(stdout, "ibis: feedback sender to %s base port %d at %d Hz\n", + log(stdout, "ibis: feedback sender to %s base port %d synchronized to simulator loop (125 Hz)\n", parser.value(ibisFeedbackAddrOpt).toStdString().c_str(), - static_cast(fbPortBase), feedbackHz); + static_cast(fbPortBase)); } rcv_thread.start(); From 813b259cb35b8695f34043614c8b613ed7140230 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 21 Apr 2026 01:20:17 +0900 Subject: [PATCH 10/22] =?UTF-8?q?fix:=20simulator-cli=E3=81=AE=E3=83=89?= =?UTF-8?q?=E3=83=AA=E3=83=96=E3=83=A9=E3=83=BC=E3=82=92perfectDribbler?= =?UTF-8?q?=E3=83=A2=E3=83=BC=E3=83=89=E3=81=AB=E5=88=87=E3=82=8A=E6=9B=BF?= =?UTF-8?q?=E3=81=88=E3=81=A6=E3=83=9C=E3=83=BC=E3=83=AB=E4=BF=9D=E6=8C=81?= =?UTF-8?q?=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ヒンジモーター経由の間接摩擦方式ではトルクがdribble_powerに比例して 低下するため、ドリブラーが非常に効きにくい問題があった。 btPoint2PointConstraintでボールを剛体拘束する既存のperfectDribblerモードを simulator-cliのデフォルトとして有効化することで修正。 - config/simulator-realism/Ibis.txtを新規作成（simulate_dribbling: false） - simulator-cliの--realismデフォルト値をRealistic→Ibisに変更 - grSimのcheckDribbleFeedbackを参考に反力 > 0.5 Nで吸着解除する機構を追加 - 解除後100msのクールダウンで即再吸着を防ぐ - 従来挙動は --realism Realistic で引き続き選択可能 --- config/simulator-realism/Ibis.txt | 21 +++++++++++++++++++++ src/amun/simulator/simrobot.cpp | 20 ++++++++++++++++++-- src/amun/simulator/simrobot.h | 1 + src/simulator/simulator.cpp | 2 +- 4 files changed, 41 insertions(+), 3 deletions(-) create mode 100644 config/simulator-realism/Ibis.txt diff --git a/config/simulator-realism/Ibis.txt b/config/simulator-realism/Ibis.txt new file mode 100644 index 0000000000..b51c8cdbc6 --- /dev/null +++ b/config/simulator-realism/Ibis.txt @@ -0,0 +1,21 @@ +stddev_ball_p: 0.0014 +stddev_robot_p: 0.0013 +stddev_robot_phi: 0.01 +stddev_ball_area: 6.5 +enable_invisible_ball: true +ball_visibility_threshold: 0.4 +camera_overlap: 1 +dribbler_ball_detections: 0.05 +camera_position_error: 0.1 +robot_command_loss: 0.03 +robot_response_loss: 0.1 +missing_ball_detections: 0.05 +vision_delay: 35000000 +vision_processing_time: 10000000 +simulate_dribbling: false +object_position_offset: 0.02 +missing_robot_detections: 0.02 +command_delay: 3000000 +robot_rotation_error: 0.5 +rotated_robot_detections_start: 0.001 +rotated_robot_detections_stop: 0.3 diff --git a/src/amun/simulator/simrobot.cpp b/src/amun/simulator/simrobot.cpp index 198ebf0bc1..5cfe92a05a 100644 --- a/src/amun/simulator/simrobot.cpp +++ b/src/amun/simulator/simrobot.cpp @@ -204,6 +204,7 @@ void SimRobot::setDribbleMode(bool perfectDribbler) stopDribbling(); } m_perfectDribbler = perfectDribbler; + m_dribblerReleaseCooldown = 0.0; } bool SimRobot::handleMoveCommand() @@ -336,10 +337,25 @@ void SimRobot::begin(SimBall *ball, double time) m_inStandby = true; } + if (m_dribblerReleaseCooldown > 0.0) { + m_dribblerReleaseCooldown = std::max(0.0, m_dribblerReleaseCooldown - time); + } + + // detect excessive constraint force and release the ball (grSim: checkDribbleFeedback, threshold 0.5 N) + if (m_holdBallConstraint) { + const float impulse = m_holdBallConstraint->getAppliedImpulse(); + const float force = impulse / SUB_TIMESTEP; + if (force > 0.5f) { + stopDribbling(); + m_dribblerReleaseCooldown = 0.1; + } + } + // enable dribbler if necessary - if (!m_inStandby && m_sslCommand.has_dribbler_speed() && m_sslCommand.dribbler_speed() > 0) { + if (!m_inStandby && m_sslCommand.has_dribbler_speed() && m_sslCommand.dribbler_speed() > 0 + && m_dribblerReleaseCooldown <= 0.0) { dribble(ball, m_sslCommand.dribbler_speed()); - } else { + } else if (m_dribblerReleaseCooldown <= 0.0) { stopDribbling(); } diff --git a/src/amun/simulator/simrobot.h b/src/amun/simulator/simrobot.h index 588a2fd77c..e9004325e2 100644 --- a/src/amun/simulator/simrobot.h +++ b/src/amun/simulator/simrobot.h @@ -113,6 +113,7 @@ class camun::simulator::SimRobot: public QObject float error_sum_omega; bool m_perfectDribbler = false; + double m_dribblerReleaseCooldown = 0.0; float m_rotationError = 0.0f; qint64 m_lastSendTime = 0; diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 85356a596a..2b704cc4e0 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -1059,7 +1059,7 @@ int main(int argc, char* argv[]) parser.addHelpOption(); QCommandLineOption geometryConfig({"g", "geometry"}, "The geometry file to load as default", "file", "2020"); - QCommandLineOption realismConfig("realism", "Simulator realism configuration (short file name without the .txt)", "realism", "Realistic"); + QCommandLineOption realismConfig("realism", "Simulator realism configuration (short file name without the .txt)", "realism", "Ibis"); QCommandLineOption localhostConfig("localhost", "Use localhost as the output address for the simulator"); parser.addOption(geometryConfig); parser.addOption(realismConfig); From a9fb9b52e64f490d0e280a512b4e3b478e4770e9 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Sep 2026 02:25:40 +0900 Subject: [PATCH 11/22] =?UTF-8?q?feat(simulator):=20ibis=E6=8C=87=E4=BB=A4?= =?UTF-8?q?=E3=81=AEcontrol=5Fmode=E3=82=92=E8=A7=A3=E9=87=88=E3=81=97?= =?UTF-8?q?=E3=83=AD=E3=83=9C=E3=83=83=E3=83=88=E5=81=B4=E4=BD=8D=E7=BD=AE?= =?UTF-8?q?=E5=88=B6=E5=BE=A1=E6=A7=8B=E6=88=90=E3=81=AB=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit crane が位置指令を送り、ロボット側の CM4 が位置制御ループを閉じて G474 へ 速度指令を渡す構成へ移行するための、simulator-cli 側の対応。 不安定で遅延が乗る無線経路を位置制御ループの外側へ出すのが狙いで、 simulator-cli は G474（STM32 メイン基板）とロボット物理を担当する。 ## control_mode を見ない復号の修正 ibisDeserialize() は CONTROL_MODE(byte 23) を無視し、CONTROL_MODE_ARGS を 常に polar velocity として復号していた。ARGS は mode により意味が変わる union のため、POSITION_TARGET(mode 4) のパケットでは terminal_velocity_x/y が polar r/theta として読まれ、無言で誤った速度指令になっていた。 CONTROL_MODE に応じて ARGS を復号するよう修正し、固定フィールドの target_global_pos / terminal_velocity と、vision_global_theta / is_vision_available も復号対象へ追加した。 ## mode 3 以外の明示的な拒否 IbisCommandAdaptor は G474 を模擬しており、G474 が実装する制御モードは POLAR_VELOCITY_TARGET(3) のみである。位置制御は CM4 側の責務なので、 mode 3 以外を受け取った場合はロボットを停止し、1秒/台のレート制限付きで 理由をログへ出す。mode 4 が直接届くのは経路に CM4 相当が入っていない 設定ミスであり、黙って誤解釈するより停止するほうが安全で切り分けも早い。 ## その他 - 送信元が制御しないスロットのゼロ埋めを ibisSlotIsEmpty() で明示的にスキップ。 従来はチーム照合の距離判定に偶然引っかかって除外されていた。 - docs/robot-side-position-control.md に crane / Orion_CM4 / G474 との 統合仕様（パケット契約・ポート割当・タイミング契約）を追加。 - data/scripts/ibis-chain-smoketest.py で実プロセスに対する回帰テストを追加。 --- README.md | 20 ++ data/scripts/ibis-chain-smoketest.py | 222 +++++++++++++++++++++ docs/robot-side-position-control.md | 287 +++++++++++++++++++++++++++ src/simulator/ibis_protocol.h | 66 +++++- src/simulator/simulator.cpp | 47 +++++ 5 files changed, 635 insertions(+), 7 deletions(-) create mode 100644 data/scripts/ibis-chain-smoketest.py create mode 100644 docs/robot-side-position-control.md diff --git a/README.md b/README.md index de7190d8f4..55ffef85e9 100644 --- a/README.md +++ b/README.md @@ -57,6 +57,26 @@ The `simulator-cli` takes three command line arguments: A `short_file_name` is just the filename without the path or the extension. For example, to start the binary with no realism and 2018 setup, call `simulator-cli -g 2018 --realism None` +#### Robot-side position control (ibis) + +The ibis command receiver emulates the robot's STM32 (G474) main board: it +accepts `POLAR_VELOCITY_TARGET` (control mode 3) only and does **not** close a +position loop. In the robot-side position control setup, `crane` sends +`POSITION_TARGET` (mode 4) commands that are consumed by the CM4 position +controller (`cm4_sim`) running between `crane` and the simulator: + +```text +crane --mode 4--> cm4_sim --mode 3--> simulator-cli + ^ | + +--- ibis feedback ---+ +``` + +A mode 4 command arriving directly at the simulator means `cm4_sim` is missing +from the chain; the simulator stops that robot and logs why instead of +misreading the mode arguments. See +[docs/robot-side-position-control.md](docs/robot-side-position-control.md) for +the packet contract and port assignment. + ### Other utilities This repo also contains various utilities: - `amun-cli` - run an AI script from the command line. diff --git a/data/scripts/ibis-chain-smoketest.py b/data/scripts/ibis-chain-smoketest.py new file mode 100644 index 0000000000..3168b77fed --- /dev/null +++ b/data/scripts/ibis-chain-smoketest.py @@ -0,0 +1,222 @@ +"""Smoke test for the ibis command path of simulator-cli. + +The simulator emulates the robot's STM32 (G474) main board: it accepts +POLAR_VELOCITY_TARGET (control mode 3) only and never closes a position loop. +POSITION_TARGET (mode 4) belongs to the robot-side CM4 controller that runs +between crane and the simulator. See docs/robot-side-position-control.md. + +Checks, both against a real simulator-cli process over UDP: + 1. a mode 3 command drives the robot + 2. a mode 4 command stops it and logs a rate-limited warning + +Usage: python3 data/scripts/ibis-chain-smoketest.py [path/to/simulator-cli] +""" + +import socket +import struct +import subprocess +import sys +import tempfile +import time +from pathlib import Path + +CMD_SIZE = 64 +SLOTS = 11 +FEEDBACK_SIZE = 128 + +MODE_POLAR_VELOCITY = 3 +MODE_POSITION_TARGET = 4 + +# The bound ports are offset well away from the defaults so a running match is +# untouched. Vision output has no port option on this branch, so it goes to the +# default address; that is send-only (no bind, no conflict), but it does inject +# extra frames into anything listening there -- do not run this during a match. +IBIS_PORT = 12397 +FEEDBACK_BASE = 50700 + + +def encode_two_byte(value, value_range): + raw = int(32767.0 * (value / value_range) + 32767.0) + raw = max(0, min(65535, raw)) + return bytes([(raw >> 8) & 0xFF, raw & 0xFF]) + + +def build_command(counter, pos, yaw, mode, args=(0.0, 0.0), target=(0.0, 0.0)): + """One 64-byte RobotCommandSerializedV2, laid out as in crane's robot_packet.h.""" + d = bytearray(CMD_SIZE) + d[1] = counter & 0xFF + d[2:4] = encode_two_byte(pos[0], 32.767) # VISION_GLOBAL_X + d[4:6] = encode_two_byte(pos[1], 32.767) # VISION_GLOBAL_Y + d[6:8] = encode_two_byte(yaw, 3.14159265) # VISION_GLOBAL_THETA + d[8:10] = encode_two_byte(yaw, 3.14159265) # TARGET_GLOBAL_THETA -> omega ~ 0 + d[12:14] = encode_two_byte(4.0, 32.767) # ACCELERATION_LIMIT + d[14:16] = encode_two_byte(3.0, 32.767) # LINEAR_VELOCITY_LIMIT + d[16:18] = encode_two_byte(5.0, 32.767) # ANGULAR_VELOCITY_LIMIT + d[22] = 0x01 # FLAGS: IS_VISION_AVAILABLE + d[23] = mode # CONTROL_MODE + d[24:26] = encode_two_byte(args[0], 32.767) # CONTROL_MODE_ARGS + d[26:28] = encode_two_byte(args[1], 32.767) + d[32:34] = encode_two_byte(target[0], 32.767) # TARGET_GLOBAL_POS_X + d[34:36] = encode_two_byte(target[1], 32.767) # TARGET_GLOBAL_POS_Y + return bytes(d) + + +def build_packet(robot_id, command): + """715-byte packet: 11 slots of (robot_id, 64-byte command), others zero-filled.""" + packet = bytearray() + for slot in range(SLOTS): + if slot == robot_id: + packet += bytes([robot_id]) + command + else: + packet += bytes([0xFF]) + bytes(CMD_SIZE) + return bytes(packet) + + +def parse_feedback(data): + if len(data) != FEEDBACK_SIZE or data[0] != 0xAB or data[1] != 0xEA: + return None + return { + "counter": data[3], + "yaw": struct.unpack_from(" 0.3 and abs(after_mode4["vx"]) < 0.05 and abs(after_mode4["vy"]) < 0.05 + return ok, (f"mode3 moved {moved3:.3f} m (want > 0.3), " + f"mode4 moved {moved4:.3f} m, " + f"final vel ({after_mode4['vx']:+.3f}, {after_mode4['vy']:+.3f}) " + f"(want ~0)") + finally: + sim.close() + + +def main(): + repo = Path(__file__).resolve().parents[2] + binary = Path(sys.argv[1]) if len(sys.argv) > 1 else repo / "build" / "bin" / "simulator-cli" + if not binary.exists(): + print(f"simulator-cli not found at {binary}", file=sys.stderr) + return 2 + + tmp = Path(tempfile.mkdtemp(prefix="ibis-smoketest-")) + log = tmp / "simulator-cli.log" + + failures = 0 + ok, detail = run_commands(binary, log) + print(f"[{'PASS' if ok else 'FAIL'}] commands: {detail}") + failures += 0 if ok else 1 + + warnings = [line for line in log.read_text().splitlines() + if "POSITION_TARGET" in line] + ok = len(warnings) > 0 + print(f"[{'PASS' if ok else 'FAIL'}] warning: {len(warnings)} POSITION_TARGET " + f"warning(s) logged (rate limited to 1/s per robot)") + failures += 0 if ok else 1 + + print(f"\nlogs: {tmp}") + return 1 if failures else 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/docs/robot-side-position-control.md b/docs/robot-side-position-control.md new file mode 100644 index 0000000000..7b146ee93c --- /dev/null +++ b/docs/robot-side-position-control.md @@ -0,0 +1,287 @@ +# ロボット側位置制御（CM4 で位置ループを閉じる構成） + +## 目的 + +従来は crane（AI）側で位置制御ループを閉じ、その出力である速度指令を無線で +ロボットへ送っていた。この構成では **不安定で遅延が乗る無線経路が位置制御 +ループの内側に入る**。 + +新構成では crane は `VisibilityGraphPlanner` が生成した **位置指令** を送り、 +ロボット側の CM4 が位置制御ループを閉じて **速度指令** をマイコン（G474）へ +渡す。無線経路はループの外側（目標値の更新経路）に移動する。 + +このドキュメントは、その構成を **シミュレータ上で実機とできるだけ同じ形** で +再現するための、3 リポジトリ間の統合仕様を定める。 + +## 構成の対応 + +### 実機 + +```text +crane ──UDP broadcast :12345 (mode 4 位置指令)──> CM4: ai_cmd_v2.out + │ 位置制御ループ + ↓ UART /dev/serial0 72B (mode 3 速度指令) + G474 (500 Hz): 加速度制御・タイヤ速度制御 + │ UART 128B feedback + ↓ + CM4: forward_robot_feedback.out + ↓ multicast 224.5.20.(100+N):50100+N + crane / host ツール +``` + +### シミュレータ + +```text +crane ──UDP :12345 (mode 4 位置指令)──> cm4_sim + │ 位置制御ループ（実機と同一コード） + ↓ UDP :12346 (mode 3 速度指令) + simulator-cli ← G474 + ロボット物理 を担当 + │ ibis feedback 128B + ↓ UDP :50100+id + cm4_sim + ↓ multicast 224.5.20.(100+id):50100+id + crane / host ツール +``` + +**対応関係** + +| 実機 | シミュレータ | 備考 | +|---|---|---| +| crane | crane | 変更なし（送信先ポートのみ切替） | +| 無線 (WiFi) | UDP + 劣化注入 | `cm4_sim` の入力側で遅延・ジッタ・ロスを注入 | +| CM4 位置制御 | `cm4_sim` | **実機と同一の position_controller ライブラリを使う** | +| UART CM4→G474 | UDP :12346 | | +| G474 の速度・加速度制御 | `simulator-cli` の `IbisCommandAdaptor` | theta P 制御 + 加速度制限 | +| ロボット物理・タイヤ | `simulator-cli` (amun/Bullet) | | +| UART G474→CM4 feedback | ibis feedback 128B (UDP) | | + +シミュレータは **G474 とロボット本体** を担当する。位置制御は行わない。 + +## パケット契約 + +### SSOT + +`RobotCommandSerializedV2`（64 バイト）の正本は +`crane/crane_sender/include/crane_sender/robot_packet.h` とする。 + +現在のオフセット一致状況（2026-09-13 実測）: + +| リポジトリ | ファイル | 状態 | +|---|---|---| +| crane | `crane_sender/include/crane_sender/robot_packet.h` | 正本 | +| G474_Orion_main | `Core/Inc/robot_packet.h` | byte 0..31 一致（32..37 は未使用のため未定義。問題なし） | +| framework | `src/simulator/ibis_protocol.h` | 一致 | +| **Orion_CM4** | `cm4/bridge/robot_packet.h` | **不一致（旧レイアウト）** | + +Orion_CM4 のコピーは `ACCELERATION_LIMIT` が無いため **byte 12 以降が 2 バイト +ずれている**（`FLAGS`=20、`CONTROL_MODE`=21）。現在これが顕在化していないのは、 +`forward_ai_cmd_v2.cpp` が受信バイト列を `memcpy` でそのまま UART へ転送する +**単なるバイト転送器** であり、デシリアライズ結果をデバッグ表示にしか使って +いないため。CM4 がパケットを解釈して制御ループを閉じた瞬間、これは正真正銘の +バグになる。**新構成の実装前に必ず統一すること。** + +### バイトオフセット（正本） + +```text + 0 HEADER + 1 CHECK_COUNTER + 2..3 VISION_GLOBAL_X (float, range 32.767) + 4..5 VISION_GLOBAL_Y (float, range 32.767) + 6..7 VISION_GLOBAL_THETA (float, range PI) + 8..9 TARGET_GLOBAL_THETA (float, range PI) +10 KICK_POWER (value * 20) +11 DRIBBLE_POWER (value * 20) +12..13 ACCELERATION_LIMIT (float, range 32.767) +14..15 LINEAR_VELOCITY_LIMIT (float, range 32.767) +16..17 ANGULAR_VELOCITY_LIMIT (float, range 32.767) +18..19 LATENCY_TIME_MS (uint16) +20..21 ELAPSED_TIME_MS_SINCE_LAST_VISION (uint16) +22 FLAGS +23 CONTROL_MODE +24..31 CONTROL_MODE_ARGS (mode により意味が変わる union) +32..33 TARGET_GLOBAL_POS_X (float, range 32.767) +34..35 TARGET_GLOBAL_POS_Y (float, range 32.767) +36..37 TERMINAL_VELOCITY (float, range 32.767) +``` + +FLAGS: bit0 `IS_VISION_AVAILABLE` / bit1 `ENABLE_CHIP` / bit3 `STOP_EMERGENCY` + +### 制御モード + +| 値 | 名前 | ARGS (24..31) | 送信元 → 受信先 | +|---|---|---|---| +| 3 | `POLAR_VELOCITY_TARGET_MODE` | `target_global_velocity_r`, `target_global_velocity_theta` | CM4 → G474 / cm4_sim → simulator-cli | +| 4 | `POSITION_TARGET_WITH_TERMINAL_VELOCITY_MODE` | `terminal_velocity_x`, `terminal_velocity_y` | crane → CM4 / crane → cm4_sim | + +**`CONTROL_MODE_ARGS` は union であり、`CONTROL_MODE` を見ずに復号してはならない。** +mode 4 のパケットを mode 3 として復号すると、`terminal_velocity_x/y` が +`r/theta` として読まれ、無言で暴走する。 + +### パケット全体 + +1 スロット = `robot_id` 1 バイト + コマンド 64 バイト = 65 バイト。 +11 スロット固定で 715 バイト。送信元が制御しないロボットのスロットは +ゼロ埋めする（受信側は `ibisSlotIsEmpty()` 相当で明示的にスキップすること）。 + +## ポート割当 + +| 経路 | アドレス:ポート | 備考 | +|---|---|---| +| crane → cm4_sim（位置指令） | `127.0.0.1:12345` | 実機の AI 指令ポートと同じ | +| cm4_sim → simulator-cli（速度指令） | `127.0.0.1:12346` | `simulator-cli --ibis-port 12346` | +| simulator-cli → cm4_sim（feedback） | `127.0.0.1:50100+id` | `--ibis-feedback-addr 127.0.0.1`（既定） | +| cm4_sim → crane / host（feedback 再配信） | `224.5.20.(100+id):50100+id` | 実機と同じ multicast | +| simulator-cli → crane（vision） | `224.5.23.2:10020`（既定・変更不可） | 変更なし | + +feedback のベースポートは **実機と同じ 50100 のまま** でよい。同一ホスト上で + +- `cm4_sim` が `127.0.0.1:50100+id` を bind（simulator-cli からの unicast を受ける） +- `crane_robot_receiver` が `224.5.20.(100+i):50100+i` を bind（multicast を受ける） + +という 2 つの bind が同居するが、**これらはポート番号が同じでも競合しない**ことを +実測で確認済み。unicast は unicast ソケットにのみ、multicast は multicast ソケットに +のみ配送され、取り違えも起きない。両者とも `SO_REUSEADDR` を設定すること。 + +simulator-cli の `--ibis-feedback-addr` の既定値が `127.0.0.1` なので、 +feedback 関連のオプション指定は不要である。 + +vision の出力先ポート・アドレスを変えるオプションは無い（`ibis` ブランチ時点）。 +`simulator-cli` は vision を **10020 番**でマルチキャストする（通常の 10006 ではなく、 +大会ネットワークでの衝突回避のため）。同一ホストで複数インスタンスを走らせると +vision が混線するので、並列実行が必要ならコンテナのネットワーク分離を使うこと。 + +また、`simulator-cli` は SSL tracker（`TrackedFrame`）を出力しない。 +crane はロボットを tracker 経路で追跡するため、**外部の auto-referee が必要**である +（`docker/scenario/docker-compose.yaml` の `autoref-tigers` がこれを担っている）。 + +## タイミング契約 + +### 各段のレート + +シミュレータは実時間で動く（wall clock 駆動）。 + +| 段 | 実機 | シミュレータ | +|---|---|---| +| crane 指令 | 約 60 Hz | vision フレーム毎 = 62.5 Hz | +| CM4 位置制御ループ | 1 kHz（`usleep(1000)` ポーリング） | `--rate-hz`（既定 1 kHz） | +| G474 メインループ | 500 Hz (`MAIN_LOOP_CYCLE`) | 125 Hz（sim 基本ループ 8 ms） | +| vision 送出 | 約 60 Hz | 62.5 Hz（基本ループ 2 回に 1 回） | +| feedback | 約 125 Hz | 125 Hz（基本ループ毎） | + +simulator-cli の基本ループは **8 ms = 125 Hz**（`Simulator::setScaling()` の +"scale default timing of 8 milliseconds (125Hz)"）で、vision はその 2 回に 1 回なので +62.5 Hz。Bullet の物理サブステップは 250 Hz（`SUB_TIMESTEP = 1/250.f`）。 + +CM4 制御ループ 1 kHz に対し G474 相当（シミュレータ基本ループ）が 125 Hz なので、 +**実機（1 kHz : 500 Hz）より内側ループが粗い**。制御ゲインを詰める際はこの差を +意識すること。 + +### feedback はループ駆動である + +`simulator-cli` の ibis feedback は独立したタイマーではなく +`Simulator::handleSimulatorTick()` の中（`src/amun/simulator/simulator.cpp`）で +**シミュレータ基本ループ 1 回につき 1 回** 送出される。位置は +`world::SimulatorState`（ground truth）由来なので毎回新鮮である +（`--ibis-feedback-hz` オプションは存在しない）。 + +新構成では **この feedback の位置が位置制御ループ内で唯一の位置信号** になる。 +従来は装飾的なテレメトリだったが、これからは制御品質を直接左右する。 + +### check_counter の扱い（重要） + +G474 の `checkConnect2AI()`（`Core/Src/ai_comm.c`）は +**`check_counter` が変化し続けること** を AI 接続生存の判定に使う。 +変化が `AI_CMD_TIMEOUT(0.5) * MAIN_LOOP_CYCLE(500)` = **250 ms** 途切れると +`connected_ai = false` になる。 + +従来の CM4 は crane のパケットをそのまま転送していたため、`check_counter` は +crane が採番したものだった。新構成では CM4 が自分の制御周期で新しい指令を +生成するので、**CM4 が `check_counter` を自分で採番する** 必要がある。 + +その結果、G474 の `connected_ai` は **crane の生存を意味しなくなる**。 +crane からのパケットが途絶えた場合の安全停止は、CM4 側で明示的に実装すること +（例: crane 無通信が一定時間続いたら `STOP_EMERGENCY` を立てる、または +速度指令をゼロにする）。これは実機・シミュレータ双方に共通の要件である。 + +### cm4_sim のペーシング + +`cm4_sim` は実機の `forward_ai_cmd_v2.cpp` と同じ構造にする。 + +- `--rate-hz`（既定 1 kHz）の固定周期で回し、最後に受信した feedback を使う。 +- **feedback 待ちでブロックしてはならない。** ソケットはノンブロッキングにし、 + 届いていなければ前回値で制御する（実機の `usleep(1000)` ポーリングループと同じ）。 +- 11 台分を **1 プロセス・1 データグラムに集約** して送る。台数分のプロセスを + 立てると、同一ポートへの送信が増えるだけで実機の構成に近づかない。 +- crane の指令は約 60 Hz でしか来ないので、CM4 制御ループはその間 + 同じ目標値に対して feedback だけを更新しながら回ることになる。これは実機と同じ。 + +## framework 側の実装状況（本リポジトリ・実装済み） + +- `src/simulator/ibis_protocol.h` + - `IBIS_MODE_POLAR_VELOCITY_TARGET` (3) / `IBIS_MODE_POSITION_TARGET` (4) を定義。 + - `ibisDeserialize()` が `CONTROL_MODE` を見て ARGS を復号するようになった。 + 従来は mode を無視して常に polar velocity として読んでいた。 + - `target_global_pos` / `terminal_velocity` / `vision_global_theta` / + `is_vision_available` を復号対象に追加。 + - `ibisSlotIsEmpty()` を追加。 +- `src/simulator/simulator.cpp` `IbisCommandAdaptor` + - 空スロットを明示的にスキップ。 + - **mode 3 以外を受け取ったらロボットを停止し、1 秒/台 のレート制限付きで警告** + を出す。mode 4 が届くのは「cm4_sim が経路に入っていない」設定ミスであり、 + 無言で誤解釈するより停止して理由を出すほうが安全かつデバッグしやすい。 + +simulator-cli 側に位置制御は **実装しない**。実装すると制御則のコピーが +4 つ目になり、`robot_packet.h` が 3 リポジトリで食い違った問題を繰り返す。 + +## 検証（A/B 比較） + +この設計の主張は「無線経路をループ外に出すと、遅延・ジッタ・ロスに強くなる」 +である。それを示すには、同じ劣化条件下で旧構成と新構成を比較する必要がある。 + +| | 旧構成 | 新構成 | +|---|---|---| +| crane | 位置ループを閉じ mode 3 を送る | mode 4 を送る | +| 経路 | crane → simulator-cli | crane → cm4_sim → simulator-cli | +| 劣化注入 | crane の送信経路 | cm4_sim の入力側 | + +`cm4_sim` の入力側に `--rx-delay-ms` / `--rx-jitter-ms` / `--rx-loss-rate` を実装し、 +両構成を同一条件で走らせて追従誤差・オーバーシュート・到達時間を比較する。 + +### simulator-cli 側のスモークテスト + +`data/scripts/ibis-chain-smoketest.py` が、実際に `simulator-cli` を起動して +UDP 越しに以下を検証する。 + +```bash +python3 data/scripts/ibis-chain-smoketest.py [path/to/simulator-cli] +``` + +1. mode 3 でロボットが動く +2. mode 4 でロボットが停止し、レート制限付き警告が出る + +`cm4_sim` を実装する前に simulator-cli 側の契約を固定するためのもの。 +既定ポートとは離れたポート（12397 / 50700 / 10097 / 11097）を使うので、 +動作中の試合には影響しない。 + +## 既知の忠実度ギャップ + +- **feedback の位置が真値である。** シミュレータの feedback は ground truth を + そのまま返すため、ノイズも vision 遅延も無い。実機の G474 は + `vision_based_position` としてタイヤオドメトリと vision を融合した推定値を返す。 + シミュレータ上の位置制御は実機より良く見える。必要ならノイズ注入を追加する。 +- **ボールセンサ・キック状態。** `kick_status` は常に 0。 +- **ローカルカメラ。** `cam_server_v3` 相当は無い。`cm4_sim` はカメラ領域を + ゼロ埋めすること（実機のカメラ未接続時と同じ扱い）。 +- **バッテリ電圧・温度等。** 固定値。 + +## 関連ファイル + +- `src/simulator/ibis_protocol.h` — プロトコル定義（framework 側） +- `data/scripts/ibis-chain-smoketest.py` — ibis コマンド経路のスモークテスト +- `src/simulator/simulator.cpp` — `IbisCommandAdaptor` / `IbisFeedbackAdaptor` +- `src/amun/simulator/simulator.cpp` — `handleSimulatorTick()`、feedback 送出 +- `Orion_CM4/cm4/bridge/robot_packet.h` — CM4 側パケット定義（要統一） +- `Orion_CM4/doc/control_packet.md` — CM4 側の制御パケット仕様 +- `G474_Orion_main/Core/Src/ai_comm.c` — `check_counter` による接続監視 +- `crane/crane_sender/src/ibis_sender_node.cpp` — mode 4 送信 +- `crane/crane_local_planner/src/visibility_graph_planner.cpp` — 位置目標の生成 diff --git a/src/simulator/ibis_protocol.h b/src/simulator/ibis_protocol.h index 53e7e693ba..207534399a 100644 --- a/src/simulator/ibis_protocol.h +++ b/src/simulator/ibis_protocol.h @@ -30,6 +30,16 @@ constexpr int IBIS_FEEDBACK_SIZE = 128; constexpr int IBIS_FEEDBACK_PORT_BASE = 50100; constexpr double IBIS_POSITION_MATCH_THRESHOLD = 0.5; // metres +// Control modes. Must match crane_sender/include/crane_sender/robot_packet.h +// and Orion_CM4/cm4/bridge/robot_packet.h (ControlMode enum). +// +// The simulator plays the role of the robot's STM32 (G474) main board, which +// only ever implements POLAR_VELOCITY_TARGET. POSITION_TARGET is closed on the +// robot's CM4 (cm4_sim in simulation), never here -- see +// docs/robot-side-position-control.md. +constexpr uint8_t IBIS_MODE_POLAR_VELOCITY_TARGET = 3; +constexpr uint8_t IBIS_MODE_POSITION_TARGET = 4; + // --------------------------------------------------------------------------- // Byte offsets in the 64-byte RobotCommandSerializedV2 (from crane's robot_packet.h) // --------------------------------------------------------------------------- @@ -80,7 +90,10 @@ enum IbisFlagBit { // --------------------------------------------------------------------------- struct IbisCommand { + uint8_t control_mode; // IBIS_MODE_* float vision_global_pos[2]; // metres, SSL vision coordinate system + float vision_global_theta; // radians + bool is_vision_available; float target_global_theta; // radians float kick_power; // 0..1 normalised float dribble_power; // 0..1 normalised @@ -89,8 +102,11 @@ struct IbisCommand { float acceleration_limit; // m/s^2 (0 means "use default") float linear_velocity_limit; // m/s (0 means "no limit") float angular_velocity_limit; // rad/s - float polar_velocity_r; // m/s - float polar_velocity_theta; // radians (global direction) + float polar_velocity_r; // m/s (mode 3 args) + float polar_velocity_theta; // radians (global direction, mode 3 args) + float terminal_velocity_xy[2]; // m/s (mode 4 args) + float target_global_pos[2]; // metres (fixed field, modes >= 4) + float terminal_velocity; // m/s (fixed field, modes >= 4) uint8_t check_counter; }; @@ -114,8 +130,10 @@ inline IbisCommand ibisDeserialize(const uint8_t* d) { IbisCommand cmd; cmd.check_counter = d[CHECK_COUNTER]; + cmd.control_mode = d[CONTROL_MODE]; cmd.vision_global_pos[0] = ibisDecodeTwoByte(d[VISION_GLOBAL_X_H], d[VISION_GLOBAL_X_L], 32.767f); cmd.vision_global_pos[1] = ibisDecodeTwoByte(d[VISION_GLOBAL_Y_H], d[VISION_GLOBAL_Y_L], 32.767f); + cmd.vision_global_theta = ibisDecodeTwoByte(d[VISION_GLOBAL_TH_H], d[VISION_GLOBAL_TH_L], static_cast(M_PI)); cmd.target_global_theta = ibisDecodeTwoByte(d[TARGET_GLOBAL_TH_H], d[TARGET_GLOBAL_TH_L], static_cast(M_PI)); cmd.kick_power = d[KICK_POWER] / 20.f; cmd.dribble_power = d[DRIBBLE_POWER] / 20.f; @@ -124,16 +142,50 @@ inline IbisCommand ibisDeserialize(const uint8_t* d) cmd.angular_velocity_limit = ibisDecodeTwoByte(d[ANGULAR_VEL_LIMIT_H], d[ANGULAR_VEL_LIMIT_L], 32.767f); uint8_t flags = d[FLAGS]; - cmd.enable_chip = (flags >> ENABLE_CHIP) & 0x01; - cmd.stop_emergency = (flags >> STOP_EMERGENCY) & 0x01; + cmd.is_vision_available = (flags >> IS_VISION_AVAILABLE) & 0x01; + cmd.enable_chip = (flags >> ENABLE_CHIP) & 0x01; + cmd.stop_emergency = (flags >> STOP_EMERGENCY) & 0x01; + + // CONTROL_MODE_ARGS (offset 24..31) is a union: its meaning depends on + // control_mode. Decoding it unconditionally as polar velocity would read + // mode 4's terminal_velocity_x/y as r/theta, which is silent garbage. + cmd.polar_velocity_r = 0.0f; + cmd.polar_velocity_theta = 0.0f; + cmd.terminal_velocity_xy[0] = 0.0f; + cmd.terminal_velocity_xy[1] = 0.0f; + switch (cmd.control_mode) { + case IBIS_MODE_POLAR_VELOCITY_TARGET: + cmd.polar_velocity_r = ibisDecodeTwoByte(d[CONTROL_MODE_ARGS + 0], d[CONTROL_MODE_ARGS + 1], 32.767f); + cmd.polar_velocity_theta = ibisDecodeTwoByte(d[CONTROL_MODE_ARGS + 2], d[CONTROL_MODE_ARGS + 3], 32.767f); + break; + case IBIS_MODE_POSITION_TARGET: + cmd.terminal_velocity_xy[0] = ibisDecodeTwoByte(d[CONTROL_MODE_ARGS + 0], d[CONTROL_MODE_ARGS + 1], 32.767f); + cmd.terminal_velocity_xy[1] = ibisDecodeTwoByte(d[CONTROL_MODE_ARGS + 2], d[CONTROL_MODE_ARGS + 3], 32.767f); + break; + default: + break; + } - // POLAR_VELOCITY_TARGET_MODE args at CONTROL_MODE_ARGS (offset 24) - cmd.polar_velocity_r = ibisDecodeTwoByte(d[CONTROL_MODE_ARGS + 0], d[CONTROL_MODE_ARGS + 1], 32.767f); - cmd.polar_velocity_theta = ibisDecodeTwoByte(d[CONTROL_MODE_ARGS + 2], d[CONTROL_MODE_ARGS + 3], 32.767f); + // Fixed fields, present regardless of mode (offsets 32..37). + cmd.target_global_pos[0] = ibisDecodeTwoByte(d[TARGET_POS_X_H], d[TARGET_POS_X_L], 32.767f); + cmd.target_global_pos[1] = ibisDecodeTwoByte(d[TARGET_POS_Y_H], d[TARGET_POS_Y_L], 32.767f); + cmd.terminal_velocity = ibisDecodeTwoByte(d[TERMINAL_VEL_H], d[TERMINAL_VEL_L], 32.767f); return cmd; } +// True when a robot slot carries no command at all. Senders zero-fill the slots +// of robots they do not control; a zero-filled slot decodes to a position of +// (-32.767, -32.767) which no team match would accept, but relying on that is +// accidental -- check explicitly instead. +inline bool ibisSlotIsEmpty(const uint8_t* d) +{ + for (int i = 0; i < IBIS_CMD_SIZE; ++i) { + if (d[i] != 0) { return false; } + } + return true; +} + // --------------------------------------------------------------------------- // 128-byte feedback packet builder // Ported from grSim BinaryFeedbackSender::buildPacket, using scalar args diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 2b704cc4e0..aad6a5f769 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -721,6 +721,10 @@ private slots: } const uint8_t* cmd_data = buf + offset + 1; + // Senders zero-fill the slots of robots they do not control. + if (ibisSlotIsEmpty(cmd_data)) { + continue; + } if (cmd_data[CHECK_COUNTER] == m_robotStates[robot_id].last_check_counter) { continue; } @@ -760,6 +764,20 @@ private slots: lv->set_angular(0.0f); m_robotStates[robot_id].prev_vx = 0.0; m_robotStates[robot_id].prev_vy = 0.0; + } else if (cmd.control_mode != IBIS_MODE_POLAR_VELOCITY_TARGET) { + // This adaptor emulates the robot's STM32 (G474) main board, which + // implements POLAR_VELOCITY_TARGET only. Any other mode means the + // chain is misconfigured -- most likely a POSITION_TARGET command + // that should have been consumed by the robot-side position loop + // (cm4_sim) before reaching the simulator. Hold the robot still and + // say why, rather than steering on reinterpreted mode args. + warnUnsupportedMode(robot_id, cmd.control_mode); + auto* lv = robotCmd->mutable_move_command()->mutable_local_velocity(); + lv->set_forward(0.0f); + lv->set_left(0.0f); + lv->set_angular(0.0f); + m_robotStates[robot_id].prev_vx = 0.0; + m_robotStates[robot_id].prev_vy = 0.0; } else { const double current_theta = vis->orientation_rad; @@ -837,12 +855,41 @@ private slots: } private: + // Logs at most once per second per robot, so a persistently misconfigured + // chain produces a readable hint instead of a flood at the command rate. + void warnUnsupportedMode(int robot_id, uint8_t mode) { + constexpr qint64 kWarnIntervalNs = 1000LL * 1000LL * 1000LL; + auto& state = m_robotStates[robot_id]; + const qint64 now = m_timer->currentTime(); + if (state.mode_warned && now - state.last_mode_warn_ns < kWarnIntervalNs) { + return; + } + state.mode_warned = true; + state.last_mode_warn_ns = now; + if (mode == IBIS_MODE_POSITION_TARGET) { + log(stdout, + "ibis: robot %d sent POSITION_TARGET (mode %u), robot stopped. The simulator " + "emulates the STM32 main board and does not close a position loop -- run the " + "CM4 position controller (cm4_sim) between crane and the simulator. " + "See docs/robot-side-position-control.md\n", + robot_id, static_cast(mode)); + } else { + log(stdout, + "ibis: robot %d sent unsupported control mode %u, robot stopped " + "(expected POLAR_VELOCITY_TARGET = %u)\n", + robot_id, static_cast(mode), + static_cast(IBIS_MODE_POLAR_VELOCITY_TARGET)); + } + } + static constexpr uint32_t kMaxRobots = 16; struct PerRobotState { double prev_vx = 0.0; double prev_vy = 0.0; uint8_t last_check_counter = 0xFF; + qint64 last_mode_warn_ns = 0; + bool mode_warned = false; }; // m_vision[0] = blue, m_vision[1] = yellow From ee2e6b4f958ea14b71b09ec9bebca91055dd4493 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Sep 2026 02:34:13 +0900 Subject: [PATCH 12/22] =?UTF-8?q?docs:=20feedback=E3=83=9D=E3=83=BC?= =?UTF-8?q?=E3=83=88=E8=A1=9D=E7=AA=81=E3=83=BB=E7=B5=82=E7=AB=AF=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E3=81=AE=E6=84=8F=E5=91=B3=E3=83=BBA/B=E6=B3=A8?= =?UTF-8?q?=E5=85=A5=E7=82=B9=E3=82=92=E4=BB=95=E6=A7=98=E3=81=B8=E8=BF=BD?= =?UTF-8?q?=E8=A8=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit crane-side セッションからの指摘を実測で検証し、誤りを訂正した。 ## 訂正: feedback ポートは「crane 側の変更不要」ではない crane_robot_receiver は sim_mode が真だと購読先を 127.0.0.1 に切り替える (robot_receiver_node.cpp:351-352)。crane_comm/unicast.hpp:81 は is_multicast() で分岐するため 127.0.0.1 は素の bind に落ちてグループ参加もせず、さらに :78-79 で SO_REUSEADDR と SO_REUSEPORT の両方を設定している。 つまり sim:=true の crane は、cm4_sim が feedback を受けるのに必要な 127.0.0.1:50100+id を同じ形で先に押さえる。SO_REUSEPORT 付き2ソケットの実測では 単一送信元フローが片方に全量入り(0 / 200)、均等分割にはならない。feedback は 位置制御ループ内で唯一の位置信号なので、これは制御が完全に死ぬか動くかの二択に なり、しかも4-tupleハッシュ次第で実行ごとに変わる。 対処として CM4-in-the-loop 構成では crane に feedback_sim_mode:=false を渡す。 multicast 受けと unicast 受けが同居する場合は完全分離されることも実測確認済み。 ## mode 4 の終端速度フィールドの意味を定義 terminal_velocity_x/y(ARGS 24..27) は到達時の速度ベクトル(フィードフォワード)、 byte 36..37 のスカラは その大きさに対する上限 であり、冗長ではない。 クランプは受信側の責務で送信側に整合義務は無く、スカラ 0 は「上限なし」であって 「停止」ではない。参照実装は calculateSimGlobalVelocity()。 position_tolerance は ibis パケットに載っていないため CM4 は到達判定を再現できず、 固定定数を使う。ARGS 28..31 が空いているので拡張は可能。 ## A/B の劣化注入点を対称化 cm4_sim が受信 mode で分岐し(3=passthrough, 4=位置制御)、劣化注入は mode に よらず入力側で常に適用する。これで独立変数が「位置ループをどこで閉じるか」だけに なる。mode 分岐は実機 CM4 にも必要な後方互換経路なのでシミュレータ専用の仕掛けでもない。 --- docs/robot-side-position-control.md | 121 +++++++++++++++++++++++++--- 1 file changed, 109 insertions(+), 12 deletions(-) diff --git a/docs/robot-side-position-control.md b/docs/robot-side-position-control.md index 7b146ee93c..5401d1e5f6 100644 --- a/docs/robot-side-position-control.md +++ b/docs/robot-side-position-control.md @@ -113,6 +113,47 @@ FLAGS: bit0 `IS_VISION_AVAILABLE` / bit1 `ENABLE_CHIP` / bit3 `STOP_EMERGENCY` | 3 | `POLAR_VELOCITY_TARGET_MODE` | `target_global_velocity_r`, `target_global_velocity_theta` | CM4 → G474 / cm4_sim → simulator-cli | | 4 | `POSITION_TARGET_WITH_TERMINAL_VELOCITY_MODE` | `terminal_velocity_x`, `terminal_velocity_y` | crane → CM4 / crane → cm4_sim | +#### mode 4 の終端速度フィールドの意味（重要） + +mode 4 は終端速度を 2 か所で運ぶが、**これらは冗長ではなく意味が違う**。 + +| フィールド | 位置 | 意味 | +|---|---|---| +| `terminal_velocity_x` / `terminal_velocity_y` | ARGS 24..27 | 目標位置に到達した瞬間の速度**ベクトル**（グローバル座標, m/s）。フィードフォワード項であり、向きに意味がある | +| `terminal_velocity`（= crane の `speed_limit_at_target`） | 32..37 の 36..37 | 上のベクトルの**大きさに対する上限**（スカラ）。名前に反して「終端速度そのもの」ではない | + +**受信側（CM4）が適用すべき規則**（`crane_sender/src/sim_position_controller.cpp` +`calculateSimGlobalVelocity()` が参照実装）: + +``` +feedforward = (terminal_velocity_x, terminal_velocity_y) +if terminal_velocity > 0: # スカラ上限。0 は「上限なし」であって「停止」ではない + feedforward = clampNorm(feedforward, terminal_velocity) +``` + +- **送信側に整合義務は無い。** クランプするのは受信側。 + `|v_xy| == terminal_velocity` を crane に要求しない。 +- `terminal_velocity == 0` は **上限なし**（クランプをスキップ）。 + 停止させたい場合は `terminal_velocity_x/y` 自体を 0 にする。実際 + `visibility_graph_planner.cpp` の到達時分岐はそうしている。 +- なお現行の VisibilityGraphPlanner は非最終ウェイポイントで + `speed_limit_at_target = terminal_speed` かつ + `terminal_velocity_xy = direction * terminal_speed` を入れるので、結果として + 両者の大きさは一致する。これは実装の都合であって仕様上の保証ではない。 + +#### 伝送されないフィールド + +`crane_msgs/msg/PositionTargetMode.msg` の `position_tolerance` は +**ibis パケットに載っていない**（`createRobotPacket()` が送っていない）。 +参照実装は到達判定に +`error.norm() <= position_tolerance && feedforward.norm() < 1e-4` を使うため、 +CM4 はこの判定をそのまま再現できない。 + +暫定方針: CM4 側の固定定数（例 0.02 m）を使う。 +ARGS には 28..31 の 4 バイトが空いているので、精度が問題になるなら +`position_tolerance` を 28..29 へ載せる拡張が可能。その場合は 4 リポジトリすべての +`robot_packet.h` を同時に更新すること。 + **`CONTROL_MODE_ARGS` は union であり、`CONTROL_MODE` を見ずに復号してはならない。** mode 4 のパケットを mode 3 として復号すると、`terminal_velocity_x/y` が `r/theta` として読まれ、無言で暴走する。 @@ -130,17 +171,54 @@ mode 4 のパケットを mode 3 として復号すると、`terminal_velocity_x | crane → cm4_sim（位置指令） | `127.0.0.1:12345` | 実機の AI 指令ポートと同じ | | cm4_sim → simulator-cli（速度指令） | `127.0.0.1:12346` | `simulator-cli --ibis-port 12346` | | simulator-cli → cm4_sim（feedback） | `127.0.0.1:50100+id` | `--ibis-feedback-addr 127.0.0.1`（既定） | -| cm4_sim → crane / host（feedback 再配信） | `224.5.20.(100+id):50100+id` | 実機と同じ multicast | +| cm4_sim → crane / host（feedback 再配信） | `224.5.20.(100+id):50100+id` | 実機と同じ multicast。**crane 側に `feedback_sim_mode:=false` が必須**（下記） | | simulator-cli → crane（vision） | `224.5.23.2:10020`（既定・変更不可） | 変更なし | -feedback のベースポートは **実機と同じ 50100 のまま** でよい。同一ホスト上で +### feedback ポートの取り合い（必読・踏むと沈黙して壊れる） + +feedback のベースポートは実機と同じ 50100 を使うが、**crane を素の `sim:=true` で +起動すると cm4_sim と衝突する。** + +`crane_robot_receiver` は `sim_mode` が真だと購読先を `127.0.0.1` に切り替える +（`robot_receiver_node.cpp:351-352`）。`crane_comm/unicast.hpp:81` は +`addr.is_multicast()` で分岐するため、`127.0.0.1` は multicast 側ではなく +`:138-139` の素の bind に落ち、**グループ参加もしない**。さらに `:78-79` で +`SO_REUSEADDR` と **`SO_REUSEPORT` の両方**を設定している。 + +つまり `sim:=true` の crane は、cm4_sim が simulator-cli の feedback を受けるために +必要な `127.0.0.1:50100+id` を、まったく同じ形で先に押さえる。 -- `cm4_sim` が `127.0.0.1:50100+id` を bind（simulator-cli からの unicast を受ける） -- `crane_robot_receiver` が `224.5.20.(100+i):50100+i` を bind（multicast を受ける） +`SO_REUSEPORT` 付きの 2 ソケットが同一ポートを bind した場合の実測（unicast 200 発、 +送信元は単一ソケット）: -という 2 つの bind が同居するが、**これらはポート番号が同じでも競合しない**ことを -実測で確認済み。unicast は unicast ソケットにのみ、multicast は multicast ソケットに -のみ配送され、取り違えも起きない。両者とも `SO_REUSEADDR` を設定すること。 +| | 受信数 | +|---|---| +| socket A | **0** | +| socket B | **200** | + +**均等分割ではなく片方が全部取る。** `SO_REUSEPORT` の振り分けは送信元を含む +4-tuple ハッシュで決まるため、simulator-cli が単一ソケットから送る feedback は +単一フローとなり、必ずどちらか一方に全量が入る。どちらが当たるかはハッシュ次第で +実行ごとに変わりうる。 + +新構成では feedback が位置制御ループ内で**唯一の位置信号**なので、これは +「cm4_sim の位置制御が完全に死ぬ」か「正常に動く」かの二択になり、しかも +実行ごとに変わるため切り分けが極めて困難になる。 + +**対処**: CM4-in-the-loop 構成では crane に **`feedback_sim_mode:=false`** を渡す +（`crane.launch.xml` に新設済み。既定は `$(var sim)` なので既存構成は不変）。 +これで crane は実機と同じ `224.5.20.(100+id):50100+id` の multicast を bind し、 +unicast は cm4_sim が独占する。 + +この条件下では両者は競合しない。multicast 受け（`0.0.0.0:port` bind + group join）と +unicast 受け（`127.0.0.1:port` bind）の同居を実測した結果: + +| 送信 | multicast socket | unicast socket | +|---|---|---| +| unicast 200 発 | 0 | 200 | +| multicast 200 発 | 200 | 0 | + +完全に分離され、取り違えも取り合いも起きない。 simulator-cli の `--ibis-feedback-addr` の既定値が `127.0.0.1` なので、 feedback 関連のオプション指定は不要である。 @@ -238,14 +316,33 @@ simulator-cli 側に位置制御は **実装しない**。実装すると制御 この設計の主張は「無線経路をループ外に出すと、遅延・ジッタ・ロスに強くなる」 である。それを示すには、同じ劣化条件下で旧構成と新構成を比較する必要がある。 +比較を成立させるには、**劣化注入点が両構成で同一**でなければならない。 +旧構成を「crane → simulator-cli 直送」にすると経路上に注入器が無く、比較にならない。 + +そこで `cm4_sim` は **受信した mode で振る舞いを切り替える**: + +| 受信 mode | cm4_sim の動作 | +|---|---| +| 3（polar velocity） | 位置制御をせずそのまま転送（passthrough）。実機の CM4 の現行動作と同じ | +| 4（position target） | 位置制御ループを回して mode 3 を生成 | + +劣化注入は mode によらず**入力側で常に適用**する。これで両構成は次のようになる。 + | | 旧構成 | 新構成 | |---|---|---| | crane | 位置ループを閉じ mode 3 を送る | mode 4 を送る | -| 経路 | crane → simulator-cli | crane → cm4_sim → simulator-cli | -| 劣化注入 | crane の送信経路 | cm4_sim の入力側 | - -`cm4_sim` の入力側に `--rx-delay-ms` / `--rx-jitter-ms` / `--rx-loss-rate` を実装し、 -両構成を同一条件で走らせて追従誤差・オーバーシュート・到達時間を比較する。 +| 経路 | crane → cm4_sim(passthrough) → simulator-cli | crane → cm4_sim(位置制御) → simulator-cli | +| 劣化注入 | cm4_sim 入力側 | cm4_sim 入力側（同一コード経路） | +| 位置ループの位置 | crane（無線がループ内） | CM4（無線がループ外） | + +**独立変数は「位置ループをどこで閉じるか」だけ**になり、転送経路・注入点・注入実装が +両者で完全に一致する。旧構成が厳密な「直送」でなくなるが、localhost の UDP 1 ホップ +追加は無線劣化に比べて無視できる。mode による分岐は実機の CM4 にも必要な後方互換 +経路なので、シミュレータ専用の仕掛けを増やすことにもならない。 + +`cm4_sim` の入力側に `--rx-delay-ms` / `--rx-jitter-ms` / `--rx-loss-rate` と +再現用のシードを実装し、両構成を同一条件で走らせて追従誤差・オーバーシュート・ +到達時間を比較する。 ### simulator-cli 側のスモークテスト From e4c1c332db1d91c752c9c7de64329bb5867bbbaa Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Sep 2026 03:01:07 +0900 Subject: [PATCH 13/22] =?UTF-8?q?fix(simulator):=20ibis=20feedback?= =?UTF-8?q?=E3=81=AE=E3=83=AC=E3=82=A4=E3=82=A2=E3=82=A6=E3=83=88=E3=82=92?= =?UTF-8?q?=E5=AE=9F=E6=A9=9FSTM32=E3=81=B8=E6=95=B4=E5=90=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit cm4-side セッションからの指摘を実機ソースと突き合わせて確認し、4箇所の 食い違いを修正した。正本は G474_Orion_main/Core/Src/ai_comm.c sendRobotInfo()。 ## byte 4..7 ヨー角: ラジアン -> 度 実機は imu->yaw_deg を書いている。シミュレータだけラジアンだったため、 この値を使う消費側の挙動が実機と 180/pi 倍ずれていた。crane_latency_estimator が vision との突き合わせに fb.yaw_angle を使っているので、実害のある差分である。 ## byte 2: ロボットID -> 定数 10 実機は "CRC, 10:dummy" として定数 10 を書いており、実チェックサムは計算しない （ホスト側 is_checksum_valid は実機パケットでも False になる）。シミュレータだけ ロボットIDを入れていた。ロボットIDはポート番号から決まるので情報は失われない。 ## byte 14: ボールセンサ複製 -> 送信サイクルカウンタ 実機は tx_cycle_count を置いている。3つ目のボールセンサではない。 消費側が ball_sensor を判定しているのは byte 12 のみ (crane robot_receiver_node.cpp:416) なので、挙動への影響はない。 ## byte 60: バージョンマーカ 0x01 -> 0 実機は camera_pos_x_div2。シミュレータ独自の「0x01 = シミュレータ」マーカを 置いていたため、消費側では camera_pos_x = 2 として復号されていた。 ローカルカメラを持たないので、カメラ未接続の実機と同じ 0 にする。 シミュレータ識別用の空きバイトは存在しない。 ## 検証 Orion_CM4 の host/lib/feedback/packet.py（実機用デコーダの正本）で シミュレータ出力を復号し、全フィールドが期待どおりになることを確認した。 checksum=10 / imu_yaw_deg=-89.85 / camera=(0,0,0,0) / byte14 が単調増加 模擬 crane -> cm4_sim -> simulator-cli の3者連結も再確認済み （目標まで 1.803 m -> 0.010 m、multicast 再配信 752 パケット、警告ゼロ）。 ## 残る忠実度ギャップ byte 3 は実機では AI 指令の check_counter エコーだが、IbisFeedbackAdaptor は 指令ストリームを見ないため自走カウンタを送る。陳腐化検出には使えるが、 特定の指令とは対応しない。コメントに明記した。 --- data/scripts/ibis-packet-tap.py | 139 ++++++++++++++++++++++++++++++++ src/simulator/ibis_protocol.h | 47 +++++++---- src/simulator/simulator.cpp | 7 +- 3 files changed, 178 insertions(+), 15 deletions(-) create mode 100644 data/scripts/ibis-packet-tap.py diff --git a/data/scripts/ibis-packet-tap.py b/data/scripts/ibis-packet-tap.py new file mode 100644 index 0000000000..aefa3f0566 --- /dev/null +++ b/data/scripts/ibis-packet-tap.py @@ -0,0 +1,139 @@ +"""Inline tap for the ibis 715-byte command stream. + +Binds a UDP port, decodes every robot slot, and optionally forwards the packet +on unchanged. Put it between two hops to see what is actually on the wire +without a packet capture -- useful where loopback capture needs root but the +chain's own ports are already held by listeners: + + crane --> cm4_sim --out-port 12399 --> [tap 12399] --> simulator-cli 12346 + +Usage: + python3 data/scripts/ibis-packet-tap.py --port 12399 \ + [--forward 127.0.0.1:12346] [--robot-ids 0,1] [--every 60] [--count 0] + +--every N print one line per N packets for the robot (default 1; 0 = summary only) +--count N stop after N datagrams (default 0 = run until Ctrl-C) + +Prints, per slot: control mode, check counter, vision pose, target pose, +mode args, and the limits, so a mode 4 -> mode 3 conversion can be read +directly off both sides of a hop. +""" + +import argparse +import socket +import struct +import sys + +CMD_SIZE = 64 +SLOTS = 11 +PACKET_SIZE = SLOTS * (CMD_SIZE + 1) + +MODE_NAMES = {3: "POLAR_VELOCITY", 4: "POSITION_TARGET"} + + +def two_byte(d, i, rng): + raw = (d[i] << 8) | d[i + 1] + return (raw - 32767.0) / 32767.0 * rng + + +def decode(d): + """Offsets follow crane_sender/include/crane_sender/robot_packet.h.""" + mode = d[23] + out = { + "counter": d[1], + "mode": mode, + "vision": (two_byte(d, 2, 32.767), two_byte(d, 4, 32.767)), + "vision_theta": two_byte(d, 6, 3.14159265), + "target_theta": two_byte(d, 8, 3.14159265), + "accel_limit": two_byte(d, 12, 32.767), + "vel_limit": two_byte(d, 14, 32.767), + "flags": d[22], + "target_pos": (two_byte(d, 32, 32.767), two_byte(d, 34, 32.767)), + "terminal_velocity": two_byte(d, 36, 32.767), + } + if mode == 3: + out["args"] = ("r", two_byte(d, 24, 32.767), "theta", two_byte(d, 26, 32.767)) + elif mode == 4: + out["args"] = ("tv_x", two_byte(d, 24, 32.767), "tv_y", two_byte(d, 26, 32.767)) + else: + out["args"] = ("raw", d[24:32].hex()) + return out + + +def is_empty(d): + return not any(d) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--port", type=int, required=True) + ap.add_argument("--bind", default="0.0.0.0") + ap.add_argument("--forward", default="", help="host:port to pass the packet on to") + ap.add_argument("--robot-ids", default="", help="only print these ids (default: all)") + ap.add_argument("--every", type=int, default=1, help="print 1 line per N packets, 0 = summary only") + ap.add_argument("--count", type=int, default=0, help="stop after N datagrams (0 = forever)") + args = ap.parse_args() + + wanted = None + if args.robot_ids: + wanted = {int(x) for x in args.robot_ids.split(",") if x.strip()} + + rx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + rx.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + rx.bind((args.bind, args.port)) + + tx = fwd = None + if args.forward: + host, _, port = args.forward.partition(":") + fwd = (host, int(port)) + tx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + + print(f"tap listening on {args.bind}:{args.port}" + + (f", forwarding to {fwd[0]}:{fwd[1]}" if fwd else ", not forwarding"), + flush=True) + + seen = 0 + mode_counts = {} + try: + while True: + data, _ = rx.recvfrom(2048) + if fwd: + tx.sendto(data, fwd) # 先に転送してチェーンを止めない + if len(data) != PACKET_SIZE: + print(f" !! size {len(data)} (expected {PACKET_SIZE})", flush=True) + continue + seen += 1 + for slot in range(SLOTS): + off = slot * (CMD_SIZE + 1) + rid = data[off] + d = data[off + 1: off + 1 + CMD_SIZE] + if is_empty(d) or rid >= SLOTS: + continue + if wanted is not None and rid not in wanted: + continue + info = decode(d) + mode_counts[info["mode"]] = mode_counts.get(info["mode"], 0) + 1 + if args.every and seen % args.every == 0: + name = MODE_NAMES.get(info["mode"], f"UNKNOWN({info['mode']})") + a = info["args"] + print( + f"#{seen:6d} id={rid} cnt={info['counter']:3d} mode={info['mode']}({name}) " + f"vision=({info['vision'][0]:+.3f},{info['vision'][1]:+.3f}) " + f"{a[0]}={a[1]:+.3f} {a[2]}={a[3]:+.3f} " + f"target=({info['target_pos'][0]:+.3f},{info['target_pos'][1]:+.3f}) " + f"term={info['terminal_velocity']:+.3f} " + f"vlim={info['vel_limit']:.2f} flags=0x{info['flags']:02x}", + flush=True) + if args.count and seen >= args.count: + break + except KeyboardInterrupt: + pass + finally: + print(f"\n--- {seen} datagrams ---", flush=True) + for m in sorted(mode_counts): + print(f" mode {m} ({MODE_NAMES.get(m, 'UNKNOWN')}): {mode_counts[m]} slots", flush=True) + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/simulator/ibis_protocol.h b/src/simulator/ibis_protocol.h index 207534399a..2032845157 100644 --- a/src/simulator/ibis_protocol.h +++ b/src/simulator/ibis_protocol.h @@ -193,21 +193,28 @@ inline bool ibisSlotIsEmpty(const uint8_t* d) // // Parameters: // buffer - 128-byte output buffer (must be pre-allocated) -// robotId - robot identifier (0..15) -// counter - rolling counter (caller increments) -// yaw_rad - orientation in radians (SSL vision convention) +// counter - AI command check counter echo (caller supplies) +// tx_cycle - rolling transmit counter (caller increments) +// yaw_rad - orientation in RADIANS; written to the wire in DEGREES // ball_detected - is ball in contact with dribbler // kick_status - 0=none, 1=flat, 2=chip // odom_x_m - position x in metres (SSL vision coords) // odom_y_m - position y in metres (SSL vision coords) // vel_x_ms - global velocity x in m/s (SSL vision coords) // vel_y_ms - global velocity y in m/s (SSL vision coords) +// +// The layout follows the real robot's STM32 main board byte for byte +// (G474_Orion_main/Core/Src/ai_comm.c sendRobotInfo()), so a consumer cannot +// tell the simulator from real hardware by parsing. Fields the simulator has +// no source for are left at the value real hardware sends when that sensor is +// absent (zero), rather than repurposed -- a simulator-only marker byte would +// decode as a real field on the consumer side. // --------------------------------------------------------------------------- inline void ibisBuildFeedbackPacket( uint8_t* buffer, - int robotId, uint8_t counter, + uint8_t tx_cycle, float yaw_rad, bool ball_detected, uint8_t kick_status, @@ -222,23 +229,32 @@ inline void ibisBuildFeedbackPacket( buffer[0] = 0xAB; buffer[1] = 0xEA; - // Robot ID (2) - buffer[2] = static_cast(robotId); + // Checksum placeholder (2). Real hardware writes the constant 10 here + // ("CRC, 10:dummy" in ai_comm.c); it never computes a real checksum, so + // host-side checksum validation fails on real packets too. Match that + // rather than inventing a value -- the robot id is implied by the port. + buffer[2] = 10; - // Counter (3) + // AI command check counter echo (3) buffer[3] = counter; - // Yaw angle in radians (4-7) - std::memcpy(&buffer[4], &yaw_rad, sizeof(float)); + // Yaw angle (4-7). The wire format is DEGREES: real hardware sends + // imu->yaw_deg here. Sending radians makes consumers that use this field + // (e.g. crane_latency_estimator) behave differently in simulation than on + // the robot, by a factor of 180/pi. + const float yaw_deg = yaw_rad * static_cast(180.0 / M_PI); + std::memcpy(&buffer[4], &yaw_deg, sizeof(float)); // Battery voltage: fixed 24.0 V (8-11) float voltage = 24.0f; std::memcpy(&buffer[8], &voltage, sizeof(float)); - // Ball detection sensors 0-2 (12-14) + // Ball detection sensors 0-1 (12-13), then the transmit cycle counter (14). + // Byte 14 is NOT a third ball sensor: real hardware puts tx_cycle_count + // there (ai_comm.c). Consumers only key the ball sensor off byte 12. buffer[12] = ball_detected ? 1 : 0; buffer[13] = ball_detected ? 1 : 0; - buffer[14] = ball_detected ? 1 : 0; + buffer[14] = tx_cycle; // Kick status (15): 0=none, 1=flat, 2=chip buffer[15] = kick_status; @@ -269,8 +285,11 @@ inline void ibisBuildFeedbackPacket( std::memcpy(&buffer[52], &vel_x_ms, sizeof(float)); std::memcpy(&buffer[56], &vel_y_ms, sizeof(float)); - // Check version byte: 0x01 = simulator (60) - buffer[60] = 0x01; + // Local camera block (60-63): camera_pos_x_div2 / camera_pos_y / + // camera_radius_div4 / camera_fps. The simulator has no local camera, so + // these stay 0 -- exactly what real hardware sends with no camera attached. + // (This byte previously carried a 0x01 "simulator" marker, which consumers + // decoded as camera_pos_x = 2.) - // Extended data (61-127): 0 (already zeroed) + // Extended data (64-127): tx_value_array on real hardware, 0 here. } diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index aad6a5f769..4ddc9a9a13 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -1008,8 +1008,12 @@ public slots: uint8_t buffer[IBIS_FEEDBACK_SIZE]; ibisBuildFeedbackPacket( buffer, - static_cast(id), + // Byte 3 echoes the AI command check counter on real hardware. + // This adaptor does not see the command stream, so it sends a + // free-running counter instead -- still usable for staleness + // detection, but it does not correlate with a specific command. m_counters[id]++, + m_txCycles[id]++, vis.orientation_rad, m_robotCache[id].ball_detected, 0, // kick_status not tracked in ER-Force simulator @@ -1072,6 +1076,7 @@ public slots: IbisVisionState m_vision[2][kMaxRobots] = {}; RobotCache m_robotCache[kMaxRobots] = {}; uint8_t m_counters[kMaxRobots] = {}; + uint8_t m_txCycles[kMaxRobots] = {}; int m_waitLogCount = 0; PacketSenderThread* m_sender; From d9e2f23b529606ff524ce6655ebc8a928ecd532c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Sep 2026 03:17:13 +0900 Subject: [PATCH 14/22] =?UTF-8?q?feat(simulator):=20ibis=E3=83=81=E3=83=BC?= =?UTF-8?q?=E3=83=A0=E8=89=B2=E3=82=92=E6=98=8E=E7=A4=BA=E6=8C=87=E5=AE=9A?= =?UTF-8?q?=E3=81=99=E3=82=8B--ibis-team-color=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit crane-side セッションの実測で、シナリオテスト環境では --ibis-use-referee が feedback を完全に停止させることが分かった。チーム色が解決するまで IbisFeedbackAdaptor::handleGroundTruth が early return するため、 rcst / autoref-tigers が送る referee にチーム名 ibis が載っていない環境では feedback が 1 パケットも出ない（実測 0 対 2480）。 新構成では feedback が位置制御ループ内で唯一の位置信号なので、これは ロボット側の位置制御が永久に閉じないことを意味する。 ## より危険な既定値の問題 --ibis-use-referee を外すと feedback は流れるが、m_ibisIsBlue の既定が true のため無条件に BLUE として扱われる。crane が team:=Yellow で動いている場合、 feedback は敵チームの ground truth を運ぶ。パケットは正常に流れ続けるので 失敗が完全に silent になる。 ## 対応 --ibis-team-color blue|yellow を追加し、referee に依存せず色を確定できるようにした。 起動時のログで3つの状態を区別する。 - 明示指定: どちらに設定されたかを出す - referee 検出: 解決するまで feedback が出ないことを明記する - どちらも無し: BLUE と仮定する旨を WARNING で出す（従来は無言だった） 解決待ちのログも「NO FEEDBACK IS BEING SENT」と結果を明示する文面に変えた。 従来は待っていることだけを伝えており、feedback が止まっている事実は 読み取れなかった。 ## 検証 --ibis-team-color blue robot0: (+4.300, -2.800) --ibis-team-color yellow robot0: (-4.300, -2.800) ← 反対サイド (指定なし) robot0: (+4.300, -2.800) ← blue と一致 指定なしが blue と一致することで、既定値の罠も裏付けた。 --- src/simulator/simulator.cpp | 53 +++++++++++++++++++++++++++++++++---- 1 file changed, 48 insertions(+), 5 deletions(-) diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 4ddc9a9a13..c4071f5540 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -949,12 +949,15 @@ private slots: class IbisFeedbackAdaptor : public QObject { Q_OBJECT public: - IbisFeedbackAdaptor(const QHostAddress& addr, quint16 portBase, bool useReferee) + // explicitColorSet: --ibis-team-color was given, so the colour is known up + // front and the referee (if also enabled) may only correct it later. + IbisFeedbackAdaptor(const QHostAddress& addr, quint16 portBase, bool useReferee, + bool explicitColorSet, bool explicitIsBlue) : m_sender(new PacketSenderThread()) , m_addr(addr) , m_portBase(portBase) - , m_ibisIsBlue(true) - , m_refereeResolved(!useReferee) + , m_ibisIsBlue(explicitColorSet ? explicitIsBlue : true) + , m_refereeResolved(explicitColorSet || !useReferee) {} ~IbisFeedbackAdaptor() override { @@ -994,8 +997,14 @@ public slots: updateTeam(state.blue_robots(), 0); updateTeam(state.yellow_robots(), 1); if (!m_refereeResolved) { + // No feedback at all goes out until the colour is known. Say so: + // a controller that closes its position loop on this feedback has + // no position signal while this message is printing. if (++m_waitLogCount % 200 == 0) { - log(stdout, "ibis: waiting for Game Controller to identify team color\n"); + log(stdout, + "ibis: NO FEEDBACK IS BEING SENT -- still waiting for the Game Controller " + "to identify team color. If the referee does not carry the team name, " + "pass --ibis-team-color blue|yellow instead of --ibis-use-referee.\n"); } return; } @@ -1123,6 +1132,7 @@ int main(int argc, char* argv[]) QCommandLineOption ibisFeedbackPortBaseOpt("ibis-feedback-port-base", "ibis feedback base port (robotId is added)", "port", QString::number(IBIS_FEEDBACK_PORT_BASE)); QCommandLineOption ibisFeedbackTeamNameOpt("ibis-feedback-team-name", "Team name to look up in Game Controller for color detection", "name", "ibis"); QCommandLineOption ibisUseRefereeOpt("ibis-use-referee", "Use Game Controller referee to auto-detect ibis team color"); + QCommandLineOption ibisTeamColorOpt("ibis-team-color", "Set the ibis team color explicitly (blue|yellow), instead of detecting it from the Game Controller", "color", ""); QCommandLineOption ibisAccSpeedupOpt("ibis-acc-speedup", "Acceleration limit for speedup [m/s^2]", "accel", "4.0"); QCommandLineOption ibisAccBrakeOpt("ibis-acc-brake", "Acceleration limit for braking [m/s^2]", "accel", "6.0"); QCommandLineOption ibisRefereePortOpt("ibis-referee-port", "Game Controller multicast port for team color detection", "port", QString::number(SSL_GAME_CONTROLLER_PORT)); @@ -1131,6 +1141,7 @@ int main(int argc, char* argv[]) parser.addOption(ibisFeedbackPortBaseOpt); parser.addOption(ibisFeedbackTeamNameOpt); parser.addOption(ibisUseRefereeOpt); + parser.addOption(ibisTeamColorOpt); parser.addOption(ibisAccSpeedupOpt); parser.addOption(ibisAccBrakeOpt); parser.addOption(ibisRefereePortOpt); @@ -1214,10 +1225,42 @@ int main(int argc, char* argv[]) const QHostAddress fbAddr = QHostAddress(parser.value(ibisFeedbackAddrOpt)); const quint16 fbPortBase = static_cast(parser.value(ibisFeedbackPortBaseOpt).toUInt()); const bool useReferee = parser.isSet(ibisUseRefereeOpt); + + // Team colour selects which team's ground truth the feedback carries. + // Getting it wrong is silent and nasty: the feedback still flows, but + // it describes the opponent's robots, so anything closing a position + // loop on it steers on the wrong positions. + const QString teamColorStr = parser.value(ibisTeamColorOpt).trimmed().toLower(); + bool explicitColorSet = false; + bool explicitIsBlue = true; + if (!teamColorStr.isEmpty()) { + if (teamColorStr == "blue") { + explicitColorSet = true; explicitIsBlue = true; + } else if (teamColorStr == "yellow") { + explicitColorSet = true; explicitIsBlue = false; + } else { + log(stdout, "ibis: unknown --ibis-team-color '%s', expected blue or yellow\n", + teamColorStr.toStdString().c_str()); + return 1; + } + } + if (explicitColorSet) { + log(stdout, "ibis: team color set to %s by --ibis-team-color\n", + explicitIsBlue ? "BLUE" : "YELLOW"); + } else if (useReferee) { + log(stdout, "ibis: team color will be detected from the Game Controller " + "(team name '%s'); no feedback is sent until it resolves\n", + parser.value(ibisFeedbackTeamNameOpt).toStdString().c_str()); + } else { + log(stdout, "ibis: WARNING no team color given and referee detection is off -- " + "assuming BLUE. If ibis plays yellow, the feedback will carry the " + "opponent's positions. Pass --ibis-team-color blue|yellow.\n"); + } const quint16 refereePort = static_cast(parser.value(ibisRefereePortOpt).toUInt()); auto* ibisCmd = new IbisCommandAdaptor(cmdPort, &timer, accSpeedup, accBrake); - auto* ibisFb = new IbisFeedbackAdaptor(fbAddr, fbPortBase, useReferee); + auto* ibisFb = new IbisFeedbackAdaptor(fbAddr, fbPortBase, useReferee, + explicitColorSet, explicitIsBlue); // IbisCommandAdaptor receives vision data to cache robot positions/orientations QObject::connect(&sim, &SimProxy::gotPacket, From df266a9867424141417ff5a27bde1ce34268e468 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Sep 2026 11:33:30 +0900 Subject: [PATCH 15/22] =?UTF-8?q?fix(simulator):=20=E5=AE=9F=E6=A9=9FG474?= =?UTF-8?q?=E3=81=A8=E5=90=8C=E3=81=98vision=E5=96=AA=E5=A4=B1=E6=99=82?= =?UTF-8?q?=E3=81=AE=E5=81=9C=E6=AD=A2=E6=9D=A1=E4=BB=B6=E3=82=92=E5=86=8D?= =?UTF-8?q?=E7=8F=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit cm4-side セッションからの指摘を実機ソースと突き合わせて確認した。 simulator-cli は is_vision_available をデシリアライズするだけで使っておらず、 実機の STM32 メイン基板が車輪を止める条件を再現できていなかった。 実機 G474_Orion_main/Core/Src/state_func.c:314 の停止条件: sys->stop_flag || ai_cmd->stop_emergency || !ai_cmd->is_vision_available || ai_cmd->elapsed_time_ms_since_last_vision > 500 simulator-cli が見ていたのは stop_emergency だけで、残り2つを無視していた （elapsed_time_ms_since_last_vision に至ってはデシリアライズもしていなかった）。 ## なぜ重要か cm4_sim を挟む新構成では CM4 側でも同条件で止めるようになったが、 A/B 比較の基準側（crane から simulator-cli への直送、mode 3）には CM4 が 経路に存在しない。そこでシミュレータが止めなければ、実機なら停車する条件で シミュレータのロボットだけが走り続ける。 とくに無線劣化注入（パケットロス）を入れると is_vision_available と elapsed_time_ms_since_last_vision はまさに発火するフィールドなので、 A/B の数値そのものが意味を失う。 ## 変更内容 - ibisDeserialize() が latency_time_ms と elapsed_time_ms_since_last_vision を 復号するようにした（byte 18..21。これらは +/-range の float 符号化ではなく 素の uint16 なのでゼロ埋めは 0 として復号される） - 停止判定を ibisShouldStop() に切り出し、実機と同じ3条件にした （sys->stop_flag はシミュレータに対応物が無い） ## 検証 実機の境界値を含めて確認した。 [OK] vision あり・新鮮 flags=0x01 elapsed= 0ms -> moved 0.422 m [OK] vision なし flags=0x00 elapsed= 0ms -> moved 0.001 m [OK] vision 古い(600ms) flags=0x01 elapsed=600ms -> moved 0.001 m [OK] vision 境界(500ms) flags=0x01 elapsed=500ms -> moved 0.423 m [OK] STOP_EMERGENCY flags=0x09 elapsed= 0ms -> moved 0.001 m 500ms で動き 600ms で止まることで、実機の `> 500` という境界も一致している。 ibis-chain-smoketest と cm4_sim を挟んだ3者連結も通過 （目標まで 1.803 m -> 0.010 m、警告ゼロ）。 --- src/simulator/ibis_protocol.h | 24 ++++++++++++++++++++++++ src/simulator/simulator.cpp | 2 +- 2 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/simulator/ibis_protocol.h b/src/simulator/ibis_protocol.h index 2032845157..710a18a29d 100644 --- a/src/simulator/ibis_protocol.h +++ b/src/simulator/ibis_protocol.h @@ -30,6 +30,10 @@ constexpr int IBIS_FEEDBACK_SIZE = 128; constexpr int IBIS_FEEDBACK_PORT_BASE = 50100; constexpr double IBIS_POSITION_MATCH_THRESHOLD = 0.5; // metres +// The real STM32 main board stops the wheels when vision has been lost for +// longer than this (G474_Orion_main/Core/Src/state_func.c). +constexpr uint16_t IBIS_VISION_LOST_TIMEOUT_MS = 500; + // Control modes. Must match crane_sender/include/crane_sender/robot_packet.h // and Orion_CM4/cm4/bridge/robot_packet.h (ControlMode enum). // @@ -102,6 +106,8 @@ struct IbisCommand { float acceleration_limit; // m/s^2 (0 means "use default") float linear_velocity_limit; // m/s (0 means "no limit") float angular_velocity_limit; // rad/s + uint16_t latency_time_ms; + uint16_t elapsed_time_ms_since_last_vision; float polar_velocity_r; // m/s (mode 3 args) float polar_velocity_theta; // radians (global direction, mode 3 args) float terminal_velocity_xy[2]; // m/s (mode 4 args) @@ -141,6 +147,11 @@ inline IbisCommand ibisDeserialize(const uint8_t* d) cmd.linear_velocity_limit = ibisDecodeTwoByte(d[LINEAR_VEL_LIMIT_H], d[LINEAR_VEL_LIMIT_L], 32.767f); cmd.angular_velocity_limit = ibisDecodeTwoByte(d[ANGULAR_VEL_LIMIT_H], d[ANGULAR_VEL_LIMIT_L], 32.767f); + // These two are plain uint16 (high, low), not the +/-range float encoding. + cmd.latency_time_ms = static_cast((d[LATENCY_MS_H] << 8) | d[LATENCY_MS_L]); + cmd.elapsed_time_ms_since_last_vision = + static_cast((d[ELAPSED_VISION_H] << 8) | d[ELAPSED_VISION_L]); + uint8_t flags = d[FLAGS]; cmd.is_vision_available = (flags >> IS_VISION_AVAILABLE) & 0x01; cmd.enable_chip = (flags >> ENABLE_CHIP) & 0x01; @@ -174,6 +185,19 @@ inline IbisCommand ibisDeserialize(const uint8_t* d) return cmd; } +// Mirrors the wheel-stop condition of the real STM32 main board +// (G474_Orion_main/Core/Src/state_func.c): it halts the wheels on emergency +// stop, on vision being unavailable, and on vision having gone stale. The +// simulator emulates that board, so it must stop for the same reasons -- +// otherwise the robot keeps driving in simulation under conditions that would +// park it on real hardware, which matters most under injected packet loss. +inline bool ibisShouldStop(const IbisCommand& cmd) +{ + return cmd.stop_emergency + || !cmd.is_vision_available + || cmd.elapsed_time_ms_since_last_vision > IBIS_VISION_LOST_TIMEOUT_MS; +} + // True when a robot slot carries no command at all. Senders zero-fill the slots // of robots they do not control; a zero-filled slot decodes to a position of // (-32.767, -32.767) which no team match would accept, but relying on that is diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index c4071f5540..6d327fc85b 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -757,7 +757,7 @@ private slots: : yellowControl->add_robot_commands(); robotCmd->set_id(robot_id); - if (cmd.stop_emergency) { + if (ibisShouldStop(cmd)) { auto* lv = robotCmd->mutable_move_command()->mutable_local_velocity(); lv->set_forward(0.0f); lv->set_left(0.0f); From 6f01e99b557732cacf9e298054a2d4d83d91543b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Sep 2026 12:10:44 +0900 Subject: [PATCH 16/22] =?UTF-8?q?feat(simulator):=20=E4=BD=8D=E7=BD=AE?= =?UTF-8?q?=E7=85=A7=E5=90=88=E3=81=AE=E5=A4=B1=E6=95=97=E3=81=A7=E6=8C=87?= =?UTF-8?q?=E4=BB=A4=E3=82=92=E7=A0=B4=E6=A3=84=E3=81=97=E3=81=9F=E3=81=93?= =?UTF-8?q?=E3=81=A8=E3=82=92=E8=AD=A6=E5=91=8A=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit simulator-cli は指令の vision_global_pos をフィールド上のロボット位置と 突き合わせてチームと機体を判定しており、0.5m 以上ずれると指令を破棄していた。 この破棄が完全に無音だったため、パケットは届き続け check_counter も進み警告も 出ないのにロボットだけが動かない、という状態になっていた。「その場で静止せよ」 と指令されている状態と区別が付かない。 1秒/台 のレート制限付きで、最も近い候補までの距離を添えて警告するようにした。 距離を併記することで「推定が少し古いだけ」なのか「まったく別の場所を指して いる」のかを切り分けられる。該当IDのロボットがまだフィールドに居ない場合は 別の文言を出す（vision 未受信と推定ずれは原因も対処も違うため）。 A/B比較にとってこの破棄は非対称な罠である。cm4_sim は --vision-echo-feedback で feedback 由来の実位置を vision_global_pos に上書きしてから下流へ流すので 新構成側は免疫があるが、基準側（crane → simulator-cli 直送）は crane の world model 推定をそのまま送るため無防備になる。推定が 0.5m ずれると基準側 だけが指令を失って動かず、A/B の差が制御方式の優劣ではなく破棄の有無で 決まってしまう。 実測（-g 2020B --realism None、mode 3 で3秒指令）: ずれ 0.00m -> 移動 1.47m / 警告 0行 ずれ 0.40m -> 移動 1.61m / 警告 0行 ずれ 0.60m -> 移動 0.00m / 警告 3行（1秒/台のレート制限どおり） ずれ 3.00m -> 移動 0.00m / 警告 3行 スモークテストに3項目を追加（位置一致時に偽陽性が出ないことの確認を含む）。 docs/robot-side-position-control.md に「指令の位置照合」節を追加した。 --- data/scripts/ibis-chain-smoketest.py | 78 ++++++++++++++++++++++++++-- docs/robot-side-position-control.md | 47 +++++++++++++++++ src/simulator/simulator.cpp | 44 +++++++++++++++- 3 files changed, 165 insertions(+), 4 deletions(-) diff --git a/data/scripts/ibis-chain-smoketest.py b/data/scripts/ibis-chain-smoketest.py index 3168b77fed..7de7d43625 100644 --- a/data/scripts/ibis-chain-smoketest.py +++ b/data/scripts/ibis-chain-smoketest.py @@ -5,9 +5,12 @@ POSITION_TARGET (mode 4) belongs to the robot-side CM4 controller that runs between crane and the simulator. See docs/robot-side-position-control.md. -Checks, both against a real simulator-cli process over UDP: +Checks, all against a real simulator-cli process over UDP: 1. a mode 3 command drives the robot 2. a mode 4 command stops it and logs a rate-limited warning + 3. a command whose vision_global_pos disagrees with the simulator is dropped, + and says so -- this drop used to be silent, which is indistinguishable from + "commanded to hold still" while packets keep arriving Usage: python3 data/scripts/ibis-chain-smoketest.py [path/to/simulator-cli] """ @@ -192,6 +195,55 @@ def drive(mode, args, seconds, counter): sim.close() +def run_position_mismatch(binary, log_path): + """A command claiming the wrong robot position is dropped, and says so. + + The simulator identifies the robot by matching the command's vision_global_pos + against the robots on the field. A sender whose own position estimate has + drifted past the threshold gets its commands dropped -- which looks exactly + like a robot commanded to hold still, so the log line is the only way to tell. + """ + sim = Simulator(binary, log_path=log_path) + try: + state = None + for _ in range(20): + if not sim.alive(): + return False, (f"simulator-cli exited (rc={sim.proc.returncode}); " + f"see {log_path}") + state = sim.recv() + if state: + break + if state is None: + return False, f"no feedback from simulator; see {log_path}" + start = dict(state) + + # Far enough past the threshold that feedback lag cannot pull it back under. + offset = 1.0 + deadline = time.time() + 2.0 + counter = 1 + while time.time() < deadline: + claimed = (state["x"] + offset, state["y"]) + cmd = build_command(counter, claimed, state["yaw"], + MODE_POLAR_VELOCITY, (1.5, 0.0)) + sim.send(build_packet(0, cmd)) + counter += 1 + time.sleep(1 / 60) + sim.rx.settimeout(0.001) + try: + while True: + got = parse_feedback(sim.rx.recv(256)) + if got: + state = got + except socket.timeout: + pass + + moved = distance(state, start) + return moved < 0.05, (f"claimed a position {offset:.1f} m off, robot moved " + f"{moved:.3f} m (want ~0)") + finally: + sim.close() + + def main(): repo = Path(__file__).resolve().parents[2] binary = Path(sys.argv[1]) if len(sys.argv) > 1 else repo / "build" / "bin" / "simulator-cli" @@ -201,19 +253,39 @@ def main(): tmp = Path(tempfile.mkdtemp(prefix="ibis-smoketest-")) log = tmp / "simulator-cli.log" + mismatch_log = tmp / "simulator-cli-mismatch.log" failures = 0 ok, detail = run_commands(binary, log) print(f"[{'PASS' if ok else 'FAIL'}] commands: {detail}") failures += 0 if ok else 1 - warnings = [line for line in log.read_text().splitlines() - if "POSITION_TARGET" in line] + text = log.read_text() + warnings = [line for line in text.splitlines() if "POSITION_TARGET" in line] ok = len(warnings) > 0 print(f"[{'PASS' if ok else 'FAIL'}] warning: {len(warnings)} POSITION_TARGET " f"warning(s) logged (rate limited to 1/s per robot)") failures += 0 if ok else 1 + # The commands above always claim the position the feedback just reported, so a + # drop here would mean the matching rejects agreeing positions. + stray = [line for line in text.splitlines() if "command dropped" in line] + ok = not stray + print(f"[{'PASS' if ok else 'FAIL'}] no false drops: {len(stray)} drop warning(s) " + f"while the claimed position agreed (want 0)") + failures += 0 if ok else 1 + + ok, detail = run_position_mismatch(binary, mismatch_log) + print(f"[{'PASS' if ok else 'FAIL'}] position mismatch: {detail}") + failures += 0 if ok else 1 + + drops = [line for line in mismatch_log.read_text().splitlines() + if "command dropped" in line] + ok = len(drops) > 0 + print(f"[{'PASS' if ok else 'FAIL'}] drop warning: {len(drops)} warning(s) logged " + f"(rate limited to 1/s per robot)") + failures += 0 if ok else 1 + print(f"\nlogs: {tmp}") return 1 if failures else 0 diff --git a/docs/robot-side-position-control.md b/docs/robot-side-position-control.md index 5401d1e5f6..c4136a87de 100644 --- a/docs/robot-side-position-control.md +++ b/docs/robot-side-position-control.md @@ -232,6 +232,47 @@ vision が混線するので、並列実行が必要ならコンテナのネッ crane はロボットを tracker 経路で追跡するため、**外部の auto-referee が必要**である （`docker/scenario/docker-compose.yaml` の `autoref-tigers` がこれを担っている）。 +### 指令の位置照合（必読・踏むと沈黙して壊れる） + +`simulator-cli` は指令に載っている `vision_global_pos`（byte 2..5）を +フィールド上のロボット位置と突き合わせて、**どちらのチームの何番か**を判定する +（`IbisCommandAdaptor`）。実機の無線には「青/黄」の区別が無く、ロボット自身が +自分宛のスロットだけを読むため、シミュレータ側はこの照合でチームを決めるしかない。 + +照合の閾値は `IBIS_POSITION_MATCH_THRESHOLD = 0.5` m（`ibis_protocol.h`）である。 +**送信側が思っているロボット位置が真の位置から 0.5 m 以上ずれると、その指令は +破棄される。** + +実測（`-g 2020B --realism None`、mode 3 で 3 秒指令）: + +| 指令の位置ずれ | 3 秒の移動量 | +|---|---| +| 0.00 m | 1.47 m | +| 0.40 m | 1.61 m | +| 0.49 m | 1.07 m | +| **0.60 m** | **0.00 m** | +| 3.00 m | 0.00 m | + +この破棄は **2026-09-13 以前は完全に無音**だった。パケットは届き続け、 +`check_counter` も進み、警告も出ず、ロボットだけが動かない。「その場で静止せよ」 +と指令されている状態と区別が付かない。現在は 1 秒/台 のレート制限付きで + +``` +ibis: robot 0 command dropped -- vision_global_pos (-3.700, -2.800) is 0.599 m +from the robot, over the 0.50 m match threshold. +``` + +を出す。最も近い候補までの距離を併記するので、「推定が少し古いだけ」（閾値を +わずかに超える）のか「まったく別の場所を指している」のかを切り分けられる。 + +**A/B 比較にとっては非対称な罠である。** `cm4_sim` は `--vision-echo-feedback` +で feedback 由来の実位置を `vision_global_pos` に上書きしてから下流へ流すため、 +**新構成側はこの破棄に免疫がある**。一方、基準側（crane → simulator-cli 直送）は +crane の world model 推定をそのまま送るので無防備である。crane の推定が 0.5 m +ずれると、**基準側だけが指令を失って動かず、新構成側は正常に動く** — A/B の差が +制御方式の優劣ではなく破棄の有無で決まってしまう。基準側の計測前に、この警告が +出ていないことを必ず確認すること。 + ## タイミング契約 ### 各段のレート @@ -307,6 +348,9 @@ crane からのパケットが途絶えた場合の安全停止は、CM4 側で - **mode 3 以外を受け取ったらロボットを停止し、1 秒/台 のレート制限付きで警告** を出す。mode 4 が届くのは「cm4_sim が経路に入っていない」設定ミスであり、 無言で誤解釈するより停止して理由を出すほうが安全かつデバッグしやすい。 + - **`vision_global_pos` の照合に失敗して指令を破棄したときも警告を出す** + （1 秒/台）。最も近い候補までの距離を併記する。 + 「指令の位置照合」節を参照。 simulator-cli 側に位置制御は **実装しない**。実装すると制御則のコピーが 4 つ目になり、`robot_packet.h` が 3 リポジトリで食い違った問題を繰り返す。 @@ -355,6 +399,9 @@ python3 data/scripts/ibis-chain-smoketest.py [path/to/simulator-cli] 1. mode 3 でロボットが動く 2. mode 4 でロボットが停止し、レート制限付き警告が出る +3. 位置が一致している間は破棄警告が出ない（偽陽性が無い） +4. `vision_global_pos` を 1.0 m ずらした指令は破棄され、ロボットが動かない +5. その破棄がレート制限付きで警告として出る `cm4_sim` を実装する前に simulator-cli 側の契約を固定するためのもの。 既定ポートとは離れたポート（12397 / 50700 / 10097 / 11097）を使うので、 diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 6d327fc85b..28febd2d66 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -736,20 +736,27 @@ private slots: // Also keeps the matched vision entry for orientation lookup below. int teamIdx = -1; const IbisVisionState* vis = nullptr; + double nearest = -1.0; for (int t = 0; t < 2; ++t) { const IbisVisionState& v = m_vision[t][robot_id]; if (!v.valid) { continue; } const float dx = v.x_mm / 1000.0f - cmd.vision_global_pos[0]; const float dy = v.y_mm / 1000.0f - cmd.vision_global_pos[1]; - if (std::hypot(dx, dy) < IBIS_POSITION_MATCH_THRESHOLD) { + const double dist = std::hypot(dx, dy); + if (nearest < 0.0 || dist < nearest) { + nearest = dist; + } + if (dist < IBIS_POSITION_MATCH_THRESHOLD) { teamIdx = t; vis = &v; break; } } if (teamIdx < 0) { + warnPositionMismatch(robot_id, cmd, nearest); continue; } + m_robotStates[robot_id].match_warned = false; const bool ibisIsBlue = (teamIdx == 0); auto* robotCmd = ibisIsBlue @@ -882,6 +889,39 @@ private slots: } } + // The command carries the sender's own estimate of where the robot is + // (vision_global_pos). When it does not match any robot on the field the command + // is dropped -- silently, until this warning was added. A silent drop is very hard + // to tell apart from "the robot is commanded to hold still": the packets keep + // arriving, check_counter keeps advancing, and nothing moves. Report the nearest + // candidate so the reader can see whether the estimate is merely stale (slightly + // over the threshold) or pointing somewhere else entirely. + void warnPositionMismatch(int robot_id, const IbisCommand& cmd, double nearest) { + constexpr qint64 kWarnIntervalNs = 1000LL * 1000LL * 1000LL; + auto& state = m_robotStates[robot_id]; + const qint64 now = m_timer->currentTime(); + if (state.match_warned && now - state.last_match_warn_ns < kWarnIntervalNs) { + return; + } + state.match_warned = true; + state.last_match_warn_ns = now; + if (nearest < 0.0) { + log(stdout, + "ibis: robot %d command dropped -- no robot with this id is on the field yet " + "(command claims the robot is at %.3f, %.3f). Commands are ignored until " + "vision reports the robot.\n", + robot_id, cmd.vision_global_pos[0], cmd.vision_global_pos[1]); + } else { + log(stdout, + "ibis: robot %d command dropped -- vision_global_pos (%.3f, %.3f) is %.3f m " + "from the robot, over the %.2f m match threshold. The sender's position " + "estimate disagrees with the simulator; the robot will not move until they " + "agree. See docs/robot-side-position-control.md\n", + robot_id, cmd.vision_global_pos[0], cmd.vision_global_pos[1], + nearest, IBIS_POSITION_MATCH_THRESHOLD); + } + } + static constexpr uint32_t kMaxRobots = 16; struct PerRobotState { @@ -890,6 +930,8 @@ private slots: uint8_t last_check_counter = 0xFF; qint64 last_mode_warn_ns = 0; bool mode_warned = false; + qint64 last_match_warn_ns = 0; + bool match_warned = false; }; // m_vision[0] = blue, m_vision[1] = yellow From 489643bec5b81495d5efcb67c834bb7af9641772 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Sep 2026 13:00:32 +0900 Subject: [PATCH 17/22] =?UTF-8?q?fix(simulator):=20=E3=83=AD=E3=82=B0?= =?UTF-8?q?=E3=82=92=E8=87=AA=E5=89=8D=E3=81=A7=E8=A1=8C=E3=83=90=E3=83=83?= =?UTF-8?q?=E3=83=95=E3=82=A1=E3=83=AA=E3=83=B3=E3=82=B0=E3=81=97=E7=95=B0?= =?UTF-8?q?=E5=B8=B8=E6=99=82=E3=81=AB=E6=B6=88=E3=81=88=E3=81=AA=E3=81=84?= =?UTF-8?q?=E3=82=88=E3=81=86=E3=81=AB=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit stdout がパイプやファイルのときブロックバッファリングされ、log() は flush しないため、SIGTERM で書きかけのログが丸ごと失われていた。Docker・systemd・ テストハーネスはいずれも stdout がパイプなので、実運用の経路すべてで起きる。 異常時こそログが消えるのが問題で、実際に公開イメージの動作確認で 「破棄警告 0 行」という誤った結論を一度出している（実際には破棄されていた）。 ログファイルは 0 バイトだった。 main() の先頭で setvbuf(stdout, nullptr, _IOLBF, 0) を呼び、呼び出し側が stdbuf -oL を付けることに依存しない形にした。公開イメージの entrypoint は tini なので、呼び出し側に任せると compose の書き方次第で消える。 実測（同一条件、setvbuf の有無だけが違う。stdbuf 無しで起動し SIGTERM）: 修正前: 0 バイト / 0 行 修正後: 1049 バイト / 6 行（破棄警告 3 行を含む） スモークテストの stdbuf は古いバイナリでも動くよう残し、コメントを実態に 合わせた。docs/robot-side-position-control.md に節を追加。 Orion_CM4 も同じ理由で cm4_sim / ai_cmd_v2 に setvbuf を入れている(d61a94d)。 --- data/scripts/ibis-chain-smoketest.py | 5 +++-- docs/robot-side-position-control.md | 19 +++++++++++++++++++ src/simulator/simulator.cpp | 11 +++++++++++ 3 files changed, 33 insertions(+), 2 deletions(-) diff --git a/data/scripts/ibis-chain-smoketest.py b/data/scripts/ibis-chain-smoketest.py index 7de7d43625..1400f68ca6 100644 --- a/data/scripts/ibis-chain-smoketest.py +++ b/data/scripts/ibis-chain-smoketest.py @@ -101,8 +101,9 @@ def __init__(self, binary, log_path=None): ] self.log_path = log_path self.log = open(log_path, "w") if log_path else subprocess.DEVNULL - # stdbuf: log() in simulator.cpp does not flush, and SIGTERM would drop - # a block-buffered pipe. + # stdbuf is redundant against a current simulator-cli, which line-buffers + # its own stdout; keep it so this script still captures logs from an older + # binary, where log() never flushed and SIGTERM dropped the whole buffer. self.proc = subprocess.Popen(["stdbuf", "-oL", "-eL"] + args, stdout=self.log, stderr=subprocess.STDOUT) self.rx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) diff --git a/docs/robot-side-position-control.md b/docs/robot-side-position-control.md index c4136a87de..7c42a4a0f6 100644 --- a/docs/robot-side-position-control.md +++ b/docs/robot-side-position-control.md @@ -273,6 +273,25 @@ crane の world model 推定をそのまま送るので無防備である。cran 制御方式の優劣ではなく破棄の有無で決まってしまう。基準側の計測前に、この警告が 出ていないことを必ず確認すること。 +### ログのバッファリング（Docker / systemd で踏む） + +`simulator-cli` は `main()` で `setvbuf(stdout, nullptr, _IOLBF, 0)` を呼び、 +**自分で行バッファリングする**。呼び出し側が `stdbuf -oL` を付ける必要は無い。 + +これが無いと、stdout がパイプやファイルのとき（Docker・systemd・テストハーネスは +すべてそう）ブロックバッファリングされ、`log()` は flush しないので **SIGTERM で +書きかけが丸ごと消える**。実測（同一条件、setvbuf の有無だけが違う）: + +| | SIGTERM 後のログ | +|---|---| +| setvbuf 無し | **0 バイト / 0 行** | +| setvbuf あり | 1049 バイト / 6 行（破棄警告 3 行を含む） | + +**異常時こそログが消える**ため、公開イメージの動作確認で「破棄警告 0 行」という +誤った結論を一度出している（実際には破棄されていた）。 + +`cm4_sim` / `ai_cmd_v2` も同じ理由で `setvbuf` するようになった（Orion_CM4 `d61a94d`）。 + ## タイミング契約 ### 各段のレート diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 28febd2d66..e85db2c7b1 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -1143,6 +1143,17 @@ public slots: int main(int argc, char* argv[]) { + // Line-buffer the logs. stdout is block-buffered when it is a pipe or a file -- + // which is how this runs under Docker, systemd, or a test harness -- and log() + // never flushes, so a SIGTERM discards everything written since the last 4 KiB + // boundary. In practice that means the log is empty exactly when something went + // wrong and someone goes looking for it: a container stopped after a failed run + // has produced zero lines here. Do it in the process rather than leaving it to + // the caller to remember `stdbuf -oL`, which the published image's own entrypoint + // does not do. + std::setvbuf(stdout, nullptr, _IOLBF, 0); + std::setvbuf(stderr, nullptr, _IOLBF, 0); + QCoreApplication app(argc, argv); app.setApplicationName("Simulator"); app.setOrganizationName("ER-Force"); From 533daf9d7bb76d3337fd00d9e5f55b29a76d996c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Sep 2026 13:05:13 +0900 Subject: [PATCH 18/22] =?UTF-8?q?fix(simulator):=20stderr=E3=81=B8?= =?UTF-8?q?=E3=81=AEsetvbuf=E3=82=92=E3=82=84=E3=82=81=E6=97=A2=E5=AE=9A?= =?UTF-8?q?=E3=81=AE=E3=83=90=E3=83=83=E3=83=95=E3=82=A1=E7=84=A1=E3=81=97?= =?UTF-8?q?=E3=81=AB=E6=88=BB=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit PR #8 で stdout と一緒に stderr にも setvbuf(_IOLBF) を入れたが、これは 目的に対して逆向きだった。glibc は stderr を既定で「バッファ無し」にしており、 _IOLBF はそれより弱い設定になる。改行で終わらない出力を抱え込むようになるため、 「異常終了時にログを残す」という PR #8 の目的に反する。 実測（改行なしで stderr へ出力してから SIGTERM。残ったバイト数）: 既定(glibc, バッファ無し): 27 setvbuf(_IOLBF) : 0 stdout の行バッファリングは PR #8 のまま維持する。stdout はパイプ・ファイル時に ブロックバッファリングされるので _IOLBF が必要で、stderr は既定で十分、という 非対称が正しい。 Orion_CM4 も同じ理由で stderr の setvbuf を外している(8f2c646)。 --- docs/robot-side-position-control.md | 11 ++++++++++- src/simulator/simulator.cpp | 12 ++++++++---- 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/docs/robot-side-position-control.md b/docs/robot-side-position-control.md index 7c42a4a0f6..2937a7b38c 100644 --- a/docs/robot-side-position-control.md +++ b/docs/robot-side-position-control.md @@ -276,7 +276,16 @@ crane の world model 推定をそのまま送るので無防備である。cran ### ログのバッファリング（Docker / systemd で踏む） `simulator-cli` は `main()` で `setvbuf(stdout, nullptr, _IOLBF, 0)` を呼び、 -**自分で行バッファリングする**。呼び出し側が `stdbuf -oL` を付ける必要は無い。 +**自分で stdout を行バッファリングする**。呼び出し側が `stdbuf -oL` を付ける必要は無い。 + +**`stderr` には設定しない。** glibc は stderr を既定で **バッファ無し**にしており、 +`_IOLBF` はそれより弱い設定になる（改行で終わらない出力を抱え込むようになる）。 +実測（改行なしで出力してから `SIGTERM`）: + +| stderr の設定 | 残ったバイト数 | +|---|---| +| 既定（glibc, バッファ無し） | 27 | +| `setvbuf(_IOLBF)` | **0** | これが無いと、stdout がパイプやファイルのとき（Docker・systemd・テストハーネスは すべてそう）ブロックバッファリングされ、`log()` は flush しないので **SIGTERM で diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index e85db2c7b1..023f8f1ef3 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -1143,16 +1143,20 @@ public slots: int main(int argc, char* argv[]) { - // Line-buffer the logs. stdout is block-buffered when it is a pipe or a file -- - // which is how this runs under Docker, systemd, or a test harness -- and log() - // never flushes, so a SIGTERM discards everything written since the last 4 KiB + // Line-buffer stdout. It is block-buffered when it is a pipe or a file -- which + // is how this runs under Docker, systemd, or a test harness -- and log() never + // flushes, so a SIGTERM discards everything written since the last 4 KiB // boundary. In practice that means the log is empty exactly when something went // wrong and someone goes looking for it: a container stopped after a failed run // has produced zero lines here. Do it in the process rather than leaving it to // the caller to remember `stdbuf -oL`, which the published image's own entrypoint // does not do. + // + // stderr is deliberately left alone: glibc leaves it unbuffered, which already + // survives an abrupt exit. Setting _IOLBF on it would be a weaker guarantee, not + // a stronger one -- it would start holding back a write that does not end in a + // newline, which is the opposite of what this is for. std::setvbuf(stdout, nullptr, _IOLBF, 0); - std::setvbuf(stderr, nullptr, _IOLBF, 0); QCoreApplication app(argc, argv); app.setApplicationName("Simulator"); From 1006a45d07166d6a1d59af905585ab7c4e271b74 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Sep 2026 13:21:35 +0900 Subject: [PATCH 19/22] =?UTF-8?q?fix:=20=E6=8C=87=E4=BB=A4=E7=A0=B4?= =?UTF-8?q?=E6=A3=84=E6=99=82=E3=81=AE=E5=81=9C=E6=AD=A2=E6=8C=99=E5=8B=95?= =?UTF-8?q?=E3=82=92=E5=AE=9F=E6=B8=AC=E3=81=AB=E5=90=88=E3=82=8F=E3=81=9B?= =?UTF-8?q?=E3=81=A6=E8=A8=82=E6=AD=A3=E3=81=97=E5=BF=A0=E5=AE=9F=E5=BA=A6?= =?UTF-8?q?=E3=82=AE=E3=83=A3=E3=83=83=E3=83=97=E3=82=92=E8=BF=BD=E8=A8=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 位置照合で指令が破棄されたとき、警告は "the robot will not move until they agree" と出していたが、実測ではロボットは 1.5 m/s から 0.573 m 進んでから 停止する。「動かない」を信じて原因を探すと誤った場所を見ることになるため、 "coasts to a stop and stays there" に改める。 あわせて、実測で判明した 2 つの忠実度ギャップを docs に追記する。 1. 指令が途切れた間の停止は摩擦による惰走であり能動制動ではない。 SimRobot::begin() は最後の指令から 0.1 s で standby に入り、車輪 PID の 手前で return するため駆動力が一切かからない。同じ 1.5 m/s からの停止でも mode 3 の r=0（能動制動）なら 0.126 m、指令途絶なら 0.573 m と 4.5 倍違う。 位置照合による破棄も経路劣化による欠落も同じ経路を通るので、シミュレータの 停止距離をそのまま実機の挙動として読んではいけない。 2. feedback の速度フィールドは指令が届いたときしか更新されない。速度は RadioResponse 由来のキャッシュで、RadioResponse は指令が届いたときだけ 生成される。一方で位置は毎周期 vision から詰め直されるため、指令が破棄 されている間は同じパケットの中で位置だけが新鮮で速度は凍る。実測でも ロボットが約 1.0 s で停止した後も速度は 1.461 m/s を返し続けた。cm4_sim は 位置だけでループを閉じるので制御には影響しないが、速度を見る診断は騙される。 --- docs/robot-side-position-control.md | 15 +++++++++++++++ src/simulator/simulator.cpp | 4 ++-- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/docs/robot-side-position-control.md b/docs/robot-side-position-control.md index 2937a7b38c..acd4a1b74a 100644 --- a/docs/robot-side-position-control.md +++ b/docs/robot-side-position-control.md @@ -441,6 +441,21 @@ python3 data/scripts/ibis-chain-smoketest.py [path/to/simulator-cli] そのまま返すため、ノイズも vision 遅延も無い。実機の G474 は `vision_based_position` としてタイヤオドメトリと vision を融合した推定値を返す。 シミュレータ上の位置制御は実機より良く見える。必要ならノイズ注入を追加する。 +- **指令が途切れた間の停止は「摩擦による惰走」で、能動制動ではない。** + `SimRobot::begin()` は最後の指令から 0.1 s で standby に入り、車輪 PID に到達する + 手前で return するため駆動力が一切かからない（`src/amun/simulator/simrobot.cpp:337` + と `:417`）。1.5 m/s から指令を止めた場合、実測で停止まで **0.573 m** 進む。同じ + 速度から mode 3 の r=0（＝能動制動）を送った場合は **0.126 m** で止まるので、 + 4.5 倍の差がある。位置照合で破棄された場合も、経路劣化で落ちた場合も同じ経路を通る。 + 停止距離をそのまま実機の挙動として読まないこと。 +- **feedback の速度は指令が届いたときしか更新されない。** 速度（byte 52/56）は + `RadioResponse` 由来のキャッシュで、`RadioResponse` は指令が届いたときにしか + 生成されない。一方で位置（byte 44/48）は毎周期 vision から詰め直される。このため + 指令が破棄されている間、**同じパケットの中で位置は新鮮なのに速度は破棄直前の値で + 凍る**。上の実測でもロボットは約 1.0 s で停止しているのに、速度フィールドは + 1.461 m/s を返し続けた。「速度が出ているのに位置が動かない」feedback を見たら、 + ロボットの挙動ではなく指令が届いているかをまず疑うこと。`cm4_sim` は位置だけを + 使ってループを閉じるので現状の制御には影響しないが、速度を見る診断は騙される。 - **ボールセンサ・キック状態。** `kick_status` は常に 0。 - **ローカルカメラ。** `cam_server_v3` 相当は無い。`cm4_sim` はカメラ領域を ゼロ埋めすること（実機のカメラ未接続時と同じ扱い）。 diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index 023f8f1ef3..b33e50a5d3 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -915,8 +915,8 @@ private slots: log(stdout, "ibis: robot %d command dropped -- vision_global_pos (%.3f, %.3f) is %.3f m " "from the robot, over the %.2f m match threshold. The sender's position " - "estimate disagrees with the simulator; the robot will not move until they " - "agree. See docs/robot-side-position-control.md\n", + "estimate disagrees with the simulator; the robot coasts to a stop and " + "stays there until they agree. See docs/robot-side-position-control.md\n", robot_id, cmd.vision_global_pos[0], cmd.vision_global_pos[1], nearest, IBIS_POSITION_MATCH_THRESHOLD); } From 5641db41a8ffaeae30bd187b7cec8eb4206a673c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Sep 2026 13:27:40 +0900 Subject: [PATCH 20/22] =?UTF-8?q?fix(simulator):=20=E5=AE=89=E5=85=A8?= =?UTF-8?q?=E5=81=9C=E6=AD=A2=E3=82=92=E5=AE=9F=E6=A9=9F=E3=81=A8=E5=90=8C?= =?UTF-8?q?=E3=81=98=E6=83=B0=E8=B5=B0=E3=81=AB=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ibisShouldStop()（stop_emergency / vision 断 / vision タイムアウト）が ゼロ速度の move_command を出していたため、シミュレータでは車輪 PID が効いて 1.5 m/s から 0.133 m で止まっていた。実機 G474 はこの条件で omniStopAll() に入り（state_func.c:314）、4輪の電圧を 0 にするだけで（omni_wheel.c:62）、 CAN フレームは duty の float のみでブレーキビットを持たない（actuator.c:12）。 つまり実機は制動ではなく惰走する。 シミュレータが実機より 4.3 倍短い距離で止まるということは、安全停止の検証が 実機より良く見えるということで、方向として危険な側に外れている。CM4 の applySafetyStop() は STOP_EMERGENCY を立てるので、crane 断・feedback 断・ vision 断の安全停止がすべてこの経路に入る。 move_command を出さないことで SimRobot の !has_move_command() 早期 return に 落ち、車輪 PID を通らず惰走する。指令途絶時の standby 経路と同じ道になる。 実測で 0.571 m となり、指令途絶時の 0.573 m と一致した。指令自体は届き続ける ので feedback の速度は正しく 0 まで減衰する。 prev_vx/prev_vy のクリアは残した。これは mode 3 の加速度制限の状態であって move_command とは無関係で、消すと停止解除後の最初の指令が停止前の速度から ランプしてしまう。 mode 4 が届いた場合の停止は能動制動のまま変更していない。この分岐は実機に 対応物が無く、構成ミスでシミュレータまで mode 4 が来たときの合図なので、 惰走させると発見が遅れる。 --- docs/robot-side-position-control.md | 35 ++++++++++++++++++++--------- src/simulator/simulator.cpp | 16 +++++++++---- 2 files changed, 36 insertions(+), 15 deletions(-) diff --git a/docs/robot-side-position-control.md b/docs/robot-side-position-control.md index acd4a1b74a..4affc88f3f 100644 --- a/docs/robot-side-position-control.md +++ b/docs/robot-side-position-control.md @@ -441,21 +441,34 @@ python3 data/scripts/ibis-chain-smoketest.py [path/to/simulator-cli] そのまま返すため、ノイズも vision 遅延も無い。実機の G474 は `vision_based_position` としてタイヤオドメトリと vision を融合した推定値を返す。 シミュレータ上の位置制御は実機より良く見える。必要ならノイズ注入を追加する。 -- **指令が途切れた間の停止は「摩擦による惰走」で、能動制動ではない。** - `SimRobot::begin()` は最後の指令から 0.1 s で standby に入り、車輪 PID に到達する - 手前で return するため駆動力が一切かからない（`src/amun/simulator/simrobot.cpp:337` - と `:417`）。1.5 m/s から指令を止めた場合、実測で停止まで **0.573 m** 進む。同じ - 速度から mode 3 の r=0（＝能動制動）を送った場合は **0.126 m** で止まるので、 - 4.5 倍の差がある。位置照合で破棄された場合も、経路劣化で落ちた場合も同じ経路を通る。 - 停止距離をそのまま実機の挙動として読まないこと。 +- **停止は2経路あり、片方は惰走。実機と構造が一致している。** 実機 G474 は + `sys->stop_flag || stop_emergency || !is_vision_available || + elapsed_time_ms_since_last_vision > 500` で `omniStopAll()` に入り + （`state_func.c:314`）、4輪の電圧を 0 にするだけ（`omni_wheel.c:62`）。CAN フレームは + duty の float 4 バイトのみでブレーキビットを持たない（`actuator.c:12`）。つまり + **実機のこの分岐は制動ではなく惰走**。一方 mode 3 で `r = 0` を送った場合は + `omniMoveIndiv` に入り車輪 PID が効く。シミュレータもこの2経路を再現している。 + + | 停止のかけ方 | 経路 | 1.5 m/s からの停止距離 | + |---|---|---| + | mode 3 で `r = 0` | 車輪 PID（能動制動） | 0.126 m | + | `STOP_EMERGENCY` / vision 断 | `move_command` を出さない（惰走） | 0.571 m | + | 指令が届かない（0.1 s で standby） | 同上（惰走） | 0.573 m | + + 残る不確定は1段下で、**duty 0 が空転なのか短絡制動なのかはモータボード側の + ファームウェアが決める**。これは `G474_Orion_main` にも CM4 側にも無いため、 + 惰走距離の絶対値は実機で測るしかない。上の 0.571 m は比較対象であって実機の + 予測値ではない。なお mode 4 が届いた場合の停止だけは能動制動のまま残してある。 + これは実機に対応物が無い「構成ミス」の合図なので、惰走させると発見が遅れるため。 - **feedback の速度は指令が届いたときしか更新されない。** 速度（byte 52/56）は `RadioResponse` 由来のキャッシュで、`RadioResponse` は指令が届いたときにしか 生成されない。一方で位置（byte 44/48）は毎周期 vision から詰め直される。このため 指令が破棄されている間、**同じパケットの中で位置は新鮮なのに速度は破棄直前の値で - 凍る**。上の実測でもロボットは約 1.0 s で停止しているのに、速度フィールドは - 1.461 m/s を返し続けた。「速度が出ているのに位置が動かない」feedback を見たら、 - ロボットの挙動ではなく指令が届いているかをまず疑うこと。`cm4_sim` は位置だけを - 使ってループを閉じるので現状の制御には影響しないが、速度を見る診断は騙される。 + 凍る**。実測ではロボットが約 1.0 s で停止した後も 1.461 m/s を返し続けた。 + 「速度が出ているのに位置が動かない」feedback を見たら、ロボットの挙動ではなく + 指令が届いているかをまず疑うこと。`cm4_sim` は位置だけを使ってループを閉じるので + 現状の制御には影響しないが、速度を見る診断は騙される。なお `STOP_EMERGENCY` に + よる停止では指令自体は届き続けるので、速度は正しく 0 まで減衰する。 - **ボールセンサ・キック状態。** `kick_status` は常に 0。 - **ローカルカメラ。** `cam_server_v3` 相当は無い。`cm4_sim` はカメラ領域を ゼロ埋めすること（実機のカメラ未接続時と同じ扱い）。 diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index b33e50a5d3..f1fa6f3ae8 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -765,10 +765,18 @@ private slots: robotCmd->set_id(robot_id); if (ibisShouldStop(cmd)) { - auto* lv = robotCmd->mutable_move_command()->mutable_local_velocity(); - lv->set_forward(0.0f); - lv->set_left(0.0f); - lv->set_angular(0.0f); + // Deliberately no move_command: the real G474 answers this condition + // with omniStopAll() (state_func.c:314), which writes duty 0 to all + // four wheels, and the CAN frame carries duty only -- there is no + // brake bit (actuator.c:12). So hardware coasts here, it does not + // brake. Leaving move_command unset lands on SimRobot's + // !has_move_command() early return, which skips the wheel PID and + // lets the robot coast the same way. Emitting a zero velocity would + // instead drive the PID and brake in 0.133 m vs 0.573 m coasting, + // making every safety stop look better in simulation than on the + // field. prev_v* still has to be cleared: it is the mode 3 + // acceleration-limiter state, so a stale value would ramp the first + // command after the stop clears from the pre-stop velocity. m_robotStates[robot_id].prev_vx = 0.0; m_robotStates[robot_id].prev_vy = 0.0; } else if (cmd.control_mode != IBIS_MODE_POLAR_VELOCITY_TARGET) { From f15d749ca0cd2dd333f69ca5094bd19541bc22af Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 13 Sep 2026 13:46:11 +0900 Subject: [PATCH 21/22] =?UTF-8?q?fix(docs):=20=E5=81=9C=E6=AD=A2=E8=B7=9D?= =?UTF-8?q?=E9=9B=A2=E3=81=AE=E5=AE=9F=E6=B8=AC=E5=80=A4=E3=82=92=E6=B8=AC?= =?UTF-8?q?=E3=82=8A=E7=9B=B4=E3=81=97=E3=81=A6=E8=A8=82=E6=AD=A3=E3=81=97?= =?UTF-8?q?=E3=80=81=E6=B8=AC=E5=AE=9A=E3=82=B9=E3=82=AF=E3=83=AA=E3=83=97?= =?UTF-8?q?=E3=83=88=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit PR #10 で記載した停止距離（能動制動 0.126 m / 緊急停止 0.571 m / 指令途絶 0.573 m）は誤りだった。測定時にロボットが壁や他のロボットに 当たっており、自由な惰走を測れていなかった。 ロボットの開始位置は y = -2.8 付近で、-y は壁、±x は他のロボットに塞がれて いる。塞がれた向きで測ると停止距離が 0.12〜0.40 m の範囲で不規則にばらつく のだが、値そのものは「それらしく」見えるため、読みからは異常と分からない。 実際 PR #10 の 0.571 と 0.573 は互いによく一致していたので、同じ経路を通った 証拠として扱ってしまった。偶然に過ぎなかった。 +y 方向で blue と yellow の両方で測り直した結果（1.45 m/s から）: | 停止のかけ方 | 停止距離 | |---|---| | mode 3 で r = 0（能動制動） | 0.13 m | | STOP_EMERGENCY / vision 断（惰走） | 0.63 m | | 指令が届かない（惰走） | 0.76 m | 両チームの差は 0.001 m 以内で、自由惰走であることが確認できる。 あわせて、緊急停止と指令途絶が一致するという記述も訂正した。指令途絶の方が 0.13 m 長いのが正しい。SimRobot は最後の指令から 0.1 s 経つまで前の指令を 実行し続けるので、その間は駆動力がかかっているため。この 0.1 s は実機の 250 ms（connected_ai タイムアウト）より短いので、指令途絶時の惰走距離は シミュレータの方が実機より短く出るという忠実度ギャップも追記した。 測定手順を data/scripts/ibis-stop-distance.py として残す。+y 方向で測ること、 1 測定 1 プロセスにすること（速度フィールドの凍結を前の測定から持ち越すと 加速せずに測定に入ってしまう）を組み込み、両チームの一致も検査する。 PR #10 のコード変更（緊急停止を惰走にする）自体は正しく、変更していない。 --- data/scripts/ibis-stop-distance.py | 201 ++++++++++++++++++++++++++++ docs/robot-side-position-control.md | 25 +++- 2 files changed, 220 insertions(+), 6 deletions(-) create mode 100644 data/scripts/ibis-stop-distance.py diff --git a/data/scripts/ibis-stop-distance.py b/data/scripts/ibis-stop-distance.py new file mode 100644 index 0000000000..1a5a946829 --- /dev/null +++ b/data/scripts/ibis-stop-distance.py @@ -0,0 +1,201 @@ +"""Measure how far a robot travels after each of the three ways it can stop. + +The simulator reproduces the robot's two hardware stop paths. The wheel PID brakes +when told to (mode 3 with r=0). Drive is cut entirely when the stop condition fires +-- the real G474 answers that with omniStopAll(), which writes duty 0, and the CAN +frame has no brake bit, so hardware coasts. A third case, commands simply not +arriving, keeps executing the last command for 0.1 s and then coasts as well. +See docs/robot-side-position-control.md. + +Two things make a naive measurement of this wrong in a way that looks right: + + * Measure in +y only, and on both teams. Robots start near y=-2.8, so -y is the + wall and both x directions are blocked by other robots. A blocked run still + produces a plausible number -- 0.12 to 0.40 m, varying run to run -- and there + is nothing in the reading that says it was blocked. Both teams agreeing to a + few mm is the signal that the coast was free. + + * One simulator process per measurement. The feedback velocity field freezes when + commands stop arriving (see the fidelity gaps in the doc above), so a previous + run's frozen value is read as "already at speed" and the next run never + accelerates -- reporting a full-speed start and a stopping distance of zero. + +Usage: python3 data/scripts/ibis-stop-distance.py [kinds] [base-port] [binary] + kinds: comma separated, any of brake,estop,drop (default: all three) +""" + +import math +import socket +import struct +import subprocess +import sys +import tempfile +import time +from pathlib import Path + +# Commands are sent at this speed; the run starts measuring once the robot reaches +# TARGET_V. Stopping distance goes with speed, so the three paths are only +# comparable if they all start from the same one. A fixed-duration acceleration +# phase does not do this: the speed reached varies with the robot's start pose. +COMMAND_SPEED = 1.5 +TARGET_V = 1.45 + +MODE_POLAR_VELOCITY = 3 +HEADING_PLUS_Y = math.pi / 2 + + +def encode_two_byte(value, value_range): + raw = int(32767.0 * (value / value_range) + 32767.0) + raw = max(0, min(65535, raw)) + return bytes([(raw >> 8) & 0xFF, raw & 0xFF]) + + +def build_packet(counter, pos, speed, theta, stop_emergency=False): + """715-byte packet driving robot 0 at `speed` along global direction `theta`.""" + d = bytearray(64) + d[1] = counter & 0xFF + d[2:4] = encode_two_byte(pos[0], 32.767) # VISION_GLOBAL_X + d[4:6] = encode_two_byte(pos[1], 32.767) # VISION_GLOBAL_Y + d[6:8] = encode_two_byte(0.0, math.pi) # VISION_GLOBAL_THETA + d[8:10] = encode_two_byte(0.0, math.pi) # TARGET_GLOBAL_THETA + d[12:14] = encode_two_byte(4.0, 32.767) # ACCELERATION_LIMIT + d[14:16] = encode_two_byte(4.0, 32.767) # LINEAR_VELOCITY_LIMIT + d[16:18] = encode_two_byte(5.0, 32.767) # ANGULAR_VELOCITY_LIMIT + d[22] = 0x01 | (0x08 if stop_emergency else 0x00) # IS_VISION_AVAILABLE | STOP_EMERGENCY + d[23] = MODE_POLAR_VELOCITY + d[24:26] = encode_two_byte(speed, 32.767) # CONTROL_MODE_ARGS: r + # theta is in the same +-32.767 range as r, not +-pi (ibis_protocol.h). Packing + # it as a pi-range angle silently drives the robot in a different direction. + d[26:28] = encode_two_byte(theta, 32.767) + packet = bytearray() + for slot in range(11): + packet += (bytes([0]) + bytes(d)) if slot == 0 else (bytes([0xFF]) + bytes(64)) + return bytes(packet) + + +def parse_feedback(data): + """(x, y, vx) in metres and m/s, or None if this is not a feedback packet.""" + if len(data) != 128 or data[0] != 0xAB or data[1] != 0xEA: + return None + return (struct.unpack_from(" 1 else ["brake", "estop", "drop"] + port = int(sys.argv[2]) if len(sys.argv) > 2 else 15600 + repo = Path(__file__).resolve().parents[2] + binary = Path(sys.argv[3]) if len(sys.argv) > 3 else repo / "build" / "bin" / "simulator-cli" + if not binary.exists(): + print(f"simulator-cli not found at {binary}", file=sys.stderr) + return 2 + + results = {} + for team in ("yellow", "blue"): + row = [] + for kind in kinds: + distance, error = measure(binary, kind, team, port) + port += 2 + results[(team, kind)] = distance + row.append(f"{kind}={'FAILED' if distance is None else format(distance, '.3f')}" + + (f" ({error})" if error else "")) + print(f"{team:6s} +y: " + " ".join(row)) + + # Both teams are different robots on different parts of the field. Agreeing means + # neither hit anything; disagreeing means at least one run was blocked and the + # numbers cannot be compared against each other or against the doc. + print() + ok = True + for kind in kinds: + a, b = results[("yellow", kind)], results[("blue", kind)] + if a is None or b is None: + print(f"[FAIL] {kind}: a run did not complete") + ok = False + continue + spread = abs(a - b) + agree = spread < 0.02 + ok = ok and agree + print(f"[{'PASS' if agree else 'FAIL'}] {kind}: teams agree to {spread:.3f} m " + f"(want < 0.020; larger means something was in the way)") + return 0 if ok else 1 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/docs/robot-side-position-control.md b/docs/robot-side-position-control.md index 4affc88f3f..a58b7b5d0f 100644 --- a/docs/robot-side-position-control.md +++ b/docs/robot-side-position-control.md @@ -449,22 +449,34 @@ python3 data/scripts/ibis-chain-smoketest.py [path/to/simulator-cli] **実機のこの分岐は制動ではなく惰走**。一方 mode 3 で `r = 0` を送った場合は `omniMoveIndiv` に入り車輪 PID が効く。シミュレータもこの2経路を再現している。 - | 停止のかけ方 | 経路 | 1.5 m/s からの停止距離 | + | 停止のかけ方 | 経路 | 1.45 m/s からの停止距離 | |---|---|---| - | mode 3 で `r = 0` | 車輪 PID（能動制動） | 0.126 m | - | `STOP_EMERGENCY` / vision 断 | `move_command` を出さない（惰走） | 0.571 m | - | 指令が届かない（0.1 s で standby） | 同上（惰走） | 0.573 m | + | mode 3 で `r = 0` | 車輪 PID（能動制動） | 0.13 m | + | `STOP_EMERGENCY` / vision 断 | `move_command` を出さない（惰走） | 0.63 m | + | 指令が届かない | 0.1 s は前の指令のまま → standby（惰走） | 0.76 m | + + **測定は必ず +y 方向で、blue と yellow の両方で取ること。** ロボットの開始位置は + y = -2.8 付近で、-y は壁、±x は他のロボットに塞がれている。塞がれた向きで測ると + 停止距離が 0.12〜0.40 m の範囲で不規則にばらつき、しかも「それらしい値」に見える。 + 両チームで測って一致すれば自由惰走、食い違えば何かに当たっている。測定は + `data/scripts/ibis-stop-distance.py` にしてあり、両チームの一致も検査する。 + + **指令途絶が `STOP_EMERGENCY` より 0.13 m 長いのは正しい。** `SimRobot` は最後の + 指令から 0.1 s 経つまで前の指令を実行し続けるので、その間は駆動力がかかっている。 + `STOP_EMERGENCY` は指令が届いた時点で即座に駆動を切るので、その分だけ短い。 + **ただしこの 0.1 s は実機の 250 ms（`connected_ai` タイムアウト）より短い**ので、 + 指令途絶時の惰走距離はシミュレータの方が実機より短く出る。 残る不確定は1段下で、**duty 0 が空転なのか短絡制動なのかはモータボード側の ファームウェアが決める**。これは `G474_Orion_main` にも CM4 側にも無いため、 - 惰走距離の絶対値は実機で測るしかない。上の 0.571 m は比較対象であって実機の + 惰走距離の絶対値は実機で測るしかない。上の 0.63 m は比較対象であって実機の 予測値ではない。なお mode 4 が届いた場合の停止だけは能動制動のまま残してある。 これは実機に対応物が無い「構成ミス」の合図なので、惰走させると発見が遅れるため。 - **feedback の速度は指令が届いたときしか更新されない。** 速度（byte 52/56）は `RadioResponse` 由来のキャッシュで、`RadioResponse` は指令が届いたときにしか 生成されない。一方で位置（byte 44/48）は毎周期 vision から詰め直される。このため 指令が破棄されている間、**同じパケットの中で位置は新鮮なのに速度は破棄直前の値で - 凍る**。実測ではロボットが約 1.0 s で停止した後も 1.461 m/s を返し続けた。 + 凍る**。実測ではロボットが約 1.0 s で停止した後も 1.46 m/s を返し続けた。 「速度が出ているのに位置が動かない」feedback を見たら、ロボットの挙動ではなく 指令が届いているかをまず疑うこと。`cm4_sim` は位置だけを使ってループを閉じるので 現状の制御には影響しないが、速度を見る診断は騙される。なお `STOP_EMERGENCY` に @@ -478,6 +490,7 @@ python3 data/scripts/ibis-chain-smoketest.py [path/to/simulator-cli] - `src/simulator/ibis_protocol.h` — プロトコル定義（framework 側） - `data/scripts/ibis-chain-smoketest.py` — ibis コマンド経路のスモークテスト +- `data/scripts/ibis-stop-distance.py` — 停止 3 経路の停止距離測定（両チーム一致で検査） - `src/simulator/simulator.cpp` — `IbisCommandAdaptor` / `IbisFeedbackAdaptor` - `src/amun/simulator/simulator.cpp` — `handleSimulatorTick()`、feedback 送出 - `Orion_CM4/cm4/bridge/robot_packet.h` — CM4 側パケット定義（要統一） From a52b6bdecd97a277544dd6f275afbf4483781e12 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 18 Sep 2026 00:50:54 +0900 Subject: [PATCH 22/22] =?UTF-8?q?fix(simulator):=20IbisCommandAdaptor?= =?UTF-8?q?=E3=82=92GC=E5=88=A4=E5=AE=9A=E3=83=81=E3=83=BC=E3=83=A0?= =?UTF-8?q?=E8=89=B2=E3=81=AB=E9=99=90=E5=AE=9A=E3=81=97=E6=8E=A5=E8=BF=91?= =?UTF-8?q?=E6=99=82=E3=81=AE=E3=83=AD=E3=83=9C=E3=83=83=E3=83=88=E5=8F=96?= =?UTF-8?q?=E3=82=8A=E9=81=95=E3=81=88=E3=82=92=E9=98=B2=E6=AD=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/simulator/simulator.cpp | 88 +++++++++++++++++++++++++------------ 1 file changed, 61 insertions(+), 27 deletions(-) diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index f1fa6f3ae8..49f5f2b38b 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -658,17 +658,33 @@ void SimProxy::handleCommand(const Command &command) { class IbisCommandAdaptor : public QObject { Q_OBJECT public: - IbisCommandAdaptor(int port, Timer* timer, double accSpeedup, double accBrake) + IbisCommandAdaptor(int port, Timer* timer, double accSpeedup, double accBrake, + bool useReferee, bool explicitColorSet, bool explicitIsBlue) : m_server(this) , m_timer(timer) , m_accSpeedup(accSpeedup) , m_accBrake(accBrake) + , m_ibisIsBlue(explicitColorSet ? explicitIsBlue : true) + , m_refereeResolved(explicitColorSet || !useReferee) { m_server.bind(QHostAddress::Any, static_cast(port)); connect(&m_server, &QUdpSocket::readyRead, this, &IbisCommandAdaptor::handleDatagrams); } public slots: + void handleRefereePacket(bool ibisIsBlue) { + if (!m_refereeResolved) { + m_ibisIsBlue = ibisIsBlue; + m_refereeResolved = true; + log(stdout, "ibis: command receiver team color resolved to %s from Game Controller\n", + ibisIsBlue ? "BLUE" : "YELLOW"); + } else if (m_ibisIsBlue != ibisIsBlue) { + m_ibisIsBlue = ibisIsBlue; + log(stdout, "ibis: command receiver team color switched to %s from Game Controller\n", + ibisIsBlue ? "BLUE" : "YELLOW"); + } + } + void handleVisionData(const QByteArray& data, qint64, QString) { SSL_WrapperPacket pkt; if (!pkt.ParseFromArray(data.data(), data.size()) || !pkt.has_detection()) { @@ -704,6 +720,13 @@ private slots: const auto& data = datagram.data(); if (data.size() != IBIS_PACKET_SIZE) { + log(stdout, "ibis: received packet of size %d, expected %d\n", + static_cast(data.size()), IBIS_PACKET_SIZE); + continue; + } + + if (!m_refereeResolved) { + // Wait until team color is known from Game Controller continue; } @@ -711,11 +734,12 @@ private slots: SSLSimRobotControl blueControl{new sslsim::RobotControl}; SSLSimRobotControl yellowControl{new sslsim::RobotControl}; - bool hasBlue = false, hasYellow = false; + bool hasBlue = false; + bool hasYellow = false; for (int slot = 0; slot < IBIS_ROBOT_SLOTS; ++slot) { const int offset = slot * IBIS_SLOT_SIZE; - const uint8_t robot_id = buf[offset]; + const int robot_id = buf[offset]; if (robot_id >= IBIS_ROBOT_SLOTS) { continue; } @@ -732,32 +756,27 @@ private slots: const IbisCommand cmd = ibisDeserialize(cmd_data); m_robotStates[robot_id].last_check_counter = cmd.check_counter; - // Team auto-detection: match vision_global_pos against cached positions. - // Also keeps the matched vision entry for orientation lookup below. - int teamIdx = -1; + // Match vision_global_pos against the designated team only. + // Do not search opponent team to prevent accidental control of opponent robots. + const int teamIdx = m_ibisIsBlue ? 0 : 1; const IbisVisionState* vis = nullptr; double nearest = -1.0; - for (int t = 0; t < 2; ++t) { - const IbisVisionState& v = m_vision[t][robot_id]; - if (!v.valid) { continue; } + const IbisVisionState& v = m_vision[teamIdx][robot_id]; + if (v.valid) { const float dx = v.x_mm / 1000.0f - cmd.vision_global_pos[0]; const float dy = v.y_mm / 1000.0f - cmd.vision_global_pos[1]; const double dist = std::hypot(dx, dy); - if (nearest < 0.0 || dist < nearest) { - nearest = dist; - } + nearest = dist; if (dist < IBIS_POSITION_MATCH_THRESHOLD) { - teamIdx = t; vis = &v; - break; } } - if (teamIdx < 0) { + if (!vis) { warnPositionMismatch(robot_id, cmd, nearest); continue; } m_robotStates[robot_id].match_warned = false; - const bool ibisIsBlue = (teamIdx == 0); + const bool ibisIsBlue = m_ibisIsBlue; auto* robotCmd = ibisIsBlue ? blueControl->add_robot_commands() @@ -913,20 +932,21 @@ private slots: } state.match_warned = true; state.last_match_warn_ns = now; + const char* teamStr = m_ibisIsBlue ? "BLUE" : "YELLOW"; if (nearest < 0.0) { log(stdout, - "ibis: robot %d command dropped -- no robot with this id is on the field yet " + "ibis: robot %d command dropped -- no %s robot with this id is on the field yet " "(command claims the robot is at %.3f, %.3f). Commands are ignored until " "vision reports the robot.\n", - robot_id, cmd.vision_global_pos[0], cmd.vision_global_pos[1]); + robot_id, teamStr, cmd.vision_global_pos[0], cmd.vision_global_pos[1]); } else { log(stdout, "ibis: robot %d command dropped -- vision_global_pos (%.3f, %.3f) is %.3f m " - "from the robot, over the %.2f m match threshold. The sender's position " + "from the %s robot, over the %.2f m match threshold. The sender's position " "estimate disagrees with the simulator; the robot coasts to a stop and " "stays there until they agree. See docs/robot-side-position-control.md\n", robot_id, cmd.vision_global_pos[0], cmd.vision_global_pos[1], - nearest, IBIS_POSITION_MATCH_THRESHOLD); + nearest, teamStr, IBIS_POSITION_MATCH_THRESHOLD); } } @@ -950,6 +970,8 @@ private slots: Timer* m_timer; double m_accSpeedup; double m_accBrake; + bool m_ibisIsBlue = true; + bool m_refereeResolved = true; }; class RefereeTeamDetector : public QObject { @@ -982,18 +1004,23 @@ private slots: auto tryMatch = [&](const SSL_Referee::TeamInfo& info, bool isBlue) { if (!info.has_name()) { return false; } if (QString::fromStdString(info.name()).toLower().trimmed() != m_teamName) { return false; } - disconnect(&m_socket, &QUdpSocket::readyRead, this, &RefereeTeamDetector::handleDatagrams); - emit teamDetected(isBlue); + if (!m_hasDetected || m_lastIsBlue != isBlue) { + m_hasDetected = true; + m_lastIsBlue = isBlue; + emit teamDetected(isBlue); + } return true; }; - if (ref.has_blue() && tryMatch(ref.blue(), true)) { return; } - if (ref.has_yellow() && tryMatch(ref.yellow(), false)) { return; } + if (ref.has_blue() && tryMatch(ref.blue(), true)) { continue; } + if (ref.has_yellow() && tryMatch(ref.yellow(), false)) { continue; } } } private: QUdpSocket m_socket; QString m_teamName; + bool m_hasDetected = false; + bool m_lastIsBlue = false; }; class IbisFeedbackAdaptor : public QObject { @@ -1114,10 +1141,14 @@ public slots: } void handleRefereePacket(bool ibisIsBlue) { - m_ibisIsBlue = ibisIsBlue; if (!m_refereeResolved) { + m_ibisIsBlue = ibisIsBlue; m_refereeResolved = true; - log(stdout, "ibis: team color resolved to %s from Game Controller\n", + log(stdout, "ibis: feedback sender team color resolved to %s from Game Controller\n", + ibisIsBlue ? "BLUE" : "YELLOW"); + } else if (m_ibisIsBlue != ibisIsBlue) { + m_ibisIsBlue = ibisIsBlue; + log(stdout, "ibis: feedback sender team color switched to %s from Game Controller\n", ibisIsBlue ? "BLUE" : "YELLOW"); } } @@ -1323,7 +1354,8 @@ int main(int argc, char* argv[]) } const quint16 refereePort = static_cast(parser.value(ibisRefereePortOpt).toUInt()); - auto* ibisCmd = new IbisCommandAdaptor(cmdPort, &timer, accSpeedup, accBrake); + auto* ibisCmd = new IbisCommandAdaptor(cmdPort, &timer, accSpeedup, accBrake, + useReferee, explicitColorSet, explicitIsBlue); auto* ibisFb = new IbisFeedbackAdaptor(fbAddr, fbPortBase, useReferee, explicitColorSet, explicitIsBlue); @@ -1348,6 +1380,8 @@ int main(int argc, char* argv[]) refereePort); QObject::connect(referee, &RefereeTeamDetector::teamDetected, ibisFb, &IbisFeedbackAdaptor::handleRefereePacket); + QObject::connect(referee, &RefereeTeamDetector::teamDetected, + ibisCmd, &IbisCommandAdaptor::handleRefereePacket); referee->moveToThread(&rcv_thread); }