init

.clangd

@@ -0,0 +1,3 @@
+CompileFlags:
+  Add:
+    - -std=c++20

.gitignore

@@ -0,0 +1,2 @@
+/.vscode
+/build

CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.15)
+
+project(robotCommand)
+
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED True)
+#set(CMAKE_VERBOSE_MAKEFILE ON)
+
+set(gcc_like_cxx "$<COMPILE_LANG_AND_ID:CXX,ARMClang,AppleClang,Clang,GNU,LCC>")
+set(msvc_cxx "$<COMPILE_LANG_AND_ID:CXX,MSVC>")
+
+add_executable(robotCommand
+    src/main.cpp
+    src/robot.cpp
+    src/jsonrpctcpclient.cpp
+    src/PosTracker.cpp
+    src/PosHistory.cpp
+)
+
+find_package(jsoncpp REQUIRED)
+find_package(SFML COMPONENTS graphics window system REQUIRED)
+target_link_libraries(robotCommand jsoncpp_lib sfml-graphics sfml-window sfml-system)
+if (WIN32)
+    target_link_libraries(robotCommand ws2_32)
+endif ()
+
+target_compile_options(robotCommand PRIVATE
+  "$<${gcc_like_cxx}:-Wall;-Wextra;-Wshadow;-Wformat=2;-Wunused>"
+  "$<${msvc_cxx}:-W3>"
+)
+
+#add_compile_definitions(JSONRPC_DEBUG)
+if (WIN32)
+    add_compile_definitions(_WIN32_WINNT=0x0501)
+endif ()

README.md

@@ -0,0 +1,14 @@
+Setup
+=====
+
+- from https://gitlab.cri.epita.fr/jeremie.graulle/ssie-s9-robot-command create a personal fork
+to be able to commit
+- clone your fork on your computer:
+`git clone git@gitlab.cri.epita.fr:<name>/ssie-s9-robot-command.git`
+- open VS code from this fork: code . &
+- Build and Run
+
+Step
+====
+
+Update the main of this projet to be able to follow the line displayed on de default simulator map.

build.sh

@@ -0,0 +1,10 @@
+#!/bin/sh
+set -e
+cd "$(dirname "$(realpath "$0")")"
+
+cp -r . /tmp/build-tp4-sim
+cd      /tmp/build-tp4-sim
+rm -fr  /tmp/build-tp4-sim/.git
+
+cmake .
+make -j$(nproc)

run.sh

@@ -0,0 +1,11 @@
+#!/bin/sh
+set -e
+cd "$(dirname "$(realpath "$0")")"
+
+if ! [ -f /tmp/build-tp4-sim/robotCommand ]
+then ./build.sh
+fi
+
+cd /tmp/build-tp4-sim/
+/tmp/build-tp4-sim/robotCommand $@
+

src/PosHistory.cpp

@@ -0,0 +1,25 @@
+#include "PosHistory.hpp"
+#include <SFML/Graphics/Vertex.hpp>
+#include <SFML/System/Vector2.hpp>
+#include <optional>
+
+bool rounded_positions_equal(sf::Vector2f& a, sf::Vector2f& b) {
+    if (int(a.x) != int(b.x)) return false;
+    if (int(a.y) != int(b.y)) return false;
+    return true;
+}
+
+void PosHistory::append(sf::Vector2f point) {
+    if (!shouldInsert(point)) return;
+    _points[_insertionIndex] = sf::Vertex(point);
+    _insertionIndex += 1;
+    if (_insertionIndex >= _capacity) _insertionIndex = 0; // note : loops over to start of the array.
+    _lastInsertion = std::optional(point);
+}
+
+bool PosHistory::shouldInsert(sf::Vector2f& point) {
+    if (!_lastInsertion.has_value()) return true;
+    auto last = _lastInsertion.value();
+    if (rounded_positions_equal(last, point)) return false;
+    return true;
+}

src/PosHistory.hpp

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <SFML/Graphics/PrimitiveType.hpp>
+#include <SFML/Graphics/VertexArray.hpp>
+#include <SFML/System/Vector2.hpp>
+#include <cstddef>
+#include <optional>
+
+class PosHistory {
+    sf::VertexArray _points;
+    std::optional<sf::Vector2f> _lastInsertion;
+    size_t _insertionIndex;
+    size_t _capacity;
+    public:
+        inline PosHistory(size_t capacity) :
+            _points(sf::VertexArray(sf::PrimitiveType::Points, capacity)),
+            _lastInsertion(std::optional<sf::Vector2f>()),
+            _insertionIndex(0),
+            _capacity(capacity)
+        {
+            // for (size_t i = 0; i < capacity_; i += 1) append(sf::Vector2f(-1, -1));
+        }
+        void append(sf::Vector2f point);
+        inline sf::VertexArray& vertices() { return _points; }
+    private:
+        bool shouldInsert(sf::Vector2f& point);
+};

src/PosTracker.cpp

