phyelds-argos 8.9.0__tar.gz

phyelds_argos-8.9.0/PKG-INFO

@@ -0,0 +1,75 @@
+Metadata-Version: 2.4
+Name: phyelds-argos
+Version: 8.9.0
+Summary: Aggregate programs on ARGoS robots, communicating over simulated radios
+License: Apache-2.0
+Author: Gianluca Aguzzi
+Author-email: gianluca.aguzzi@unibo.it
+Requires-Python: >=3.12,<3.14
+Classifier: License :: OSI Approved :: Apache Software License
+Classifier: Programming Language :: Python :: 3
+Classifier: Programming Language :: Python :: 3.12
+Classifier: Programming Language :: Python :: 3.13
+Requires-Dist: phyelds (>=8.9.0,<9.0.0)
+Project-URL: Repository, https://github.com/phyelds/phyelds
+Description-Content-Type: text/markdown
+
+# phyelds-argos
+
+Run [phyelds](https://github.com/phyelds/phyelds) aggregate programs on any ARGoS3 robot:
+foot-bot, e-puck, eye-bot, Spiri, Pi-Puck, BuilderBot, drone, block or prototype.
+ARGoS owns the robots, physics and radios. Each robot runs a `phyelds.vm.device.Device`;
+its bytes travel through the robot's simulated radio, one round per tick.
+
+Install this package into Python 3.12 or 3.13, with ARGoS3, a C++17 compiler and
+CMake 3.18+ installed. Build the plugin with the same Python that has your programs:
+
+```bash
+python -m phyelds.argos.build --build-dir build
+# Without Qt/OpenGL (the controller still drives the LEDs):
+python -m phyelds.argos.build --build-dir build-headless --no-gui
+```
+
+The package ships C++ sources, not a binary tied to a particular ARGoS/Python ABI.
+The command compiles `libphyelds_argos.so` and, unless `--no-gui` is used,
+`libphyelds_argos_qt.so`. It uses the calling interpreter and its development
+headers/shared library, rather than whichever `python3` happens to be on the PATH.
+Rebuild after moving or replacing that Python environment.
+
+In an ARGoS experiment use `phyelds_controller`, with
+`library="build/libphyelds_argos"` and `<params program="your_module:your_function"
+source="0" max_speed="0.1" />`. Robot names must end in a unique unsigned 32-bit
+integer, for example `fb0`, `fb1`. The controller uses the devices the experiment
+declares: it needs the `positioning` sensor and a `range_and_bearing` or
+`simple_radios` radio (actuator and sensor); motion (`differential_steering`,
+`pipuck_differential_drive`, `builderbot_differential_drive`, `quadrotor_speed`,
+`drone_flight_system`), proximity sensors and LEDs are optional, and outputs a robot
+cannot act on are ignored. Positioning supplies **yaw only**, unless `gps="true"`.
+
+Programs read `time` (seconds), `source` and `heading`; `obstacle` with a proximity
+sensor or rangefinders; the `neighbors_distances` and `neighbors_vectors` maps, keyed
+by numeric robot ID, with the range-and-bearing radio; and `position` with
+`gps="true"`, which simple radios need for geometry. Distances are metres; vectors and
+headings share the global compass frame. The standard geometry helpers use the
+measured sensors without GPS.
+
+Programs write `velocity` (an iterable with two or three components, m/s), `led`
+(three integer RGB channels, 0–255) and `label` (text), through `store`. Missing
+outputs mean zero velocity, black LEDs and an empty label. With the GUI enabled,
+`<user_functions library="build/libphyelds_argos_qt" label="phyelds_qtuser_functions" />`
+draws the LED colour as a halo and the label above each robot.
+
+Radio frames are `[u32 sender][u16 length][Device payload]`, with the header encoded by
+ARGoS `CByteArray`, padded with zeros on the range-and-bearing radio. Compact paths are
+used by default; `paths="debug"` is available for debugging, but all communicating
+robots must use the same mode and program version. Use a sufficiently large
+`rab_data_size` (4096 in the examples).
+Oversize messages fail explicitly; they are never truncated. Bad frame lengths are
+ignored. Payload/codec errors stop the experiment with the robot ID and Python traceback.
+`trace="true"` logs each robot's label after every round for diagnostics.
+
+See the [integration guide](https://phyelds.github.io/phyelds/integration/argos/) for
+an experiment and the five-example project. The host build needs no Boost.Python,
+pybind11, FCPP or phyelds simulator. Python execution is protected by the GIL even
+when ARGoS uses worker threads; individual rounds are not executed in parallel.
+

phyelds_argos-8.9.0/README.md

@@ -0,0 +1,58 @@
+# phyelds-argos
+
+Run [phyelds](https://github.com/phyelds/phyelds) aggregate programs on any ARGoS3 robot:
+foot-bot, e-puck, eye-bot, Spiri, Pi-Puck, BuilderBot, drone, block or prototype.
+ARGoS owns the robots, physics and radios. Each robot runs a `phyelds.vm.device.Device`;
+its bytes travel through the robot's simulated radio, one round per tick.
+
+Install this package into Python 3.12 or 3.13, with ARGoS3, a C++17 compiler and
+CMake 3.18+ installed. Build the plugin with the same Python that has your programs:
+
+```bash
+python -m phyelds.argos.build --build-dir build
+# Without Qt/OpenGL (the controller still drives the LEDs):
+python -m phyelds.argos.build --build-dir build-headless --no-gui
+```
+
+The package ships C++ sources, not a binary tied to a particular ARGoS/Python ABI.
+The command compiles `libphyelds_argos.so` and, unless `--no-gui` is used,
+`libphyelds_argos_qt.so`. It uses the calling interpreter and its development
+headers/shared library, rather than whichever `python3` happens to be on the PATH.
+Rebuild after moving or replacing that Python environment.
+
+In an ARGoS experiment use `phyelds_controller`, with
+`library="build/libphyelds_argos"` and `<params program="your_module:your_function"
+source="0" max_speed="0.1" />`. Robot names must end in a unique unsigned 32-bit
+integer, for example `fb0`, `fb1`. The controller uses the devices the experiment
+declares: it needs the `positioning` sensor and a `range_and_bearing` or
+`simple_radios` radio (actuator and sensor); motion (`differential_steering`,
+`pipuck_differential_drive`, `builderbot_differential_drive`, `quadrotor_speed`,
+`drone_flight_system`), proximity sensors and LEDs are optional, and outputs a robot
+cannot act on are ignored. Positioning supplies **yaw only**, unless `gps="true"`.
+
+Programs read `time` (seconds), `source` and `heading`; `obstacle` with a proximity
+sensor or rangefinders; the `neighbors_distances` and `neighbors_vectors` maps, keyed
+by numeric robot ID, with the range-and-bearing radio; and `position` with
+`gps="true"`, which simple radios need for geometry. Distances are metres; vectors and
+headings share the global compass frame. The standard geometry helpers use the
+measured sensors without GPS.
+
+Programs write `velocity` (an iterable with two or three components, m/s), `led`
+(three integer RGB channels, 0–255) and `label` (text), through `store`. Missing
+outputs mean zero velocity, black LEDs and an empty label. With the GUI enabled,
+`<user_functions library="build/libphyelds_argos_qt" label="phyelds_qtuser_functions" />`
+draws the LED colour as a halo and the label above each robot.
+
+Radio frames are `[u32 sender][u16 length][Device payload]`, with the header encoded by
+ARGoS `CByteArray`, padded with zeros on the range-and-bearing radio. Compact paths are
+used by default; `paths="debug"` is available for debugging, but all communicating
+robots must use the same mode and program version. Use a sufficiently large
+`rab_data_size` (4096 in the examples).
+Oversize messages fail explicitly; they are never truncated. Bad frame lengths are
+ignored. Payload/codec errors stop the experiment with the robot ID and Python traceback.
+`trace="true"` logs each robot's label after every round for diagnostics.
+
+See the [integration guide](https://phyelds.github.io/phyelds/integration/argos/) for
+an experiment and the five-example project. The host build needs no Boost.Python,
+pybind11, FCPP or phyelds simulator. Python execution is protected by the GIL even
+when ARGoS uses worker threads; individual rounds are not executed in parallel.

phyelds_argos-8.9.0/pyproject.toml

@@ -0,0 +1,21 @@
+[tool.poetry]
+name = "phyelds-argos"
+version = "8.9.0"
+description = "Aggregate programs on ARGoS robots, communicating over simulated radios"
+license = "Apache-2.0"
+authors = ["Gianluca Aguzzi <gianluca.aguzzi@unibo.it>", "Davide Domini <davide.domini@unibo.it>"]
+repository = "https://github.com/phyelds/phyelds"
+readme = "README.md"
+packages = [{ include = "phyelds/argos", from = "src" }]
+include = [{ path = "src/phyelds/argos/native/**", format = ["sdist", "wheel"] }]
+
+[tool.poetry.dependencies]
+python = ">=3.12,<3.14"
+phyelds = "^8.9.0"
+
+[tool.pytest.ini_options]
+testpaths = ["tests"]
+
+[build-system]
+requires = ["poetry-core>=2.0.0,<3.0.0"]
+build-backend = "poetry.core.masonry.api"

phyelds_argos-8.9.0/src/phyelds/argos/__init__.py

@@ -0,0 +1 @@
+"""ARGoS integration: build the native host with ``python -m phyelds.argos.build``."""

phyelds_argos-8.9.0/src/phyelds/argos/build.py

@@ -0,0 +1,47 @@
+"""Build the ARGoS plugin against the Python running this command."""
+import argparse
+from pathlib import Path
+import subprocess
+import sys
+
+
+def build_plugin(build_dir: Path, *, gui: bool = True) -> Path:
+    """Compile the host and return the controller plugin path.
+
+    Args:
+        build_dir: Directory for CMake artifacts and the compiled plugins.
+        gui: Whether to also build the Qt/OpenGL halo and label renderer.
+
+    Returns:
+        The absolute path of the controller library.
+
+    Raises:
+        subprocess.CalledProcessError: If configuration or compilation fails.
+        FileNotFoundError: If CMake or the built controller cannot be found.
+    """
+    build_dir = Path(build_dir).absolute()
+    source = Path(__file__).parent / "native"
+    subprocess.run([
+        "cmake", "-S", str(source), "-B", str(build_dir),
+        f"-DPython3_EXECUTABLE={sys.executable}", "-DCMAKE_BUILD_TYPE=Release",
+        f"-DPHYELDS_ARGOS_GUI={'ON' if gui else 'OFF'}",
+    ], check=True)
+    subprocess.run(["cmake", "--build", str(build_dir), "--parallel"], check=True)
+    suffix = ".dylib" if sys.platform == "darwin" else ".so"
+    plugin = build_dir / f"libphyelds_argos{suffix}"
+    if not plugin.is_file():
+        raise FileNotFoundError(plugin)
+    return plugin
+
+
+def main() -> None:
+    """Compile the plugins using command-line options."""
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--build-dir", type=Path, default=Path("build"))
+    parser.add_argument("--no-gui", action="store_true", help="do not require Qt/OpenGL")
+    args = parser.parse_args()
+    print(build_plugin(args.build_dir, gui=not args.no_gui))
+
+
+if __name__ == "__main__":
+    main()

phyelds_argos-8.9.0/src/phyelds/argos/native/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.18)
+project(phyelds_argos CXX)
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+# ARGoSConfig.cmake looks for the modules installed beside it only under the system
+# prefixes: an ARGoS found through CMAKE_PREFIX_PATH would not find its own modules.
+find_path(ARGOS_CMAKE_DIR FindARGoSQTOpenGL.cmake
+    PATHS ${CMAKE_PREFIX_PATH} ENV CMAKE_PREFIX_PATH
+    PATH_SUFFIXES share/argos3/cmake NO_DEFAULT_PATH)
+find_package(ARGoS REQUIRED)
+# Find both together so an interpreter can never be paired with another Python's ABI.
+set(Python3_USE_STATIC_LIBS FALSE)
+find_package(Python3 3.12 REQUIRED COMPONENTS Interpreter Development.Embed)
+if(Python3_VERSION VERSION_GREATER_EQUAL "3.14")
+    message(FATAL_ERROR "phyelds currently supports Python 3.12 and 3.13")
+endif()
+include_directories(${ARGOS_INCLUDE_DIRS})
+link_directories(${ARGOS_LIBRARY_DIR})
+
+add_library(phyelds_argos SHARED controller.cpp devices.cpp)
+target_compile_definitions(phyelds_argos PRIVATE
+    PHYELDS_PYTHON_EXECUTABLE="${Python3_EXECUTABLE}")
+target_compile_options(phyelds_argos PRIVATE -Wall -Wextra)
+target_link_libraries(phyelds_argos PRIVATE Python3::Python
+    argos3core_simulator argos3plugin_simulator_footbot argos3plugin_simulator_epuck
+    argos3plugin_simulator_eyebot argos3plugin_simulator_pipuck argos3plugin_simulator_builderbot
+    argos3plugin_simulator_drone
+    argos3plugin_simulator_genericrobot ${ARGOS_LDFLAGS})
+
+option(PHYELDS_ARGOS_GUI "Build Qt/OpenGL halo and label renderer" ON)
+if(PHYELDS_ARGOS_GUI)
+    find_package(ARGoSQTOpenGL REQUIRED)
+    add_library(phyelds_argos_qt SHARED gui.cpp)
+    target_include_directories(phyelds_argos_qt PRIVATE ${ARGOS_QTOPENGL_INCLUDE_DIRS})
+    target_link_libraries(phyelds_argos_qt PRIVATE phyelds_argos
+        argos3plugin_simulator_qtopengl ${ARGOS_QTOPENGL_LIBRARIES})
+endif()

phyelds_argos-8.9.0/src/phyelds/argos/native/controller.cpp

@@ -0,0 +1,176 @@
+#include <cmath>
+#include <algorithm>
+#include <limits>
+#include <regex>
+#include "controller.hpp"
+#include "python.hpp"
+#include <argos3/core/simulator/simulator.h>
+#include <argos3/core/simulator/space/space.h>
+#include <argos3/core/utility/logging/argos_log.h>
+
+using namespace argos;
+using namespace phyelds_argos;
+
+CPhyeldsController::~CPhyeldsController() {
+    if (m_device && Py_IsInitialized()) {
+        Gil gil;
+        Py_DECREF(m_device);
+    }
+    // The interpreter belongs to the process, never to an individual robot.
+}
+
+void CPhyeldsController::Init(TConfigurationNode& node) {
+    try {
+        GetNodeAttribute(node, "program", m_program);
+        GetNodeAttributeOrDefault(node, "source", m_source, m_source);
+        GetNodeAttributeOrDefault(node, "max_speed", m_max_speed, m_max_speed);
+        GetNodeAttributeOrDefault(node, "paths", m_paths, m_paths);
+        GetNodeAttributeOrDefault(node, "gps", m_gps, m_gps);
+        GetNodeAttributeOrDefault(node, "trace", m_trace, m_trace);
+        if (!std::isfinite(m_max_speed) || m_max_speed < 0)
+            throw std::runtime_error("max_speed must be finite and nonnegative");
+        if (m_paths != "compact" && m_paths != "debug")
+            throw std::runtime_error("paths must be compact or debug");
+        std::smatch match;
+        auto id = GetId();
+        if (!std::regex_match(id, match, std::regex(".*?([0-9]+)$")))
+            throw std::runtime_error("robot IDs must end in a unique integer (e.g. fb0)");
+        auto uid = std::stoull(match[1].str());
+        if (uid > std::numeric_limits<UInt32>::max()) throw std::runtime_error("robot UID exceeds u32");
+        m_uid = static_cast<UInt32>(uid);
+        m_devices = FindDevices(*this);
+        InitializePython();
+        Reset();
+    } catch (std::exception& error) {
+        THROW_ARGOSEXCEPTION_NESTED(GetId() << " initializing " << m_program, error);
+    }
+}
+
+void CPhyeldsController::Reset() {
+    try {
+        Gil gil;
+        auto separator = m_program.find(':');
+        if (separator == std::string::npos || separator == 0 || separator + 1 == m_program.size())
+            throw std::runtime_error("program must be module:function");
+        auto module = Checked(PyImport_ImportModule(m_program.substr(0, separator).c_str()));
+        auto program = Checked(PyObject_GetAttrString(module.get(), m_program.substr(separator + 1).c_str()));
+        if (!PyCallable_Check(program.get())) throw std::runtime_error("program is not callable");
+        auto runtime = Checked(PyImport_ImportModule("phyelds.vm.device"));
+        auto device = Checked(PyObject_GetAttrString(runtime.get(), "Device"));
+        auto args = Checked(Py_BuildValue("(IO)", m_uid, program.get()));
+        auto kwargs = Checked(Py_BuildValue("{s:s}", "paths", m_paths.c_str()));
+        auto replacement = Checked(PyObject_Call(device.get(), args.get(), kwargs.get()));
+        Py_XDECREF(m_device);
+        m_device = replacement.release();
+        Label.clear();
+        Color = CColor::BLACK;
+        if (m_devices.Drive) m_devices.Drive(CVector3::ZERO);
+        if (m_devices.Light) m_devices.Light(Color);
+        m_devices.Send(CByteArray());
+    } catch (std::exception& error) {
+        THROW_ARGOSEXCEPTION_NESTED(GetId() << " resetting " << m_program, error);
+    }
+}
+
+void CPhyeldsController::ControlStep() {
+    try {
+        Gil gil;
+        Step();
+    } catch (std::exception& error) {
+        THROW_ARGOSEXCEPTION_NESTED(GetId() << " executing " << m_program, error);
+    }
+}
+
+void CPhyeldsController::Step() {
+    auto const& pose = m_devices.Positioning->GetReading();
+    CRadians yaw, pitch, roll;
+    pose.Orientation.ToEulerAngles(yaw, pitch, roll);
+    auto ranges = Checked(PyDict_New());
+    auto vectors = Checked(PyDict_New());
+    for (auto& heard : m_devices.Receive()) {
+        CByteArray& data = heard.Data;
+        if (data.Size() < 6) continue;
+        UInt32 sender;
+        UInt16 length;
+        data >> sender >> length;
+        if (!length || length > data.Size() || sender == m_uid) continue;
+        auto received = Checked(PyObject_CallMethod(m_device, "receive", "Iy#", sender,
+            reinterpret_cast<char const*>(data.ToCArray()), static_cast<Py_ssize_t>(length)));
+        if (!heard.Offset) continue;
+        CVector3 offset = heard.Offset->Rotate(pose.Orientation);
+        auto key = Checked(PyLong_FromUnsignedLong(sender));
+        auto distance = Checked(PyFloat_FromDouble(offset.Length()));
+        auto vector = Checked(Py_BuildValue("(ddd)", offset.GetX(), offset.GetY(), offset.GetZ()));
+        Check(PyDict_SetItem(ranges.get(), key.get(), distance.get()));
+        Check(PyDict_SetItem(vectors.get(), key.get(), vector.get()));
+    }
+    auto sensors = Checked(PyDict_New());
+    Real time = CSimulator::GetInstance().GetSpace().GetSimulationClock()
+        * CPhysicsEngine::GetSimulationClockTick();
+    Put(sensors.get(), "time", Checked(PyFloat_FromDouble(time)));
+    Put(sensors.get(), "source", Checked(PyBool_FromLong(m_source == m_uid)));
+    Put(sensors.get(), "heading", Checked(Py_BuildValue("(ddd)", Cos(yaw), Sin(yaw), 0.0)));
+    if (m_devices.Obstacle) {
+        CVector2 obstacle = m_devices.Obstacle().Rotate(yaw);
+        Put(sensors.get(), "obstacle", Checked(Py_BuildValue("(ddd)", obstacle.GetX(), obstacle.GetY(), 0.0)));
+    }
+    if (m_devices.Measures) {
+        Put(sensors.get(), "neighbors_distances", std::move(ranges));
+        Put(sensors.get(), "neighbors_vectors", std::move(vectors));
+    }
+    if (m_gps) {
+        auto const& position = pose.Position;
+        Put(sensors.get(), "position",
+            Checked(Py_BuildValue("(ddd)", position.GetX(), position.GetY(), position.GetZ())));
+    }
+    auto payload = Checked(PyObject_CallMethod(m_device, "run_round", "O", sensors.get()));
+    if (!PyBytes_Check(payload.get())) throw std::runtime_error("Device.run_round must return bytes");
+    auto length = static_cast<size_t>(PyBytes_Size(payload.get()));
+    if (length > std::numeric_limits<UInt16>::max())
+        throw std::runtime_error("payload of " + std::to_string(length) + " bytes exceeds the 16-bit frame length");
+    CByteArray data;
+    data << m_uid << static_cast<UInt16>(length);
+    data.AddBuffer(reinterpret_cast<UInt8 const*>(PyBytes_AsString(payload.get())), length);
+    m_devices.Send(data);
+
+    auto outputs = Checked(PyObject_GetAttrString(m_device, "outputs"));
+    Label.clear();
+    if (auto label = PyDict_GetItemString(outputs.get(), "label")) {
+        auto text = Checked(PyObject_Str(label));
+        auto utf8 = PyUnicode_AsUTF8(text.get());
+        if (!utf8) Checked(nullptr);
+        Label = utf8;
+    }
+    Color = CColor::BLACK;
+    if (auto led = PyDict_GetItemString(outputs.get(), "led")) {
+        auto channels = Checked(PySequence_Fast(led, "led must be an RGB iterable"));
+        if (PySequence_Fast_GET_SIZE(channels.get()) != 3) throw std::runtime_error("led must have three channels");
+        UInt8 rgb[3];
+        for (size_t index = 0; index < 3; ++index) {
+            double value = Number(PySequence_Fast_GET_ITEM(channels.get(), index));
+            if (value < 0 || value > 255) throw std::runtime_error("led channels must be in [0,255]");
+            rgb[index] = static_cast<UInt8>(value);
+        }
+        Color = CColor(rgb[0], rgb[1], rgb[2]);
+    }
+    if (m_devices.Light) m_devices.Light(Color);
+    CVector3 velocity;
+    if (auto value = PyDict_GetItemString(outputs.get(), "velocity")) {
+        auto components = Checked(PySequence_Fast(value, "velocity must be an iterable"));
+        auto size = PySequence_Fast_GET_SIZE(components.get());
+        if (size != 2 && size != 3) throw std::runtime_error("velocity must have two or three components");
+        velocity.Set(Number(PySequence_Fast_GET_ITEM(components.get(), 0)),
+                     Number(PySequence_Fast_GET_ITEM(components.get(), 1)),
+                     size == 3 ? Number(PySequence_Fast_GET_ITEM(components.get(), 2)) : 0);
+        if (velocity.Length() > m_max_speed) velocity *= m_max_speed / velocity.Length();
+    }
+    if (m_devices.Drive) m_devices.Drive(velocity);
+    if (m_trace) LOG << GetId() << " round: " << Label << std::endl;
+}
+
+void CPhyeldsController::Destroy() {
+    LOG << GetId() << " final: " << Label << std::endl;
+    LOG.Flush();
+}
+
+REGISTER_CONTROLLER(CPhyeldsController, "phyelds_controller")

phyelds_argos-8.9.0/src/phyelds/argos/native/controller.hpp

@@ -0,0 +1,32 @@
+// Native host only: all aggregate state and serialization belong to phyelds Device.
+#pragma once
+#include <argos3/core/control_interface/ci_controller.h>
+#include <argos3/core/utility/datatypes/color.h>
+#include <string>
+#include "devices.hpp"
+
+struct _object;
+
+class CPhyeldsController : public argos::CCI_Controller {
+public:
+    ~CPhyeldsController() override;
+    void Init(argos::TConfigurationNode& node) override;
+    void Reset() override;
+    void ControlStep() override;
+    void Destroy() override;
+
+    std::string Label;
+    argos::CColor Color = argos::CColor::BLACK;
+
+private:
+    void Step();
+    phyelds_argos::Devices m_devices;
+    _object* m_device = nullptr;
+    std::string m_program;
+    std::string m_paths = "compact";
+    argos::UInt32 m_uid = 0;
+    argos::SInt64 m_source = -1;
+    argos::Real m_max_speed = 0.1;
+    bool m_gps = false;
+    bool m_trace = false;
+};

phyelds_argos-8.9.0/src/phyelds/argos/native/devices.cpp

@@ -0,0 +1,220 @@
+#include "devices.hpp"
+#include <algorithm>
+#include <cmath>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <argos3/plugins/robots/builderbot/control_interface/ci_builderbot_differential_drive_actuator.h>
+#include <argos3/plugins/robots/builderbot/control_interface/ci_builderbot_leds_actuator.h>
+#include <argos3/plugins/robots/builderbot/control_interface/ci_builderbot_rangefinders_sensor.h>
+#include <argos3/plugins/robots/drone/control_interface/ci_drone_flight_system_actuator.h>
+#include <argos3/plugins/robots/drone/control_interface/ci_drone_flight_system_sensor.h>
+#include <argos3/plugins/robots/drone/control_interface/ci_drone_leds_actuator.h>
+#include <argos3/plugins/robots/e-puck/control_interface/ci_epuck_proximity_sensor.h>
+#include <argos3/plugins/robots/eye-bot/control_interface/ci_eyebot_proximity_sensor.h>
+#include <argos3/plugins/robots/foot-bot/control_interface/ci_footbot_proximity_sensor.h>
+#include <argos3/plugins/robots/generic/control_interface/ci_differential_steering_actuator.h>
+#include <argos3/plugins/robots/generic/control_interface/ci_directional_leds_actuator.h>
+#include <argos3/plugins/robots/generic/control_interface/ci_leds_actuator.h>
+#include <argos3/plugins/robots/generic/control_interface/ci_quadrotor_speed_actuator.h>
+#include <argos3/plugins/robots/generic/control_interface/ci_range_and_bearing_actuator.h>
+#include <argos3/plugins/robots/generic/control_interface/ci_range_and_bearing_sensor.h>
+#include <argos3/plugins/robots/generic/control_interface/ci_simple_radios_actuator.h>
+#include <argos3/plugins/robots/generic/control_interface/ci_simple_radios_sensor.h>
+#include <argos3/plugins/robots/pi-puck/control_interface/ci_pipuck_differential_drive_actuator.h>
+#include <argos3/plugins/robots/pi-puck/control_interface/ci_pipuck_leds_actuator.h>
+#include <argos3/plugins/robots/pi-puck/control_interface/ci_pipuck_rangefinders_sensor.h>
+
+using namespace argos;
+
+namespace phyelds_argos {
+namespace {
+constexpr Real CM_PER_M = 100; // range-and-bearing ranges and differential_steering speeds are in cm
+constexpr Real BUILDERBOT_RANGE = 0.05; // what its rangefinders report when they see nothing
+
+CRadians Yaw(CCI_PositioningSensor const* positioning) {
+    CRadians yaw, pitch, roll;
+    positioning->GetReading().Orientation.ToEulerAngles(yaw, pitch, roll);
+    return yaw;
+}
+
+// Wheel speeds (m/s) that turn a differential drive towards a velocity in its own frame,
+// and drive it forward as its heading aligns.
+std::pair<Real, Real> Wheels(CVector2 const& velocity) {
+    Real speed = velocity.Length();
+    if (speed < 0.005) return {0, 0};
+    Real angle = velocity.Angle().GetValue();
+    Real forward = speed * std::max<Real>(0, std::cos(angle));
+    Real turn = speed * angle;
+    return {forward - turn, forward + turn};
+}
+
+// A rangefinder distance as a proximity reading: exp(-distance) in range, as ARGoS
+// computes the readings of the foot-bot, e-puck and eye-bot, and 0 beyond it.
+Real Closeness(Real distance, Real range) {
+    return distance < range ? std::exp(-distance) : 0;
+}
+
+// The horizontal direction a rangefinder looks along, in the frame of the robot.
+CVector2 Looking(CQuaternion const& orientation) {
+    CVector3 ray(CVector3::Z);
+    ray.Rotate(orientation);
+    return {ray.GetX(), ray.GetY()};
+}
+
+// The repulsion of a ring of proximity sensors with angles, as the foot-bot's.
+template <class Sensor> std::function<CVector2()> Ring(Sensor* sensor) {
+    return [sensor] {
+        CVector2 repulsion;
+        for (auto const& reading : sensor->GetReadings()) repulsion -= CVector2(reading.Value, reading.Angle);
+        return repulsion;
+    };
+}
+
+void FindRadio(CCI_Controller& robot, Devices& devices) {
+    if (robot.HasActuator("range_and_bearing")) {
+        auto* out = robot.GetActuator<CCI_RangeAndBearingActuator>("range_and_bearing");
+        auto* in = robot.GetSensor<CCI_RangeAndBearingSensor>("range_and_bearing");
+        devices.Measures = true;
+        devices.Receive = [in] {
+            std::vector<Heard> heard;
+            for (auto const& packet : in->GetReadings()) {
+                CVector3 offset;
+                offset.FromSphericalCoords(packet.Range / CM_PER_M,
+                    CRadians::PI_OVER_TWO - packet.VerticalBearing, packet.HorizontalBearing);
+                heard.push_back({packet.Data, offset});
+            }
+            return heard;
+        };
+        devices.Send = [out](CByteArray const& frame) {
+            if (frame.Size() > out->GetSize())
+                throw std::runtime_error("payload of " + std::to_string(frame.Size() - 6)
+                    + " bytes exceeds rab_data_size (including 6-byte header)");
+            CByteArray data(frame);
+            data.Resize(out->GetSize());
+            out->SetData(data);
+        };
+    } else if (robot.HasActuator("simple_radios")) {
+        auto* out = robot.GetActuator<CCI_SimpleRadiosActuator>("simple_radios");
+        auto* in = robot.GetSensor<CCI_SimpleRadiosSensor>("simple_radios");
+        // Every interface: a block has a radio on each face, a BuilderBot both wifi and NFC.
+        devices.Receive = [in] {
+            std::vector<Heard> heard;
+            for (auto const& interface : in->GetInterfaces())
+                for (auto const& message : interface.Messages) heard.push_back({message, std::nullopt});
+            return heard;
+        };
+        devices.Send = [out](CByteArray const& frame) {
+            if (frame.Size() == 0) return; // simple radios send each message once
+            for (auto& interface : out->GetInterfaces()) interface.Messages.push_back(frame);
+        };
+    } else {
+        throw std::runtime_error("the robot needs a range_and_bearing or a simple_radios radio");
+    }
+}
+
+void FindLocomotion(CCI_Controller& robot, Devices& devices) {
+    auto* positioning = devices.Positioning;
+    auto local = [positioning](CVector3 const& velocity) {
+        return CVector2(velocity.GetX(), velocity.GetY()).Rotate(-Yaw(positioning));
+    };
+    if (robot.HasActuator("differential_steering")) {
+        auto* wheels = robot.GetActuator<CCI_DifferentialSteeringActuator>("differential_steering");
+        devices.Drive = [wheels, local](CVector3 const& velocity) {
+            auto [left, right] = Wheels(local(velocity));
+            wheels->SetLinearVelocity(left * CM_PER_M, right * CM_PER_M);
+        };
+    } else if (robot.HasActuator("pipuck_differential_drive")) {
+        auto* wheels = robot.GetActuator<CCI_PiPuckDifferentialDriveActuator>("pipuck_differential_drive");
+        devices.Drive = [wheels, local](CVector3 const& velocity) {
+            auto [left, right] = Wheels(local(velocity));
+            wheels->SetLinearVelocity(left, right);
+        };
+    } else if (robot.HasActuator("builderbot_differential_drive")) {
+        auto* wheels = robot.GetActuator<CCI_BuilderBotDifferentialDriveActuator>("builderbot_differential_drive");
+        // Its right wheel is mirrored: (v, -v) drives the BuilderBot straight ahead.
+        devices.Drive = [wheels, local](CVector3 const& velocity) {
+            auto [left, right] = Wheels(local(velocity));
+            wheels->SetTargetVelocityLeft(left);
+            wheels->SetTargetVelocityRight(-right);
+        };
+    } else if (robot.HasActuator("quadrotor_speed")) {
+        auto* rotors = robot.GetActuator<CCI_QuadRotorSpeedActuator>("quadrotor_speed");
+        devices.Drive = [rotors](CVector3 const& velocity) { rotors->SetLinearVelocity(velocity); };
+    } else if (robot.HasActuator("drone_flight_system")) {
+        auto* flight = robot.GetActuator<CCI_DroneFlightSystemActuator>("drone_flight_system");
+        auto* state = robot.GetSensor<CCI_DroneFlightSystemSensor>("drone_flight_system");
+        // The flight system flies towards a target, in the frame the drone started in, at
+        // 1/s of the distance: a target one second ahead is flown at the velocity. At zero
+        // velocity the drone keeps its last target, so it holds its place without drifting.
+        devices.Drive = [flight, state, positioning](CVector3 velocity) {
+            if (velocity == CVector3::ZERO) return;
+            velocity.RotateZ(CRadians(state->GetOrientation().GetZ()) - Yaw(positioning));
+            flight->SetTargetPosition(state->GetPosition() + velocity);
+        };
+    }
+}
+
+void FindObstacles(CCI_Controller& robot, Devices& devices) {
+    if (robot.HasSensor("footbot_proximity")) {
+        devices.Obstacle = Ring(robot.GetSensor<CCI_FootBotProximitySensor>("footbot_proximity"));
+    } else if (robot.HasSensor("epuck_proximity")) {
+        devices.Obstacle = Ring(robot.GetSensor<CCI_EPuckProximitySensor>("epuck_proximity"));
+    } else if (robot.HasSensor("eyebot_proximity")) {
+        devices.Obstacle = Ring(robot.GetSensor<CCI_EyeBotProximitySensor>("eyebot_proximity"));
+    } else if (robot.HasSensor("pipuck_rangefinders")) {
+        auto* sensor = robot.GetSensor<CCI_PiPuckRangefindersSensor>("pipuck_rangefinders");
+        devices.Obstacle = [sensor] {
+            CVector2 repulsion;
+            sensor->Visit([&repulsion](auto const& rangefinder) {
+                auto const& [anchor, position, orientation, range] = rangefinder.Configuration;
+                repulsion -= Looking(orientation) * Closeness(rangefinder.Proximity, range);
+            });
+            return repulsion;
+        };
+    } else if (robot.HasSensor("builderbot_rangefinders")) {
+        auto* sensor = robot.GetSensor<CCI_BuilderBotRangefindersSensor>("builderbot_rangefinders");
+        devices.Obstacle = [sensor] {
+            CVector2 repulsion;
+            // The ring on its body, but for the front pair (6, 7) that faces its own end
+            // effector, as do the rangefinders on the end effector.
+            for (auto const* rangefinder : sensor->GetInterfaces())
+                if (rangefinder->Anchor == "origin" && rangefinder->Label != "6" && rangefinder->Label != "7")
+                    repulsion -= Looking(rangefinder->OrientationOffset)
+                        * Closeness(rangefinder->Proximity, BUILDERBOT_RANGE);
+            return repulsion;
+        };
+    }
+}
+
+void FindLights(CCI_Controller& robot, Devices& devices) {
+    if (robot.HasActuator("leds")) {
+        auto* leds = robot.GetActuator<CCI_LEDsActuator>("leds");
+        devices.Light = [leds](CColor const& color) { leds->SetAllColors(color); };
+    } else if (robot.HasActuator("directional_leds")) {
+        auto* leds = robot.GetActuator<CCI_DirectionalLEDsActuator>("directional_leds");
+        devices.Light = [leds](CColor const& color) { leds->SetAllColors(color); };
+    } else if (robot.HasActuator("builderbot_leds")) {
+        auto* leds = robot.GetActuator<CCI_BuilderBotLEDsActuator>("builderbot_leds");
+        devices.Light = [leds](CColor const& color) { leds->SetLEDs(color); };
+    } else if (robot.HasActuator("drone_leds")) {
+        auto* leds = robot.GetActuator<CCI_DroneLEDsActuator>("drone_leds");
+        devices.Light = [leds](CColor const& color) { leds->SetLEDs(color); };
+    } else if (robot.HasActuator("pipuck_leds")) {
+        // The Pi-Puck ring LEDs are only on or off.
+        auto* leds = robot.GetActuator<CCI_PiPuckLEDsActuator>("pipuck_leds");
+        devices.Light = [leds](CColor const& color) { leds->SetRingLEDs(color != CColor::BLACK); };
+    }
+}
+}
+
+Devices FindDevices(CCI_Controller& robot) {
+    Devices devices;
+    devices.Positioning = robot.GetSensor<CCI_PositioningSensor>("positioning");
+    FindRadio(robot, devices);
+    FindLocomotion(robot, devices);
+    FindObstacles(robot, devices);
+    FindLights(robot, devices);
+    return devices;
+}
+}

phyelds_argos-8.9.0/src/phyelds/argos/native/devices.hpp

@@ -0,0 +1,34 @@
+// The devices of any ARGoS robot, behind the few operations an aggregate round needs.
+#pragma once
+#include <argos3/core/control_interface/ci_controller.h>
+#include <argos3/core/utility/datatypes/byte_array.h>
+#include <argos3/core/utility/datatypes/color.h>
+#include <argos3/core/utility/math/vector2.h>
+#include <argos3/core/utility/math/vector3.h>
+#include <argos3/plugins/robots/generic/control_interface/ci_positioning_sensor.h>
+#include <functional>
+#include <optional>
+#include <vector>
+
+namespace phyelds_argos {
+// A frame from the radio; a range-and-bearing radio also measures where its sender is.
+struct Heard {
+    argos::CByteArray Data;
+    std::optional<argos::CVector3> Offset; // metres, in the frame of the robot
+};
+
+// What a robot can do, from the devices its controller declares. A device the robot
+// lacks is an empty function: its outputs are ignored, so one program fits a mixed swarm.
+struct Devices {
+    argos::CCI_PositioningSensor* Positioning = nullptr;
+    bool Measures = false; // whether Receive measures where the senders are
+    std::function<std::vector<Heard>()> Receive;
+    std::function<void(argos::CByteArray const&)> Send; // an empty frame silences the radio
+    std::function<argos::CVector2()> Obstacle; // repulsion, in the frame of the robot
+    std::function<void(argos::CVector3 const&)> Drive; // m/s, in the global frame
+    std::function<void(argos::CColor const&)> Light;
+};
+
+// Find the devices declared for a robot; it needs a radio and the positioning sensor.
+Devices FindDevices(argos::CCI_Controller& robot);
+}

phyelds_argos-8.9.0/src/phyelds/argos/native/gui.cpp

@@ -0,0 +1,33 @@
+#include <algorithm>
+#include <argos3/core/simulator/entity/composable_entity.h>
+#include <argos3/core/simulator/entity/controllable_entity.h>
+#include <argos3/core/simulator/entity/embodied_entity.h>
+#include <argos3/core/simulator/simulator.h>
+#include <argos3/core/simulator/space/space.h>
+#include <argos3/plugins/simulator/visualizations/qt-opengl/qtopengl_user_functions.h>
+#include "controller.hpp"
+
+// Under every robot a phyelds controller drives, whatever its type, a halo of its LED
+// colour, and its label above it.
+class CPhyeldsQTUserFunctions : public argos::CQTOpenGLUserFunctions {
+public:
+    void DrawInWorld() override {
+        QFont font;
+        font.setPixelSize(12);
+        for (auto& [id, entity] : argos::CSimulator::GetInstance().GetSpace().GetEntitiesByType("controller")) {
+            auto& controllable = *argos::any_cast<argos::CControllableEntity*>(entity);
+            auto* controller = dynamic_cast<CPhyeldsController*>(&controllable.GetController());
+            auto& robot = controllable.GetParent();
+            if (!controller || !robot.HasComponent("body")) continue;
+            auto const& box = robot.GetComponent<argos::CEmbodiedEntity>("body").GetBoundingBox();
+            argos::CVector3 base = (box.MinCorner + box.MaxCorner) * 0.5;
+            base.SetZ(box.MinCorner.GetZ());
+            argos::Real radius = 0.75 * std::max(box.MaxCorner.GetX() - box.MinCorner.GetX(),
+                                                 box.MaxCorner.GetY() - box.MinCorner.GetY());
+            DrawCylinder(base, argos::CQuaternion(), radius, 0.01, controller->Color);
+            DrawText(argos::CVector3(base.GetX(), base.GetY(), box.MaxCorner.GetZ() + 0.05),
+                     controller->Label, argos::CColor::BLACK, font);
+        }
+    }
+};
+REGISTER_QTOPENGL_USER_FUNCTIONS(CPhyeldsQTUserFunctions, "phyelds_qtuser_functions")

phyelds_argos-8.9.0/src/phyelds/argos/native/python.hpp

@@ -0,0 +1,70 @@
+// The GIL must outlive every owned Python reference in its scope.
+#pragma once
+#define PY_SSIZE_T_CLEAN
+#include <Python.h>
+#include <cmath>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <string>
+
+namespace phyelds_argos {
+struct PythonDeleter {
+    void operator()(PyObject* value) const { Py_XDECREF(value); }
+};
+using Object = std::unique_ptr<PyObject, PythonDeleter>;
+
+inline Object Checked(PyObject* value) {
+    if (!value) {
+        // Preserve the original traceback; the controller adds its ID and program.
+        PyErr_Print();
+        throw std::runtime_error("Python call failed (see traceback above)");
+    }
+    return Object(value);
+}
+
+inline void Check(int result) {
+    if (result < 0) Checked(nullptr);
+}
+
+inline void InitializePython() {
+    static std::once_flag once;
+    std::call_once(once, [] {
+        if (Py_IsInitialized()) return;
+        PyConfig config;
+        PyConfig_InitPythonConfig(&config);
+        config.parse_argv = 0;
+        config.install_signal_handlers = 0;
+        PyStatus status = PyConfig_SetBytesString(&config, &config.program_name, PHYELDS_PYTHON_EXECUTABLE);
+        if (!PyStatus_Exception(status)) status = Py_InitializeFromConfig(&config);
+        std::string error;
+        if (PyStatus_Exception(status))
+            error = status.err_msg ? status.err_msg : "could not initialize Python";
+        PyConfig_Clear(&config);
+        if (!error.empty()) throw std::runtime_error(error);
+        // ARGoS can dispatch controllers to worker threads. Each call takes the GIL.
+        PyEval_SaveThread();
+    });
+}
+
+class Gil {
+public:
+    Gil() : m_state(PyGILState_Ensure()) {}
+    ~Gil() { PyGILState_Release(m_state); }
+    Gil(Gil const&) = delete;
+    Gil& operator=(Gil const&) = delete;
+private:
+    PyGILState_STATE m_state;
+};
+
+inline void Put(PyObject* dict, char const* name, Object value) {
+    Check(PyDict_SetItemString(dict, name, value.get()));
+}
+
+inline double Number(PyObject* value) {
+    double number = PyFloat_AsDouble(value);
+    if (PyErr_Occurred()) Checked(nullptr);
+    if (!std::isfinite(number)) throw std::runtime_error("actuator components must be finite");
+    return number;
+}
+}