feat: 実機用バイナリフィードバック送信機能を追加

CMakeLists.txt

@@ -200,6 +200,7 @@ set(SOURCES
     src/logger.cpp
     src/robotwidget.cpp
     src/getpositionwidget.cpp
+    src/binary_feedback_sender.cpp
 )
 
 set(HEADERS
@@ -225,6 +226,7 @@ set(HEADERS
     include/getpositionwidget.h
     include/common.h
     include/config.h
+    include/binary_feedback_sender.h
 )
 
 # files to be compiled

include/binary_feedback_sender.h

@@ -0,0 +1,46 @@
+/*
+grSim - RoboCup Small Size Soccer Robots Simulator
+Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim)
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef BINARY_FEEDBACK_SENDER_H
+#define BINARY_FEEDBACK_SENDER_H
+
+#include <QUdpSocket>
+#include <stdint.h>
+#include "robot.h"
+#include "configwidget.h"
+#include "config.h"
+
+class BinaryFeedbackSender {
+public:
+    BinaryFeedbackSender(ConfigWidget* cfg);
+    ~BinaryFeedbackSender();
+
+    void sendFeedback(int robotId, Robot* robot);
+    void setEnabled(bool enabled);
+    bool isEnabled() const { return enabled; }
+
+private:
+    void buildPacket(uint8_t* buffer, int robotId, Robot* robot);
+
+    QUdpSocket* socket;
+    uint8_t counters[MAX_ROBOT_COUNT * 2];  // Blue and Yellow teams
+    ConfigWidget* cfg;
+    bool enabled;
+};
+
+#endif // BINARY_FEEDBACK_SENDER_H

include/configwidget.h

@@ -215,6 +215,9 @@ class ConfigWidget : public VarTreeView
 
   DEF_VALUE(std::string, String, ColorRobotBlue)
   DEF_VALUE(std::string, String, ColorRobotYellow)
+  DEF_VALUE(bool, Bool, BinaryFeedbackEnabled)
+  DEF_VALUE(std::string, String, BinaryFeedbackAddr)
+  DEF_VALUE(int, Int, BinaryFeedbackPortBase)
 
   void loadRobotSettings(QString team);
 public slots:  

include/sslworld.h

@@ -45,6 +45,7 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim)
 #include "ssl_simulation_control.pb.h"
 #include "ssl_simulation_robot_control.pb.h"
 #include "ssl_simulation_robot_feedback.pb.h"
+#include "binary_feedback_sender.h"
 
 #define WALL_COUNT 10
 
@@ -111,6 +112,7 @@ class SSLWorld : public QObject
     QUdpSocket *simControlSocket;
     QUdpSocket *blueControlSocket;
     QUdpSocket *yellowControlSocket;
+    BinaryFeedbackSender *binaryFeedback{};
 
     QElapsedTimer elapsedLastPackageBlue;
     QElapsedTimer elapsedLastPackageYellow;

src/binary_feedback_sender.cpp

@@ -0,0 +1,135 @@
+/*
+grSim - RoboCup Small Size Soccer Robots Simulator
+Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim)
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "binary_feedback_sender.h"
+#include <QHostAddress>
+#include <cstring>
+#include <cmath>
+
+BinaryFeedbackSender::BinaryFeedbackSender(ConfigWidget* _cfg)
+    : cfg(_cfg), enabled(false)
+{
+    socket = new QUdpSocket();
+    memset(counters, 0, sizeof(counters));
+}
+
+BinaryFeedbackSender::~BinaryFeedbackSender()
+{
+    delete socket;
+}
+
+void BinaryFeedbackSender::setEnabled(bool _enabled)
+{
+    enabled = _enabled;
+}
+
+void BinaryFeedbackSender::buildPacket(uint8_t* buffer, int robotId, Robot* robot)
+{
+    // 128バイトをゼロクリア
+    memset(buffer, 0, 128);
+
+    // ヘッダー (0-1)
+    buffer[0] = 0xAB;
+    buffer[1] = 0xEA;
+
+    // Robot ID (2)
+    buffer[2] = static_cast<uint8_t>(robotId);
+
+    // Counter (3)
+    buffer[3] = counters[robotId];
+    counters[robotId]++;
+
+    // Yaw角 (4-7) - deg→rad変換
+    dReal dir = robot->getDir();
+    float yaw_rad = static_cast<float>(dir * M_PI / 180.0);
+    memcpy(&buffer[4], &yaw_rad, sizeof(float));
+
+    // 電圧 (8-11) - 固定値 24.0V
+    float voltage = 24.0f;
+    memcpy(&buffer[8], &voltage, sizeof(float));
+
+    // ボール検出0-2 (12-14)
+    bool ball_detected = robot->kicker->isTouchingBall();
+    buffer[12] = ball_detected ? 1 : 0;
+    buffer[13] = ball_detected ? 1 : 0;
+    buffer[14] = ball_detected ? 1 : 0;
+
+    // キック状態 (15)
+    KickStatus kick_status = robot->kicker->isKicking();
+    buffer[15] = static_cast<uint8_t>(kick_status);
+
+    // エラー情報 (16-23) - 0 (エラーなし)
+    // 既にmemsetで0初期化済み
+
+    // モーター電流 (24-27) - 0
+    // 既にmemsetで0初期化済み
+
+    // ボール検出3 (28) - 0
+    // 既にmemsetで0初期化済み
+
+    // 温度 (29-35) - 25℃
+    for (int i = 29; i <= 35; i++) {
+        buffer[i] = 25;
+    }
+
+    // 角度差分 (36-39) - 0
+    float angle_diff = 0.0f;
+    memcpy(&buffer[36], &angle_diff, sizeof(float));
+
+    // キャパシタ電圧 (40-43) - 200.0V
+    float capacitor_voltage = 200.0f;
+    memcpy(&buffer[40], &capacitor_voltage, sizeof(float));
+
+    // オドメトリXY (44-51)
+    dReal x, y;
+    robot->getXY(x, y);
+    float odom_x = static_cast<float>(x);
+    float odom_y = static_cast<float>(y);
+    memcpy(&buffer[44], &odom_x, sizeof(float));
+    memcpy(&buffer[48], &odom_y, sizeof(float));
+
+    // 速度XY (52-59)
+    const dReal* linear_vel = dBodyGetLinearVel(robot->chassis->body);
+    float vel_x = static_cast<float>(linear_vel[0]);
+    float vel_y = static_cast<float>(linear_vel[1]);
+    memcpy(&buffer[52], &vel_x, sizeof(float));
+    memcpy(&buffer[56], &vel_y, sizeof(float));
+
+    // チェックバージョン (60) - 0x01 (シミュレータ識別)
+    buffer[60] = 0x01;
+
+    // 拡張データ (61-127) - 0
+    // 既にmemsetで0初期化済み
+}
+
+void BinaryFeedbackSender::sendFeedback(int robotId, Robot* robot)
+{
+    if (!enabled || robot == nullptr) {
+        return;
+    }
+
+    uint8_t buffer[128];
+    buildPacket(buffer, robotId, robot);
+
+    // 送信先アドレスとポート
+    // ポート = 50100 + robot_id
+    QHostAddress addr("127.0.0.1");
+    int port = 50100 + robotId;
+
+    socket->writeDatagram(reinterpret_cast<const char*>(buffer), 128, addr, port);
+}

src/configwidget.cpp

@@ -141,6 +141,11 @@ ConfigWidget::ConfigWidget() {
     ADD_VALUE(comm_vars,Int,YellowControlListenPort,10302,"Yellow team control port")
     ADD_VALUE(comm_vars,Int,sendDelay,0,"Sending delay (milliseconds)")
     ADD_VALUE(comm_vars,Int,sendGeometryEvery,120,"Send geometry every X frames")
+    VarListPtr binary_feedback_vars(new VarList("Binary Feedback"));
+        comm_vars->addChild(binary_feedback_vars);
+        ADD_VALUE(binary_feedback_vars,Bool,BinaryFeedbackEnabled,true,"Enable Binary Feedback")
+        ADD_VALUE(binary_feedback_vars,String,BinaryFeedbackAddr,"127.0.0.1","Binary Feedback Address")
+        ADD_VALUE(binary_feedback_vars,Int,BinaryFeedbackPortBase,50100,"Binary Feedback Port Base")
     VarListPtr gauss_vars(new VarList("Gaussian noise"));
         comm_vars->addChild(gauss_vars);
         ADD_VALUE(gauss_vars,Bool,noise,false,"Noise")

src/mainwindow.cpp

@@ -38,6 +38,7 @@ Copyright (C) 2011, Parsian Robotic Center (eew.aut.ac.ir/~parsian/grsim)
 
 #include "mainwindow.h"
 #include "logger.h"
+#include "binary_feedback_sender.h"
 
 int MainWindow::getInterval()
 {
@@ -101,6 +102,8 @@ MainWindow::MainWindow(QWidget *parent)
     glwidget->ssl->simControlSocket = simControlSocket;
     glwidget->ssl->blueControlSocket = blueControlSocket;
     glwidget->ssl->yellowControlSocket = yellowControlSocket;
+    glwidget->ssl->binaryFeedback = new BinaryFeedbackSender(glwidget->cfg);
+    glwidget->ssl->binaryFeedback->setEnabled(glwidget->cfg->BinaryFeedbackEnabled());
 
     robotwidget = new RobotWidget(this, configwidget);
     robotwidget->setObjectName("RobotWidget");
@@ -487,6 +490,8 @@ void MainWindow::restartSimulator()
     newWorld->simControlSocket = simControlSocket;
     newWorld->blueControlSocket = blueControlSocket;
     newWorld->yellowControlSocket = yellowControlSocket;
+    newWorld->binaryFeedback = new BinaryFeedbackSender(glwidget->cfg);
+    newWorld->binaryFeedback->setEnabled(glwidget->cfg->BinaryFeedbackEnabled());
 
     auto oldWorld = glwidget->ssl;
     glwidget->ssl = newWorld;

src/sslworld.cpp

@@ -324,6 +324,7 @@ int SSLWorld::robotIndex(int robot,int team) {
 }
 
 SSLWorld::~SSLWorld() {
+    delete binaryFeedback;
     delete g;
     delete p;
 }
@@ -521,6 +522,16 @@ void SSLWorld::step(dReal dt) {
 
 
     sendVisionBuffer();
+
+    // Binary feedback送信
+    if (binaryFeedback && binaryFeedback->isEnabled()) {
+        for (int k = 0; k < cfg->Robots_Count() * 2; k++) {
+            if (robots[k] && robots[k]->on) {
+                binaryFeedback->sendFeedback(k, robots[k]);
+            }
+        }
+    }
+
     frame_num ++;
 }