@@ -0,0 +1,28 @@
+#include "PosTracker.hpp"
+
+using sf::Vector2f;
+using sf::Transform;
+
+void PosTracker::reset(Vector2f initialPos, float initialRot) {
+    transform = transformFromPosRot(initialPos, initialRot);
+}
+
+void PosTracker::update(float newLeftValue, float newRightValue) {
+    auto leftDistance = (newLeftValue - _lastLeftValue) * wheelIncrementDistance();
+    auto rightDistance = (newRightValue - _lastRightValue) * wheelIncrementDistance();
+    updateTransform(leftDistance, rightDistance);
+    _lastLeftValue = newLeftValue;
+    _lastRightValue = newRightValue;
+}
+
+void PosTracker::updateTransform(float leftDistance, float rightDistance) {
+    float linearStep =  (leftDistance + rightDistance) / 2.0;
+    float angularStep = (leftDistance - rightDistance) / _wheelDistance * 180.0 / std::numbers::pi;
+
+    auto orientation = Transform(sf::Transform().rotate(transform.getRotation()));
+    auto new_ = transformFromPosRot(
+        transform.getPosition() + orientation.transformPoint(sf::Vector2f(linearStep, 0.0)),
+        transform.getRotation() + angularStep
+    );
+    transform = new_;
+}

src/PosTracker.hpp

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <SFML/Graphics/RectangleShape.hpp>
+#include <SFML/Graphics/RenderWindow.hpp>
+#include <SFML/Graphics/Transform.hpp>
+#include <SFML/Graphics/Transformable.hpp>
+#include <SFML/System/Vector2.hpp>
+#include <SFML/Window/Event.hpp>
+
+inline sf::Transformable transformFromPosRot(sf::Vector2f position, float rotation) {
+    auto result = sf::Transformable();
+    result.setPosition(position);
+    result.setRotation(rotation);
+    return result;
+}
+
+class PosTracker {
+    /// Wheel encoder values.
+    float _wheelDistance, _wheelDiameter;
+    float _wheelStepsPerTurn;
+    float _lastLeftValue, _lastRightValue;
+    sf::Transformable transform;
+    public:
+        PosTracker(sf::Vector2f initialPos, float initialRot, float wheelDistance, float wheelDiameter, float wheelStepsPerTurn) :
+            _wheelDistance(wheelDistance),
+            _wheelDiameter(wheelDiameter),
+            _wheelStepsPerTurn(wheelStepsPerTurn),
+            _lastLeftValue(0.),
+            _lastRightValue(0.),
+            transform(transformFromPosRot(initialPos, initialRot))
+        {}
+        void reset(sf::Vector2f initialPos, float initialRot);
+        void update(float newLeftValue, float newRightValue);
+
+        inline sf::Vector2f getPosition() const { return transform.getPosition(); }
+        inline float getRotation() const { return transform.getRotation(); }
+    private:
+        inline float wheelIncrementDistance() const {
+            return _wheelDiameter * std::numbers::pi / _wheelStepsPerTurn;
+        }
+        void updateTransform(float leftDistance, float rightDistance);
+};

src/filter.cpp

@@ -0,0 +1,25 @@
+#include <cmath>
+#include "filter.hpp"
+
+void MotorJitterFilter::set(float value) {
+    _value = value;
+}
+
+float MotorJitterFilter::get(float delta) {
+    update(delta);
+    if (_stateIsOn) return 1;
+    else return 0;
+}
+
+float MotorJitterFilter::delayOfState() {
+    float result = 0;
+    if (_stateIsOn) result = _delay * _value;
+    else result = _delay * (1 - _value);
+    return std::fmax(result, 0);
+}
+
+void MotorJitterFilter::update(float delta) {
+    _delaySinceState += delta;
+    if (_delaySinceState < delayOfState()) return;
+    _stateIsOn = !_stateIsOn;
+}

src/filter.hpp

@@ -0,0 +1,17 @@
+#pragma once
+
+class MotorJitterFilter {
+    public:
+        inline MotorJitterFilter(float delay) : _delay(delay), _delaySinceState(0), _stateIsOn(false), _value(0) {}
+        void set(float value);
+        float get(float delta);
+
+    private:
+        float _value;
+        float _delay;
+        float _delaySinceState;
+        bool _stateIsOn;
+
+        float delayOfState();
+        void update(float delta);
+};

src/jsonrpctcpclient.cpp

