diff --git a/CMakeLists.txt b/CMakeLists.txt
index f9ffc9e6..0a936ec5 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/binary_feedback_sender.h b/include/binary_feedback_sender.h
new file mode 100644
index 00000000..e52cbacf
--- /dev/null
+++ b/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 .
+*/
+
+#ifndef BINARY_FEEDBACK_SENDER_H
+#define BINARY_FEEDBACK_SENDER_H
+
+#include
+#include
+#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
diff --git a/include/configwidget.h b/include/configwidget.h
index df7cece2..62dcc075 100644
--- a/include/configwidget.h
+++ b/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:
diff --git a/include/sslworld.h b/include/sslworld.h
index 60915835..fba55a5b 100644
--- a/include/sslworld.h
+++ b/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;
diff --git a/src/binary_feedback_sender.cpp b/src/binary_feedback_sender.cpp
new file mode 100644
index 00000000..c9b4cf64
--- /dev/null
+++ b/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 .
+*/
+
+#include "binary_feedback_sender.h"
+#include
+#include
+#include
+
+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(robotId);
+
+ // Counter (3)
+ buffer[3] = counters[robotId];
+ counters[robotId]++;
+
+ // Yaw角 (4-7) - deg→rad変換
+ dReal dir = robot->getDir();
+ float yaw_rad = static_cast(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(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(x);
+ float odom_y = static_cast(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(linear_vel[0]);
+ float vel_y = static_cast(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(buffer), 128, addr, port);
+}
diff --git a/src/configwidget.cpp b/src/configwidget.cpp
index c88d732e..3934c1fa 100644
--- a/src/configwidget.cpp
+++ b/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")
diff --git a/src/mainwindow.cpp b/src/mainwindow.cpp
index 0460c905..271f6e4a 100644
--- a/src/mainwindow.cpp
+++ b/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;
diff --git a/src/sslworld.cpp b/src/sslworld.cpp
index 2cb626e8..f8040f15 100644
--- a/src/sslworld.cpp
+++ b/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 ++;
}