diff --git a/.github/workflows/docker-publish.yaml b/.github/workflows/docker-publish.yaml new file mode 100644 index 000000000..bb9aaa21e --- /dev/null +++ b/.github/workflows/docker-publish.yaml @@ -0,0 +1,82 @@ +name: Build and Push Docker Image + +on: + push: + branches: + - ibis + tags: + - 'v*.*.*' + pull_request: + branches: + - ibis + 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 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 + 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 diff --git a/CMakeLists.txt b/CMakeLists.txt index 144366ffd..695a6584d 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/config/simulator-realism/Ibis.txt b/config/simulator-realism/Ibis.txt new file mode 100644 index 000000000..b51c8cdbc --- /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/data/docker/Dockerfile.simulatorcli b/data/docker/Dockerfile.simulatorcli index a7ab54fab..e225360b8 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/*; @@ -22,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 @@ -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/*; @@ -47,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 f7f9b4b3a..499656851 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 0088c422d..000000000 --- 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/amun/simulator/include/simulator/simulator.h b/src/amun/simulator/include/simulator/simulator.h index e41c8040e..4b76d0977 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; @@ -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 125Hz, no noise void sendSSLSimError(const QList& errors, ErrorSource source); public slots: @@ -117,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/simrobot.cpp b/src/amun/simulator/simrobot.cpp index 198ebf0bc..5cfe92a05 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 588a2fd77..e9004325e 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/amun/simulator/simulator.cpp b/src/amun/simulator/simulator.cpp index e8f5132ae..6cd47fd6a 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,9 +274,24 @@ void Simulator::process() m_data->dynamicsWorld->stepSimulation(timeDelta, 10, SUB_TIMESTEP); m_time = current_time; - // 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) { + // Emit ground truth robot positions at full simulation rate (125Hz), 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); + } + + // 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(); @@ -297,8 +312,6 @@ void Simulator::process() timer->start(timeout); m_visionTimers.enqueue(timer); } - - m_lastSentStatusTime = m_time; } // send timing information @@ -785,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); } @@ -950,8 +964,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 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/CMakeLists.txt b/src/simulator/CMakeLists.txt index d1b44a175..5cd05e181 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 @@ -29,3 +30,5 @@ target_link_libraries(simulator-cli Qt6::Widgets amun::simulator ) + +install(TARGETS simulator-cli RUNTIME DESTINATION bin) diff --git a/src/simulator/ibis_protocol.h b/src/simulator/ibis_protocol.h new file mode 100644 index 000000000..53e7e693b --- /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 000000000..015865c38 --- /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 000000000..5786efb64 --- /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 004b9e84f..2b704cc4e 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -46,6 +47,12 @@ #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" +#include "protobuf/world.pb.h" /** * Stand alone Erforce simulator @@ -592,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 125Hz void gotCommand(const Command &command); // internal void handleRadioCommands(const SSLSimRobotControl& control, bool isBlue, qint64 processingStart); // in public slots: @@ -637,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(); @@ -646,6 +655,385 @@ 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) { + // 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); + } + } + + 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, quint16 port = SSL_GAME_CONTROLLER_PORT) + : m_socket(this) + , m_teamName(teamName.toLower().trimmed()) + { + m_socket.bind(QHostAddress::AnyIPv4, + 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: + // 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 を算出 + void handleGroundTruth(const QByteArray& data) { + world::SimulatorState state; + if (!state.ParseFromArray(data.data(), data.size())) { + return; + } + 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); + 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; } + 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; } + + // ロボットローカル速度（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(); + 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: + 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; + + PacketSenderThread* m_sender; + QHostAddress m_addr; + quint16 m_portBase; + bool m_ibisIsBlue; + bool m_refereeResolved; +}; + #include "simulator.moc" @@ -671,12 +1059,30 @@ 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); 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"); + 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); + parser.addOption(ibisFeedbackTeamNameOpt); + parser.addOption(ibisUseRefereeOpt); + parser.addOption(ibisAccSpeedupOpt); + parser.addOption(ibisAccBrakeOpt); + parser.addOption(ibisRefereePortOpt); + parser.process(app); auto* desc = sslsim::RobotSpecs::descriptor(); @@ -748,6 +1154,51 @@ 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); + const quint16 refereePort = static_cast(parser.value(ibisRefereePortOpt).toUInt()); + + 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 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 + QObject::connect(&sim, &SimProxy::sendRadioResponses, + ibisFb, &IbisFeedbackAdaptor::handleRobotResponse); + + if (useReferee) { + auto* referee = new RefereeTeamDetector( + parser.value(ibisFeedbackTeamNameOpt), + parser.isSet(localhostConfig), + refereePort); + 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 synchronized to simulator loop (125 Hz)\n", + parser.value(ibisFeedbackAddrOpt).toStdString().c_str(), + static_cast(fbPortBase)); + } rcv_thread.start();