@@ -0,0 +1,161 @@
+#include "jsonrpctcpclient.hpp"
+
+#include <utility>
+#include <memory>
+#include <json/json.h>
+#include <iostream>
+#include <asio/streambuf.hpp>
+#include <asio/write.hpp>
+#include <asio/read_until.hpp>
+#include <asio/read.hpp>
+#include <asio/buffer.hpp>
+#include <thread>
+
+
+JsonRpcTcpClient::JsonRpcTcpClient(const std::string & hostIpAddress, unsigned short tcpPort)
+    : _ioc()
+    , _socket(_ioc)
+    , _jsonRpcId(1)
+    , _tcpStreambuf()
+    , _tcpOutStream(&_tcpStreambuf)
+    , _jsonStreamWriter(nullptr)
+    , _receiveMethodResponseSem(0)
+    , _isStartReceive(false)
+{
+    Json::StreamWriterBuilder jsonStreamWriterBuilder;
+    jsonStreamWriterBuilder["indentation"] = "";
+    _jsonStreamWriter.reset(jsonStreamWriterBuilder.newStreamWriter());
+
+    _socket.connect(asio::ip::tcp::endpoint(asio::ip::address::from_string(hostIpAddress), tcpPort));
+}
+
+JsonRpcTcpClient::~JsonRpcTcpClient()
+{
+    _socket.close();
+}
+
+void JsonRpcTcpClient::bindNotification(const std::string & methodName, const std::function<void(Json::Value)> & notificationHandle)
+{
+    assert(!_isStartReceive);
+    _notificationHandles.insert(std::make_pair(methodName, notificationHandle));
+}
+
+void JsonRpcTcpClient::startReceive()
+{
+    _isStartReceive = true;
+    std::thread([](JsonRpcTcpClient * thus){thus->receive();}, this).detach();
+}
+
+void JsonRpcTcpClient::callNotification(const char * methodName, const Json::Value & params)
+{
+    // Prepare message
+    Json::Value message;
+    message["jsonrpc"] = "2.0";
+    message["method"] = methodName;
+    message["params"] = params;
+
+    // Send message
+    _jsonStreamWriter->write(message, &_tcpOutStream);
+    _tcpOutStream << static_cast<char>(0x0A);
+    asio::write(_socket, _tcpStreambuf);
+
+#ifdef JSONRPC_DEBUG
+    // Print message
+    std::cout << "send message ";
+    _jsonStreamWriter->write(message, &std::cout);
+    std::cout << std::endl;
+#endif
+}
+
+Json::Value JsonRpcTcpClient::callMethod(const char * methodName, const Json::Value & param)
+{
+    // Prepare message
+    Json::Value message;
+    message["jsonrpc"] = "2.0";
+    message["method"] = methodName;
+    message["params"] = param;
+    message["id"] = _jsonRpcId;
+
+    // Send message
+    _jsonStreamWriter->write(message, &_tcpOutStream);
+    _tcpOutStream << static_cast<char>(0x0A);
+    asio::write(_socket, _tcpStreambuf);
+
+#ifdef JSONRPC_DEBUG
+    // Print message
+    std::cout << "send message ";
+    _jsonStreamWriter->write(message, &std::cout);
+    std::cout << std::endl;
+#endif
+
+    // Wait response
+#ifdef JSONRPC_DEBUG
+    std::cout << "wait response..." << std::endl;
+#endif
+    _receiveMethodResponseSem.acquire();
+    Json::Value responseJson = _receiveMethodResponseJsonValue;
+    assert(_jsonRpcId == responseJson["id"].asInt());
+    _receiveMethodResponseJsonValue = Json::Value();
+
+#ifdef JSONRPC_DEBUG
+    // Print response
+    std::cout << "Receive response ";
+    _jsonStreamWriter->write(responseJson, &std::cout);
+    std::cout << std::endl;
+#endif
+
+    _jsonRpcId++;
+    return responseJson["result"];
+}
+
+void JsonRpcTcpClient::receive()
+{
+    asio::streambuf tcpStreambuf;
+    std::istream tcpInStream(&tcpStreambuf);
+
+    while (true)
+    {
+        // Wait message
+        asio::read_until(_socket, tcpStreambuf, 0x0A);
+#ifdef JSONRPC_DEBUG
+        std::cout << "wait message..." << std::endl;
+#endif
+
+        // Extract one json message
+        std::string messageJsonStr;
+        std::getline(tcpInStream, messageJsonStr, static_cast<char>(0x0A));
+
+        // Parse this json message
+        Json::CharReaderBuilder builder;
+        JSONCPP_STRING errs;
+        Json::Value messageJson;
+        const std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
+        if (!reader->parse(messageJsonStr.c_str(), messageJsonStr.c_str()+messageJsonStr.size(),
+                &messageJson, &errs))
+            throw std::runtime_error(errs);
+
+        // If method response
+        if (messageJson.isMember("id"))
+        {
+            assert(_receiveMethodResponseJsonValue.isNull());
+            _receiveMethodResponseJsonValue = messageJson;
+            _receiveMethodResponseSem.release();
+        }
+        // If notification
+        else
+        {
+#ifdef JSONRPC_DEBUG
+            // Print notification
+            std::cout << "Receive notification ";
+            _jsonStreamWriter->write(messageJson, &std::cout);
+            std::cout << std::endl;
+#endif
+            // Find the notification and execute it
+            auto it = _notificationHandles.find(messageJson["method"].asString());
+            if (it != _notificationHandles.end())
+            {
+                it->second(messageJson["params"]);
+            }
+        }
+    }
+}

