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 ++; }