From c219e83dec6199b2d841a2f4f0712b6853ccaef8 Mon Sep 17 00:00:00 2001
From: William Ha <60044853+williamckha@users.noreply.github.com>
Date: Tue, 24 Sep 2024 18:17:50 -0700
Subject: [PATCH 01/14] Remove Qt dependency from ER Force Simulator (#3311)
* Update to python 3.10.0 and bazel 5.4.0
* No need to import `typing` for collections as of python 3.9
* Update black formatter to 24.4.2
* Run fix_formatting.sh
* Remove PyOpenGL-accelerate dependency
* Install latest pip in setup_software.sh
* Install latest pip in setup_software.sh
* Testing
* Testing
* Remove testing
* Bump pyqtgraph to 0.13.3
* Update setup_software.sh
* Try fixing precommit
* Try fixing precommit
* Try fixing precommit
* Replace black/autoflake with ruff
* Remove black binary
* Remove git attribute
* Remove unused check_formatting_ci.sh
* Run fix_formatting.sh
* Add simulated/field test fixtures to conftest.py
* Address #3251
* Python code cleanup and fix Thunderscope
* Enable pydocstyle rules in ruff
* Add compile pip requirements to pre-commit
* Formatting and fix pre-commit
* Fix pre-commit
* Fix pre-commit
* Fix pre-commit and fsm_diagram_generator.py
* Update docstrings
* Formatting
* Fix type errors
* Nuke jetson_nano/display :(
* Switch to pyqtdarktheme
* Bump pytqtgraph to 0.13.7
* If build fails, download autoref from Google Drive mirror
* Fix setup_software.sh
* If build fails, download autoref from releases in github fork
* Remove Qt deps from ER Force Simulator and improve code quality
* Remove Qt from bazel and setup_software.sh
* Misc code quality changes
* Rename formatting_scripts to scripts
* Fix docstring
* FIx docstring
* [pre-commit.ci lite] apply automatic fixes
* Add ASCII diagram to robotmesh.cpp
* Fix pyqtdarktheme dep missing
* Misc changes
* Nits
* Use python dataclasses, remove unused stuff, nits
* [pre-commit.ci lite] apply automatic fixes
---------
Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com>
---
environment_setup/setup_software.sh | 3 -
src/WORKSPACE | 16 -
src/extlibs/er_force_sim/README.md | 7 +-
.../er_force_sim/src/amun/simulator/BUILD | 14 +-
.../src/amun/simulator/erroraggregator.cpp | 40 --
.../src/amun/simulator/erroraggregator.h | 50 --
.../er_force_sim/src/amun/simulator/mesh.cpp | 96 ----
.../er_force_sim/src/amun/simulator/mesh.h | 60 ---
.../src/amun/simulator/robotmesh.cpp | 135 +++++
.../sslsim.h => amun/simulator/robotmesh.h} | 23 +-
.../src/amun/simulator/simball.cpp | 175 +++---
.../er_force_sim/src/amun/simulator/simball.h | 76 +--
.../src/amun/simulator/simfield.cpp | 106 ++--
.../src/amun/simulator/simfield.h | 19 +-
.../src/amun/simulator/simrobot.cpp | 250 ++++-----
.../src/amun/simulator/simrobot.h | 91 ++--
.../src/amun/simulator/simulator.cpp | 503 +++++-------------
.../src/amun/simulator/simulator.h | 155 +++---
src/extlibs/er_force_sim/src/core/BUILD | 11 +-
.../er_force_sim/src/core/configuration.h | 54 --
.../src/core/protobuffilereader.cpp | 62 ---
.../src/core/protobuffilereader.h | 44 --
.../src/core/protobuffilesaver.cpp | 68 ---
.../er_force_sim/src/core/protobuffilesaver.h | 53 --
src/extlibs/er_force_sim/src/core/rng.h | 6 +-
src/extlibs/er_force_sim/src/protobuf/BUILD | 9 +-
.../er_force_sim/src/protobuf/command.cpp | 54 --
.../er_force_sim/src/protobuf/command.h | 40 --
.../er_force_sim/src/protobuf/ssl_referee.cpp | 37 --
.../er_force_sim/src/protobuf/ssl_referee.h | 28 -
.../er_force_sim/src/protobuf/world.proto | 9 -
src/software/simulation/BUILD | 2 +-
.../simulation/er_force_simulator.cpp | 20 +-
src/software/simulation/er_force_simulator.h | 4 +-
34 files changed, 712 insertions(+), 1608 deletions(-)
delete mode 100644 src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.cpp
delete mode 100644 src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.h
delete mode 100644 src/extlibs/er_force_sim/src/amun/simulator/mesh.cpp
delete mode 100644 src/extlibs/er_force_sim/src/amun/simulator/mesh.h
create mode 100644 src/extlibs/er_force_sim/src/amun/simulator/robotmesh.cpp
rename src/extlibs/er_force_sim/src/{protobuf/sslsim.h => amun/simulator/robotmesh.h} (76%)
delete mode 100644 src/extlibs/er_force_sim/src/core/configuration.h
delete mode 100644 src/extlibs/er_force_sim/src/core/protobuffilereader.cpp
delete mode 100644 src/extlibs/er_force_sim/src/core/protobuffilereader.h
delete mode 100644 src/extlibs/er_force_sim/src/core/protobuffilesaver.cpp
delete mode 100644 src/extlibs/er_force_sim/src/core/protobuffilesaver.h
delete mode 100644 src/extlibs/er_force_sim/src/protobuf/command.cpp
delete mode 100644 src/extlibs/er_force_sim/src/protobuf/command.h
delete mode 100644 src/extlibs/er_force_sim/src/protobuf/ssl_referee.cpp
delete mode 100644 src/extlibs/er_force_sim/src/protobuf/ssl_referee.h
diff --git a/environment_setup/setup_software.sh b/environment_setup/setup_software.sh
index d9531f5173..e38c51d1ea 100755
--- a/environment_setup/setup_software.sh
+++ b/environment_setup/setup_software.sh
@@ -90,15 +90,12 @@ if [[ $(lsb_release -rs) == "20.04" ]]; then
host_software_packages+=(llvm-6.0)
host_software_packages+=(libclang-6.0-dev)
host_software_packages+=(libncurses5)
- host_software_packages+=(qt5-default)
# This fixes missing headers by notifying the linker
ldconfig
fi
if [[ $(lsb_release -rs) == "22.04" ]]; then
- host_software_packages+=(qtbase5-dev)
-
wget -nc https://github.com/UBC-Thunderbots/Software-External-Dependencies/blob/main/85-brltty.rules -O /tmp/85-brltty.rules
sudo mv /tmp/85-brltty.rules /usr/lib/udev/rules.d/85-brltty.rules
fi
diff --git a/src/WORKSPACE b/src/WORKSPACE
index 790a388d2c..1c7fc50d7e 100644
--- a/src/WORKSPACE
+++ b/src/WORKSPACE
@@ -214,22 +214,6 @@ git_repository(
shallow_since = "1571237073 -0500",
)
-# Qt bazel rules from https://github.com/justbuchanan/bazel_rules_qt
-git_repository(
- name = "bazel_rules_qt",
- commit = "7665177f47bf514176d5f8575a7334f030203e3d",
- remote = "https://github.com/justbuchanan/bazel_rules_qt.git",
- shallow_since = "1614109311 -0800",
-)
-
-# Right now qt5 is installed on the system using the setup_software.sh script
-# We assume we can find the headers in the "standard" path used in this rule
-new_local_repository(
- name = "qt",
- build_file = "@bazel_rules_qt//:qt.BUILD",
- path = "/usr/include/x86_64-linux-gnu/qt5/",
-)
-
new_local_repository(
name = "linux_gcc",
build_file = "@//extlibs:linux_gcc.BUILD",
diff --git a/src/extlibs/er_force_sim/README.md b/src/extlibs/er_force_sim/README.md
index c6a65ed338..257a2e5dbe 100644
--- a/src/extlibs/er_force_sim/README.md
+++ b/src/extlibs/er_force_sim/README.md
@@ -1,7 +1,11 @@
# The Simulator from Roboterfußballmannschaft der Friedrich-Alexander-Universität Erlangen-Nürnberg Robotics Erlangen's SSL-Team ER-Force's Framework
-The simulator was adapted from [robotics-erlangen/framework](https://github.com/robotics-erlangen/framework). It was modified so that time steps could be controlled by a stepSimulation function.
+
+The simulator is adapted from [robotics-erlangen/framework](https://github.com/robotics-erlangen/framework). It contains several bug fixes and general code quality improvements, and it is modified so that time steps can be controlled by a `stepSimulation` function.
+
## Copyright
+
+```
/***************************************************************************
* Copyright 2020 Michael Eischer, Philipp Nordhus, Andreas Wendler *
* Robotics Erlangen e.V. *
@@ -21,3 +25,4 @@ The simulator was adapted from [robotics-erlangen/framework](https://github.com/
* You should have received a copy of the GNU General Public License *
* along with this program. If not, see . *
***************************************************************************/
+```
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/BUILD b/src/extlibs/er_force_sim/src/amun/simulator/BUILD
index 76473358a4..69a9a7b732 100644
--- a/src/extlibs/er_force_sim/src/amun/simulator/BUILD
+++ b/src/extlibs/er_force_sim/src/amun/simulator/BUILD
@@ -1,21 +1,15 @@
-load("@bazel_rules_qt//:qt.bzl", "qt_cc_library")
-
package(default_visibility = ["//visibility:public"])
-qt_cc_library(
- name = "simulator_qt",
+cc_library(
+ name = "simulator",
srcs = glob(["*.cpp"]),
hdrs = glob(["*.h"]),
deps = [
- "//extlibs/er_force_sim/src/core:core_qt",
- "//extlibs/er_force_sim/src/protobuf:protobuf_qt",
+ "//extlibs/er_force_sim/src/core",
+ "//extlibs/er_force_sim/src/protobuf",
"//proto:ssl_simulation_cc_proto",
"//shared:constants",
"@bullet",
- "@qt//:qt_core",
- "@qt//:qt_gui",
- "@qt//:qt_widgets",
],
- #linkstatic = True,
alwayslink = True,
)
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.cpp b/src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.cpp
deleted file mode 100644
index 850e63c24d..0000000000
--- a/src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/****************************************************************************
- * Copyright 2021 Tobias Heineken *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 "erroraggregator.h"
-
-#include
-
-#include "simulator.h"
-
-using namespace camun::simulator;
-
-void ErrorAggregator::aggregate(SSLSimError error, ErrorSource source)
-{
- m_data[source].push_back(std::move(error));
-}
-
-QList ErrorAggregator::getAggregates(ErrorSource source)
-{
- QList &ref = m_data[source];
- QList out = ref;
- ref.clear();
- return out;
-}
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.h b/src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.h
deleted file mode 100644
index 7f10d3e39d..0000000000
--- a/src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/****************************************************************************
- * Copyright 2021 Tobias Heineken *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 SIM_AGGREGATOR_H
-#define SIM_AGGREGATOR_H
-#include
-#include
-
-#include "extlibs/er_force_sim/src/protobuf/sslsim.h"
-
-namespace camun
-{
- namespace simulator
- {
- enum class ErrorSource;
- class ErrorAggregator : public QObject
- {
- Q_OBJECT
- public:
- ErrorAggregator(QObject* parent) : QObject(parent) {}
-
- public slots:
- void aggregate(SSLSimError eror, ErrorSource e);
-
- public:
- QList getAggregates(ErrorSource e);
-
- private:
- QMap> m_data;
- };
- } // namespace simulator
-} // namespace camun
-#endif
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/mesh.cpp b/src/extlibs/er_force_sim/src/amun/simulator/mesh.cpp
deleted file mode 100644
index 599090fe94..0000000000
--- a/src/extlibs/er_force_sim/src/amun/simulator/mesh.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-/***************************************************************************
- * Copyright 2015 Michael Eischer, Philipp Nordhus *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 "mesh.h"
-
-#include
-
-using namespace camun::simulator;
-
-/*!
- * \class Mesh
- * \ingroup simulator
- * \brief A 3D mesh
- */
-
-Mesh::Mesh(float radius, float height, float angle, float holeSize, float boxHeight)
- : m_radius(radius), m_height(height), m_angle(angle), m_holeSize(holeSize)
-{
- const float frontPlateLength = std::sin(angle / 2.0f) * radius;
- const float frontPlatePos = radius * std::cos(angle / 2.0f);
- const float holePlatePos = frontPlatePos - holeSize;
- const float outerAngle = std::acos(holePlatePos / radius) * 2;
- const float angleDiff = (outerAngle - angle) / 2.0f;
- const float halfOuterAngle = outerAngle / 2.0f;
- const float outerAngleStart = halfOuterAngle + M_PI_2;
- const float outerAngleStop = 2.0f * M_PI - halfOuterAngle + M_PI_2;
- addRobotCover(20, outerAngleStart, outerAngleStop);
-
- // right pillar
- addRobotCover(5, outerAngleStart - angleDiff, outerAngleStart);
- m_hull.back().append(QVector3D(-frontPlateLength, holePlatePos, m_height / 2.0f));
- m_hull.back().append(QVector3D(-frontPlateLength, holePlatePos, -m_height / 2.0f));
-
- // left pillar
- addRobotCover(5, outerAngleStop, outerAngleStop + angleDiff);
- m_hull.back().append(QVector3D(frontPlateLength, holePlatePos, m_height / 2.0f));
- m_hull.back().append(QVector3D(frontPlateLength, holePlatePos, -m_height / 2.0f));
-
- // the remaining box
- QList boxPart;
- boxPart.append(QVector3D(frontPlateLength, holePlatePos, m_height / 2.0f));
- boxPart.append(QVector3D(-frontPlateLength, holePlatePos, m_height / 2.0f));
- boxPart.append(
- QVector3D(frontPlateLength, holePlatePos, -m_height / 2.0f + boxHeight));
- boxPart.append(
- QVector3D(-frontPlateLength, holePlatePos, -m_height / 2.0f + boxHeight));
- boxPart.append(QVector3D(frontPlateLength, frontPlatePos, m_height / 2.0f));
- boxPart.append(QVector3D(-frontPlateLength, frontPlatePos, m_height / 2.0f));
- boxPart.append(
- QVector3D(frontPlateLength, frontPlatePos, -m_height / 2.0f + boxHeight));
- boxPart.append(
- QVector3D(-frontPlateLength, frontPlatePos, -m_height / 2.0f + boxHeight));
- m_hull.append(boxPart);
-}
-
-/*!
- * \brief Adds triangles for the outer hull
- * \param radius Radius of the robot
- * \param height Height of the robot
- * \param num Number of segments
- * \param angle Angle of the hull start
- * \param angleStep Step size in rad
- */
-void Mesh::addRobotCover(uint num, float startAngle, float endAngle)
-{
- QList covers;
- float angle = startAngle;
- float angleStep = (endAngle - startAngle) / num;
- for (uint i = 0; i <= num; i++)
- {
- covers.append(
- QVector3D(m_radius * cos(angle), m_radius * sin(angle), m_height / 2.0f));
- covers.append(
- QVector3D(m_radius * cos(angle), m_radius * sin(angle), -m_height / 2.0f));
-
- angle += angleStep;
- }
- m_hull.append(covers);
-}
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/mesh.h b/src/extlibs/er_force_sim/src/amun/simulator/mesh.h
deleted file mode 100644
index e25ad2c019..0000000000
--- a/src/extlibs/er_force_sim/src/amun/simulator/mesh.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/***************************************************************************
- * Copyright 2015 Michael Eischer, Philipp Nordhus *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 MESH_H
-#define MESH_H
-
-#include
-#include
-#include
-#include
-
-namespace camun
-{
- namespace simulator
- {
- class Mesh;
- }
-} // namespace camun
-
-class camun::simulator::Mesh
-{
- public:
- Mesh(float radius, float height, float angle, float holeSize, float boxHeight);
- void createRobotMeshes(float radius, float height, float angle);
- const QList> &hull() const
- {
- return m_hull;
- }
-
- private:
- void addRobotCover(uint num, float startAngle, float endAngle);
- void addSidePart(uint num, float angleStart, float angleStop, bool right);
-
- private:
- QList> m_hull;
-
- const float m_radius;
- const float m_height;
- const float m_angle;
- const float m_holeSize;
-};
-
-#endif // MESH_H
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.cpp b/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.cpp
new file mode 100644
index 0000000000..d06d66390c
--- /dev/null
+++ b/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.cpp
@@ -0,0 +1,135 @@
+/***************************************************************************
+ * Copyright 2015 Michael Eischer, Philipp Nordhus *
+ * Robotics Erlangen e.V. *
+ * http://www.robotics-erlangen.de/ *
+ * info@robotics-erlangen.de *
+ * *
+ * 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 *
+ * 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 "robotmesh.h"
+
+#include
+
+using namespace camun::simulator;
+
+static std::vector> generateRobotShellPoints(
+ unsigned int numSegments, float startAngle, float endAngle, float radius,
+ float height)
+{
+ std::vector> points;
+
+ float angle = startAngle;
+ const float angleStep = (endAngle - startAngle) / numSegments;
+
+ for (unsigned int i = 0; i <= numSegments; ++i)
+ {
+ const float x = radius * std::cos(angle);
+ const float y = radius * std::sin(angle);
+
+ points.emplace_back(x, y, height / 2.0);
+ points.emplace_back(x, y, -height / 2.0);
+
+ angle += angleStep;
+ }
+
+ return points;
+}
+
+std::vector>>
+camun::simulator::createRobotMesh(float radius, float height, float angle,
+ float holeDepth, float holeHeight)
+{
+ static constexpr unsigned int NUM_SEGMENTS_HULL = 20;
+ static constexpr unsigned int NUM_SEGMENTS_PILLAR = 5;
+
+ // Diagram of robot (top view)
+ //
+ // ┌────────┬────frontPlatePos
+ // ┌───┬─────────holePlatePos
+ // ┌────┬────holeDepth
+ // , - ~ - ,
+ // , ' . ' ,
+ // , . .| ─┐
+ // , . . | │
+ // , .\. | │
+ // , + )─┐ | ├─frontPlateLength
+ // , `/` │ | │
+ // , │ │' | │
+ // , │` │ .| │
+ // , │ `│ , ' ─┘
+ // ' - , _│, │'
+ // │ └─angle
+ // └────angleDiff
+ // outerAngle = angle + angleDiff + angleDiff
+ //
+ const float frontPlateLength = std::sin(angle / 2.0) * radius;
+ const float frontPlatePos = radius * std::cos(angle / 2.0);
+ const float holePlatePos = frontPlatePos - holeDepth;
+ const float outerAngle = std::acos(holePlatePos / radius) * 2;
+ const float angleDiff = (outerAngle - angle) / 2.0;
+ const float halfOuterAngle = outerAngle / 2.0;
+ const float outerAngleStart = halfOuterAngle + M_PI_2;
+ const float outerAngleStop = 2.0 * M_PI - halfOuterAngle + M_PI_2;
+
+ std::vector>> meshParts;
+
+ // Parts of robot (top view)
+ //
+ // , - ~ - , Right pillar
+ // , ' | ' ,
+ // , ├────┐
+ // , | |
+ // , | |
+ // , Main hull | Front plate and dribbler hole
+ // , | |
+ // , | |
+ // , ├────┘
+ // , | .'
+ // ' - , _ , ' Left pillar
+ //
+
+ // Main hull
+ meshParts.push_back(generateRobotShellPoints(NUM_SEGMENTS_HULL, outerAngleStart,
+ outerAngleStop, radius, height));
+
+ // Left pillar
+ auto leftPillar = generateRobotShellPoints(
+ NUM_SEGMENTS_PILLAR, outerAngleStop, outerAngleStop + angleDiff, radius, height);
+ leftPillar.emplace_back(frontPlateLength, holePlatePos, height / 2.0);
+ leftPillar.emplace_back(frontPlateLength, holePlatePos, -height / 2.0);
+ meshParts.push_back(leftPillar);
+
+ // Right pillar
+ auto rightPillar =
+ generateRobotShellPoints(NUM_SEGMENTS_PILLAR, outerAngleStart - angleDiff,
+ outerAngleStart, radius, height);
+ rightPillar.emplace_back(-frontPlateLength, holePlatePos, height / 2.0);
+ rightPillar.emplace_back(-frontPlateLength, holePlatePos, -height / 2.0);
+ meshParts.push_back(rightPillar);
+
+ // The remaining front plate and dribbler "hole"
+ meshParts.push_back({
+ {frontPlateLength, holePlatePos, height / 2.0},
+ {-frontPlateLength, holePlatePos, height / 2.0},
+ {frontPlateLength, holePlatePos, -height / 2.0 + holeHeight},
+ {-frontPlateLength, holePlatePos, -height / 2.0 + holeHeight},
+ {frontPlateLength, frontPlatePos, height / 2.0},
+ {-frontPlateLength, frontPlatePos, height / 2.0},
+ {frontPlateLength, frontPlatePos, -height / 2.0 + holeHeight},
+ {-frontPlateLength, frontPlatePos, -height / 2.0 + holeHeight},
+ });
+
+ return meshParts;
+}
diff --git a/src/extlibs/er_force_sim/src/protobuf/sslsim.h b/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.h
similarity index 76%
rename from src/extlibs/er_force_sim/src/protobuf/sslsim.h
rename to src/extlibs/er_force_sim/src/amun/simulator/robotmesh.h
index 8bdc8aac12..3670e3f81e 100644
--- a/src/extlibs/er_force_sim/src/protobuf/sslsim.h
+++ b/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.h
@@ -1,5 +1,5 @@
/***************************************************************************
- * Copyright 2021 Tobias Heineken *
+ * Copyright 2015 Michael Eischer, Philipp Nordhus *
* Robotics Erlangen e.V. *
* http://www.robotics-erlangen.de/ *
* info@robotics-erlangen.de *
@@ -18,14 +18,19 @@
* along with this program. If not, see . *
***************************************************************************/
-#ifndef PB_SSLSIM_H
-#define PB_SSLSIM_H
+#ifndef ROBOTMESH_H
+#define ROBOTMESH_H
-#include
+#include
+#include
-#include "extlibs/er_force_sim/src/protobuf/ssl_simulation_error.pb.h"
-#include "extlibs/er_force_sim/src/protobuf/ssl_simulation_robot_control.pb.h"
+namespace camun
+{
+ namespace simulator
+ {
+ std::vector>> createRobotMesh(
+ float radius, float height, float angle, float holeDepth, float holeHeight);
+ } // namespace simulator
+} // namespace camun
-typedef QSharedPointer SSLSimRobotControl;
-typedef QSharedPointer SSLSimError;
-#endif
+#endif // ROBOTMESH_H
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simball.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simball.cpp
index 054a1d8b24..81c40e84c3 100644
--- a/src/extlibs/er_force_sim/src/amun/simulator/simball.cpp
+++ b/src/extlibs/er_force_sim/src/amun/simulator/simball.cpp
@@ -20,30 +20,26 @@
#include "simball.h"
-#include
#include
#include "extlibs/er_force_sim/src/core/coordinates.h"
-#include "extlibs/er_force_sim/src/core/rng.h"
#include "extlibs/er_force_sim/src/core/vector.h"
-#include "proto/ssl_vision_detection.pb.h"
#include "simulator.h"
using namespace camun::simulator;
-SimBall::SimBall(RNG *rng, btDiscreteDynamicsWorld *world)
- : m_rng(rng),
- m_world(world),
- current_ball_state(STATIONARY),
- set_transition_speed(true),
- rolling_speed(-1.0f)
+SimBall::SimBall(std::shared_ptr world)
+ : m_world(world),
+ m_rolling_speed(-1.0f),
+ m_set_transition_speed(true),
+ m_current_ball_state(STATIONARY)
{
// see http://robocup.mi.fu-berlin.de/buch/rolling.pdf for correct modelling
- m_sphere =
- new btSphereShape(static_cast(BALL_MAX_RADIUS_METERS) * SIMULATOR_SCALE);
+ m_sphere = std::make_unique(
+ static_cast(BALL_MAX_RADIUS_METERS) * SIMULATOR_SCALE);
- btVector3 localInertia(0, 0, 0);
// FIXME measure inertia coefficient
+ btVector3 localInertia(0, 0, 0);
m_sphere->calculateLocalInertia(BALL_MASS_KG, localInertia);
btTransform startWorldTransform;
@@ -51,30 +47,28 @@ SimBall::SimBall(RNG *rng, btDiscreteDynamicsWorld *world)
startWorldTransform.setOrigin(
btVector3(0.0f, 0.0f, static_cast(BALL_MAX_RADIUS_METERS)) *
SIMULATOR_SCALE);
- m_motionState = new btDefaultMotionState(startWorldTransform);
+ m_motionState = std::make_unique(startWorldTransform);
- btRigidBody::btRigidBodyConstructionInfo rbInfo(BALL_MASS_KG, m_motionState, m_sphere,
- localInertia);
+ btRigidBody::btRigidBodyConstructionInfo rbInfo(BALL_MASS_KG, m_motionState.get(),
+ m_sphere.get(), localInertia);
// parameters seem to be ignored...
- m_body = new btRigidBody(rbInfo);
- // see simulator.cpp
- // TODO (#2512): Check these values with real life
+ m_body = std::make_unique(rbInfo);
+
+ // See simulator.h
m_body->setRestitution(BALL_RESTITUTION);
m_body->setFriction(BALL_SLIDING_FRICTION_NEWTONS);
// \mu_r = -a / g = 0.0357 (while rolling)
// rollingFriction in bullet is too unstable to be useful
// use custom implementation in begin()
- m_world->addRigidBody(m_body);
+
+ m_world->addRigidBody(m_body.get());
}
SimBall::~SimBall()
{
- m_world->removeRigidBody(m_body);
- delete m_body;
- delete m_sphere;
- delete m_motionState;
+ m_world->removeRigidBody(m_body.get());
}
void SimBall::begin(bool robot_collision)
@@ -86,48 +80,47 @@ void SimBall::begin(bool robot_collision)
{ // ball is on the ground
bool is_stationary =
velocity.length() < STATIONARY_BALL_SPEED_METERS_PER_SECOND * SIMULATOR_SCALE;
- bool should_roll = velocity.length() < rolling_speed * SIMULATOR_SCALE;
+ bool should_roll = velocity.length() < m_rolling_speed * SIMULATOR_SCALE;
if (robot_collision)
{
- current_ball_state = ROBOT_COLLISION;
+ m_current_ball_state = ROBOT_COLLISION;
}
else if (is_stationary)
{
- current_ball_state = STATIONARY;
+ m_current_ball_state = STATIONARY;
}
- else if ((current_ball_state == SLIDING && should_roll) ||
- current_ball_state == ROLLING)
+ else if ((m_current_ball_state == SLIDING && should_roll) ||
+ m_current_ball_state == ROLLING)
{
- current_ball_state = ROLLING;
+ m_current_ball_state = ROLLING;
}
else
{
- current_ball_state = SLIDING;
+ m_current_ball_state = SLIDING;
}
- switch (current_ball_state)
+ switch (m_current_ball_state)
{
case STATIONARY:
m_body->setLinearVelocity(btVector3(0, 0, 0));
m_body->setFriction(BALL_SLIDING_FRICTION_NEWTONS);
- set_transition_speed = true;
+ m_set_transition_speed = true;
break;
case ROBOT_COLLISION:
m_body->setFriction(BALL_SLIDING_FRICTION_NEWTONS);
- set_transition_speed = true;
+ m_set_transition_speed = true;
break;
case SLIDING:
m_body->setFriction(BALL_SLIDING_FRICTION_NEWTONS);
- if (set_transition_speed)
+ if (m_set_transition_speed)
{
- rolling_speed =
+ m_rolling_speed =
FRICTION_TRANSITION_FACTOR * velocity.length() / SIMULATOR_SCALE;
}
- set_transition_speed = false;
+ m_set_transition_speed = false;
break;
case ROLLING:
-
// just apply rolling friction, normal friction is somehow handled by
// bullet
const btScalar rollingDeceleration =
@@ -139,54 +132,32 @@ void SimBall::begin(bool robot_collision)
SUB_TIMESTEP);
m_body->setFriction(0.0f);
- set_transition_speed = false;
+ m_set_transition_speed = false;
break;
}
}
- bool moveCommand = false;
- auto sendPartialCoordError = [this](const char *msg) {
- SSLSimError error{new sslsim::SimulatorError};
- error->set_code("PARTIAL_COORD");
- std::string message = "Partial coordinates are not implemented yet";
- error->set_message(message + msg);
- emit this->sendSSLSimError(error, ErrorSource::CONFIG);
- };
- if (m_move.has_x())
+ if (m_move.has_x() != m_move.has_y())
{
- if (!m_move.has_y())
- {
- sendPartialCoordError(": position ball");
- return;
- }
- moveCommand = true;
+ std::cerr << "Partial coordinates are not implemented yet: ball position"
+ << std::endl;
+ return;
}
- else if (m_move.has_y() || m_move.has_z())
+
+ if (m_move.has_vx() != m_move.has_vy())
{
- sendPartialCoordError(": position ball (not x)");
+ std::cerr << "Partial coordinates are not implemented yet: ball velocity"
+ << std::endl;
return;
}
- if (m_move.has_vx())
+ if (m_move.by_force() && (m_move.vx() != 0 || m_move.vy() != 0 || m_move.vz() != 0))
{
- if (!m_move.has_vy())
- {
- sendPartialCoordError(": velocity ball");
- return;
- }
- if (m_move.by_force() && (m_move.vx() != 0 || m_move.vy() != 0 ||
- (m_move.has_vz() && m_move.vz() != 0)))
- {
- SSLSimError error{new sslsim::SimulatorError};
- error->set_code("VELOCITY_FORCE");
- error->set_message("Velocities != 0 and by_force are incompatible");
- emit sendSSLSimError(error, ErrorSource::CONFIG);
- return;
- }
- moveCommand = true;
+ std::cerr << "Velocities != 0 and by_force are incompatible" << std::endl;
+ return;
}
- if (moveCommand)
+ if (m_move.has_x() || m_move.has_vx())
{
if (m_move.by_force())
{
@@ -232,9 +203,9 @@ void SimBall::begin(bool robot_collision)
m_body->setLinearVelocity(linVel * SIMULATOR_SCALE);
// override ballState
- current_ball_state = SLIDING;
- rolling_speed = linVel.length() * FRICTION_TRANSITION_FACTOR;
- set_transition_speed = false;
+ m_current_ball_state = SLIDING;
+ m_rolling_speed = linVel.length() * FRICTION_TRANSITION_FACTOR;
+ m_set_transition_speed = false;
m_body->setAngularVelocity(btVector3(0, 0, 0));
}
@@ -328,7 +299,7 @@ static float positionOfVisiblePixels(btVector3 &p, const btVector3 &simulatorBal
return static_cast(cameraHitCounter) / static_cast(maxHits);
}
-bool SimBall::update(SSLProto::SSL_DetectionBall *ball, float stddev, float stddevArea,
+bool SimBall::update(SSLProto::SSL_DetectionBall &ball, float stddev, float stddevArea,
const btVector3 &cameraPosition, bool enableInvisibleBall,
float visibilityThreshold, btVector3 positionOffset)
{
@@ -340,15 +311,15 @@ bool SimBall::update(SSLProto::SSL_DetectionBall *ball, float stddev, float stdd
enableInvisibleBall, visibilityThreshold, positionOffset);
}
-bool SimBall::addDetection(SSLProto::SSL_DetectionBall *ball, btVector3 pos, float stddev,
+bool SimBall::addDetection(SSLProto::SSL_DetectionBall &ball, btVector3 pos, float stddev,
float stddevArea, const btVector3 &cameraPosition,
bool enableInvisibleBall, float visibilityThreshold,
btVector3 positionOffset)
{
// setup ssl-vision ball detection
- ball->set_confidence(1.0f);
- ball->set_pixel_x(0.0f);
- ball->set_pixel_y(0.0f);
+ ball.set_confidence(1.0f);
+ ball.set_pixel_x(0.0f);
+ ball.set_pixel_y(0.0f);
btTransform transform;
m_motionState->getWorldTransform(transform);
@@ -365,7 +336,7 @@ bool SimBall::addDetection(SSLProto::SSL_DetectionBall *ball, btVector3 pos, flo
{
// if the visibility is lower than the threshold the ball disappears
visibility = positionOfVisiblePixels(pos, transform.getOrigin(),
- simulatorCameraPosition, m_world);
+ simulatorCameraPosition, m_world.get());
if (visibility < visibilityThreshold)
{
return false;
@@ -398,21 +369,15 @@ bool SimBall::addDetection(SSLProto::SSL_DetectionBall *ball, btVector3 pos, flo
float area =
visibility *
std::max(0.0f, (basePixelArea +
- static_cast(m_rng->normal(stddevArea)) / PIXEL_PER_AREA));
- ball->set_area(area * PIXEL_PER_AREA);
-
- // if (height > 0.1f) {
- // qDebug() << "simball" << p.x() << p.y() << height << "ttt" << ball_x <<
- // ball_y;
- // }
+ static_cast(m_rng.normal(stddevArea)) / PIXEL_PER_AREA));
+ ball.set_area(area * PIXEL_PER_AREA);
// add noise to coordinates
- // to convert from bullet coordinate system to ssl-vision rotate by 90 degree
- // ccw
- const ErForceVector noise = m_rng->normalVector(stddev);
- coordinates::toVision(ErForceVector(modX, modY) + noise, *ball);
+ // to convert from bullet coordinate system to ssl-vision rotate by 90 degree ccw
+ const ErForceVector noise = m_rng.normalVector(stddev);
+ coordinates::toVision(ErForceVector(modX, modY) + noise, ball);
- ball->set_z(modZ * 1000); // modZ is in kilometres, need to convert to metres
+ ball.set_z(modZ * 1000); // modZ is in kilometres, need to convert to metres
return true;
}
@@ -433,21 +398,21 @@ btVector3 SimBall::speed() const
return m_body->getLinearVelocity();
}
-void SimBall::writeBallState(world::SimBall *ball) const
+void SimBall::writeBallState(world::SimBall &ball) const
{
const btVector3 ballPosition =
m_body->getWorldTransform().getOrigin() / SIMULATOR_SCALE;
- ball->set_p_x(ballPosition.getX());
- ball->set_p_y(ballPosition.getY());
- ball->set_p_z(ballPosition.getZ());
+ ball.set_p_x(ballPosition.getX());
+ ball.set_p_y(ballPosition.getY());
+ ball.set_p_z(ballPosition.getZ());
const btVector3 ballSpeed = speed() / SIMULATOR_SCALE;
- ball->set_v_x(ballSpeed.getX());
- ball->set_v_y(ballSpeed.getY());
- ball->set_v_z(ballSpeed.getZ());
+ ball.set_v_x(ballSpeed.getX());
+ ball.set_v_y(ballSpeed.getY());
+ ball.set_v_z(ballSpeed.getZ());
const btVector3 angularVelocity = m_body->getAngularVelocity();
- ball->set_angular_x(angularVelocity.x());
- ball->set_angular_y(angularVelocity.y());
- ball->set_angular_z(angularVelocity.z());
+ ball.set_angular_x(angularVelocity.x());
+ ball.set_angular_y(angularVelocity.y());
+ ball.set_angular_z(angularVelocity.z());
}
void SimBall::restoreState(const world::SimBall &ball)
@@ -479,8 +444,4 @@ void SimBall::kick(const btVector3 &power)
{
m_body->activate();
m_body->applyCentralForce(power);
- // btTransform transform;
- // m_motionState->getWorldTransform(transform);
- // const btVector3 p = transform.getOrigin() / SIMULATOR_SCALE;
- // qDebug() << "kick at" << p.x() << p.y();
}
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simball.h b/src/extlibs/er_force_sim/src/amun/simulator/simball.h
index a7000400a6..1d9022c837 100644
--- a/src/extlibs/er_force_sim/src/amun/simulator/simball.h
+++ b/src/extlibs/er_force_sim/src/amun/simulator/simball.h
@@ -23,78 +23,83 @@
#include
-#include
-
+#include "extlibs/er_force_sim/src/core/rng.h"
#include "extlibs/er_force_sim/src/protobuf/command.pb.h"
-#include "extlibs/er_force_sim/src/protobuf/sslsim.h"
+#include "extlibs/er_force_sim/src/protobuf/world.pb.h"
+#include "proto/ssl_vision_detection.pb.h"
#include "shared/constants.h"
-#include "simfield.h"
-
-class RNG;
-namespace SSLProto
-{
- class SSL_DetectionBall;
-}
namespace camun
{
namespace simulator
{
class SimBall;
- enum class ErrorSource;
} // namespace simulator
} // namespace camun
-class camun::simulator::SimBall : public QObject
+class camun::simulator::SimBall
{
- Q_OBJECT
public:
- SimBall(RNG *rng, btDiscreteDynamicsWorld *world);
+ SimBall(std::shared_ptr world);
~SimBall();
- SimBall(const SimBall &) = delete;
- SimBall &operator=(const SimBall &) = delete;
-
- signals:
- void sendSSLSimError(const SSLSimError &error, ErrorSource s);
public:
/**
- * processes velocity and forces to be applied on the ball
- * @param robot_collision whether the ball collides with a robot in this simulation
- * tick
+ * Processes velocity and forces to be applied on the ball
+ *
+ * @param robot_collision whether the ball collides with a robot in
+ * this simulation tick
*/
void begin(bool robot_collision);
- bool update(SSLProto::SSL_DetectionBall *ball, float stddev, float stddevArea,
+
+ bool update(SSLProto::SSL_DetectionBall &ball, float stddev, float stddevArea,
const btVector3 &cameraPosition, bool enableInvisibleBall,
float visibilityThreshold, btVector3 positionOffset);
+
void move(const sslsim::TeleportBall &ball);
+
void kick(const btVector3 &power);
- // returns the ball position projected onto the floor (z component is not included)
+
+ /**
+ * Returns the ball's position projected onto the floor (z component is not included)
+ *
+ * @return the ball position
+ */
btVector3 position() const;
+
+ /**
+ * Returns the ball's linear velocity
+ *
+ * @return the ball velocity
+ */
btVector3 speed() const;
- void writeBallState(world::SimBall *ball) const;
+
+ void writeBallState(world::SimBall &ball) const;
+
void restoreState(const world::SimBall &ball);
+
btRigidBody *body() const
{
- return m_body;
+ return m_body.get();
}
+
bool isInvalid() const;
// can be used to add ball mis-detections
- bool addDetection(SSLProto::SSL_DetectionBall *ball, btVector3 pos, float stddev,
+ bool addDetection(SSLProto::SSL_DetectionBall &ball, btVector3 pos, float stddev,
float stddevArea, const btVector3 &cameraPosition,
bool enableInvisibleBall, float visibilityThreshold,
btVector3 positionOffset);
private:
- RNG *m_rng;
- btDiscreteDynamicsWorld *m_world;
- btCollisionShape *m_sphere;
- btRigidBody *m_body;
- btMotionState *m_motionState;
+ RNG m_rng;
+ std::shared_ptr m_world;
+ std::unique_ptr m_sphere;
+ std::unique_ptr m_body;
+ std::unique_ptr m_motionState;
sslsim::TeleportBall m_move;
- double rolling_speed;
- bool set_transition_speed;
+ double m_rolling_speed;
+ bool m_set_transition_speed;
enum BallState
{
@@ -103,7 +108,8 @@ class camun::simulator::SimBall : public QObject
SLIDING,
ROLLING
};
- BallState current_ball_state;
+
+ BallState m_current_ball_state;
};
#endif // SIMBALL_H
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp
index 78b4e9e510..38acbaff55 100644
--- a/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp
+++ b/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp
@@ -24,109 +24,99 @@
using namespace camun::simulator;
-SimField::SimField(btDiscreteDynamicsWorld *world, const world::Geometry &geometry)
+SimField::SimField(std::shared_ptr world,
+ const world::Geometry &geometry)
: m_world(world)
{
const float totalWidth = geometry.field_width() / 2.0f + geometry.boundary_width();
const float totalHeight = geometry.field_height() / 2.0f + geometry.boundary_width();
- // upper boundary
- const float roomHeight = 8.0f;
- const float height = geometry.field_height() / 2.0f - geometry.line_width();
+ const float roomHeight = 8.0f; // Upper boundary of "room" that the field lives in
+ const float height = geometry.field_height() / 2.0f - geometry.line_width();
const float goalWidthHalf = geometry.goal_width() / 2.0f + geometry.goal_wall_width();
const float goalHeightHalf = geometry.goal_height() / 2.0f;
const float goalDepth = geometry.goal_depth() + geometry.goal_wall_width();
const float goalDepthHalf = goalDepth / 2.0f;
const float goalWallHalf = geometry.goal_wall_width() / 2.0f;
- // obstacle prototypes
- m_plane = new btStaticPlaneShape(btVector3(0, 0, 1), 0);
- m_goalSide = new btBoxShape(btVector3(goalWallHalf, goalDepthHalf, goalHeightHalf) *
- SIMULATOR_SCALE);
- m_goalBack = new btBoxShape(btVector3(goalWidthHalf, goalWallHalf, goalHeightHalf) *
- SIMULATOR_SCALE);
+ // Collision shapes
+ m_plane = std::make_unique(btVector3(0, 0, 1), 0);
+ m_goalSide = std::make_unique(
+ btVector3(goalWallHalf, goalDepthHalf, goalHeightHalf) * SIMULATOR_SCALE);
+ m_goalBack = std::make_unique(
+ btVector3(goalWidthHalf, goalWallHalf, goalHeightHalf) * SIMULATOR_SCALE);
- // build field cube
- // floor
- addObject(m_plane,
+ // Create the "room" that the field lives in
+ // Floor
+ addObject(m_plane.get(),
btTransform(btQuaternion(btVector3(1, 0, 0), 0),
btVector3(0, 0, 0) * SIMULATOR_SCALE),
0.56, 0.35);
- // others
- addObject(m_plane,
+ // Roof
+ addObject(m_plane.get(),
btTransform(btQuaternion(btVector3(1, 0, 0), M_PI),
btVector3(0, 0, roomHeight) * SIMULATOR_SCALE),
0.3, 0.35);
-
- addObject(m_plane,
+ // Walls
+ addObject(m_plane.get(),
btTransform(btQuaternion(btVector3(1, 0, 0), M_PI_2),
btVector3(0, totalHeight, 0) * SIMULATOR_SCALE),
0.3, 0.35);
- addObject(m_plane,
+ addObject(m_plane.get(),
btTransform(btQuaternion(btVector3(1, 0, 0), -M_PI_2),
btVector3(0, -totalHeight, 0) * SIMULATOR_SCALE),
0.3, 0.35);
-
- addObject(m_plane,
+ addObject(m_plane.get(),
btTransform(btQuaternion(btVector3(0, 1, 0), M_PI_2),
btVector3(-totalWidth, 0, 0) * SIMULATOR_SCALE),
0.3, 0.35);
- addObject(m_plane,
+ addObject(m_plane.get(),
btTransform(btQuaternion(btVector3(0, 1, 0), -M_PI_2),
btVector3(totalWidth, 0, 0) * SIMULATOR_SCALE),
0.3, 0.35);
- // create goals
- for (int goal = 0; goal < 2; goal++)
+ // Create goals
+ for (const float side : {-1.0f, 1.0f})
{
- const float side = (goal == 0) ? -1.0f : 1.0f;
- const btQuaternion rot = btQuaternion::getIdentity();
-
- addObject(
- m_goalSide,
- btTransform(rot, btVector3((goalWidthHalf - goalWallHalf),
- side * (height + goalDepthHalf), goalHeightHalf) *
- SIMULATOR_SCALE),
- 0.3, 0.5);
- addObject(
- m_goalSide,
- btTransform(rot, btVector3(-(goalWidthHalf - goalWallHalf),
- side * (height + goalDepthHalf), goalHeightHalf) *
- SIMULATOR_SCALE),
- 0.3, 0.5);
- addObject(
- m_goalBack,
- btTransform(rot, btVector3(0.0f, side * (height + goalDepth - goalWallHalf),
- goalHeightHalf) *
- SIMULATOR_SCALE),
- 0.1, 0.5);
+ addObject(m_goalSide.get(),
+ btTransform(btQuaternion::getIdentity(),
+ btVector3((goalWidthHalf - goalWallHalf),
+ side * (height + goalDepthHalf), goalHeightHalf) *
+ SIMULATOR_SCALE),
+ 0.3, 0.5);
+ addObject(m_goalSide.get(),
+ btTransform(btQuaternion::getIdentity(),
+ btVector3(-(goalWidthHalf - goalWallHalf),
+ side * (height + goalDepthHalf), goalHeightHalf) *
+ SIMULATOR_SCALE),
+ 0.3, 0.5);
+ addObject(m_goalBack.get(),
+ btTransform(btQuaternion::getIdentity(),
+ btVector3(0.0f, side * (height + goalDepth - goalWallHalf),
+ goalHeightHalf) *
+ SIMULATOR_SCALE),
+ 0.1, 0.5);
}
}
SimField::~SimField()
{
- foreach (btCollisionObject *object, m_objects)
+ for (const auto &object : m_objects)
{
- m_world->removeCollisionObject(object);
- delete object;
+ m_world->removeCollisionObject(object.get());
}
-
- delete m_goalSide;
- delete m_goalBack;
- delete m_plane;
}
void SimField::addObject(btCollisionShape *shape, const btTransform &transform,
float restitution, float friction)
{
- // create new obstacle
- btCollisionObject *object = new btCollisionObject;
+ std::unique_ptr object = std::make_unique();
+
object->setCollisionShape(shape);
- // damp ball a bit if it hits an obstacle
+ object->setWorldTransform(transform);
object->setRestitution(restitution);
- // the friction is multiplied with the colliding obstacle ones
object->setFriction(friction);
object->setRollingFriction(friction);
- object->setWorldTransform(transform);
- m_world->addCollisionObject(object);
- m_objects.append(object);
+
+ m_world->addCollisionObject(object.get());
+ m_objects.push_back(std::move(object));
}
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simfield.h b/src/extlibs/er_force_sim/src/amun/simulator/simfield.h
index 8582be8a21..b7743d2f43 100644
--- a/src/extlibs/er_force_sim/src/amun/simulator/simfield.h
+++ b/src/extlibs/er_force_sim/src/amun/simulator/simfield.h
@@ -23,8 +23,6 @@
#include
-#include
-
#include "extlibs/er_force_sim/src/protobuf/world.pb.h"
namespace camun
@@ -32,27 +30,26 @@ namespace camun
namespace simulator
{
class SimField;
- }
+ } // namespace simulator
} // namespace camun
class camun::simulator::SimField
{
public:
- SimField(btDiscreteDynamicsWorld *world, const world::Geometry &geometry);
+ SimField(std::shared_ptr world,
+ const world::Geometry &geometry);
~SimField();
- SimField(const SimField &) = delete;
- SimField &operator=(const SimField &) = delete;
private:
void addObject(btCollisionShape *shape, const btTransform &transform,
float restitution, float friction);
private:
- btDiscreteDynamicsWorld *m_world;
- btCollisionShape *m_plane;
- btCollisionShape *m_goalSide;
- btCollisionShape *m_goalBack;
- QList m_objects;
+ std::shared_ptr m_world;
+ std::unique_ptr m_plane;
+ std::unique_ptr m_goalSide;
+ std::unique_ptr m_goalBack;
+ std::vector> m_objects;
};
#endif // SIMFIELD_H
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp
index ddd3767992..571dfd38e9 100644
--- a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp
+++ b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp
@@ -20,57 +20,51 @@
#include "simrobot.h"
+#include
#include
#include "extlibs/er_force_sim/src/core/coordinates.h"
-#include "extlibs/er_force_sim/src/core/rng.h"
-#include "mesh.h"
-#include "proto/ssl_vision_detection.pb.h"
-#include "simball.h"
+#include "robotmesh.h"
#include "simulator.h"
using namespace camun::simulator;
const float MAX_SPEED = 1000;
-float boundSpeed(float speed)
-{
- return qBound(-MAX_SPEED, speed, MAX_SPEED);
-}
-
-SimRobot::SimRobot(RNG *rng, const robot::Specs &specs, btDiscreteDynamicsWorld *world,
- const btVector3 &pos, float dir)
- : m_rng(rng),
- m_specs(specs),
+SimRobot::SimRobot(const robot::Specs &specs,
+ std::shared_ptr world, const btVector3 &pos,
+ float dir)
+ : m_specs(specs),
m_world(world),
m_charge(false),
m_isCharged(false),
m_inStandby(false),
m_shootTime(0.0),
m_commandTime(0.0),
- error_sum_v_s(0),
- error_sum_v_f(0),
- error_sum_omega(0)
+ m_error_sum_v_s(0),
+ m_error_sum_v_f(0),
+ m_error_sum_omega(0)
{
- btCompoundShape *wholeShape = new btCompoundShape;
+ std::unique_ptr wholeShape = std::make_unique();
btTransform robotShapeTransform;
robotShapeTransform.setIdentity();
// subtract collision margin from dimensions
- Mesh mesh(m_specs.radius() - COLLISION_MARGIN / SIMULATOR_SCALE,
- m_specs.height() - 2 * COLLISION_MARGIN / SIMULATOR_SCALE, m_specs.angle(),
- 0.04f, m_specs.dribbler_height() + 0.02f);
- for (const QList &hullPart : mesh.hull())
+ auto mesh =
+ createRobotMesh(m_specs.radius() - COLLISION_MARGIN / SIMULATOR_SCALE,
+ m_specs.height() - 2 * COLLISION_MARGIN / SIMULATOR_SCALE,
+ m_specs.angle(), 0.04f, m_specs.dribbler_height() + 0.02f);
+ for (const auto &hullPart : mesh)
{
- btConvexHullShape *hullPartShape = new btConvexHullShape;
- m_shapes.append(hullPartShape);
- for (const QVector3D &v : hullPart)
+ std::unique_ptr hullPartShape =
+ std::make_unique();
+ for (const auto &[x, y, z] : hullPart)
{
- hullPartShape->addPoint(btVector3(v.x(), v.y(), v.z()) * SIMULATOR_SCALE);
+ hullPartShape->addPoint(btVector3(x, y, z) * SIMULATOR_SCALE);
}
- wholeShape->addChildShape(robotShapeTransform, hullPartShape);
+ wholeShape->addChildShape(robotShapeTransform, hullPartShape.get());
+ m_shapes.push_back(std::move(hullPartShape));
}
- m_shapes.append(wholeShape);
btTransform startWorldTransform;
startWorldTransform.setIdentity();
@@ -78,24 +72,25 @@ SimRobot::SimRobot(RNG *rng, const robot::Specs &specs, btDiscreteDynamicsWorld
SIMULATOR_SCALE);
startWorldTransform.setOrigin(robotBasePos);
startWorldTransform.setRotation(btQuaternion(btVector3(0, 0, 1), dir - M_PI_2));
- m_motionState = new btDefaultMotionState(startWorldTransform);
+ m_motionState = std::make_unique(startWorldTransform);
// set robot dynamics and move to start position
btVector3 localInertia(0, 0, 0);
const float robotMassProportion = 49.0f / 50.0f;
wholeShape->calculateLocalInertia(robotMassProportion * m_specs.mass(), localInertia);
- btRigidBody::btRigidBodyConstructionInfo rbInfo(
- robotMassProportion * m_specs.mass(), m_motionState, wholeShape, localInertia);
+ btRigidBody::btRigidBodyConstructionInfo rbInfo(robotMassProportion * m_specs.mass(),
+ m_motionState.get(), wholeShape.get(),
+ localInertia);
- m_body = new btRigidBody(rbInfo);
+ m_body = std::make_unique(rbInfo);
// see simulator.cpp
m_body->setRestitution(0.6f);
m_body->setFriction(0.22f);
- m_world->addRigidBody(m_body);
+ m_world->addRigidBody(m_body.get());
- btCylinderShape *dribblerShape = new btCylinderShapeX(
+ std::unique_ptr dribblerShape = std::make_unique(
btVector3(m_specs.dribbler_width() / 2.0f, 0.007f, 0.007f) * SIMULATOR_SCALE);
- m_shapes.append(dribblerShape);
+
// WARNING: hack, instead of 0.02 should be the dribbler height
// the ball seems to get instable if the dribbler is at correct height
// possibly the ball gets 'sucked' onto the robot
@@ -111,15 +106,14 @@ SimRobot::SimRobot(RNG *rng, const robot::Specs &specs, btDiscreteDynamicsWorld
dribblerShape->calculateLocalInertia((1 - robotMassProportion) * m_specs.mass(),
dribblerInertia);
btRigidBody::btRigidBodyConstructionInfo rbDribInfo(
- (1 - robotMassProportion) * m_specs.mass(), nullptr, dribblerShape,
+ (1 - robotMassProportion) * m_specs.mass(), nullptr, dribblerShape.get(),
dribblerInertia);
rbDribInfo.m_startWorldTransform = dribblerStartTransform;
- btRigidBody *dribblerBody = new btRigidBody(rbDribInfo);
- dribblerBody->setRestitution(0.2f);
- dribblerBody->setFriction(1.5f);
- m_dribblerBody = dribblerBody;
- m_world->addRigidBody(dribblerBody);
+ m_dribblerBody = std::make_unique(rbDribInfo);
+ m_dribblerBody->setRestitution(0.2f);
+ m_dribblerBody->setFriction(1.5f);
+ m_world->addRigidBody(m_dribblerBody.get());
btTransform localA, localB;
localA.setIdentity();
@@ -127,9 +121,13 @@ SimRobot::SimRobot(RNG *rng, const robot::Specs &specs, btDiscreteDynamicsWorld
localA.setOrigin(m_dribblerCenter);
localA.setRotation(btQuaternion(btVector3(0, 1, 0), M_PI_2));
localB.setRotation(btQuaternion(btVector3(0, 1, 0), M_PI_2));
- m_dribblerConstraint = new btHingeConstraint(*m_body, *dribblerBody, localA, localB);
+ m_dribblerConstraint =
+ std::make_unique(*m_body, *m_dribblerBody, localA, localB);
m_dribblerConstraint->enableAngularMotor(false, 0, 0);
- m_world->addConstraint(m_dribblerConstraint, true);
+ m_world->addConstraint(m_dribblerConstraint.get(), true);
+
+ m_shapes.push_back(std::move(wholeShape));
+ m_shapes.push_back(std::move(dribblerShape));
}
SimRobot::~SimRobot()
@@ -138,14 +136,9 @@ SimRobot::~SimRobot()
{
m_world->removeConstraint(m_holdBallConstraint.get());
}
- m_world->removeConstraint(m_dribblerConstraint);
- m_world->removeRigidBody(m_dribblerBody);
- m_world->removeRigidBody(m_body);
- delete m_dribblerConstraint;
- delete m_body;
- delete m_dribblerBody;
- delete m_motionState;
- qDeleteAll(m_shapes);
+ m_world->removeConstraint(m_dribblerConstraint.get());
+ m_world->removeRigidBody(m_dribblerBody.get());
+ m_world->removeRigidBody(m_body.get());
}
void SimRobot::calculateDribblerMove(const btVector3 pos, const btQuaternion rot,
@@ -165,7 +158,7 @@ void SimRobot::calculateDribblerMove(const btVector3 pos, const btQuaternion rot
m_dribblerBody->setAngularVelocity(btVector3(0, 0, 0));
}
-void SimRobot::dribble(SimBall *ball, float speed)
+void SimRobot::dribble(const SimBall &ball, float speed)
{
if (m_perfectDribbler)
{
@@ -176,12 +169,12 @@ void SimRobot::dribble(SimBall *ball, float speed)
localB.setIdentity();
auto worldToRobot = m_body->getWorldTransform().inverse();
- localA.setOrigin(worldToRobot * ball->position());
+ localA.setOrigin(worldToRobot * ball.position());
localA.setRotation(btQuaternion(worldToRobot * btVector3(0, 1, 0), M_PI_2));
localB.setRotation(btQuaternion(worldToRobot * btVector3(0, 1, 0), M_PI_2));
- m_holdBallConstraint.reset(
- new btHingeConstraint(*m_body, *ball->body(), localA, localB));
+ m_holdBallConstraint = std::make_unique(
+ *m_body, *ball.body(), localA, localB);
m_world->addConstraint(m_holdBallConstraint.get(), true);
}
}
@@ -191,7 +184,7 @@ void SimRobot::dribble(SimBall *ball, float speed)
const float max_rotation_speed = 150.f / 2 / M_PI * 60;
float dribbler = speed / max_rotation_speed;
// rad/s is limited to 150
- float boundedDribbler = qBound(0.0f, dribbler, 1.0f);
+ float boundedDribbler = std::clamp(dribbler, 0.0f, 1.0f);
m_dribblerConstraint->enableAngularMotor(true, 150 * boundedDribbler,
20 * boundedDribbler);
}
@@ -217,7 +210,7 @@ void SimRobot::setDribbleMode(bool perfectDribbler)
m_perfectDribbler = perfectDribbler;
}
-void SimRobot::begin(SimBall *ball, double time)
+void SimRobot::begin(SimBall &ball, double time)
{
m_commandTime += time;
m_inStandby = false;
@@ -243,20 +236,14 @@ void SimRobot::begin(SimBall *ball, double time)
}
auto sendPartialCoordError = [this](const std::string &msg) {
- SSLSimError error{new sslsim::SimulatorError};
- error->set_code("PARTIAL_COORD");
- std::string message = "Partial coordinates are not implemented yet";
- error->set_message(message + msg);
- emit this->sendSSLSimError(error, ErrorSource::CONFIG);
+ std::cerr << "Partial coordinates are not implemented yet" << msg << std::endl;
if (!m_move.has_by_force() || !m_move.by_force())
{
m_move.Clear();
}
};
bool moveCommand = false;
- std::string message = " for robot (";
- message += std::to_string(m_specs.id());
- message += ')';
+ std::string message = " for robot (" + std::to_string(m_specs.id()) + ")";
if (m_move.has_x())
{
@@ -309,10 +296,7 @@ void SimRobot::begin(SimBall *ball, double time)
}
if (sendError)
{
- SSLSimError error{new sslsim::SimulatorError};
- error->set_code("VELOCITY_FORCE");
- error->set_message("Velocities != 0 and by_force are incompatible");
- emit sendSSLSimError(error, ErrorSource::CONFIG);
+ std::cerr << "Velocities != 0 and by_force are incompatible" << std::endl;
return;
}
} // TODO: check for force and orientation
@@ -425,23 +409,23 @@ void SimRobot::begin(SimBall *ball, double time)
// we subtract the current speed of the ball from the intended kick speed
// this ensures the ball leaves the robot at exactly the speed we want
float kickSpeed = 0.0f;
- if (ball->speed().length() < kickSpeedBoundThreshold)
+ if (ball.speed().length() < kickSpeedBoundThreshold)
{
- kickSpeed = m_sslCommand.kick_speed() -
- (ball->speed().length() / SIMULATOR_SCALE);
+ kickSpeed =
+ m_sslCommand.kick_speed() - (ball.speed().length() / SIMULATOR_SCALE);
}
else
{
kickSpeed = m_sslCommand.kick_speed();
}
- power = qBound(0.05f, kickSpeed, m_specs.shot_linear_max());
+ power = std::clamp(kickSpeed, 0.05f, m_specs.shot_linear_max());
}
else
{
// FIXME: for now we just recalc the max distance based on the given angle
const float maxShootSpeed =
coordinates::chipVelFromChipDistance(m_specs.shot_chip_max());
- power = qBound(0.05f, m_sslCommand.kick_speed(), maxShootSpeed);
+ power = std::clamp(m_sslCommand.kick_speed(), 0.05f, maxShootSpeed);
}
const auto getSpeedCompensation = [&]() -> float {
@@ -453,15 +437,16 @@ void SimRobot::begin(SimBall *ball, double time)
{
// if the ball hits the robot the chip distance actually decreases
const btVector3 relBallSpeed = relativeBallSpeed(ball) / SIMULATOR_SCALE;
- return std::max((btScalar)0, relBallSpeed.y()) -
- qBound((btScalar)0, (btScalar)0.5 * relBallSpeed.y(),
- (btScalar)0.5 * dirFloor);
+ return std::max(static_cast(0), relBallSpeed.y()) -
+ std::clamp(static_cast(0.5) * relBallSpeed.y(),
+ static_cast(0),
+ static_cast(0.5) * dirFloor);
}
};
const float speedCompensation = getSpeedCompensation();
- ball->kick(t * btVector3(0, dirFloor * power + speedCompensation, dirUp * power) *
- (1.0f / static_cast(time)) * SIMULATOR_SCALE *
- static_cast(BALL_MASS_KG));
+ ball.kick(t * btVector3(0, dirFloor * power + speedCompensation, dirUp * power) *
+ (1.0f / static_cast(time)) * SIMULATOR_SCALE *
+ static_cast(BALL_MASS_KG));
// discharge
m_isCharged = false;
m_shootTime = 0.0;
@@ -478,7 +463,8 @@ void SimRobot::begin(SimBall *ball, double time)
float output_omega = m_sslCommand.move_command().local_velocity().angular();
btVector3 v_local(t.inverse() * m_body->getLinearVelocity());
- btVector3 v_d_local(boundSpeed(output_v_s), boundSpeed(output_v_f), 0);
+ btVector3 v_d_local(std::clamp(output_v_s, -MAX_SPEED, MAX_SPEED),
+ std::clamp(output_v_f, -MAX_SPEED, MAX_SPEED), 0);
float v_f = v_local.y() / SIMULATOR_SCALE;
float v_s = v_local.x() / SIMULATOR_SCALE;
@@ -486,15 +472,15 @@ void SimRobot::begin(SimBall *ball, double time)
const float error_v_s = v_d_local.x() - v_s;
const float error_v_f = v_d_local.y() - v_f;
- const float error_omega = boundSpeed(output_omega) - omega;
+ const float error_omega = std::clamp(output_omega, -MAX_SPEED, MAX_SPEED) - omega;
- error_sum_v_s += error_v_s;
- error_sum_v_f += error_v_f;
- error_sum_omega += error_omega;
+ m_error_sum_v_s += error_v_s;
+ m_error_sum_v_f += error_v_f;
+ m_error_sum_omega += error_omega;
- const float error_sum_limit = 20.0f;
- error_sum_v_s = qBound(-error_sum_limit, error_sum_v_s, error_sum_limit);
- error_sum_v_f = qBound(-error_sum_limit, error_sum_v_f, error_sum_limit);
+ const float m_error_sum_limit = 20.0f;
+ m_error_sum_v_s = std::clamp(m_error_sum_v_s, -m_error_sum_limit, m_error_sum_limit);
+ m_error_sum_v_f = std::clamp(m_error_sum_v_f, -m_error_sum_limit, m_error_sum_limit);
// (1-(1-linear_damping)^timestep)/timestep - compensates damping
const float V = 1.200f; // keep current speed
@@ -504,8 +490,8 @@ void SimRobot::begin(SimBall *ball, double time)
// as a certain part of the acceleration is required to compensate damping,
// the robot will run into a speed limit! bound acceleration the speed limit
// is acceleration * accelScale / V
- float a_f = V * v_f + K * error_v_f + K_I * error_sum_v_f;
- float a_s = V * v_s + K * error_v_s + K_I * error_sum_v_s;
+ float a_f = V * v_f + K * error_v_f + K_I * m_error_sum_v_f;
+ float a_s = V * v_s + K * error_v_s + K_I * m_error_sum_v_s;
const float accelScale =
2.f; // let robot accelerate / brake faster than the accelerator does
@@ -524,7 +510,7 @@ void SimRobot::begin(SimBall *ball, double time)
1.f / time * 0.5f; // correct half the error during each subtimestep
const float K_I_phi = /*0*0.2/1000; //*/ 0.f;
- const float a_phi = V_phi * omega + K_phi * error_omega + K_I_phi * error_sum_omega;
+ const float a_phi = V_phi * omega + K_phi * error_omega + K_I_phi * m_error_sum_omega;
const float a_phi_bound =
bound(a_phi, omega, accelScale * m_specs.strategy().a_speedup_phi_max(),
accelScale * m_specs.strategy().a_brake_phi_max());
@@ -546,19 +532,19 @@ float SimRobot::bound(float acceleration, float oldSpeed, float speedupLimit,
if ((std::signbit(acceleration) == std::signbit(oldSpeed)) || (oldSpeed == 0))
{
// the acceleration needs to be bounded with values for speeding up.
- return qBound(-speedupLimit, acceleration, speedupLimit);
+ return std::clamp(acceleration, -speedupLimit, speedupLimit);
}
else
{
// bound braking acceleration, in order to avoid fallover
- return qBound(-brakeLimit, acceleration, brakeLimit);
+ return std::clamp(acceleration, -brakeLimit, brakeLimit);
}
}
-btVector3 SimRobot::relativeBallSpeed(SimBall *ball) const
+btVector3 SimRobot::relativeBallSpeed(const SimBall &ball) const
{
btTransform t = m_body->getWorldTransform();
- const btVector3 ballSpeed = ball->speed();
+ const btVector3 ballSpeed = ball.speed();
const btQuaternion robotDir = t.getRotation();
const btVector3 diff = (ballSpeed).rotate(robotDir.getAxis(), -robotDir.getAngle());
@@ -566,9 +552,9 @@ btVector3 SimRobot::relativeBallSpeed(SimBall *ball) const
return diff;
}
-bool SimRobot::canKickBall(SimBall *ball) const
+bool SimRobot::canKickBall(const SimBall &ball) const
{
- const btVector3 ballPos = ball->position();
+ const btVector3 ballPos = ball.position();
// can't kick jumping ball
if (ballPos.z() > 0.05f * SIMULATOR_SCALE)
{
@@ -588,8 +574,8 @@ bool SimRobot::canKickBall(SimBall *ball) const
m_world->getDispatcher()->getManifoldByIndexInternal(i);
btCollisionObject *objectA = (btCollisionObject *)(contactManifold->getBody0());
btCollisionObject *objectB = (btCollisionObject *)(contactManifold->getBody1());
- if ((objectA == m_dribblerBody && objectB == ball->body()) ||
- (objectA == ball->body() && objectB == m_dribblerBody))
+ if ((objectA == m_dribblerBody.get() && objectB == ball.body()) ||
+ (objectA == ball.body() && objectB == m_dribblerBody.get()))
{
int numContacts = contactManifold->getNumContacts();
for (int j = 0; j < numContacts; ++j)
@@ -606,7 +592,7 @@ bool SimRobot::canKickBall(SimBall *ball) const
}
robot::RadioResponse SimRobot::setCommand(const SSLSimulationProto::RobotCommand &command,
- SimBall *ball, bool charge, float rxLoss,
+ const SimBall &ball, bool charge, float rxLoss,
float txLoss)
{
m_sslCommand = command;
@@ -639,31 +625,31 @@ robot::RadioResponse SimRobot::setCommand(const SSLSimulationProto::RobotCommand
return response;
}
-void SimRobot::update(SSLProto::SSL_DetectionRobot *robot, float stddev_p,
- float stddev_phi, qint64 time, btVector3 positionOffset)
+void SimRobot::update(SSLProto::SSL_DetectionRobot &robot, float stddev_p,
+ float stddev_phi, int64_t time, btVector3 positionOffset)
{
// setup vision packet
- robot->set_robot_id(m_specs.id());
- robot->set_confidence(1.0);
- robot->set_pixel_x(0);
- robot->set_pixel_y(0);
+ robot.set_robot_id(m_specs.id());
+ robot.set_confidence(1.0);
+ robot.set_pixel_x(0);
+ robot.set_pixel_y(0);
// add noise
btTransform transform;
m_motionState->getWorldTransform(transform);
const btVector3 p = transform.getOrigin() / SIMULATOR_SCALE + positionOffset;
- const ErForceVector p_noise = m_rng->normalVector(stddev_p);
- robot->set_x((p.y() + p_noise.x) * 1000.0f);
- robot->set_y(-(p.x() + p_noise.y) * 1000.0f);
+ const ErForceVector p_noise = m_rng.normalVector(stddev_p);
+ robot.set_x((p.y() + p_noise.x) * 1000.0f);
+ robot.set_y(-(p.x() + p_noise.y) * 1000.0f);
const btQuaternion q = transform.getRotation();
const btVector3 dir = btMatrix3x3(q).getColumn(0);
- robot->set_orientation(atan2(dir.y(), dir.x()) + m_rng->normal(stddev_phi));
+ robot.set_orientation(atan2(dir.y(), dir.x()) + m_rng.normal(stddev_phi));
m_lastSendTime = time;
}
-bool SimRobot::touchesBall(SimBall *ball) const
+bool SimRobot::touchesBall(const SimBall &ball) const
{
// for some reason btHingeConstraints, which is used when dribbling, are not always
// detected as contact by bullet. so if the ball is being dribbled then we assume it
@@ -684,10 +670,10 @@ bool SimRobot::touchesBall(SimBall *ball) const
// determine if the two objects are the ball and robot body/dribbler
btCollisionObject *objectA = (btCollisionObject *)(contact_manifold->getBody0());
btCollisionObject *objectB = (btCollisionObject *)(contact_manifold->getBody1());
- if ((objectA == m_dribblerBody && objectB == ball->body()) ||
- (objectA == ball->body() && objectB == m_dribblerBody) ||
- (objectA == m_body && objectB == ball->body()) ||
- (objectA == ball->body() && objectB == m_body))
+ if ((objectA == m_dribblerBody.get() && objectB == ball.body()) ||
+ (objectA == ball.body() && objectB == m_dribblerBody.get()) ||
+ (objectA == m_body.get() && objectB == ball.body()) ||
+ (objectA == ball.body() && objectB == m_body.get()))
{
// check if the points are in contact now
int num_contacts = contact_manifold->getNumContacts();
@@ -705,18 +691,18 @@ bool SimRobot::touchesBall(SimBall *ball) const
return false;
}
-void SimRobot::update(world::SimRobot *robot, SimBall *ball) const
+void SimRobot::update(world::SimRobot &robot, const SimBall &ball) const
{
btTransform transform;
m_motionState->getWorldTransform(transform);
const btVector3 position = transform.getOrigin() / SIMULATOR_SCALE;
- robot->set_p_x(position.x());
- robot->set_p_y(position.y());
- robot->set_p_z(position.z());
- robot->set_id(m_specs.id());
+ robot.set_p_x(position.x());
+ robot.set_p_y(position.y());
+ robot.set_p_z(position.z());
+ robot.set_id(m_specs.id());
const btQuaternion q = transform.getRotation();
- auto *rotation = robot->mutable_rotation();
+ auto *rotation = robot.mutable_rotation();
rotation->set_real(q.getX());
rotation->set_i(q.getY());
rotation->set_j(q.getZ());
@@ -727,17 +713,17 @@ void SimRobot::update(world::SimRobot *robot, SimBall *ball) const
float y = 0;
float z = 0;
q.getEulerZYX(z, y, x);
- robot->set_angle(z);
+ robot.set_angle(z);
const btVector3 velocity = m_body->getLinearVelocity() / SIMULATOR_SCALE;
- robot->set_v_x(velocity.x());
- robot->set_v_y(velocity.y());
- robot->set_v_z(velocity.z());
+ robot.set_v_x(velocity.x());
+ robot.set_v_y(velocity.y());
+ robot.set_v_z(velocity.z());
const btVector3 angular = m_body->getAngularVelocity();
- robot->set_r_x(angular.x());
- robot->set_r_y(angular.y());
- robot->set_r_z(angular.z());
+ robot.set_r_x(angular.x());
+ robot.set_r_y(angular.y());
+ robot.set_r_z(angular.z());
bool ballTouchesRobot = false;
int numManifolds = m_world->getDispatcher()->getNumManifolds();
@@ -747,15 +733,15 @@ void SimRobot::update(world::SimRobot *robot, SimBall *ball) const
m_world->getDispatcher()->getManifoldByIndexInternal(i);
btCollisionObject *objectA = (btCollisionObject *)(contactManifold->getBody0());
btCollisionObject *objectB = (btCollisionObject *)(contactManifold->getBody1());
- if ((objectA == m_dribblerBody && objectB == ball->body()) ||
- (objectA == ball->body() && objectB == m_dribblerBody) ||
- (objectA == m_body && objectB == ball->body()) ||
- (objectA == ball->body() && objectB == m_body))
+ if ((objectA == m_dribblerBody.get() && objectB == ball.body()) ||
+ (objectA == ball.body() && objectB == m_dribblerBody.get()) ||
+ (objectA == m_body.get() && objectB == ball.body()) ||
+ (objectA == ball.body() && objectB == m_body.get()))
{
ballTouchesRobot = true;
}
}
- robot->set_touches_ball(ballTouchesRobot);
+ robot.set_touches_ball(ballTouchesRobot);
}
void SimRobot::restoreState(const world::SimRobot &robot)
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h
index 89c60002cb..c156f80ee5 100644
--- a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h
+++ b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h
@@ -23,62 +23,62 @@
#include
-#include
-
+#include "extlibs/er_force_sim/src/core/rng.h"
#include "extlibs/er_force_sim/src/protobuf/command.pb.h"
#include "extlibs/er_force_sim/src/protobuf/robot.pb.h"
-#include "extlibs/er_force_sim/src/protobuf/sslsim.h"
+#include "extlibs/er_force_sim/src/protobuf/world.pb.h"
#include "proto/ssl_simulation_robot_control.pb.h"
-
-class RNG;
-namespace SSLProto
-{
- class SSL_DetectionRobot;
-}
+#include "proto/ssl_vision_detection.pb.h"
+#include "simball.h"
namespace camun
{
namespace simulator
{
- class SimBall;
class SimRobot;
- enum class ErrorSource;
} // namespace simulator
} // namespace camun
-class camun::simulator::SimRobot : public QObject
+class camun::simulator::SimRobot
{
- Q_OBJECT
public:
- SimRobot(RNG *rng, const robot::Specs &specs, btDiscreteDynamicsWorld *world,
+ SimRobot(const robot::Specs &specs, std::shared_ptr world,
const btVector3 &pos, float dir);
~SimRobot();
- SimRobot(const SimRobot &) = delete;
- SimRobot &operator=(const SimRobot &) = delete;
-
- signals:
- void sendSSLSimError(const SSLSimError &error, ErrorSource s);
public:
- void begin(SimBall *ball, double time);
- bool canKickBall(SimBall *ball) const;
- void tryKick(SimBall *ball, float power, double time);
+ void begin(SimBall &ball, double time);
+
+ bool canKickBall(const SimBall &ball) const;
+
+ void tryKick(const SimBall &ball, float power, double time);
+
robot::RadioResponse setCommand(const SSLSimulationProto::RobotCommand &command,
- SimBall *ball, bool charge, float rxLoss,
+ const SimBall &ball, bool charge, float rxLoss,
float txLoss);
- void update(SSLProto::SSL_DetectionRobot *robot, float stddev_p, float stddev_phi,
- qint64 time, btVector3 positionOffset);
- void update(world::SimRobot *robot, SimBall *ball) const;
+
+ void update(SSLProto::SSL_DetectionRobot &robot, float stddev_p, float stddev_phi,
+ int64_t time, btVector3 positionOffset);
+
+ void update(world::SimRobot &robot, const SimBall &ball) const;
+
void restoreState(const world::SimRobot &robot);
+
void move(const sslsim::TeleportRobot &robot);
+
bool isFlipped();
+
btVector3 position() const;
+
btVector3 dribblerCorner(bool left) const;
- qint64 getLastSendTime() const
+
+ int64_t getLastSendTime() const
{
return m_lastSendTime;
}
+
void setDribbleMode(bool perfectDribbler);
+
void stopDribbling();
const robot::Specs &specs() const
@@ -92,34 +92,27 @@ class camun::simulator::SimRobot : public QObject
* @param ball the ball in play
* @return true if the ball is in contact with the robot, false otherwise
*/
- bool touchesBall(SimBall *ball) const;
+ bool touchesBall(const SimBall &ball) const;
private:
- btVector3 relativeBallSpeed(SimBall *ball) const;
+ btVector3 relativeBallSpeed(const SimBall &ball) const;
float bound(float acceleration, float oldSpeed, float speedupLimit,
float brakeLimit) const;
void calculateDribblerMove(const btVector3 pos, const btQuaternion rot,
const btVector3 linVel, float omega);
- void dribble(SimBall *ball, float speed);
+ void dribble(const SimBall &ball, float speed);
- RNG *m_rng;
+ RNG m_rng;
robot::Specs m_specs;
- btDiscreteDynamicsWorld *m_world;
- btRigidBody *m_body;
- btRigidBody *m_dribblerBody;
- btHingeConstraint *m_dribblerConstraint;
- QList m_shapes;
- btMotionState *m_motionState;
- btVector3 m_dribblerCenter;
+ std::shared_ptr m_world;
+ std::unique_ptr m_body;
+ std::unique_ptr m_dribblerBody;
+ std::unique_ptr m_dribblerConstraint;
+ std::vector> m_shapes;
+ std::unique_ptr m_motionState;
std::unique_ptr m_holdBallConstraint;
-
- struct Wheel
- {
- float angle;
- btVector3 pos;
- btVector3 dir;
- };
+ btVector3 m_dribblerCenter;
sslsim::TeleportRobot m_move;
SSLSimulationProto::RobotCommand m_sslCommand;
@@ -129,13 +122,13 @@ class camun::simulator::SimRobot : public QObject
double m_shootTime;
double m_commandTime;
- float error_sum_v_s;
- float error_sum_v_f;
- float error_sum_omega;
+ float m_error_sum_v_s;
+ float m_error_sum_v_f;
+ float m_error_sum_omega;
bool m_perfectDribbler = false;
- qint64 m_lastSendTime = 0;
+ int64_t m_lastSendTime = 0;
};
#endif // SIMROBOT_H
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp
index 2058c8a3ae..a37333174e 100644
--- a/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp
+++ b/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp
@@ -20,135 +20,52 @@
#include "simulator.h"
-#include
-#include
-#include
-#include
#include
#include
-#include "erroraggregator.h"
#include "extlibs/er_force_sim/src/core/coordinates.h"
-#include "extlibs/er_force_sim/src/core/rng.h"
#include "extlibs/er_force_sim/src/protobuf/geometry.h"
-#include "simball.h"
-#include "simfield.h"
-#include "simrobot.h"
-
using namespace camun::simulator;
-/* Friction and restitution between robots, ball and field: (empirical
- * measurments) Ball vs. Robot: Restitution: about 0.60 Friction: trial and
- * error in simulator 0.18 (similar results as in reality)
- *
- * Ball vs. Floor:
- * Restitution: sqrt(h'/h) = sqrt(0.314) = 0.56
- * Friction: \mu_k = -a / g (while slipping) = 0.35
- *
- * Robot vs. Floor:
- * Restitution and Friction should be as low as possible
- *
- * Calculations:
- * Variables: r: restitution, f: friction
- * Indices: b: ball; f: floor; r: robot
- *
- * r_b * r_f = 0.56
- * r_b * r_r = 0.60
- * r_f * r_r = small
- * => r_b = 1; r_f = 0.56; r_r = 0.60
- *
- * f_b * f_f = 0.35
- * f_b * f_r = 0.22
- * f_f * f_r = very small
- * => f_b = 1; f_f = 0.35; f_r = 0.22
- */
-
-struct camun::simulator::SimulatorData
-{
- RNG rng;
- btDefaultCollisionConfiguration *collision;
- btCollisionDispatcher *dispatcher;
- btBroadphaseInterface *overlappingPairCache;
- btSequentialImpulseConstraintSolver *solver;
- btDiscreteDynamicsWorld *dynamicsWorld;
- world::Geometry geometry;
- QVector reportedCameraSetup;
- QVector cameraPositions;
- SimField *field;
- SimBall *ball;
- Simulator::RobotMap robotsBlue;
- Simulator::RobotMap robotsYellow;
- QMap specsBlue;
- QMap specsYellow;
- bool flip;
- float stddevBall;
- float stddevBallArea;
- float stddevRobot;
- float stddevRobotPhi;
- float ballDetectionsAtDribbler; // per robot per second
- bool enableInvisibleBall;
- float ballVisibilityThreshold;
- float cameraOverlap;
- float cameraPositionError;
- float objectPositionOffset;
- float robotCommandPacketLoss;
- float robotReplyPacketLoss;
- float missingBallDetections;
- bool dribblePerfect;
-};
-
-static void simulatorTickCallback(btDynamicsWorld *world, btScalar timeStep)
-{
- Simulator *sim = reinterpret_cast(world->getWorldUserInfo());
- sim->handleSimulatorTick(timeStep);
-}
-
-/*!
- * \class Simulator
- * \ingroup simulator
- * \brief %Simulator interface
- */
-
Simulator::Simulator(const amun::SimulatorSetup &setup)
- : m_time(0),
- m_lastSentStatusTime(0),
+ : m_data(std::make_unique()),
m_enabled(false),
m_charge(true),
+ m_time(0),
m_visionDelay(35 * 1000 * 1000),
- m_visionProcessingTime(5 * 1000 * 1000),
- m_aggregator(new ErrorAggregator(this))
+ m_visionProcessingTime(5 * 1000 * 1000)
{
- // setup bullet
- m_data = new SimulatorData;
- m_data->collision = new btDefaultCollisionConfiguration();
- m_data->dispatcher = new btCollisionDispatcher(m_data->collision);
- m_data->overlappingPairCache = new btDbvtBroadphase();
- m_data->solver = new btSequentialImpulseConstraintSolver;
- m_data->dynamicsWorld =
- new btDiscreteDynamicsWorld(m_data->dispatcher, m_data->overlappingPairCache,
- m_data->solver, m_data->collision);
+ m_data->collision = std::make_unique();
+ m_data->dispatcher = std::make_unique(m_data->collision.get());
+ m_data->overlappingPairCache = std::make_unique();
+ m_data->solver = std::make_unique();
+ m_data->dynamicsWorld = std::make_shared(
+ m_data->dispatcher.get(), m_data->overlappingPairCache.get(),
+ m_data->solver.get(), m_data->collision.get());
m_data->dynamicsWorld->setGravity(btVector3(0.0f, 0.0f, -9.81f * SIMULATOR_SCALE));
- m_data->dynamicsWorld->setInternalTickCallback(simulatorTickCallback, this, true);
+ m_data->dynamicsWorld->setInternalTickCallback(
+ [](btDynamicsWorld *world, btScalar timeStep) {
+ Simulator *sim = reinterpret_cast(world->getWorldUserInfo());
+ sim->handleSimulatorTick(timeStep);
+ },
+ this, true);
m_data->geometry.CopyFrom(setup.geometry());
for (const auto &camera : setup.camera_setup())
{
- m_data->reportedCameraSetup.append(camera);
+ m_data->reportedCameraSetup.push_back(camera);
ErForceVector visionPosition(camera.derived_camera_world_tx(),
camera.derived_camera_world_ty());
btVector3 truePosition;
coordinates::fromVision(visionPosition, truePosition);
truePosition.setZ(camera.derived_camera_world_tz() / 1000.0f);
- m_data->cameraPositions.append(truePosition);
+ m_data->cameraPositions.push_back(truePosition);
}
- // add field and ball
- m_data->field = new SimField(m_data->dynamicsWorld, m_data->geometry);
- m_data->ball = new SimBall(&m_data->rng, m_data->dynamicsWorld);
- connect(m_data->ball, &SimBall::sendSSLSimError, m_aggregator,
- &ErrorAggregator::aggregate);
- m_data->flip = false;
+ m_data->field = std::make_unique(m_data->dynamicsWorld, m_data->geometry);
+ m_data->ball = std::make_shared(m_data->dynamicsWorld);
+ m_data->flip = false;
m_data->stddevBall = 0.0f;
m_data->stddevBallArea = 0.0f;
m_data->stddevRobot = 0.0f;
@@ -167,32 +84,6 @@ Simulator::Simulator(const amun::SimulatorSetup &setup)
// no robots after initialisation
}
-// does delete all Simrobots in the RobotMap, does not clear map
-// (just like qDeleteAll would)
-static void deleteAll(const Simulator::RobotMap &map)
-{
- for (const auto &e : map)
- {
- delete e.first;
- }
-}
-
-Simulator::~Simulator()
-{
- resetVisionPackets();
-
- deleteAll(m_data->robotsBlue);
- deleteAll(m_data->robotsYellow);
- delete m_data->ball;
- delete m_data->field;
- delete m_data->dynamicsWorld;
- delete m_data->solver;
- delete m_data->overlappingPairCache;
- delete m_data->dispatcher;
- delete m_data->collision;
- delete m_data;
-}
-
std::vector Simulator::acceptBlueRobotControlCommand(
const SSLSimulationProto::RobotControl &control)
{
@@ -211,65 +102,35 @@ std::vector Simulator::acceptRobotControlCommand(
// collect responses from robots
std::vector responses;
+ Simulator::RobotMap &robotMap = isBlue ? m_data->robotsBlue : m_data->robotsYellow;
+
for (const SSLSimulationProto::RobotCommand &command : control.robot_commands())
{
- // pass radio command to robot that matches the id
- const auto id = command.id();
- SimulatorData *data = m_data;
- auto time = m_time;
- auto charge = m_charge;
- auto fabricateResponse = [data, &responses, time, charge, &id, &command](
- const Simulator::RobotMap &map, const bool *isBlue) {
- if (!map.contains(id))
- return;
- robot::RadioResponse response = map[id].first->setCommand(
- command, data->ball, charge, data->robotCommandPacketLoss,
- data->robotReplyPacketLoss);
- response.set_time(time);
-
- if (isBlue != nullptr)
- {
- response.set_is_blue(*isBlue);
- }
- // only collect valid responses
- if (response.IsInitialized())
- {
- if (data->robotReplyPacketLoss == 0 ||
- data->rng.uniformFloat(0, 1) > data->robotReplyPacketLoss)
- {
- responses.emplace_back(response);
- }
- }
- };
- if (isBlue)
+ if (!robotMap.contains(command.id()))
{
- fabricateResponse(m_data->robotsBlue, &isBlue);
+ return responses;
}
- else
+
+ // pass radio command to robot that matches the id
+ robot::RadioResponse response = robotMap.at(command.id())
+ ->setCommand(command, *m_data->ball, m_charge,
+ m_data->robotCommandPacketLoss,
+ m_data->robotReplyPacketLoss);
+ response.set_time(m_time);
+ response.set_is_blue(isBlue);
+
+ // only collect valid responses
+ if (response.IsInitialized())
{
- fabricateResponse(m_data->robotsYellow, &isBlue);
+ if (m_data->robotReplyPacketLoss == 0 ||
+ m_data->rng.uniformFloat(0, 1) > m_data->robotReplyPacketLoss)
+ {
+ responses.emplace_back(response);
+ }
}
}
- return responses;
-}
-void Simulator::sendSSLSimErrorInternal(ErrorSource source)
-{
- QList errors = m_aggregator->getAggregates(source);
- if (errors.size() == 0)
- return;
- emit sendSSLSimError(errors, source);
-}
-
-static void createRobot(Simulator::RobotMap &list, float x, float y, uint32_t id,
- const ErrorAggregator *agg, SimulatorData *data,
- const QMap &teamSpecs)
-{
- SimRobot *robot = new SimRobot(&data->rng, teamSpecs[id], data->dynamicsWorld,
- btVector3(x, y, 0), 0.f);
- robot->setDribbleMode(data->dribblePerfect);
- robot->connect(robot, &SimRobot::sendSSLSimError, agg, &ErrorAggregator::aggregate);
- list[id] = {robot, teamSpecs[id].generation()};
+ return responses;
}
void Simulator::resetFlipped(Simulator::RobotMap &robots, float side)
@@ -278,22 +139,13 @@ void Simulator::resetFlipped(Simulator::RobotMap &robots, float side)
const float x = m_data->geometry.field_width() / 2 - 0.2;
float y = m_data->geometry.field_height() / 2 - 0.2;
- for (RobotMap::iterator it = robots.begin(); it != robots.end(); ++it)
+ for (auto &[robotId, robot] : robots)
{
- SimRobot *robot = it.value().first;
if (robot->isFlipped())
{
- SimRobot *new_robot =
- new SimRobot(&m_data->rng, robot->specs(), m_data->dynamicsWorld,
- btVector3(x, side * y, 0), 0.0f);
- delete robot;
- connect(new_robot, &SimRobot::sendSSLSimError, m_aggregator,
- &ErrorAggregator::aggregate); // TODO? use createRobot instead of
- // this. However, doing so naively
- // will break the iteration, so I
- // left it for now.
- new_robot->setDribbleMode(m_data->dribblePerfect);
- it.value().first = new_robot;
+ robot = std::make_unique(robot->specs(), m_data->dynamicsWorld,
+ btVector3(x, side * y, 0), 0.0f);
+ robot->setDribbleMode(m_data->dribblePerfect);
}
y -= 0.3;
}
@@ -314,17 +166,14 @@ void Simulator::handleSimulatorTick(double time_s)
resetFlipped(m_data->robotsYellow, -1.0f);
if (m_data->ball->isInvalid())
{
- delete m_data->ball;
- m_data->ball = new SimBall(&m_data->rng, m_data->dynamicsWorld);
- connect(m_data->ball, &SimBall::sendSSLSimError, m_aggregator,
- &ErrorAggregator::aggregate);
+ m_data->ball = std::make_shared(m_data->dynamicsWorld);
}
// find out if ball and any robot collide
- auto robot_ball_collision = [this](QPair elem) {
- return elem.first->touchesBall(this->m_data->ball);
+ auto robot_ball_collision = [&](const auto &kv_pair) {
+ auto &[robotId, robot] = kv_pair;
+ return robot->touchesBall(*m_data->ball);
};
-
bool ball_collision = std::any_of(m_data->robotsBlue.begin(),
m_data->robotsBlue.end(), robot_ball_collision) ||
std::any_of(m_data->robotsYellow.begin(),
@@ -332,13 +181,13 @@ void Simulator::handleSimulatorTick(double time_s)
// apply commands and forces to ball and robots
m_data->ball->begin(ball_collision);
- for (const auto &pair : m_data->robotsBlue)
+ for (auto &[robotId, robot] : m_data->robotsBlue)
{
- pair.first->begin(m_data->ball, time_s);
+ robot->begin(*m_data->ball, time_s);
}
- for (const auto &pair : m_data->robotsYellow)
+ for (auto &[robotId, robot] : m_data->robotsYellow)
{
- pair.first->begin(m_data->ball, time_s);
+ robot->begin(*m_data->ball, time_s);
}
// add gravity to all ACTIVE objects
@@ -347,7 +196,8 @@ void Simulator::handleSimulatorTick(double time_s)
}
static bool checkCameraID(const int cameraId, const btVector3 &p,
- const QVector &cameraPositions, const float overlap)
+ const std::vector &cameraPositions,
+ const float overlap)
{
float minDistance = std::numeric_limits::max();
float ownDistance = 0;
@@ -366,13 +216,13 @@ static bool checkCameraID(const int cameraId, const btVector3 &p,
return ownDistance <= minDistance + 2 * overlap;
}
-void Simulator::initializeDetection(SSLProto::SSL_DetectionFrame *detection,
- std::size_t cameraId)
+void Simulator::initializeDetection(SSLProto::SSL_DetectionFrame &detection,
+ size_t cameraId)
{
- detection->set_frame_number(m_lastFrameNumber[cameraId]++);
- detection->set_camera_id(cameraId);
- detection->set_t_capture((m_time + m_visionDelay - m_visionProcessingTime) * 1E-9);
- detection->set_t_sent((m_time + m_visionDelay) * 1E-9);
+ detection.set_frame_number(m_lastFrameNumber[cameraId]++);
+ detection.set_camera_id(cameraId);
+ detection.set_t_capture((m_time + m_visionDelay - m_visionProcessingTime) * 1E-9);
+ detection.set_t_sent((m_time + m_visionDelay) * 1E-9);
}
static btVector3 positionOffsetForCamera(float offsetStrength, btVector3 cameraPos)
@@ -398,7 +248,7 @@ std::vector Simulator::getWrapperPackets()
std::vector detections(numCameras);
for (std::size_t i = 0; i < numCameras; i++)
{
- initializeDetection(&detections[i], i);
+ initializeDetection(detections[i], i);
}
bool missingBall = m_data->missingBallDetections > 0 &&
@@ -421,7 +271,7 @@ std::vector Simulator::getWrapperPackets()
const btVector3 positionOffset = positionOffsetForCamera(
m_data->objectPositionOffset, m_data->cameraPositions[cameraId]);
bool visible = m_data->ball->update(
- detections[cameraId].add_balls(), m_data->stddevBall,
+ *detections[cameraId].add_balls(), m_data->stddevBall,
m_data->stddevBallArea, m_data->cameraPositions[cameraId],
m_data->enableInvisibleBall, m_data->ballVisibilityThreshold,
positionOffset);
@@ -433,14 +283,12 @@ std::vector Simulator::getWrapperPackets()
}
// get robot positions
- for (bool teamIsBlue : {true, false})
+ for (auto &[teamIsBlue, team] :
+ {std::make_tuple(true, std::ref(m_data->robotsBlue)),
+ std::make_tuple(false, std::ref(m_data->robotsYellow))})
{
- auto &team = teamIsBlue ? m_data->robotsBlue : m_data->robotsYellow;
-
- for (const auto &it : team)
+ for (auto &[robotId, robot] : team)
{
- SimRobot *robot = it.first;
-
if (m_time - robot->getLastSendTime() >= m_minRobotDetectionTime)
{
const float timeDiff = (m_time - robot->getLastSendTime()) * 1E-9;
@@ -458,13 +306,13 @@ std::vector Simulator::getWrapperPackets()
m_data->objectPositionOffset, m_data->cameraPositions[cameraId]);
if (teamIsBlue)
{
- robot->update(detections[cameraId].add_robots_blue(),
+ robot->update(*detections[cameraId].add_robots_blue(),
m_data->stddevRobot, m_data->stddevRobotPhi, m_time,
positionOffset);
}
else
{
- robot->update(detections[cameraId].add_robots_yellow(),
+ robot->update(*detections[cameraId].add_robots_yellow(),
m_data->stddevRobot, m_data->stddevRobotPhi, m_time,
positionOffset);
}
@@ -478,7 +326,7 @@ std::vector Simulator::getWrapperPackets()
{
// always on the right side of the dribbler for now
if (!m_data->ball->addDetection(
- detections[cameraId].add_balls(),
+ *detections[cameraId].add_balls(),
robot->dribblerCorner(false) / SIMULATOR_SCALE,
m_data->stddevRobot, 0, m_data->cameraPositions[cameraId],
false, 0, positionOffset))
@@ -555,70 +403,46 @@ world::SimulatorState Simulator::getSimulatorState()
const std::size_t numCameras = m_data->reportedCameraSetup.size();
world::SimulatorState simState;
- auto *ball = simState.mutable_ball();
- m_data->ball->writeBallState(ball);
+ m_data->ball->writeBallState(*simState.mutable_ball());
// get robot positions
for (bool teamIsBlue : {true, false})
{
auto &team = teamIsBlue ? m_data->robotsBlue : m_data->robotsYellow;
- for (const auto &it : team)
+ for (auto &[robotId, robot] : team)
{
- SimRobot *robot = it.first;
-
// convert coordinates from ER Force
btVector3 robotPos = robot->position() / SIMULATOR_SCALE;
btVector3 newRobotPos;
coordinates::toVision(robotPos, newRobotPos);
- auto *robotProto =
- teamIsBlue ? simState.add_blue_robots() : simState.add_yellow_robots();
+ auto &robotProto =
+ teamIsBlue ? *simState.add_blue_robots() : *simState.add_yellow_robots();
- robot->update(robotProto, m_data->ball);
+ robot->update(robotProto, *m_data->ball);
// Convert mm to m
- robotProto->set_p_x(newRobotPos.x() / 1000);
- robotProto->set_p_y(newRobotPos.y() / 1000);
+ robotProto.set_p_x(newRobotPos.x() / 1000);
+ robotProto.set_p_y(newRobotPos.y() / 1000);
// Convert velocity
- coordinates::toVisionVelocity(*robotProto, *robotProto);
- robotProto->set_v_x(robotProto->v_x() / 1000);
- robotProto->set_v_y(robotProto->v_y() / 1000);
+ coordinates::toVisionVelocity(robotProto, robotProto);
+ robotProto.set_v_x(robotProto.v_x() / 1000);
+ robotProto.set_v_y(robotProto.v_y() / 1000);
}
}
return simState;
}
-void Simulator::resetVisionPackets()
-{
- qDeleteAll(m_visionTimers);
- m_visionTimers.clear();
- m_visionPackets.clear();
-}
-
-void Simulator::handleRadioCommands(const SSLSimRobotControl &commands, bool isBlue,
- qint64 processingStart)
-{
- m_radioCommands.enqueue(std::make_tuple(commands, processingStart, isBlue));
-}
-
-void Simulator::setTeam(Simulator::RobotMap &list, float side, const robot::Team &team,
- QMap &teamSpecs)
+void Simulator::setTeam(Simulator::RobotMap &robotMap, float side,
+ const robot::Team &team,
+ std::map &teamSpecs)
{
// remove old team
- deleteAll(list);
- list.clear();
-
- // changing a team is also triggering a tracking reset
- // thus the old robots will disappear immediatelly
- // however if the delayed vision packets arrive the old robots will be tracked
- // again thus after removing a robot from a team it can take 1 simulated
- // second for the robot to disappear to prevent this remove outdated vision
- // packets
- resetVisionPackets();
+ robotMap.clear();
// align robots on a line
const float x = m_data->geometry.field_width() / 2 - 0.2;
@@ -630,14 +454,17 @@ void Simulator::setTeam(Simulator::RobotMap &list, float side, const robot::Team
const auto id = specs.id();
// (color, robot id) must be unique
- if (list.contains(id))
+ if (robotMap.contains(id))
{
std::cerr << "Error: Two ids for the same color, aborting!" << std::endl;
continue;
}
teamSpecs[id].CopyFrom(specs);
- createRobot(list, x, side * y, id, m_aggregator, m_data, teamSpecs);
+ robotMap[id] = std::make_unique(teamSpecs[id], m_data->dynamicsWorld,
+ btVector3(x, side * y, 0), 0.f);
+ robotMap[id]->setDribbleMode(m_data->dribblePerfect);
+
y -= 0.3;
}
}
@@ -656,12 +483,13 @@ void Simulator::moveBall(const sslsim::TeleportBall &ball)
// remove the dribbling constraint
if (!ball.has_by_force() || !ball.by_force())
{
- for (const auto &robotList : {m_data->robotsBlue, m_data->robotsYellow})
+ for (auto &[robotId, robot] : m_data->robotsBlue)
{
- for (const auto &it : robotList)
- {
- it.first->stopDribbling();
- }
+ robot->stopDribbling();
+ }
+ for (auto &[robotId, robot] : m_data->robotsYellow)
+ {
+ robot->stopDribbling();
}
}
@@ -690,12 +518,11 @@ void Simulator::moveRobot(const sslsim::TeleportRobot &robot)
if (!robot.id().has_id())
return;
- bool is_blue = robot.id().team() == gameController::Team::BLUE;
-
- RobotMap &list = is_blue ? m_data->robotsBlue : m_data->robotsYellow;
- bool isPresent = list.contains(robot.id().id());
- QMap &teamSpecs =
- is_blue ? m_data->specsBlue : m_data->specsYellow;
+ bool isBlue = robot.id().team() == gameController::Team::BLUE;
+ RobotMap &robotMap = isBlue ? m_data->robotsBlue : m_data->robotsYellow;
+ bool isPresent = robotMap.contains(robot.id().id());
+ std::map &teamSpecs =
+ isBlue ? m_data->specsBlue : m_data->specsYellow;
if (robot.has_present())
{
@@ -704,39 +531,31 @@ void Simulator::moveRobot(const sslsim::TeleportRobot &robot)
// add the requested robot
if (!teamSpecs.contains(robot.id().id()))
{
- SSLSimError error{new sslsim::SimulatorError};
- error->set_code("CREATE_UNSPEC_ROBOT");
- std::string message =
- "trying to create robot " + std::to_string(robot.id().id());
- message += ", but no spec for this robot was found";
- error->set_message(std::move(message));
- m_aggregator->aggregate(error, ErrorSource::CONFIG);
+ std::cerr << "Trying to create robot " << std::to_string(robot.id().id())
+ << ", but no spec for this robot was found" << std::endl;
}
else if (!robot.has_x() || !robot.has_y())
{
- SSLSimError error{new sslsim::SimulatorError};
- error->set_code("CREATE_NOPOS_ROBOT");
- std::string message =
- "trying to create robot " + std::to_string(robot.id().id());
- message += " without giving a position";
- error->set_message(std::move(message));
- m_aggregator->aggregate(error, ErrorSource::CONFIG);
+ std::cerr << "Trying to create robot " << std::to_string(robot.id().id())
+ << ", without giving a position" << std::endl;
}
else
{
ErForceVector targetPos;
coordinates::fromVision(robot, targetPos);
// TODO: check if the given position is fine
- createRobot(list, targetPos.x, targetPos.y, robot.id().id(), m_aggregator,
- m_data, teamSpecs);
+
+ robotMap[robot.id().id()] = std::make_unique(
+ teamSpecs[robot.id().id()], m_data->dynamicsWorld,
+ btVector3(targetPos.x, targetPos.y, 0), 0.f);
+ robotMap[robot.id().id()]->setDribbleMode(m_data->dribblePerfect);
}
}
else if (!robot.present() && isPresent)
{
// remove the robot
- auto val = list.take(robot.id().id());
- val.first->stopDribbling();
- delete val.first;
+ robotMap.at(robot.id().id())->stopDribbling();
+ robotMap.erase(robot.id().id());
return;
}
else if (!robot.present() && !isPresent)
@@ -752,8 +571,8 @@ void Simulator::moveRobot(const sslsim::TeleportRobot &robot)
return;
}
- if (!list.contains(robot.id().id()))
- return; // Recheck the list in case the has_present paragraph did change it.
+ if (!robotMap.contains(robot.id().id()))
+ return; // Recheck the robotMap in case the has_present paragraph did change it.
sslsim::TeleportRobot r = robot;
@@ -765,17 +584,11 @@ void Simulator::moveRobot(const sslsim::TeleportRobot &robot)
FLIP(r, v_y);
}
- SimRobot *sim_robot = list[robot.id().id()].first;
if (!r.has_by_force() || !r.by_force())
{
- sim_robot->stopDribbling();
+ robotMap[robot.id().id()]->stopDribbling();
}
- sim_robot->move(r);
-}
-
-void Simulator::setFlipped(bool flipped)
-{
- m_data->flip = flipped;
+ robotMap[robot.id().id()]->move(r);
}
void Simulator::handleSimulatorSetupCommand(const std::unique_ptr &command)
@@ -785,6 +598,7 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr
if (command->has_simulator())
{
const amun::CommandSimulator &sim = command->simulator();
+
if (sim.has_enable())
{
m_enabled = sim.enable();
@@ -793,6 +607,7 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr
if (sim.has_realism_config())
{
auto realism = sim.realism_config();
+
if (realism.has_stddev_ball_p())
{
m_data->stddevBall = realism.stddev_ball_p();
@@ -860,13 +675,12 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr
if (realism.has_vision_delay())
{
- m_visionDelay = std::max((qint64)0, (qint64)realism.vision_delay());
+ m_visionDelay = std::max(0l, realism.vision_delay());
}
if (realism.has_vision_processing_time())
{
- m_visionProcessingTime =
- std::max((qint64)0, (qint64)realism.vision_processing_time());
+ m_visionProcessingTime = std::max(0l, realism.vision_processing_time());
}
if (realism.has_simulate_dribbling())
@@ -914,7 +728,7 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr
{
if (map.contains(robot.id()))
{
- map[robot.id()].first->restoreState(robot);
+ map[robot.id()]->restoreState(robot);
}
}
};
@@ -926,10 +740,9 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr
if (command->has_transceiver())
{
- const amun::CommandTransceiver &t = command->transceiver();
- if (t.has_charge())
+ if (command->transceiver().has_charge())
{
- m_charge = t.charge();
+ m_charge = command->transceiver().charge();
}
}
@@ -948,73 +761,13 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr
if (teamOrPerfectDribbleChanged)
{
- for (const auto &robotList : {m_data->robotsBlue, m_data->robotsYellow})
+ for (auto &[robotId, robot] : m_data->robotsBlue)
{
- for (const auto &it : robotList)
- {
- SimRobot *robot = it.first;
- robot->setDribbleMode(m_data->dribblePerfect);
- }
+ robot->setDribbleMode(m_data->dribblePerfect);
}
- }
-}
-
-void Simulator::seedPRGN(uint32_t seed)
-{
- m_data->rng.seed(seed);
-}
-
-static bool overlapCheck(const btVector3 &p0, const float &r0, const btVector3 &p1,
- const float &r1)
-{
- const float distance = (p1 - p0).length();
- return distance <= r0 + r1;
-}
-
-// uses the real world scale
-void Simulator::teleportRobotToFreePosition(SimRobot *robot)
-{
- btVector3 robotPos = robot->position() / SIMULATOR_SCALE;
- btVector3 direction =
- (robotPos - m_data->ball->position() / SIMULATOR_SCALE).normalize();
- float distance =
- 2.0f * (static_cast(BALL_MAX_RADIUS_METERS) + robot->specs().radius());
- bool valid = true;
- do
- {
- valid = true;
- robotPos = robotPos + 2 * direction * distance;
-
- for (const auto &robotList : {m_data->robotsBlue, m_data->robotsYellow})
+ for (auto &[robotId, robot] : m_data->robotsYellow)
{
- for (const auto &it : robotList)
- {
- SimRobot *robot2 = it.first;
- if (robot == robot2)
- {
- continue;
- }
-
- btVector3 tmp = robot2->position() / SIMULATOR_SCALE;
- if (overlapCheck(robotPos, robot->specs().radius(), tmp,
- robot2->specs().radius()))
- {
- valid = false;
- break;
- }
- }
- if (!valid)
- {
- break;
- }
+ robot->setDribbleMode(m_data->dribblePerfect);
}
- } while (!valid);
-
- sslsim::TeleportRobot robotCommand;
- robotCommand.mutable_id()->set_id(robot->specs().id());
- coordinates::toVision(robotPos, robotCommand);
-
- robotCommand.set_v_x(0);
- robotCommand.set_v_y(0);
- robot->move(robotCommand);
+ }
}
diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simulator.h b/src/extlibs/er_force_sim/src/amun/simulator/simulator.h
index f9cbb77432..48d3560c15 100644
--- a/src/extlibs/er_force_sim/src/amun/simulator/simulator.h
+++ b/src/extlibs/er_force_sim/src/amun/simulator/simulator.h
@@ -21,19 +21,16 @@
#ifndef SIMULATOR_H
#define SIMULATOR_H
-#include
-#include
-#include
-#include
-#include
+#include
#include
-#include
+#include
-#include "extlibs/er_force_sim/src/protobuf/command.h"
-#include "extlibs/er_force_sim/src/protobuf/sslsim.h"
+#include "extlibs/er_force_sim/src/core/rng.h"
#include "proto/ssl_simulation_robot_control.pb.h"
#include "proto/ssl_vision_wrapper.pb.h"
-
+#include "simball.h"
+#include "simfield.h"
+#include "simrobot.h"
// higher values break the rolling friction of the ball
const float SIMULATOR_SCALE = 10.0f;
@@ -41,37 +38,19 @@ const float SUB_TIMESTEP = 1 / 200.f;
const float COLLISION_MARGIN = 0.04f;
const float FOCAL_LENGTH = 390.f;
-
-class QByteArray;
-class QTimer;
-class Timer;
-class SSL_GeometryFieldSize;
-
namespace camun
{
namespace simulator
{
- class SimRobot;
class Simulator;
- class ErrorAggregator;
struct SimulatorData;
-
- enum class ErrorSource
- {
- BLUE,
- YELLOW,
- CONFIG
- };
} // namespace simulator
} // namespace camun
-class camun::simulator::Simulator : public QObject
+class camun::simulator::Simulator
{
- Q_OBJECT
-
public:
- typedef QMap>
- RobotMap; /*First int: ID, Second int: Generation*/
+ typedef std::map> RobotMap;
/**
* Creates a simulator with the given set up
@@ -79,22 +58,6 @@ class camun::simulator::Simulator : public QObject
* @param setup the simulator set up
*/
explicit Simulator(const amun::SimulatorSetup &setup);
- ~Simulator() override;
- Simulator(const Simulator &) = delete;
- Simulator &operator=(const Simulator &) = delete;
-
- /**
- * Seeds the pseudorandom generator
- *
- * @param seed
- */
- void seedPRGN(uint32_t seed);
-
- signals:
- void gotPacket(const QByteArray &data, qint64 time, QString sender);
- void sendRadioResponses(const QList &responses);
- void sendRealData(const QByteArray &data); // sends amun::SimulatorState
- void sendSSLSimError(const QList &errors, ErrorSource source);
public:
/**
@@ -145,11 +108,6 @@ class camun::simulator::Simulator : public QObject
*/
void handleSimulatorSetupCommand(const std::unique_ptr &command);
- public slots:
- void handleRadioCommands(const SSLSimRobotControl &control, bool isBlue,
- qint64 processingStart);
- void setFlipped(bool flipped);
-
private:
/**
* Accepts and executes a blue or yellow robot control command
@@ -161,42 +119,91 @@ class camun::simulator::Simulator : public QObject
*/
std::vector acceptRobotControlCommand(
const SSLSimulationProto::RobotControl &control, bool isBlue);
- void sendSSLSimErrorInternal(ErrorSource source);
+
void resetFlipped(RobotMap &robots, float side);
- std::tuple, QByteArray, qint64> createVisionPacket();
- void resetVisionPackets();
void setTeam(RobotMap &list, float side, const robot::Team &team,
- QMap &specs);
+ std::map &specs);
void moveBall(const sslsim::TeleportBall &ball);
void moveRobot(const sslsim::TeleportRobot &robot);
- void teleportRobotToFreePosition(SimRobot *robot);
- void initializeDetection(SSLProto::SSL_DetectionFrame *detection,
- std::size_t cameraId);
+ void initializeDetection(SSLProto::SSL_DetectionFrame &detection, size_t cameraId);
private:
- typedef std::tuple RadioCommand;
- SimulatorData *m_data;
- QQueue m_radioCommands;
- QQueue, QByteArray, qint64>> m_visionPackets;
- QQueue m_visionTimers;
- QTimer *m_trigger;
- qint64 m_time;
- qint64 m_lastSentStatusTime;
+ std::unique_ptr m_data;
+
bool m_enabled;
bool m_charge;
- // systemDelay + visionProcessingTime = visionDelay
- qint64 m_visionDelay;
- qint64 m_visionProcessingTime;
-
- qint64 m_minRobotDetectionTime = 0;
- qint64 m_minBallDetectionTime = 0;
- qint64 m_lastBallSendTime = 0;
- std::map m_lastFrameNumber;
- ErrorAggregator *m_aggregator;
+ int64_t m_time;
+ int64_t m_visionDelay; // systemDelay + visionProcessingTime = visionDelay
+ int64_t m_visionProcessingTime;
+ int64_t m_minRobotDetectionTime = 0;
+ int64_t m_minBallDetectionTime = 0;
+ int64_t m_lastBallSendTime = 0;
+ std::map m_lastFrameNumber;
std::mt19937 rand_shuffle_src = std::mt19937(std::random_device()());
};
+
+/* Friction and restitution between robots, ball and field: (empirical
+ * measurments) Ball vs. Robot: Restitution: about 0.60 Friction: trial and
+ * error in simulator 0.18 (similar results as in reality)
+ *
+ * Ball vs. Floor:
+ * Restitution: sqrt(h'/h) = sqrt(0.314) = 0.56
+ * Friction: \mu_k = -a / g (while slipping) = 0.35
+ *
+ * Robot vs. Floor:
+ * Restitution and Friction should be as low as possible
+ *
+ * Calculations:
+ * Variables: r: restitution, f: friction
+ * Indices: b: ball; f: floor; r: robot
+ *
+ * r_b * r_f = 0.56
+ * r_b * r_r = 0.60
+ * r_f * r_r = small
+ * => r_b = 1; r_f = 0.56; r_r = 0.60
+ *
+ * f_b * f_f = 0.35
+ * f_b * f_r = 0.22
+ * f_f * f_r = very small
+ * => f_b = 1; f_f = 0.35; f_r = 0.22
+ */
+
+struct camun::simulator::SimulatorData
+{
+ RNG rng;
+ std::unique_ptr collision;
+ std::unique_ptr dispatcher;
+ std::unique_ptr overlappingPairCache;
+ std::unique_ptr solver;
+ std::shared_ptr dynamicsWorld;
+ world::Geometry geometry;
+ std::vector reportedCameraSetup;
+ std::vector cameraPositions;
+ std::unique_ptr field;
+ std::shared_ptr ball;
+ Simulator::RobotMap robotsBlue;
+ Simulator::RobotMap robotsYellow;
+ std::map specsBlue;
+ std::map specsYellow;
+ bool flip;
+ float stddevBall;
+ float stddevBallArea;
+ float stddevRobot;
+ float stddevRobotPhi;
+ float ballDetectionsAtDribbler; // per robot per second
+ bool enableInvisibleBall;
+ float ballVisibilityThreshold;
+ float cameraOverlap;
+ float cameraPositionError;
+ float objectPositionOffset;
+ float robotCommandPacketLoss;
+ float robotReplyPacketLoss;
+ float missingBallDetections;
+ bool dribblePerfect;
+};
+
#endif // SIMULATOR_H
diff --git a/src/extlibs/er_force_sim/src/core/BUILD b/src/extlibs/er_force_sim/src/core/BUILD
index 517319b07a..475d773201 100644
--- a/src/extlibs/er_force_sim/src/core/BUILD
+++ b/src/extlibs/er_force_sim/src/core/BUILD
@@ -1,16 +1,9 @@
-load("@bazel_rules_qt//:qt.bzl", "qt_cc_library")
-
package(default_visibility = ["//visibility:public"])
-qt_cc_library(
- name = "core_qt",
+cc_library(
+ name = "core",
srcs = glob(
["*.cpp"],
),
hdrs = glob(["*.h"]),
- deps = [
- "@qt//:qt_core",
- "@qt//:qt_gui",
- "@qt//:qt_widgets",
- ],
)
diff --git a/src/extlibs/er_force_sim/src/core/configuration.h b/src/extlibs/er_force_sim/src/core/configuration.h
deleted file mode 100644
index 0cfb4fe8a7..0000000000
--- a/src/extlibs/er_force_sim/src/core/configuration.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/****************************************************************************
- * Copyright 2021 Andreas Wendler *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 CONFIGURATION_H
-#define CONFIGURATION_H
-
-#include
-#include
-
-#include
-#include
-#include
-
-#include "src/config/config.h"
-
-inline bool loadConfiguration(const QString &configFile,
- google::protobuf::Message *message, bool allowPartial)
-{
- QString fullFilename = QString(ERFORCE_CONFDIR) + configFile + ".txt";
- QFile file(fullFilename);
- if (!file.open(QFile::ReadOnly))
- {
- std::cout << "Could not open configuration file " << fullFilename.toStdString()
- << std::endl;
- return false;
- }
- QString str = file.readAll();
- file.close();
- std::string s = qPrintable(str);
-
- google::protobuf::TextFormat::Parser parser;
- parser.AllowPartialMessage(allowPartial);
- parser.ParseFromString(s, message);
- return true;
-}
-
-#endif // CONFIGURATION_H
diff --git a/src/extlibs/er_force_sim/src/core/protobuffilereader.cpp b/src/extlibs/er_force_sim/src/core/protobuffilereader.cpp
deleted file mode 100644
index 80e9f00f62..0000000000
--- a/src/extlibs/er_force_sim/src/core/protobuffilereader.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/***************************************************************************
- * Copyright 2020 Andreas Wendler *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 "protobuffilereader.h"
-
-ProtobufFileReader::ProtobufFileReader() : m_stream(&m_file) {}
-
-bool ProtobufFileReader::open(QString filename, QString filePrefix)
-{
- m_file.setFileName(filename);
- if (!m_file.open(QIODevice::ReadOnly))
- {
- return false;
- }
-
- // ensure compatibility across qt versions
- m_stream.setVersion(QDataStream::Qt_4_6);
-
- QString fileType;
- int version;
- m_stream >> fileType >> version;
-
- if (fileType != filePrefix || version != 0)
- {
- return false;
- }
-
- return true;
-}
-
-bool ProtobufFileReader::readNext(google::protobuf::Message &message)
-{
- if (m_stream.atEnd())
- {
- return false;
- }
- QByteArray data;
- m_stream >> data;
-
- if (!message.ParseFromArray(data.data(), data.size()))
- {
- return false;
- }
- return true;
-}
diff --git a/src/extlibs/er_force_sim/src/core/protobuffilereader.h b/src/extlibs/er_force_sim/src/core/protobuffilereader.h
deleted file mode 100644
index 957c2e4a20..0000000000
--- a/src/extlibs/er_force_sim/src/core/protobuffilereader.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/***************************************************************************
- * Copyright 2020 Andreas Wendler *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 PROTOBUFFILEREADER_H
-#define PROTOBUFFILEREADER_H
-
-#include
-
-#include
-#include
-
-class ProtobufFileReader : public QObject
-{
- public:
- ProtobufFileReader();
-
- bool open(QString filename, QString filePrefix);
-
- // returns true if the message has be sucessfully parsed
- bool readNext(google::protobuf::Message &message);
-
- private:
- QFile m_file;
- QDataStream m_stream;
-};
-
-#endif // PROTOBUFFILEREADER_H
diff --git a/src/extlibs/er_force_sim/src/core/protobuffilesaver.cpp b/src/extlibs/er_force_sim/src/core/protobuffilesaver.cpp
deleted file mode 100644
index 87a3a8c5db..0000000000
--- a/src/extlibs/er_force_sim/src/core/protobuffilesaver.cpp
+++ /dev/null
@@ -1,68 +0,0 @@
-/***************************************************************************
- * Copyright 2020 Andreas Wendler *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 "protobuffilesaver.h"
-
-#include
-
-ProtobufFileSaver::ProtobufFileSaver(QString filename, QString filePrefix,
- QObject *parent)
- : QObject(parent),
- m_filename(filename),
- m_filePrefix(filePrefix),
- m_stream(&m_file),
- m_mutex(QMutex::Recursive)
-{
-}
-
-void ProtobufFileSaver::open()
-{
- if (m_file.isOpen())
- {
- return;
- }
- m_file.setFileName(m_filename);
- if (!m_file.open(QIODevice::WriteOnly | QIODevice::Truncate))
- {
- qDebug() << "Could not open path input file for saving";
- return;
- }
-
- // ensure compatibility across qt versions
- m_stream.setVersion(QDataStream::Qt_4_6);
-
- m_stream << QString(m_filePrefix);
- m_stream << (int)0; // file version
-}
-
-void ProtobufFileSaver::saveMessage(const google::protobuf::Message &message)
-{
- QByteArray data;
- data.resize(message.ByteSize());
- if (!message.IsInitialized() || !message.SerializeToArray(data.data(), data.size()))
- {
- return;
- }
-
- QMutexLocker locker(&m_mutex);
- open();
-
- m_stream << data;
-}
diff --git a/src/extlibs/er_force_sim/src/core/protobuffilesaver.h b/src/extlibs/er_force_sim/src/core/protobuffilesaver.h
deleted file mode 100644
index 75d16c10ff..0000000000
--- a/src/extlibs/er_force_sim/src/core/protobuffilesaver.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/***************************************************************************
- * Copyright 2020 Andreas Wendler *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 PROTOBUFFILESAVER_H
-#define PROTOBUFFILESAVER_H
-
-#include
-
-#include
-#include
-#include
-#include
-#include
-
-class ProtobufFileSaver : QObject
-{
- Q_OBJECT
- public:
- // The file is created once saveMessage is called for the first time
- ProtobufFileSaver(QString filename, QString filePrefix, QObject *parent = nullptr);
-
- // may be called from any thread
- void saveMessage(const google::protobuf::Message &message);
-
- private:
- void open();
-
- private:
- QString m_filename;
- QString m_filePrefix;
- QFile m_file;
- QDataStream m_stream;
- QMutex m_mutex;
-};
-
-#endif // PROTOBUFFILESAVER_H
diff --git a/src/extlibs/er_force_sim/src/core/rng.h b/src/extlibs/er_force_sim/src/core/rng.h
index b3f858f93d..319cb4566a 100644
--- a/src/extlibs/er_force_sim/src/core/rng.h
+++ b/src/extlibs/er_force_sim/src/core/rng.h
@@ -52,12 +52,12 @@ class RNG
*/
inline double RNG::uniform()
{
- return uniformInt() / 4294967296.0;
+ return static_cast(uniformInt()) / 4294967296.0;
}
inline float RNG::uniformFloat(float min, float max)
{
- return min + (uniformInt() / 4294967296.0f) * (max - min);
+ return min + (static_cast(uniformInt()) / 4294967296.0f) * (max - min);
}
/*!
@@ -68,7 +68,7 @@ inline float RNG::uniformFloat(float min, float max)
*/
inline ErForceVector RNG::uniformVector()
{
- return ErForceVector(uniform(), uniform());
+ return ErForceVector(static_cast(uniform()), static_cast(uniform()));
}
/*!
diff --git a/src/extlibs/er_force_sim/src/protobuf/BUILD b/src/extlibs/er_force_sim/src/protobuf/BUILD
index 1d28897496..e8f3dc17f0 100644
--- a/src/extlibs/er_force_sim/src/protobuf/BUILD
+++ b/src/extlibs/er_force_sim/src/protobuf/BUILD
@@ -1,5 +1,3 @@
-load("@bazel_rules_qt//:qt.bzl", "qt_cc_library")
-
package(default_visibility = ["//visibility:public"])
load("@rules_proto//proto:defs.bzl", "proto_library")
@@ -35,16 +33,13 @@ py_proto_library(
],
)
-qt_cc_library(
- name = "protobuf_qt",
+cc_library(
+ name = "protobuf",
srcs = glob(["*.cpp"]),
hdrs = glob(["*.h"]),
linkstatic = True,
deps = [
":protobuf_cc_lib",
"//proto:ssl_simulation_cc_proto",
- "@qt//:qt_core",
- "@qt//:qt_gui",
- "@qt//:qt_widgets",
],
)
diff --git a/src/extlibs/er_force_sim/src/protobuf/command.cpp b/src/extlibs/er_force_sim/src/protobuf/command.cpp
deleted file mode 100644
index ae8cbaaece..0000000000
--- a/src/extlibs/er_force_sim/src/protobuf/command.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-/***************************************************************************
- * Copyright 2021 Andreas Wendler *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 "command.h"
-
-#include "geometry.h"
-
-SSL_GeometryCameraCalibration createDefaultCamera(int cameraId, float x, float y, float z)
-{
- SSL_GeometryCameraCalibration calibration;
-
- calibration.set_camera_id(cameraId);
- // DUMMY VALUES
- calibration.set_distortion(0.2);
- calibration.set_focal_length(390);
- calibration.set_principal_point_x(300);
- calibration.set_principal_point_y(300);
- calibration.set_q0(0.7);
- calibration.set_q1(0.7);
- calibration.set_q2(0.7);
- calibration.set_q3(0.7);
- calibration.set_tx(0);
- calibration.set_ty(0);
- calibration.set_tz(3500);
-
- calibration.set_derived_camera_world_tx(y * 1000);
- calibration.set_derived_camera_world_ty(-x * 1000);
- calibration.set_derived_camera_world_tz(z * 1000);
-
- return calibration;
-}
-
-void simulatorSetupSetDefault(amun::SimulatorSetup &setup)
-{
- geometrySetDefault(setup.mutable_geometry(), true);
- setup.add_camera_setup()->CopyFrom(createDefaultCamera(0, 0.0f, 0.0f, 4.0f));
-}
diff --git a/src/extlibs/er_force_sim/src/protobuf/command.h b/src/extlibs/er_force_sim/src/protobuf/command.h
deleted file mode 100644
index 0a72807e68..0000000000
--- a/src/extlibs/er_force_sim/src/protobuf/command.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/***************************************************************************
- * Copyright 2015 Philipp Nordhus *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 COMMAND_H
-#define COMMAND_H
-
-#include
-
-#include "extlibs/er_force_sim/src/protobuf/command.pb.h"
-
-//! @file command.h
-//! @addtogroup protobuf
-//! @{
-
-void simulatorSetupSetDefault(amun::SimulatorSetup &setup);
-
-// position is in meters in our coordinate system
-SSL_GeometryCameraCalibration createDefaultCamera(int cameraId, float x, float y,
- float z);
-
-//! @}
-
-#endif // COMMAND_H
diff --git a/src/extlibs/er_force_sim/src/protobuf/ssl_referee.cpp b/src/extlibs/er_force_sim/src/protobuf/ssl_referee.cpp
deleted file mode 100644
index 25bf123c5b..0000000000
--- a/src/extlibs/er_force_sim/src/protobuf/ssl_referee.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-/***************************************************************************
- * Copyright 2015 Michael Eischer *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 "ssl_referee.h"
-
-/*!
- * \brief Initializes team info with default values
- * \param teamInfo The team info to initialize
- */
-void teamInfoSetDefault(SSLProto::Referee::TeamInfo *teamInfo)
-{
- teamInfo->set_name("");
- teamInfo->set_score(0);
- teamInfo->set_red_cards(0);
- teamInfo->set_yellow_cards(0);
- teamInfo->set_timeouts(4);
- teamInfo->set_timeout_time(5 * 60 * 1000 * 1000);
- teamInfo->set_goalkeeper(0);
- assert(teamInfo->IsInitialized());
-}
diff --git a/src/extlibs/er_force_sim/src/protobuf/ssl_referee.h b/src/extlibs/er_force_sim/src/protobuf/ssl_referee.h
deleted file mode 100644
index c2b498636d..0000000000
--- a/src/extlibs/er_force_sim/src/protobuf/ssl_referee.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/***************************************************************************
- * Copyright 2015 Michael Eischer *
- * Robotics Erlangen e.V. *
- * http://www.robotics-erlangen.de/ *
- * info@robotics-erlangen.de *
- * *
- * 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 *
- * 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 SSL_REFEREE_H
-#define SSL_REFEREE_H
-
-#include "proto/ssl_gc_referee_message.pb.h"
-
-void teamInfoSetDefault(SSLProto::Referee::TeamInfo *teamInfo);
-
-#endif // SSL_REFEREE_H
diff --git a/src/extlibs/er_force_sim/src/protobuf/world.proto b/src/extlibs/er_force_sim/src/protobuf/world.proto
index 65610f5810..1f0402f60d 100644
--- a/src/extlibs/er_force_sim/src/protobuf/world.proto
+++ b/src/extlibs/er_force_sim/src/protobuf/world.proto
@@ -39,15 +39,6 @@ message Geometry
optional Division division = 19 [default = A];
}
-message DivisionDimensions
-{
- required float field_width_a = 1;
- required float field_height_a = 2;
-
- required float field_width_b = 3;
- required float field_height_b = 4;
-}
-
message BallPosition
{
required int64 time = 1;
diff --git a/src/software/simulation/BUILD b/src/software/simulation/BUILD
index e10251bfdd..a84c1d64fd 100644
--- a/src/software/simulation/BUILD
+++ b/src/software/simulation/BUILD
@@ -9,7 +9,7 @@ cc_library(
"//extlibs/er_force_sim/config/simulator:2020B.txt",
],
deps = [
- "//extlibs/er_force_sim/src/amun/simulator:simulator_qt",
+ "//extlibs/er_force_sim/src/amun/simulator",
"//proto/message_translation:ssl_detection",
"//proto/message_translation:ssl_geometry",
"//proto/message_translation:ssl_simulation_robot_control",
diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp
index fff9b082a2..eb06a08bff 100644
--- a/src/software/simulation/er_force_simulator.cpp
+++ b/src/software/simulation/er_force_simulator.cpp
@@ -3,8 +3,6 @@
#include
#include
-#include
-#include
#include
#include "extlibs/er_force_sim/src/protobuf/robot.h"
@@ -34,7 +32,7 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type,
yellow_robot_with_ball(std::nullopt),
ramping(ramping)
{
- QString full_filename = CONFIG_DIRECTORY;
+ std::string full_filename = CONFIG_DIRECTORY;
if (field_type == TbotsProto::FieldType::DIV_A)
{
@@ -46,19 +44,19 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type,
full_filename = full_filename + CONFIG_FILE + "B.txt";
}
- QFile file(full_filename);
- if (!file.open(QFile::ReadOnly))
+ std::ifstream config_file(full_filename);
+ if (!config_file)
{
- LOG(FATAL) << "Could not open configuration file " << full_filename.toStdString()
- << std::endl;
+ LOG(FATAL) << "Could not open configuration file " << full_filename;
}
- QString str = file.readAll();
- file.close();
+ // Read in the config file
+ std::stringstream config_ss;
+ config_ss << config_file.rdbuf();
+ std::string config_str = config_ss.str();
- std::string s = qPrintable(str);
google::protobuf::TextFormat::Parser parser;
- parser.ParseFromString(s, &er_force_sim_setup);
+ parser.ParseFromString(config_str, &er_force_sim_setup);
er_force_sim = std::make_unique(er_force_sim_setup);
auto simulator_setup_command = std::make_unique();
simulator_setup_command->mutable_simulator()->set_enable(true);
diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h
index 632fc97560..1fdfb3cf3e 100644
--- a/src/software/simulation/er_force_simulator.h
+++ b/src/software/simulation/er_force_simulator.h
@@ -230,6 +230,6 @@ class ErForceSimulator
bool ramping;
- const QString CONFIG_FILE = "simulator/2020";
- const QString CONFIG_DIRECTORY = "extlibs/er_force_sim/config/";
+ const std::string CONFIG_FILE = "simulator/2020";
+ const std::string CONFIG_DIRECTORY = "extlibs/er_force_sim/config/";
};
From 3a385a688ad6f10378f2194f1c577bf706308178 Mon Sep 17 00:00:00 2001
From: potatoisagender <63363206+potatoisagender@users.noreply.github.com>
Date: Sun, 29 Sep 2024 18:47:39 -0700
Subject: [PATCH 02/14] Remove unused `ROBOT_STATUS` log filter (#3325)
* Removed unused ROBOT-STATUS log filter
* Removed additional ROBOT_STATUS log filter references from coloured_cout_sink_test.cpp and csv_sink_test.cpp
* Changed the id of the remaining custom log levels in custom_logging_levels.h to match the removal of ROBOT_STATUS log filter.
(all id levels reduced by 1)
* [pre-commit.ci lite] apply automatic fixes
---------
Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com>
---
src/software/logger/coloured_cout_sink_test.cpp | 9 ++++-----
src/software/logger/csv_sink_test.cpp | 1 -
src/software/logger/custom_logging_levels.h | 7 +++----
src/software/logger/logger.h | 7 +++----
4 files changed, 10 insertions(+), 14 deletions(-)
diff --git a/src/software/logger/coloured_cout_sink_test.cpp b/src/software/logger/coloured_cout_sink_test.cpp
index 4d50442277..87d7a14f56 100644
--- a/src/software/logger/coloured_cout_sink_test.cpp
+++ b/src/software/logger/coloured_cout_sink_test.cpp
@@ -73,8 +73,7 @@ TEST_P(ColouredCoutSinkTest, testLogInfoNoDetails)
// LOG(FATAL) isn't tested because it causes a SIGABRT
INSTANTIATE_TEST_CASE_P(
All, ColouredCoutSinkTest,
- ::testing::Values(
- std::make_tuple(LEVELS(INFO), FG_Colour::WHITE),
- std::make_tuple(LEVELS(DEBUG), FG_Colour::GREEN),
- std::make_tuple(LEVELS(ROBOT_STATUS), FG_Colour::WHITE),
- std::make_tuple(LEVELS(WARNING), FG_Colour::YELLOW)));
+ ::testing::Values(std::make_tuple(LEVELS(INFO), FG_Colour::WHITE),
+ std::make_tuple(LEVELS(DEBUG), FG_Colour::GREEN),
+ std::make_tuple(LEVELS(WARNING),
+ FG_Colour::YELLOW)));
diff --git a/src/software/logger/csv_sink_test.cpp b/src/software/logger/csv_sink_test.cpp
index 98187dbe66..ad9e45693a 100644
--- a/src/software/logger/csv_sink_test.cpp
+++ b/src/software/logger/csv_sink_test.cpp
@@ -54,5 +54,4 @@ TEST_P(CSVSinkTest, test_csv_log_levels_not_logging)
INSTANTIATE_TEST_CASE_P(All, CSVSinkTest,
::testing::Values(std::make_tuple(LEVELS(INFO)),
std::make_tuple(LEVELS(DEBUG)),
- std::make_tuple(LEVELS(ROBOT_STATUS)),
std::make_tuple(LEVELS(WARNING))));
diff --git a/src/software/logger/custom_logging_levels.h b/src/software/logger/custom_logging_levels.h
index 0d62273c3c..06fad07935 100644
--- a/src/software/logger/custom_logging_levels.h
+++ b/src/software/logger/custom_logging_levels.h
@@ -8,7 +8,6 @@
// diagnostic information like battery voltage and wheel encoder errors. These messages
// are separate because they are shown separately in the FullSystemGUI.
// over radio
-const LEVELS ROBOT_STATUS{INFO.value + 1, {"ROBOT_STATUS"}};
-const LEVELS CSV{INFO.value + 2, {"CSV"}};
-const LEVELS VISUALIZE{INFO.value + 3, {"VISUALIZE"}};
-const LEVELS PLOTJUGGLER{INFO.value + 4, {"PLOTJUGGLER"}};
+const LEVELS CSV{INFO.value + 1, {"CSV"}};
+const LEVELS VISUALIZE{INFO.value + 2, {"VISUALIZE"}};
+const LEVELS PLOTJUGGLER{INFO.value + 3, {"PLOTJUGGLER"}};
diff --git a/src/software/logger/logger.h b/src/software/logger/logger.h
index 0d121383e3..87a35103b9 100644
--- a/src/software/logger/logger.h
+++ b/src/software/logger/logger.h
@@ -122,10 +122,9 @@ class LoggerSingleton
}
// levels is this vector are filtered out of the filtered log rotate sink
- std::vector filtered_level_filter = {DEBUG, VISUALIZE, CSV,
- INFO, ROBOT_STATUS, PLOTJUGGLER};
- std::vector default_level_filter = {VISUALIZE, CSV, ROBOT_STATUS,
- PLOTJUGGLER};
+ std::vector filtered_level_filter = {DEBUG, VISUALIZE, CSV, INFO,
+ PLOTJUGGLER};
+ std::vector default_level_filter = {VISUALIZE, CSV, PLOTJUGGLER};
const std::string filter_suffix = "_filtered";
const std::string log_name = "thunderbots";
std::unique_ptr logWorker;
From bc475ca6932cd3335ec82477d17f10b8eaa710d9 Mon Sep 17 00:00:00 2001
From: Minghao Li
Date: Mon, 30 Sep 2024 10:20:23 -0700
Subject: [PATCH 03/14] Remove GenevaController message (#3326)
---
src/proto/primitive.proto | 6 ------
1 file changed, 6 deletions(-)
diff --git a/src/proto/primitive.proto b/src/proto/primitive.proto
index b679a65127..c2408c8955 100644
--- a/src/proto/primitive.proto
+++ b/src/proto/primitive.proto
@@ -92,12 +92,6 @@ message PowerControl
};
}
- message GenevaControl
- {
- float angle_deg = 1;
- float rotation_speed_rpm = 2;
- }
-
ChickerControl chicker = 1;
Geneva.Slot geneva_slot = 2;
}
From 7b086f43fac2cbb2c5b0aa3eed09a15bd0389d5e Mon Sep 17 00:00:00 2001
From: Minghao Li
Date: Thu, 3 Oct 2024 09:35:02 -0700
Subject: [PATCH 04/14] Optimize replay mode seek bar stuttering (#3327)
* Add basic functions for chunk index
* Fix index building failure
* Update documentations
* Revise logging statements
* Add documentations
* Add tests
* Optimize import statements
* Remove end timestamp in index
* Fix index building function
* Add prompt message for user to enable cache function
* Revise documentation and log messages
* Fix broken test code
* Fix broken test code
* Build index when saving the clip
* Build index when saving the clip
* [pre-commit.ci lite] apply automatic fixes
* Fix type hints
* Fix PathLike
* [pre-commit.ci lite] apply automatic fixes
* Add error handling in index building
* [pre-commit.ci lite] apply automatic fixes
* Add version number
* Fix typo
* [pre-commit.ci lite] apply automatic fixes
---------
Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com>
---
.../thunderscope/replay/proto_player.py | 133 ++++++++++++++++--
src/software/thunderscope/replay/test/BUILD | 19 +++
.../replay/test/replay_corruption_test.py | 4 +-
.../replay/test/replay_indexing_test.py | 126 +++++++++++++++++
4 files changed, 269 insertions(+), 13 deletions(-)
create mode 100644 src/software/thunderscope/replay/test/replay_indexing_test.py
diff --git a/src/software/thunderscope/replay/proto_player.py b/src/software/thunderscope/replay/proto_player.py
index 1a11354372..5f6c246630 100644
--- a/src/software/thunderscope/replay/proto_player.py
+++ b/src/software/thunderscope/replay/proto_player.py
@@ -43,8 +43,12 @@ class ProtoPlayer:
"""
PLAY_PAUSE_POLL_INTERVAL_SECONDS = 0.1
+ CHUNK_INDEX_FILENAME = "chunks.index"
+ CHUNK_INDEX_FILE_VERSION = 1
- def __init__(self, log_folder_path: str, proto_unix_io: ProtoUnixIO) -> None:
+ def __init__(
+ self, log_folder_path: os.PathLike, proto_unix_io: ProtoUnixIO
+ ) -> None:
"""Creates a proto player that plays back all protos
:param log_folder_path: The path to the log file.
@@ -76,6 +80,9 @@ def __init__(self, log_folder_path: str, proto_unix_io: ProtoUnixIO) -> None:
self.sorted_chunks[0]
)
+ # build or load index for chunks
+ self.chunks_indices = self.get_chunk_index()
+
# We can get the total runtime of the log from the last entry in the last chunk
self.end_time = self.find_actual_endtime()
@@ -93,14 +100,14 @@ def __init__(self, log_folder_path: str, proto_unix_io: ProtoUnixIO) -> None:
self.error_bit_flag = ProtoPlayerFlags.NO_ERROR_FLAG
@staticmethod
- def sort_and_get_replay_files(log_folder_path):
+ def sort_and_get_replay_files(log_folder_path: os.PathLike):
"""Sorting the replay files
:param log_folder_path: the path to the folder that we are going to be sorting!
:return: the sorted replay files
"""
# Load up all replay files in the log folder
- replay_files = glob.glob(log_folder_path + f"/*.{REPLAY_FILE_EXTENSION}")
+ replay_files = glob.glob(f"{log_folder_path}/*.{REPLAY_FILE_EXTENSION}")
if len(replay_files) == 0:
raise ValueError(
@@ -109,7 +116,7 @@ def sort_and_get_replay_files(log_folder_path):
)
# Sort the files by their chunk index
- def __sort_replay_chunks(file_path: str):
+ def __sort_replay_chunks(file_path: os.PathLike):
head, tail = os.path.split(file_path)
replay_index, _ = tail.split(".")
return int(replay_index)
@@ -130,6 +137,99 @@ def is_log_entry_corrupt(log_entry: bytes, version: int) -> bool:
except Exception:
return True
+ def build_chunk_index(self, folder_path: os.PathLike) -> dict[str, float]:
+ """Build the chunk index and store the index into the index file
+
+ Note:
+ ----
+ A chunk index entry is a key value pair [filename: start timestamp]
+ - Start timestamp: timestamp of the first log entry in the chunk
+ - Filename: replay chunk file name (%d.replay)
+
+ :param folder_path: folder containing all the replay files
+
+ :return: a dictionary for mapping replay filename to the start
+ timestamp of the first entry in the chunk.
+
+ """
+ chunk_indices: dict[str, float] = dict()
+ for chunk_name in self.sorted_chunks:
+ chunk_data = ProtoPlayer.load_replay_chunk(chunk_name, self.version)
+ if chunk_data:
+ start_timestamp, _, _ = ProtoPlayer.unpack_log_entry(
+ chunk_data[0], self.version
+ )
+ chunk_indices[os.path.basename(chunk_name)] = start_timestamp
+ if chunk_indices:
+ try:
+ with open(
+ os.path.join(folder_path, ProtoPlayer.CHUNK_INDEX_FILENAME), "w"
+ ) as index_file:
+ index_file.write(
+ f"Version: {ProtoPlayer.CHUNK_INDEX_FILE_VERSION}, "
+ f"Generated on {time.time():.0f}\n"
+ )
+ for filename, start_timestamp in chunk_indices.items():
+ index_file.write(f"{start_timestamp}, {filename}\n")
+ logging.info("Created chunk index file successfully.")
+ except Exception as e:
+ logging.warning(f"Failed to build chunk index for {folder_path}: {e}")
+ else:
+ logging.warning(
+ f"Failed to build chunk index for {folder_path} : No chunk data."
+ )
+ return chunk_indices
+
+ def is_chunk_indexed(self) -> bool:
+ """Returns true if the chunk index is already built.
+
+ :return: if the chunk index file exists
+ """
+ return os.path.exists(
+ os.path.join(self.log_folder_path, ProtoPlayer.CHUNK_INDEX_FILENAME)
+ )
+
+ def load_chunk_index(self) -> dict[str, float]:
+ """Loads the chunk index file.
+
+ :return: the chunk indices.
+ """
+ chunk_indices: dict[str, float] = dict()
+ try:
+ with open(
+ os.path.join(self.log_folder_path, ProtoPlayer.CHUNK_INDEX_FILENAME),
+ "r",
+ ) as index_file:
+ # skip the first timestamp line
+ index_file.readline()
+
+ for line in index_file:
+ start_timestamp, chunk_name = line.split(",")
+ start_timestamp = float(start_timestamp)
+ chunk_name = chunk_name.strip()
+ chunk_indices[chunk_name] = start_timestamp
+ logging.info("Pre-existing chunk index file found and loaded.")
+ except Exception as e:
+ logging.warning(f"An Exception occurred when loading chunk index file {e}")
+ return chunk_indices
+
+ def get_chunk_index(self) -> dict[str, float]:
+ """Returns the chunk indices.
+ NOTE: if the chunk index was not built, this function will automatically build one.
+
+ :return: chunk indices.
+ """
+ if not self.is_chunk_indexed():
+ return self.build_chunk_index(self.log_folder_path)
+
+ try:
+ return self.load_chunk_index()
+ except Exception as e:
+ logging.warning(
+ f"Exception occurred when loading chunk index, trying to rebuild. Message: {e}"
+ )
+ return self.build_chunk_index(self.log_folder_path)
+
def is_proto_player_playing(self) -> bool:
"""Return whether or not the proto player is being played.
@@ -163,7 +263,7 @@ def find_actual_endtime(self) -> float:
return 0.0
@staticmethod
- def load_replay_chunk(replay_chunk_path: str, version: int) -> list:
+ def load_replay_chunk(replay_chunk_path: os.PathLike, version: int) -> list:
"""Reads a replay chunk.
:param replay_chunk_path: The path to the replay chunk.
@@ -202,7 +302,7 @@ def load_replay_chunk(replay_chunk_path: str, version: int) -> list:
return cached_data
@staticmethod
- def get_replay_chunk_format_version(replay_chunk_path: str) -> int:
+ def get_replay_chunk_format_version(replay_chunk_path: os.PathLike) -> int:
"""Reads a replay chunk.
:param replay_chunk_path: The path to the replay chunk.
@@ -329,6 +429,7 @@ def save_clip(self, filename: str, start_time: float, end_time: float) -> None:
self.current_entry_index += 1
if self.current_packet_time >= end_time:
logging.info("Clip saved!")
+ self.build_chunk_index(directory)
return
# Load the next chunk
self.current_chunk_index += 1
@@ -418,14 +519,24 @@ def seek(self, seek_time: float) -> None:
# Let's binary search through the chunks to find the chunk that starts
# with a timestamp less than (but closest to) the seek_time we want
# to seek to.
- def __bisect_chunks_by_timestamp(chunk: str) -> None:
- chunk = ProtoPlayer.load_replay_chunk(chunk, self.version)
- start_timestamp, _, _ = ProtoPlayer.unpack_log_entry(chunk[0], self.version)
- return start_timestamp
+ def __get_timestamp_for_chunk(chunk: str) -> float:
+ if self.chunks_indices and os.path.basename(chunk) in self.chunks_indices:
+ return self.chunks_indices[os.path.basename(chunk)]
+ else:
+ logging.warning(
+ "Use old algorithm to jump, which might result in lagging "
+ + f"Please try deleting {ProtoPlayer.CHUNK_INDEX_FILENAME} in the replay file folder"
+ + " and re-run Thunderscope to enable the indexing for faster speed!"
+ )
+ chunk = ProtoPlayer.load_replay_chunk(chunk, self.version)
+ start_timestamp, _, _ = ProtoPlayer.unpack_log_entry(
+ chunk[0], self.version
+ )
+ return start_timestamp
with self.replay_controls_mutex:
self.current_chunk_index = ProtoPlayer.binary_search(
- self.sorted_chunks, seek_time, key=__bisect_chunks_by_timestamp
+ self.sorted_chunks, seek_time, key=__get_timestamp_for_chunk
)
# Let's binary search through the entries in the chunk to find the closest
diff --git a/src/software/thunderscope/replay/test/BUILD b/src/software/thunderscope/replay/test/BUILD
index 3a9408c3ad..159879cd73 100644
--- a/src/software/thunderscope/replay/test/BUILD
+++ b/src/software/thunderscope/replay/test/BUILD
@@ -19,3 +19,22 @@ py_test(
requirement("pytest"),
],
)
+
+py_test(
+ name = "replay_indexing_test",
+ srcs = [
+ "replay_indexing_test.py",
+ ],
+ data = [
+ "//software:py_constants.so",
+ ],
+ deps = [
+ "//extlibs/er_force_sim/src/protobuf:erforce_py_proto",
+ "//proto:import_all_protos",
+ "//software:conftest",
+ "//software/thunderscope:proto_unix_io",
+ "//software/thunderscope/replay:proto_player",
+ "//software/thunderscope/replay/test:replay_corruption_test",
+ requirement("pytest"),
+ ],
+)
diff --git a/src/software/thunderscope/replay/test/replay_corruption_test.py b/src/software/thunderscope/replay/test/replay_corruption_test.py
index b54565cf06..da16eb8b2d 100644
--- a/src/software/thunderscope/replay/test/replay_corruption_test.py
+++ b/src/software/thunderscope/replay/test/replay_corruption_test.py
@@ -9,7 +9,7 @@
2. we create invalid entries of two type: the data is actually corrupted, and we are missing a delimiter. We
mixed those replay entries with valid replay entries to check if proto player could actually player those
entries
- 4. we check to see if there are uncaught exception. If there are, we know fore sure proto player wouldn't work!
+ 3. we check to see if there are uncaught exception. If there are, we know for sure proto player wouldn't work!
"""
import time
@@ -94,7 +94,7 @@ def make_part_replay_chunks(
save_path: str,
duration: float,
start_time: float,
- gen_log_entry_func: Callable[[Message, float], None],
+ gen_log_entry_func: Callable[[Message, float], str],
frequency=0.1,
):
"""Making a part of the replay chunks and appending it to the 0.replay file.
diff --git a/src/software/thunderscope/replay/test/replay_indexing_test.py b/src/software/thunderscope/replay/test/replay_indexing_test.py
new file mode 100644
index 0000000000..04513235c0
--- /dev/null
+++ b/src/software/thunderscope/replay/test/replay_indexing_test.py
@@ -0,0 +1,126 @@
+"""This test is for testing the chunk indexing function of the proto player.
+We are going to examine the index-building and index-loading functions.
+Those functions are first introduced to optimize the response time of the progress bar in the replay mode.
+"""
+
+from typing import Callable
+import math
+import shutil
+import gzip
+import os
+from google.protobuf.message import Message
+from software.py_constants import *
+
+from software.thunderscope.replay.proto_player import ProtoPlayer
+from software.thunderscope.proto_unix_io import ProtoUnixIO
+from software.simulated_tests.simulated_test_fixture import pytest_main
+from software.thunderscope.replay.test.replay_corruption_test import (
+ create_valid_log_entry,
+ create_random_proto,
+)
+
+# location to store the generated files
+TMP_REPLAY_SAVE_PATH = "/tmp/test_indexing"
+
+# number of chunk files being generated
+CHUNK_FILES_NUM = 100
+
+# number of entries per replay file
+ENTRY_NUM_PER_FILE = 100
+
+# length of time that every chunk represents
+DURATION_PER_CHUNK = 5
+
+
+def create_test_player() -> ProtoPlayer:
+ """Create a dummy protoplayer for testing
+ :return: a protoplayer for testing
+ """
+ return ProtoPlayer(TMP_REPLAY_SAVE_PATH, ProtoUnixIO())
+
+
+def cleanup():
+ """Clean up this test by deleting the replay files generated"""
+ shutil.rmtree(TMP_REPLAY_SAVE_PATH)
+
+
+def generate_chunk(
+ list_of_protos: [Message],
+ save_path: os.PathLike,
+ filename: str,
+ duration: float,
+ start_time: float,
+ gen_log_entry_func: Callable[[Message, float], str],
+):
+ """Generate one replay chunk for testing.
+
+ :param list_of_protos: the list of proto being referenced when generating log entries
+ :param save_path: directory for saving the chunk file
+ :param filename: the name of the chunk file
+ :param duration: the length of the chunk (in seconds)
+ :param start_time: the start time (in seconds)
+ :param gen_log_entry_func: function used to generate log entries
+ """
+ os.makedirs(save_path, exist_ok=True)
+ path_to_replay_file = os.path.join(save_path, filename)
+
+ with gzip.open(path_to_replay_file, "wb") as log_file:
+ for i, proto in enumerate(list_of_protos):
+ proto_time = start_time + duration / len(list_of_protos) * i
+ log_entry = gen_log_entry_func(proto, proto_time)
+ log_file.write(bytes(log_entry, encoding="utf-8"))
+
+
+def test_for_building_index_on_valid_chunks():
+ """Test building index file on correctly formatted replay files
+
+ Test steps:
+ 1. generate correctly formatted replay files (chunks)
+ 2. Build index files
+ 3. Load index into memory and validate
+ 3. Test ProtoPlayer.seek() to check if the player can jump correctly to the correct chunk
+ """
+ # generate correctly formatted replay files
+ replay_proto = []
+ for _ in range(ENTRY_NUM_PER_FILE):
+ replay_proto.append(create_random_proto())
+ for i in range(CHUNK_FILES_NUM):
+ generate_chunk(
+ replay_proto,
+ TMP_REPLAY_SAVE_PATH,
+ f"{i}.replay",
+ DURATION_PER_CHUNK,
+ DURATION_PER_CHUNK * i,
+ create_valid_log_entry,
+ )
+
+ player = create_test_player()
+
+ # generate chunk index file
+ player.build_chunk_index(player.log_folder_path)
+
+ # validate index with load index function
+ chunk_indices = player.load_chunk_index()
+ assert len(chunk_indices) == CHUNK_FILES_NUM
+ assert player.chunks_indices
+ for filename, start_timestamp in chunk_indices.items():
+ index_of_file = int(filename.replace(".replay", ""))
+ assert math.isclose(start_timestamp, DURATION_PER_CHUNK * index_of_file)
+
+ # test seeking a timestamp
+ # jump to the middle chunk
+ player.seek(DURATION_PER_CHUNK * CHUNK_FILES_NUM / 2)
+ assert player.current_chunk_index == int(CHUNK_FILES_NUM / 2)
+
+ # jump to the start chunk
+ player.seek(0.0)
+ assert player.current_chunk_index == 0
+
+ # jump to the end chunk
+ player.seek(DURATION_PER_CHUNK * CHUNK_FILES_NUM - 1)
+ assert player.current_chunk_index == CHUNK_FILES_NUM - 1
+ cleanup()
+
+
+if __name__ == "__main__":
+ pytest_main(__file__)
From 67e3c23572eb6e77da9e005c2e70dc4f771a9b19 Mon Sep 17 00:00:00 2001
From: Michael Ben-Zvi
Date: Fri, 4 Oct 2024 15:36:10 -0700
Subject: [PATCH 05/14] Raspberry Pi 5 Support (#3253)
* update gpio pin #s
* update GPIO to use new kernel interface
* wip
* wip
* new linux configs
* spi config
* partial refactor
* wip
* cleaning up ansible script
* cleaning up ansible script
* fixup pi setup ansible script
* wip
* basic stuff
* wip
* update to use new config.txt in ansible setup
* transfer with gusto!
* fixed up perms
* fix build errors and add power_service back
* fix logger build issue and run formatting
* Split new GPIO into new class
* Refactored motor for variable platform
* Added flag to tbots py
* Fixed bazel sync
* Restructured constants
* [pre-commit.ci lite] apply automatic fixes
* tracy on pi fix
* update const-ness of constants -- wip not yet complete
* Move raspberry pi build and config selection to //software/embedded
* [pre-commit.ci lite] apply automatic fixes
* add some documentation and fixup failing tests
* [pre-commit.ci lite] apply automatic fixes
* fix c++ test
* cleanup docs and tests
* remove underflow in gpio char dev implementation
* update use of Jetson word in docs
* [pre-commit.ci lite] apply automatic fixes
* fix tbots.py for flashing pi
* fix tbots.py for flashing pi
* [pre-commit.ci lite] apply automatic fixes
* Address PR comments, clean up docs and update temperature handling
* address PR comments and cleanup comments
* fixup docs
* fix mermaid diagram
* fix mermaid v2
* update mermaid to use tbots network for Pis
* fix mermaid broken
* Update useful-robot-commands.md
* Update useful-robot-commands.md
* remove unintentional changes
* fix ansible requirements build failure
* Merge in rpi IP changes from robocup
* Update useful-robot-commands.md
* Fix wrong cpu temp path in jetson_constants.h
* Add network connection test from robocup
* [pre-commit.ci lite] apply automatic fixes
* Fix failing build
* update thunderloop build issue
* [pre-commit.ci lite] apply automatic fixes
---------
Co-authored-by: arun
Co-authored-by: arun
Co-authored-by: Nima Zareian
Co-authored-by: pre-commit-ci-lite[bot] <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com>
Co-authored-by: itsarune <42703774+itsarune@users.noreply.github.com>
Co-authored-by: williamckha
---
.github/workflows/main.yml | 20 +-
docs/getting-started.md | 33 ++-
docs/robot-software-architecture.md | 16 +-
docs/useful-robot-commands.md | 85 +++---
scripts/compile_pip_requirements.sh | 2 +-
src/MODULE.bazel | 1 +
src/WORKSPACE | 2 +-
src/extlibs/tracy.BUILD | 1 +
src/shared/BUILD | 16 +-
src/shared/constants.h | 7 +-
src/software/{jetson_nano => embedded}/BUILD | 56 +++-
.../{jetson_nano => embedded}/ansible/BUILD | 22 +-
.../ansible/playbooks/deploy_powerboard.yml | 0
.../playbooks/deploy_robot_software.yml} | 26 --
.../ansible/playbooks/example_playbook.yml | 0
.../ansible/playbooks/misc.yml | 0
.../playbooks/robot_auto_test_playbook.yml | 0
.../embedded/ansible/playbooks/setup_nano.yml | 137 ++++++++++
.../embedded/ansible/playbooks/setup_pi.yml | 128 +++++++++
.../ansible/playbooks/shared_setup.yml | 6 +
.../ansible/requirements.in | 0
.../ansible/requirements_lock.txt | 4 +-
.../ansible/run_ansible.py | 0
.../ansible/tasks/add_user_to_dialout.yml | 8 +
.../embedded/ansible/tasks/check_internet.yml | 8 +
.../enable_password_less_sudo_for_rsync.yml | 10 +
.../embedded/ansible/tasks/reboot.yml | 14 +
.../ansible/tasks/set_user_hostname.yml | 18 ++
.../tasks/setup_robot_software_deps.yml | 30 +++
.../embedded/ansible/tasks/setup_systemd.yml | 41 +++
.../ansible/tasks/setup_wifi_interface.yml | 20 ++
.../battery_test.cpp | 2 +-
src/software/embedded/constants/BUILD | 17 ++
src/software/embedded/constants/constants.h | 7 +
.../embedded/constants/jetson_constants.h | 13 +
.../embedded/constants/pi_constants.h | 13 +
src/software/embedded/gpio.h | 25 ++
src/software/embedded/gpio_char_dev.cpp | 111 ++++++++
src/software/embedded/gpio_char_dev.h | 54 ++++
.../gpio.cpp => embedded/gpio_sysfs.cpp} | 11 +-
.../gpio.h => embedded/gpio_sysfs.h} | 21 +-
src/software/embedded/linux_configs/BUILD | 1 +
.../embedded/linux_configs/jetson_nano/BUILD | 9 +
.../jetson_nano}/device_tree.zip | Bin
.../linux_configs/jetson_nano}/extlinux.conf | 0
src/software/embedded/linux_configs/pi/BUILD | 9 +
.../embedded/linux_configs/pi/config.txt | 50 ++++
.../embedded/linux_configs/pi/setup.zip | Bin 0 -> 639512 bytes
.../systemd/10-persistent-wifi5-net.link | 5 +
.../systemd/10-persistent-wifi6-net.link | 5 +
.../linux_configs/systemd/BUILD | 1 +
.../linux_configs/systemd/thunderloop.service | 0
src/software/embedded/platform.h | 5 +
.../primitive_executor.cpp | 2 +-
.../primitive_executor.h | 0
.../{jetson_nano => embedded}/redis/BUILD | 0
.../redis/redis_client.cpp | 2 +-
.../redis/redis_client.h | 0
.../redis/redis_client_test.cpp | 2 +-
.../{jetson_nano => embedded}/services/BUILD | 10 +-
.../services/motor.cpp | 54 ++--
.../services/motor.h | 53 +++-
.../services/network/BUILD | 0
.../services/network/network.cpp | 2 +-
.../services/network/network.h | 2 +-
.../services/network/proto_tracker.cpp | 2 +-
.../services/network/proto_tracker.h | 0
.../services/network/proto_tracker_test.cpp | 2 +-
.../services/power.cpp | 2 +-
.../services/power.h | 0
.../services/robot_auto_test.cpp | 29 ++-
.../setup_robot_software_deps.sh} | 7 +-
.../{jetson_nano => embedded}/thunderloop.cpp | 35 ++-
.../{jetson_nano => embedded}/thunderloop.h | 20 +-
.../thunderloop_main.cpp | 2 +-
.../ansible/playbooks/setup_nano.yml | 242 ------------------
src/software/jetson_nano/linux_configs/BUILD | 11 -
.../linux_configs/systemd/thunderbots.service | 16 --
src/software/logger/coloured_cout_sink.cpp | 17 +-
src/software/logger/coloured_cout_sink.h | 4 +-
src/software/logger/logger.h | 17 +-
src/software/simulation/BUILD | 2 +-
src/software/simulation/er_force_simulator.h | 2 +-
src/tbots.py | 22 +-
84 files changed, 1134 insertions(+), 495 deletions(-)
create mode 100644 src/MODULE.bazel
rename src/software/{jetson_nano => embedded}/BUILD (61%)
rename src/software/{jetson_nano => embedded}/ansible/BUILD (62%)
rename src/software/{jetson_nano => embedded}/ansible/playbooks/deploy_powerboard.yml (100%)
rename src/software/{jetson_nano/ansible/playbooks/deploy_nano.yml => embedded/ansible/playbooks/deploy_robot_software.yml} (73%)
rename src/software/{jetson_nano => embedded}/ansible/playbooks/example_playbook.yml (100%)
rename src/software/{jetson_nano => embedded}/ansible/playbooks/misc.yml (100%)
rename src/software/{jetson_nano => embedded}/ansible/playbooks/robot_auto_test_playbook.yml (100%)
create mode 100644 src/software/embedded/ansible/playbooks/setup_nano.yml
create mode 100644 src/software/embedded/ansible/playbooks/setup_pi.yml
create mode 100644 src/software/embedded/ansible/playbooks/shared_setup.yml
rename src/software/{jetson_nano => embedded}/ansible/requirements.in (100%)
rename src/software/{jetson_nano => embedded}/ansible/requirements_lock.txt (99%)
rename src/software/{jetson_nano => embedded}/ansible/run_ansible.py (100%)
create mode 100644 src/software/embedded/ansible/tasks/add_user_to_dialout.yml
create mode 100644 src/software/embedded/ansible/tasks/check_internet.yml
create mode 100644 src/software/embedded/ansible/tasks/enable_password_less_sudo_for_rsync.yml
create mode 100644 src/software/embedded/ansible/tasks/reboot.yml
create mode 100644 src/software/embedded/ansible/tasks/set_user_hostname.yml
create mode 100644 src/software/embedded/ansible/tasks/setup_robot_software_deps.yml
create mode 100644 src/software/embedded/ansible/tasks/setup_systemd.yml
create mode 100644 src/software/embedded/ansible/tasks/setup_wifi_interface.yml
rename src/software/{jetson_nano => embedded}/battery_test.cpp (94%)
create mode 100644 src/software/embedded/constants/BUILD
create mode 100644 src/software/embedded/constants/constants.h
create mode 100644 src/software/embedded/constants/jetson_constants.h
create mode 100644 src/software/embedded/constants/pi_constants.h
create mode 100644 src/software/embedded/gpio.h
create mode 100644 src/software/embedded/gpio_char_dev.cpp
create mode 100644 src/software/embedded/gpio_char_dev.h
rename src/software/{jetson_nano/gpio.cpp => embedded/gpio_sysfs.cpp} (83%)
rename src/software/{jetson_nano/gpio.h => embedded/gpio_sysfs.h} (61%)
create mode 100644 src/software/embedded/linux_configs/BUILD
create mode 100644 src/software/embedded/linux_configs/jetson_nano/BUILD
rename src/software/{jetson_nano/linux_configs => embedded/linux_configs/jetson_nano}/device_tree.zip (100%)
rename src/software/{jetson_nano/linux_configs => embedded/linux_configs/jetson_nano}/extlinux.conf (100%)
create mode 100644 src/software/embedded/linux_configs/pi/BUILD
create mode 100755 src/software/embedded/linux_configs/pi/config.txt
create mode 100644 src/software/embedded/linux_configs/pi/setup.zip
create mode 100644 src/software/embedded/linux_configs/systemd/10-persistent-wifi5-net.link
create mode 100644 src/software/embedded/linux_configs/systemd/10-persistent-wifi6-net.link
rename src/software/{jetson_nano => embedded}/linux_configs/systemd/BUILD (88%)
rename src/software/{jetson_nano => embedded}/linux_configs/systemd/thunderloop.service (100%)
create mode 100644 src/software/embedded/platform.h
rename src/software/{jetson_nano => embedded}/primitive_executor.cpp (99%)
rename src/software/{jetson_nano => embedded}/primitive_executor.h (100%)
rename src/software/{jetson_nano => embedded}/redis/BUILD (100%)
rename src/software/{jetson_nano => embedded}/redis/redis_client.cpp (98%)
rename src/software/{jetson_nano => embedded}/redis/redis_client.h (100%)
rename src/software/{jetson_nano => embedded}/redis/redis_client_test.cpp (98%)
rename src/software/{jetson_nano => embedded}/services/BUILD (80%)
rename src/software/{jetson_nano => embedded}/services/motor.cpp (96%)
rename src/software/{jetson_nano => embedded}/services/motor.h (89%)
rename src/software/{jetson_nano => embedded}/services/network/BUILD (100%)
rename src/software/{jetson_nano => embedded}/services/network/network.cpp (98%)
rename src/software/{jetson_nano => embedded}/services/network/network.h (98%)
rename src/software/{jetson_nano => embedded}/services/network/proto_tracker.cpp (96%)
rename src/software/{jetson_nano => embedded}/services/network/proto_tracker.h (100%)
rename src/software/{jetson_nano => embedded}/services/network/proto_tracker_test.cpp (96%)
rename src/software/{jetson_nano => embedded}/services/power.cpp (97%)
rename src/software/{jetson_nano => embedded}/services/power.h (100%)
rename src/software/{jetson_nano => embedded}/services/robot_auto_test.cpp (79%)
rename src/software/{jetson_nano/setup_nano.sh => embedded/setup_robot_software_deps.sh} (81%)
rename src/software/{jetson_nano => embedded}/thunderloop.cpp (95%)
rename src/software/{jetson_nano => embedded}/thunderloop.h (91%)
rename src/software/{jetson_nano => embedded}/thunderloop_main.cpp (98%)
delete mode 100644 src/software/jetson_nano/ansible/playbooks/setup_nano.yml
delete mode 100644 src/software/jetson_nano/linux_configs/BUILD
delete mode 100644 src/software/jetson_nano/linux_configs/systemd/thunderbots.service
diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml
index 815c00e5b4..3e0758ddcb 100644
--- a/.github/workflows/main.yml
+++ b/.github/workflows/main.yml
@@ -30,12 +30,17 @@ jobs:
-//software/simulated_tests/... \
-//software/ai/hl/... \
-//software/field_tests/... \
- -//software/jetson_nano/... \
+ -//software/embedded/... \
- name: Jetson Nano Build Test
run: |
cd src
- bazel build --cpu=jetson_nano //software/jetson_nano:thunderloop_main --copt=-O3
+ bazel build --cpu=jetson_nano //software/embedded:thunderloop_main --copt=-O3 --//software/embedded:host_platform=NANO
+
+ - name: Raspberry Pi Build Test
+ run: |
+ cd src
+ bazel build --cpu=jetson_nano //software/embedded:thunderloop_main --copt=-O3 --//software/embedded:host_platform=PI
software-tests:
name: Software Tests
@@ -58,8 +63,8 @@ jobs:
-//software/field_tests/... \
-//software/ai/navigator/...
- jetson-nano-tests:
- name: Jetson Nano Software Tests
+ robot-tests:
+ name: Robot Software Tests
runs-on: ubuntu-20.04
steps:
# checks-out the repository under $GITHUB_WORKSPACE
@@ -85,7 +90,12 @@ jobs:
- name: Jetson Nano Build
run: |
cd src
- bazel build --cpu=jetson_nano //software/jetson_nano:thunderloop_main --copt=-O3
+ bazel build --cpu=jetson_nano //software/embedded:thunderloop_main --copt=-O3 --//software/embedded:host_platform=NANO
+
+ - name: Raspberry Pi Build
+ run: |
+ cd src
+ bazel build --cpu=jetson_nano //software/embedded:thunderloop_main --copt=-O3 --//software/embedded:host_platform=PI
simulated-gameplay-tests:
name: Simulated Gameplay Tests
diff --git a/docs/getting-started.md b/docs/getting-started.md
index c9aea63a5a..59dd069ed1 100644
--- a/docs/getting-started.md
+++ b/docs/getting-started.md
@@ -1,10 +1,5 @@
-Table of Contents
-=================
-
+Table of Contents
+=================
* [Software Setup](#software-setup)
* [Introduction](#introduction)
@@ -30,8 +25,8 @@ Table of Contents
* [Profiling](#profiling)
* [Callgrind](#callgrind)
* [Tracy](#tracy)
- * [Building for Jetson Nano](#building-for-jetson-nano)
- * [Deploying to Jetson Nano](#deploying-to-jetson-nano)
+ * [Building for the robot](#building-for-the-robot)
+ * [Deploying Robot Software to the robot](#deploying-robot-software-to-the-robot)
* [Setting up Virtual Robocup 2021](#setting-up-virtual-robocup-2021)
* [Setting up the SSL Simulation Environment](#setting-up-the-ssl-simulation-environment)
* [Pushing a Dockerfile to dockerhub](#pushing-a-dockerfile-to-dockerhub)
@@ -49,6 +44,14 @@ Table of Contents
* [Example Workflow](#example-workflow)
* [Testing](#testing)
+
+
# Software Setup
## Introduction
@@ -339,17 +342,19 @@ Tracy also samples call stacks. If the profiled binary is run with root permissi
./tbots.py run thunderscope_main --tracy --sudo
-## Building for Jetson Nano
+## Building for the robot
-To build for the Jetson Nano, build the target with the `--cpu=jetson_nano` flag and the toolchain will automatically build using the ARM toolchain for Jetson Nano. For example, `bazel build --cpu=jetson_nano //software/geom/...`.
+To build for the robot computer, build the target with the `--cpu=jetson_nano` flag and the toolchain will automatically build using the ARM toolchain. For example, `bazel build --cpu=jetson_nano //software/geom/...`.
-## Deploying to Jetson Nano
+## Deploying Robot Software to the robot
-We use ansible to automatically update software running on the Jetson Nano. [More info here.](useful-robot-commands.md#flashing-the-nano)
+We use Ansible to automatically update software running on the robot. [More info here.](useful-robot-commands.md#flashing-the-robots-compute-module)
To update binaries on a working robot, you can run:
-`bazel run //software/jetson_nano/ansible:run_ansible --cpu=jetson_nano -- --playbook deploy_nano.yml --hosts --ssh_pass `
+`bazel run //software/embedded/ansible:run_ansible --cpu=jetson_nano --//software/embedded:host_platform= -- --playbook deploy_robot_software.yml --hosts --ssh_pass `
+
+Where `` is the robot platform you are deploying to (`PI` or `NANO`), and `` is the IP address of the robot you are deploying to. The `robot_password` is the password used to login to the `robot` user on the robot.
## Setting up Virtual Robocup 2021
diff --git a/docs/robot-software-architecture.md b/docs/robot-software-architecture.md
index 8bd11d84d7..f806e76e1c 100644
--- a/docs/robot-software-architecture.md
+++ b/docs/robot-software-architecture.md
@@ -16,11 +16,11 @@
## Ansible
-[Ansible](https://www.ansible.com/overview/how-ansible-works) allows us to run actions on multiple robots at once. Actions are communicated through YAML files called playbooks. Playbooks contain a series of tasks (ex move a file, run this script, output this command) and logic dictating dependencies between tasks. When playbooks are run, Ansible establishes an SSH connection between the user's computer and target Jetson Nanos, allowing it to run the tasks in the playbook. Output from each task, and any other requested output, is displayed on the console
+[Ansible](https://www.ansible.com/overview/how-ansible-works) allows us to run actions on multiple robots at once. Actions are communicated through YAML files called playbooks. Playbooks contain a series of tasks (eg. move a file, run this script, output this command) and logic dictating dependencies between tasks. When playbooks are run, Ansible establishes an SSH connection between the user's computer and robot, allowing it to run the tasks in the playbook. Output from each task, and any other requested output, is displayed on the console
For a more detailed look at how Ansible works, [see the RFC](https://docs.google.com/document/d/1hN3Us2Vjr8z6ihqUVp_3L7rrjKc-EZ-l2hZJc31gNOc/edit)
-Example command: `bazel run //software/jetson_nano/ansible:run_ansible --cpu=jetson_nano -- --playbook deploy_nano.yml --hosts --ssh_pass `
+Example command: `bazel run //software/embedded/ansible:run_ansible --cpu=jetson_nano --//software/embedded:host_platform= -- --playbook deploy_robot_software.yml --hosts --ssh_pass `
More commands available [here](useful-robot-commands.md#off-robot-commands)
@@ -36,14 +36,6 @@ To learn more about how it works, [see the RFC](https://docs.google.com/document
# Thunderloop
-Thunderloop is software that runs in a loop. It continuously polls services (unrelated from Systemd) sending relevant control proto (`PowerControl`, `MotorControl`) and receiving back status proto. Currently we have a [Network](https://github.com/UBC-Thunderbots/Software/blob/master/src/software/jetson_nano/services/network/network.cpp), [Power](https://github.com/UBC-Thunderbots/Software/blob/master/src/software/jetson_nano/services/power.cpp) and [Motor service](https://github.com/UBC-Thunderbots/Software/blob/master/src/software/jetson_nano/services/motor.cpp). Thunderloop also receives `World` and `PrimitiveSet` Proto from AI and sends back Robot Status.
+Thunderloop is software that runs in a loop. It continuously polls services (unrelated from Systemd), sending relevant control protos (`PowerControl`, `MotorControl`) and receiving back status protos from the power and motor boards. Currently we have a [Network](https://github.com/UBC-Thunderbots/Software/blob/master/src/software/embedded/services/network/network.cpp), [Power](https://github.com/UBC-Thunderbots/Software/blob/master/src/software/embedded/services/power.cpp) and [Motor](https://github.com/UBC-Thunderbots/Software/blob/master/src/software/embedded/services/motor.cpp) services. Thunderloop also receives `World` and `PrimitiveSet` protos from AI and sends back `Robot Status` protos.
-Motor and Power service both interface with their respective electrical boards over different communication interfaces, namely SPI and UART respectively.
-
-# Announcements
-
-Broadcasts packets for what robot IDs are connected to wifi and what IPs they have. Combined with `robot_broadcast_receiver.py` allows us to see all robots on the network.
-
-# Display
-
-Displays information about the robot for us. Uses redis to receive info from other processes. Can also change values with physical buttons.
+Motor and Power services both interface with their respective electrical boards over different communication interfaces, namely SPI and UART respectively.
diff --git a/docs/useful-robot-commands.md b/docs/useful-robot-commands.md
index e5399fa8d6..5dd76fdc69 100644
--- a/docs/useful-robot-commands.md
+++ b/docs/useful-robot-commands.md
@@ -1,32 +1,40 @@
-# Common Robot Commands
+Table of Contents
+=================
-# Table of Contents
+* [Table of Contents](#table-of-contents)
* [Common Debugging Steps](#common-debugging-steps)
* [Off Robot Commands](#off-robot-commands)
* [Wifi Disclaimer](#wifi-disclaimer)
- * [Miscellaneous Ansible Tasks & Options](#miscellaneous-ansible-tasks--options)
- * [Flashing the nano](#flashing-the-nano)
+ * [Miscellaneous Ansible Tasks & Options](#miscellaneous-ansible-tasks--options)
+ * [Flashing the robot's compute module](#flashing-the-robots-compute-module)
* [Flashing the powerboard](#flashing-the-powerboard)
- * [Setting up nano](#setting-up-nano)
+ * [Setting up the embedded host](#setting-up-the-embedded-host)
+ * [Jetson Nano](#jetson-nano)
+ * [Raspberry Pi](#raspberry-pi)
* [Robot Diagnostics](#robot-diagnostics)
+ * [For Just Diagnostics](#for-just-diagnostics)
+ * [For AI + Diagnostics](#for-ai--diagnostics)
* [Robot Auto Test](#robot-auto-test)
* [On Robot Commands](#on-robot-commands)
* [Systemd Services](#systemd-services)
* [Debugging Uart](#debugging-uart)
* [Redis](#redis)
+
+
+
# Common Debugging Steps
```mermaid
---
title: Robot Debugging Steps
---
flowchart TD
- ssh(Can you SSH into the robot?
- `ssh robot@192.168.0.20RobotID` OR `ssh robot@robot_name.local`
- E.g. `ssh robot@192.168.0.203` or `ssh robot@robert.local`
- for a robot called robert with robot id 3)
+ ssh("Can you SSH into the robot?
+ `ssh robot@192.168.0.20RobotID` (for Nanos) OR `ssh robot@192.168.5.20RobotID` (for Pis) OR `ssh robot@robot_name.local`
+ e.g. `ssh robot@192.168.0.203` (for Nanos) or `ssh robot@192.168.5.203` (for Pis) or `ssh robot@robert.local`
+ for a robot called robert with robot id 3")
ssh ---> |Yes| tloop_status
- ssh --> |No - Second Try| monitor("`Connect Jetson to an external monitor and check wifi connection _or_ SSH using an ethernet cable`")
+ ssh --> |No - Second Try| monitor("Connect Jetson or Pi to an external monitor and check wifi connection or SSH using an ethernet cable")
ssh --> |No - First Try| restart(Restart robot)
restart --> ssh
@@ -53,12 +61,12 @@ flowchart TD
`service thunderloop restart`)
tloop_status --> |Running| tloop_logs(Check Thunderloop logs for errors
`journalctl -fu thunderloop -n 300`)
- tloop_logs --> |No Errors| check_redis(Does `redis-cli get /network_interface` return 'wlan0',
+ tloop_logs --> |No Errors| check_redis(Does `redis-cli get /network_interface` return 'wlan0' or 'tbots',
and does `redis-cli get /channel_id` return '0'?)
- tloop_logs --> |Contains Errors| rip2("`Fix errors or check errors with a lead`")
- check_redis --> |No| update_redis(Update Redis constants by running:
- `redis-cli set /network_interface 'wlan0'`
- `redis-cli set /channel_id '0'`)
+ tloop_logs --> |Contains Errors| rip2("Fix errors or check errors with a lead")
+ check_redis --> |No| update_redis("Update Redis constants by running:
+ `redis-cli set /network_interface 'wlan0'` (for Nanos) OR `redis-cli set /network_interface 'tbots'` (for Pis)
+ `redis-cli set /channel_id '0'`")
check_redis --> |Yes| rip3(Check with a lead)
update_redis --> tloop_restart
tloop_restart --> tloop_status
@@ -79,13 +87,13 @@ The IP address of the robots on the tbots network is `192.168.0.20` so
Individual miscellaneous tasks (ex reboot, shutdown, rtt test) can be run through the `misc.yml` playbook by specifying the corresponding tag.
To view a list of supported arguments, run
-`bazel run //software/jetson_nano/ansible:run_ansible --cpu=jetson_nano -- -h`
+`bazel run //software/embedded/ansible:run_ansible --cpu=jetson_nano -- -h`
If desired, the `-ho`, `--hosts` argument can be replaced with `-p`, `--port`, defining a port to listen to for Announcements from hosts.
The `--tags` argument can be used to specify which actions to perform and on which services.
-## Flashing the nano
+## Flashing the robot's compute module
This will stop the current Systemd services, replace and restart them. Binaries that are used for Systemd services are located in a folder in `home/robot` (the default directory) called `thunderbots_binaries`.
@@ -93,15 +101,21 @@ This will stop the current Systemd services, replace and restart them. Binaries
This will trigger motor calibration meaning the wheels may spin. Please elevate the robot so the wheels are not touching the ground for proper calibration.
-`bazel run //software/jetson_nano/ansible:run_ansible --cpu=jetson_nano -- --playbook deploy_nano.yml --hosts --ssh_pass `
+`bazel run //software/embedded/ansible:run_ansible --cpu=jetson_nano --//software/embedded:host_platform= -- --playbook deploy_robot_software.yml --hosts --ssh_pass `
+* \ is the host platform on the robot (either `PI` or `NANO`)
+* is the IP address of the robot
+*