src/jsonrpctcpclient.hpp

@@ -0,0 +1,49 @@
+#ifndef JSONRPCTCPCLIENT_HPP
+#define JSONRPCTCPCLIENT_HPP
+
+#include <asio/ip/tcp.hpp>
+#include <asio/streambuf.hpp>
+#include <asio/io_context.hpp>
+#include <semaphore>
+#include <json/value.h>
+
+namespace Json
+{
+    class StreamWriter;
+}
+
+
+class JsonRpcTcpClient
+{
+public:
+    JsonRpcTcpClient(const std::string & hostIpAddress, unsigned short tcpPort);
+    ~JsonRpcTcpClient();
+
+    using NotificationHandle = std::function<void(Json::Value)>;
+    void bindNotification(const std::string & methodName, const NotificationHandle & notificationHandle);
+
+    //! @warning start receive only after bind all notification
+    void startReceive();
+
+    void callNotification(const char * methodName, const Json::Value & param);
+    Json::Value callMethod(const char * methodName, const Json::Value & param);
+
+private:
+    JsonRpcTcpClient(const JsonRpcTcpClient &) = delete;
+    JsonRpcTcpClient & operator=(const JsonRpcTcpClient &) = delete;
+
+    void receive();
+
+    asio::io_context _ioc;
+    asio::ip::tcp::socket _socket;
+    int _jsonRpcId;
+    asio::streambuf _tcpStreambuf;
+    std::ostream _tcpOutStream;
+    std::unique_ptr<Json::StreamWriter> _jsonStreamWriter;
+    std::binary_semaphore _receiveMethodResponseSem;
+    Json::Value _receiveMethodResponseJsonValue;
+    std::map<std::string, NotificationHandle> _notificationHandles;
+    bool _isStartReceive;
+};
+
+#endif

src/main.cpp

@@ -0,0 +1,157 @@
+#include "PosHistory.hpp"
+#include "PosTracker.hpp"
+#include "filter.hpp"
+#include "robot.hpp"
+#include "utils.hpp"
+
+#include <SFML/Graphics/PrimitiveType.hpp>
+#include <SFML/Graphics/RectangleShape.hpp>
+#include <SFML/Graphics/RenderWindow.hpp>
+#include <SFML/Graphics/Transform.hpp>
+#include <SFML/Graphics/Transformable.hpp>
+#include <SFML/Graphics/Vertex.hpp>
+#include <SFML/Graphics/VertexArray.hpp>
+#include <SFML/System/Vector2.hpp>
+#include <SFML/Window/Event.hpp>
+#include <SFML/Window/Keyboard.hpp>
+#include <csignal>
+#include <iostream>
+#include <sstream>
+
+using sf::Vector2f;
+
+using namespace std::chrono_literals;
+
+constexpr auto THRESHOLD = 10;
+
+void controller_follow_border_step(Robot& robot, float speed_factor) {
+    auto line_value = DBG(int(robot.getLineTracksValue(0)));
+    auto detects_line = line_value > THRESHOLD;
+    if (detects_line) robot.setMotorsPower(0.2 * speed_factor, 0.1 * speed_factor);
+    else robot.setMotorsPower(0.1 * speed_factor, 0.2 * speed_factor);
+}
+
+void controller_follow_border(Robot &robot, float speed_factor) {
+    while (true) {
+        auto events = {EventType::LINE_TRACKS_IS_DETECTED,EventType::SWITCHS_IS_DETECTED};
+        robot.waitChanged(events, 1s);
+        controller_follow_border_step(robot, speed_factor);
+    }
+}
+
+// note : Ne marche pas car remplace le stack du code interrompu.
+static bool do_stop = false;
+void stop_handler(int) {
+    do_stop = true;
+}
+
+void event(sf::RenderWindow & window, Robot& robot) {
+    bool do_stop_ = do_stop;
+    sf::Event event;
+    while (window.pollEvent(event)) {
+        if (event.type == sf::Event::Resized) {
+            sf::FloatRect visibleArea(0.f, 0.f, event.size.width, event.size.height);
+            window.setView(sf::View(visibleArea));
+        }
+        if (event.type == sf::Event::Closed) do_stop_ = true;
+        if (event.type == sf::Event::KeyPressed && event.key.code == sf::Keyboard::Escape) do_stop_ = true;
+    }
+    if (do_stop_) {
+        robot.setMotorPower(Robot::MotorIndex::LEFT, 0);
+        robot.setMotorPower(Robot::MotorIndex::RIGHT, 0);
+        window.close();
+    }
+}
+
+
+constexpr float ROBOT_SIZE = 20.0;
+constexpr float WHEEL_DIAMETER = 6.5;
+constexpr float LATICE_COUNT = 20;
+
+
+class ControlWindow {
+    private:
+        Robot& _robot;
+        sf::RenderWindow _window;
+        sf::RectangleShape _robotShape;
+        PosTracker _tracker;
+        PosHistory _history;
+        MotorJitterFilter _filter;
+        float _speed_factor;
+
+    public:
+        ControlWindow(Robot& robot, Vector2f robot_size, float wheel_diameter, float latice_count, int history_count, float speed_factor, float filter_delay) :
+            _robot(robot),
+            _window(sf::RenderWindow(
+            sf::VideoMode(800u, 600u),
+            "robot-command",
+            sf::Style::Titlebar | sf::Style::Close
+            )),
+            _robotShape(sf::RectangleShape(robot_size)),
+            _tracker(PosTracker(Vector2f(90, 90), 0., robot_size.y, wheel_diameter, latice_count)),
+            _history(PosHistory(history_count)),
+            _speed_factor(speed_factor),
+            _filter(filter_delay)
+        {
+            _window.setVerticalSyncEnabled(true);
+            _robotShape.setOrigin(sf::Vector2f(robot_size.x / 2, robot_size.y / 2));
+            _robotShape.setFillColor(sf::Color::Blue);
+            _tracker.update(robot.getEncoderWheelValue(1), robot.getEncoderWheelValue(0));
+            _tracker.reset(Vector2f(90, 90), 0.);
+        }
+
+        void run() {
+            while (_window.isOpen()) {
+                update();
+                controller_follow_border_step(_robot, _speed_factor);
+            }
+        }
+
+    private:
+        void update() {
+            _tracker.update(_robot.getEncoderWheelValue(1), _robot.getEncoderWheelValue(0));
+
+            _robotShape.setPosition(_tracker.getPosition());
+            _robotShape.setRotation(_tracker.getRotation());
+            _history.append(_tracker.getPosition());
+
+            event(_window, _robot);
+            _window.clear();
+            _window.draw(_robotShape);
+            _window.draw(_history.vertices());
+            _window.display();
+        }
+};
+
+
+int main(int argc, char ** argv)
+{
+    signal(SIGKILL, stop_handler);
+    std::string hostIpAddress("127.0.0.1");
+    uint16_t tcpPort = 6543;
+
+    if (argc == 3)
+    {
+        hostIpAddress = std::string(argv[1]);
+        std::istringstream iss(argv[2]);
+        iss.exceptions(std::istringstream::failbit);
+        iss >> tcpPort;
+    }
+
+    std::cout << "Try to connect to " << hostIpAddress << ":" << tcpPort << " ..." << std::endl;
+    Robot robot(hostIpAddress, tcpPort);
+    robot.waitReady();
+
+    try {
+        // #define STEP_1
+        #ifdef STEP_1
+        controller_follow_border(robot);
+        #else
+        ControlWindow(robot, Vector2f(ROBOT_SIZE, ROBOT_SIZE), WHEEL_DIAMETER, LATICE_COUNT, 4000, 0.78, 1).run();
+        #endif
+    } catch (std::exception &e) {
+      std::cerr << e.what() << std::endl;
+    }
+
+    return 0;
+}

src/robot.cpp

@@ -0,0 +1,137 @@
+#include "robot.hpp"
+
+#include <json/value.h>
+
+
+Robot::Robot(const std::string & hostIpAddress, uint16_t tcpPort)
+    : _jsonRpcTcpClient(hostIpAddress, tcpPort)
+    , _irProximitysDistanceDetected(this)
+    , _lineTracksIsDetected(this, 1, false)
+    , _lineTracksValue(this, 1, 0)
+    , _encoderWheelsValue(this, 2, 0)
+    , _switchsIsDetected(this)
+    , _ultrasoundsDistanceDetected(this)
+    , _isReadySemaphore(0)
+    , _eventCvMutex()
+    , _eventCv()
+{
+    _jsonRpcTcpClient.bindNotification("irProximityDistanceDetected", [this](const Json::Value & params){
+        std::size_t index = params["index"].asUInt();
+        _irProximitysDistanceDetected.set(index, params["value"].asUInt(), params["changedCount"].asInt());
+    });
+    _jsonRpcTcpClient.bindNotification("lineTrackIsDetected", [this](const Json::Value & params){
+        std::size_t index = params["index"].asUInt();
+        _lineTracksIsDetected.set(index, params["value"].asBool(), params["changedCount"].asInt());
+    });
+    _jsonRpcTcpClient.bindNotification("lineTrackValue", [this](const Json::Value & params){
+        std::size_t index = params["index"].asUInt();
+        _lineTracksValue.set(index, params["value"].asUInt(), params["changedCount"].asInt());
+    });
+    _jsonRpcTcpClient.bindNotification("encoderWheelValue", [this](const Json::Value & params){
+        std::size_t index = params["index"].asUInt();
+        _encoderWheelsValue.set(index, params["value"].asUInt(), params["changedCount"].asInt());
+    });
+    _jsonRpcTcpClient.bindNotification("switchIsDetected", [this](const Json::Value & params){
+        std::size_t index = params["index"].asUInt();
+        _switchsIsDetected.set(index, params["value"].asBool(), params["changedCount"].asInt());
+    });
+    _jsonRpcTcpClient.bindNotification("ultrasoundDistanceDetected", [this](const Json::Value & params){
+        std::size_t index = params["index"].asUInt();
+        _ultrasoundsDistanceDetected.set(index, params["value"].asUInt(), params["changedCount"].asInt());
+    });
+    _jsonRpcTcpClient.bindNotification("setIsReady", [this](const Json::Value & params){
+        assert(params.isNull());
+        _isReadySemaphore.release();
+    });
+    _jsonRpcTcpClient.startReceive();
+}
+
+Robot::~Robot()
+{}
+
+void Robot::setMotorPower(MotorIndex motorIndex, float value)
+{
+    Json::Value params;
+    params["motorIndex"] = motorIndexToStringHelper(motorIndex);
+    params["value"] = value;
+    _jsonRpcTcpClient.callNotification("setMotorPower", params);
+
+}
+void Robot::setMotorsPower(float rightValue, float leftValue)
+{
+    Json::Value params;
+    params["rightValue"] = rightValue;
+    params["leftValue"] = leftValue;
+    _jsonRpcTcpClient.callNotification("setMotorsPower", params);
+}
+
+void Robot::waitChanged(EventType eventType) {
+    auto eventTypes = waitParamHelper({eventType});
+    waitChangedHelper(eventTypes);
+}
+
+EventType Robot::waitChanged(const std::set<EventType> & eventTypes)
+{
+    auto eventTypesWithChangedCount = waitParamHelper(eventTypes);
+    return waitChangedHelper(eventTypesWithChangedCount);
+}
+
+void Robot::notify(EventType eventType, int changedCount)
+{
+    {
+        std::lock_guard<std::mutex> lk(_eventCvMutex);
+        _lastNotifiedEventType.insert_or_assign(eventType, changedCount);
+    }
+    _eventCv.notify_all();
+}
+
+std::string Robot::motorIndexToStringHelper(MotorIndex motorIndex)
+{
+    switch (motorIndex)
+    {
+        case MotorIndex::RIGHT:
+            return "RIGHT";
+        case MotorIndex::LEFT:
+            return "LEFT";
+    }
+    throw std::invalid_argument(std::string("Cannot convert ")
+            + std::to_string(static_cast<int>(motorIndex)) + " into Robot::MotorIndex");
+}
+
+void Robot::waitChangedHelper(EventType eventType, int & changedCount) {
+    std::map<EventType, int> eventTypes{{eventType, changedCount}};
+    waitChangedHelper(eventTypes);
+    changedCount = eventTypes.at(eventType);
+}
+
+EventType Robot::waitChangedHelper(std::map<EventType, int> & eventTypes) {
+    std::unique_lock<std::mutex> lk(_eventCvMutex);
+    EventType notifiedEventType;
+    _eventCv.wait(lk, [eventTypes, &notifiedEventType, lastNotifiedEventType = std::ref(_lastNotifiedEventType)]{
+        for (auto eventType : eventTypes) {
+            if (lastNotifiedEventType.get().at(eventType.first)>eventType.second)
+            {
+                lastNotifiedEventType.get().at(eventType.first) = eventType.second;
+                notifiedEventType = eventType.first;
+                return true;
+            }
+        }
+        return false;
+    });
+    return notifiedEventType;
+}
+
+std::map<EventType, int> Robot::waitParamHelper(std::set<EventType> eventTypes)
+{
+    std::map<EventType, int> toReturn;
+    std::unique_lock<std::mutex> lk(_eventCvMutex);
+    for (auto eventType : eventTypes)
+    {
+        int changedCount = -1;
+        auto it = _lastNotifiedEventType.find(eventType);
+        if (it != _lastNotifiedEventType.end())
+            changedCount = it->second;
+        toReturn.insert(std::make_pair(eventType, changedCount));
+    }
+    return toReturn;
+}

src/robot.hpp

@@ -0,0 +1,158 @@
+#ifndef ROBOT_HPP
+#define ROBOT_HPP
+
+#include "jsonrpctcpclient.hpp"
+#include "values.hpp"
+
+#include <string>
+#include <optional>
+#include <semaphore>
+#include <condition_variable>
+#include <set>
+#include <chrono>
+
+enum class EventType
+{
+    IR_PROXIMITYS_DISTANCE_DETECTED, //!< A new value of IR distance have been received
+    LINE_TRACKS_IS_DETECTED, //!< A new value of the boolean is line detected have been received
+    LINE_TRACKS_VALUE, //!< A new value of the raw line color have been received
+    ENCODER_WHEELS_VALUE, //!< A new value of the encoder wheels have been received
+    SWITCHS_IS_DETECTED, //!< A new value of the switch have been received
+    ULTRASOUNDS_DISTANCE_DETECTED //!< A new value of ultrasound distance have been received
+};
+
+class Robot : public IRobot<EventType>
+{
+public:
+    //! @brief Create a new robot connexion with a robot server (simu or reel).
+    Robot(const std::string & hostIpAddress, uint16_t tcpPort);
+
+    //! @brief Close the robot connexion.
+    virtual ~Robot();
+
+    //! @brief Wait for the robot server to be ready to send or receive messages.
+    inline void waitReady() {using namespace std::chrono_literals; _isReadySemaphore.acquire();
+            std::this_thread::sleep_for(100ms);}
+
+    //! @return The last IR distance receive from robot server (in pixel on simu).
+    //! @param index The index of this sensor ont he robot starting from 0.
+    std::size_t getIrProximitysDistanceDetected(std::size_t index) const {return _irProximitysDistanceDetected.get(index);}
+
+    //! @return True if the line has been detected in the last receive message from robot server.
+    //! @param index The index of this sensor ont he robot starting from 0.
+    bool getLineTracksIsDetected(std::size_t index) const {return _lineTracksIsDetected.get(index);}
+
+    //! @return The last raw line color receive from robot server (0 to 255).
+    //! @param index The index of this sensor ont he robot starting from 0.
+    std::uint8_t getLineTracksValue(std::size_t index) const {return _lineTracksValue.get(index);}
+
+    //! @return The last encoder wheel latice count receive from robot server.
+    //! @param index The index of this sensor ont he robot starting from 0.
+    std::size_t getEncoderWheelValue(std::size_t index) const {return _encoderWheelsValue.get(index);}
+
+    //! @return True if last switch state receive from robot server is pressed.
+    //! @param index The index of this sensor ont he robot starting from 0.
+    bool getSwitchsIsDetected(std::size_t index) const {return _switchsIsDetected.get(index);}
+
+    //! @return The last ultrasound distance receive from robot server (in pixel on simu).
+    //! @param index The index of this sensor ont he robot starting from 0.
+    std::size_t getUltrasoundsDistanceDetected(std::size_t index) const {return _ultrasoundsDistanceDetected.get(index);}
+
+    //! @brief Set current motor(s) power(s).
+    //! @param value PWM between -1.0 and 1.0.
+    //! \{
+    enum class MotorIndex {RIGHT = 0, LEFT = 1};
+    void setMotorPower(MotorIndex motorIndex, float value);
+    void setMotorsPower(float rightValue, float leftValue);
+    //! \}
+
+    //! @brief Wait until this specific event has been received
+    void waitChanged(EventType eventType);
+
+    //! @brief Wait until one of the listed event has been received
+    //! @return The received event
+    EventType waitChanged(const std::set<EventType> & eventType);
+
+    //! @brief Wait until this specific event has been received or timeout
+    //! @return True if this event has been receive or false if timeout
+    template<typename _Rep, typename _Period>
+    bool waitChanged(EventType eventType, const std::chrono::duration<_Rep, _Period> & duration);
+
+    //! @brief Wait until one of the listed event has been received
+    //! @return The received event if a event of the list has been received or an empty value if timeout
+    template<typename _Rep, typename _Period>
+    std::optional<EventType> waitChanged(const std::set<EventType> & eventTypes, const std::chrono::duration<_Rep, _Period> & duration);
+
+private:
+    Robot(const Robot &) = delete;
+    Robot & operator=(const Robot &) = delete;
+
+    void notify(EventType eventType, int changedCount) override;
+
+    static std::string motorIndexToStringHelper(MotorIndex motorIndex);
+    void waitChangedHelper(EventType eventType, int & changedCount);
+    EventType waitChangedHelper(std::map<EventType, int> & eventTypes);
+    template<typename _Rep, typename _Period>
+    bool waitChangedHelper(EventType eventType, int & changedCount, const std::chrono::duration<_Rep, _Period> & duration);
+    template<typename _Rep, typename _Period>
+    std::optional<EventType> waitChangedHelper(std::map<EventType, int> & eventTypes, const std::chrono::duration<_Rep, _Period> & duration);
+    std::map<EventType, int> waitParamHelper(std::set<EventType> eventTypes);
+
+    JsonRpcTcpClient _jsonRpcTcpClient;
+    Values<std::size_t, EventType, EventType::IR_PROXIMITYS_DISTANCE_DETECTED> _irProximitysDistanceDetected;
+    Values<bool, EventType, EventType::LINE_TRACKS_IS_DETECTED> _lineTracksIsDetected;
+    Values<std::uint8_t, EventType, EventType::LINE_TRACKS_VALUE> _lineTracksValue;
+    Values<std::size_t, EventType, EventType::ENCODER_WHEELS_VALUE> _encoderWheelsValue;
+    Values<bool, EventType, EventType::SWITCHS_IS_DETECTED> _switchsIsDetected;
+    Values<std::size_t, EventType, EventType::ULTRASOUNDS_DISTANCE_DETECTED> _ultrasoundsDistanceDetected;
+    std::binary_semaphore _isReadySemaphore;
+    std::mutex _eventCvMutex;
+    std::condition_variable _eventCv;
+    std::map<EventType, int> _lastNotifiedEventType;
+};
+
+
+template<typename _Rep, typename _Period>
+bool Robot::waitChanged(EventType eventType, const std::chrono::duration<_Rep, _Period> & duration)
+{
+    auto eventTypes = waitParamHelper({eventType});
+    return waitChangedHelper(eventTypes, duration).has_value();
+}
+
+template<typename _Rep, typename _Period>
+bool Robot::waitChangedHelper(EventType eventType, int & changedCount, const std::chrono::duration<_Rep, _Period> & duration)
+{
+    std::map<EventType, int> eventTypes{{eventType, changedCount}};
+    bool toReturn = waitChangedHelper(eventTypes, duration).has_value();
+    changedCount = eventTypes.at(eventType);
+    return toReturn;
+}
+
+template<typename _Rep, typename _Period>
+std::optional<EventType> Robot::waitChanged(const std::set<EventType> & eventTypes, const std::chrono::duration<_Rep, _Period> & duration)
+{
+    auto eventTypesWithChangedCount = waitParamHelper(eventTypes);
+    return waitChangedHelper(eventTypesWithChangedCount, duration);
+}
+
+template<typename _Rep, typename _Period>
+std::optional<EventType> Robot::waitChangedHelper(std::map<EventType, int> & eventTypes, const std::chrono::duration<_Rep, _Period> & duration)
+{
+    std::unique_lock<std::mutex> lk(_eventCvMutex);
+    EventType notifiedEventType;
+    if (!_eventCv.wait_for(lk, duration, [eventTypes, &notifiedEventType, lastNotifiedEventType = std::ref(_lastNotifiedEventType)]{
+        for (auto eventType : eventTypes) {
+            if (lastNotifiedEventType.get().at(eventType.first)>eventType.second)
+            {
+                lastNotifiedEventType.get().at(eventType.first) = eventType.second;
+                notifiedEventType = eventType.first;
+                return true;
+            }
+        }
+        return false;
+    }))
+        return {};
+    return notifiedEventType;
+}
+
+#endif

src/utils.hpp

@@ -0,0 +1,5 @@
+#pragma once
+
+#include <iostream>
+
+#define DBG(X) ({ auto value = X; std::cout << "(DBG) [" << __FILE__ << ":" << __LINE__ << "] (" << #X << ") = " << value << "\n"; value;})

src/values.hpp

@@ -0,0 +1,63 @@
+#ifndef VALUES_HPP
+#define VALUES_HPP
+
+#include <cstddef>
+#include <mutex>
+#include <optional>
+#include <vector>
+
+
+template<typename EventType>
+class IRobot
+{
+public:
+    virtual ~IRobot() {}
+    virtual void notify(EventType eventType, int changedCount) = 0;
+};
+
+
+template<typename T, typename EventType, EventType EVENT_TYPE_VALUE>
+class Values
+{
+public:
+    Values(IRobot<EventType> * robot) : _robot(robot) {}
+    Values(IRobot<EventType> * robot, size_t values_count, T defaults) : _robot(robot) {
+        for (size_t i = 0; i < values_count; i += 1) {
+            auto value = Value();
+            value._value = defaults;
+            this->_values.push_back(value);
+        }
+    }
+
+    inline T get(std::size_t index) const {const std::lock_guard<std::mutex> lock(_mutex); return _values.at(index)._value.value();}
+    inline int getChangedCount(std::size_t index) const {const std::lock_guard<std::mutex> lock(_mutex); return _values.at(index)._changedCount.value();}
+    void set(std::size_t index, T v, int changedCount)
+    {
+        const std::lock_guard<std::mutex> lock(_mutex);
+        if (index>=_values.size())
+            _values.resize(index+1);
+        Value & value = _values.at(index);
+        value._value = v;
+        if (value._changedCount.has_value())
+        {
+            value._changedCount.value()++;
+            assert(value._changedCount == changedCount);
+        }
+        else
+            value._changedCount = changedCount;
+        _robot->notify(EVENT_TYPE_VALUE, changedCount);
+    }
+
+private:
+    struct Value
+    {
+        Value() : _value(), _changedCount() {}
+        std::optional<T> _value;
+        std::optional<int> _changedCount;
+    };
+    std::vector<Value> _values;
+    mutable std::mutex _mutex;
+    IRobot<EventType> * _robot;
+};
+
+#endif 

watch.sh

@@ -0,0 +1,5 @@
+#!/bin/sh
+set -e
+cd "$(dirname "$(realpath "$0")")"
+
+nodemon -w src -e c,cpp,h,hpp -x './build.sh && ./run.sh'