commit 8d45088064b8ecf07e1845580d1312466b6ffa77 Author: JOLIMAITRE Matthieu Date: Wed May 29 15:18:03 2024 +0200 init diff --git a/.clangd b/.clangd new file mode 100644 index 0000000..06c75bf --- /dev/null +++ b/.clangd @@ -0,0 +1,3 @@ +CompileFlags: + Add: + - -std=c++20 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..00a6291 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +/.vscode +/build diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..8afb7d4 --- /dev/null +++ b/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 "$") +set(msvc_cxx "$") + +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 () diff --git a/README.md b/README.md new file mode 100644 index 0000000..c61d66f --- /dev/null +++ b/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:/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. diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..61dc0a3 --- /dev/null +++ b/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) diff --git a/run.sh b/run.sh new file mode 100755 index 0000000..5ec5ee0 --- /dev/null +++ b/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 $@ + diff --git a/src/PosHistory.cpp b/src/PosHistory.cpp new file mode 100644 index 0000000..ba80318 --- /dev/null +++ b/src/PosHistory.cpp @@ -0,0 +1,25 @@ +#include "PosHistory.hpp" +#include +#include +#include + +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; +} diff --git a/src/PosHistory.hpp b/src/PosHistory.hpp new file mode 100644 index 0000000..e48f1fd --- /dev/null +++ b/src/PosHistory.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include +#include +#include +#include +#include + +class PosHistory { + sf::VertexArray _points; + std::optional _lastInsertion; + size_t _insertionIndex; + size_t _capacity; + public: + inline PosHistory(size_t capacity) : + _points(sf::VertexArray(sf::PrimitiveType::Points, capacity)), + _lastInsertion(std::optional()), + _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); +}; diff --git a/src/PosTracker.cpp b/src/PosTracker.cpp new file mode 100644 index 0000000..1d49f2e --- /dev/null +++ b/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_; +} diff --git a/src/PosTracker.hpp b/src/PosTracker.hpp new file mode 100644 index 0000000..7400634 --- /dev/null +++ b/src/PosTracker.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +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); +}; diff --git a/src/filter.cpp b/src/filter.cpp new file mode 100644 index 0000000..04994c1 --- /dev/null +++ b/src/filter.cpp @@ -0,0 +1,25 @@ +#include +#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; +} diff --git a/src/filter.hpp b/src/filter.hpp new file mode 100644 index 0000000..91cecd5 --- /dev/null +++ b/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); +}; diff --git a/src/jsonrpctcpclient.cpp b/src/jsonrpctcpclient.cpp new file mode 100644 index 0000000..9b0dcea --- /dev/null +++ b/src/jsonrpctcpclient.cpp @@ -0,0 +1,161 @@ +#include "jsonrpctcpclient.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +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 & 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(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(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(0x0A)); + + // Parse this json message + Json::CharReaderBuilder builder; + JSONCPP_STRING errs; + Json::Value messageJson; + const std::unique_ptr 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"]); + } + } + } +} diff --git a/src/jsonrpctcpclient.hpp b/src/jsonrpctcpclient.hpp new file mode 100644 index 0000000..f61b0c2 --- /dev/null +++ b/src/jsonrpctcpclient.hpp @@ -0,0 +1,49 @@ +#ifndef JSONRPCTCPCLIENT_HPP +#define JSONRPCTCPCLIENT_HPP + +#include +#include +#include +#include +#include + +namespace Json +{ + class StreamWriter; +} + + +class JsonRpcTcpClient +{ +public: + JsonRpcTcpClient(const std::string & hostIpAddress, unsigned short tcpPort); + ~JsonRpcTcpClient(); + + using NotificationHandle = std::function; + 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 _jsonStreamWriter; + std::binary_semaphore _receiveMethodResponseSem; + Json::Value _receiveMethodResponseJsonValue; + std::map _notificationHandles; + bool _isStartReceive; +}; + +#endif \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..efe2370 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,157 @@ +#include "PosHistory.hpp" +#include "PosTracker.hpp" +#include "filter.hpp" +#include "robot.hpp" +#include "utils.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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; +} diff --git a/src/robot.cpp b/src/robot.cpp new file mode 100644 index 0000000..e2eb84d --- /dev/null +++ b/src/robot.cpp @@ -0,0 +1,137 @@ +#include "robot.hpp" + +#include + + +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 & eventTypes) +{ + auto eventTypesWithChangedCount = waitParamHelper(eventTypes); + return waitChangedHelper(eventTypesWithChangedCount); +} + +void Robot::notify(EventType eventType, int changedCount) +{ + { + std::lock_guard 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(motorIndex)) + " into Robot::MotorIndex"); +} + +void Robot::waitChangedHelper(EventType eventType, int & changedCount) { + std::map eventTypes{{eventType, changedCount}}; + waitChangedHelper(eventTypes); + changedCount = eventTypes.at(eventType); +} + +EventType Robot::waitChangedHelper(std::map & eventTypes) { + std::unique_lock lk(_eventCvMutex); + EventType notifiedEventType; + _eventCv.wait(lk, [eventTypes, ¬ifiedEventType, 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 Robot::waitParamHelper(std::set eventTypes) +{ + std::map toReturn; + std::unique_lock 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; +} diff --git a/src/robot.hpp b/src/robot.hpp new file mode 100644 index 0000000..fc9a6fc --- /dev/null +++ b/src/robot.hpp @@ -0,0 +1,158 @@ +#ifndef ROBOT_HPP +#define ROBOT_HPP + +#include "jsonrpctcpclient.hpp" +#include "values.hpp" + +#include +#include +#include +#include +#include +#include + +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 +{ +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); + + //! @brief Wait until this specific event has been received or timeout + //! @return True if this event has been receive or false if timeout + template + 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 + std::optional waitChanged(const std::set & 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 & eventTypes); + template + bool waitChangedHelper(EventType eventType, int & changedCount, const std::chrono::duration<_Rep, _Period> & duration); + template + std::optional waitChangedHelper(std::map & eventTypes, const std::chrono::duration<_Rep, _Period> & duration); + std::map waitParamHelper(std::set eventTypes); + + JsonRpcTcpClient _jsonRpcTcpClient; + Values _irProximitysDistanceDetected; + Values _lineTracksIsDetected; + Values _lineTracksValue; + Values _encoderWheelsValue; + Values _switchsIsDetected; + Values _ultrasoundsDistanceDetected; + std::binary_semaphore _isReadySemaphore; + std::mutex _eventCvMutex; + std::condition_variable _eventCv; + std::map _lastNotifiedEventType; +}; + + +template +bool Robot::waitChanged(EventType eventType, const std::chrono::duration<_Rep, _Period> & duration) +{ + auto eventTypes = waitParamHelper({eventType}); + return waitChangedHelper(eventTypes, duration).has_value(); +} + +template +bool Robot::waitChangedHelper(EventType eventType, int & changedCount, const std::chrono::duration<_Rep, _Period> & duration) +{ + std::map eventTypes{{eventType, changedCount}}; + bool toReturn = waitChangedHelper(eventTypes, duration).has_value(); + changedCount = eventTypes.at(eventType); + return toReturn; +} + +template +std::optional Robot::waitChanged(const std::set & eventTypes, const std::chrono::duration<_Rep, _Period> & duration) +{ + auto eventTypesWithChangedCount = waitParamHelper(eventTypes); + return waitChangedHelper(eventTypesWithChangedCount, duration); +} + +template +std::optional Robot::waitChangedHelper(std::map & eventTypes, const std::chrono::duration<_Rep, _Period> & duration) +{ + std::unique_lock lk(_eventCvMutex); + EventType notifiedEventType; + if (!_eventCv.wait_for(lk, duration, [eventTypes, ¬ifiedEventType, 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 diff --git a/src/utils.hpp b/src/utils.hpp new file mode 100644 index 0000000..eb02f33 --- /dev/null +++ b/src/utils.hpp @@ -0,0 +1,5 @@ +#pragma once + +#include + +#define DBG(X) ({ auto value = X; std::cout << "(DBG) [" << __FILE__ << ":" << __LINE__ << "] (" << #X << ") = " << value << "\n"; value;}) diff --git a/src/values.hpp b/src/values.hpp new file mode 100644 index 0000000..1c00bb5 --- /dev/null +++ b/src/values.hpp @@ -0,0 +1,63 @@ +#ifndef VALUES_HPP +#define VALUES_HPP + +#include +#include +#include +#include + + +template +class IRobot +{ +public: + virtual ~IRobot() {} + virtual void notify(EventType eventType, int changedCount) = 0; +}; + + +template +class Values +{ +public: + Values(IRobot * robot) : _robot(robot) {} + Values(IRobot * 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 lock(_mutex); return _values.at(index)._value.value();} + inline int getChangedCount(std::size_t index) const {const std::lock_guard lock(_mutex); return _values.at(index)._changedCount.value();} + void set(std::size_t index, T v, int changedCount) + { + const std::lock_guard 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 _value; + std::optional _changedCount; + }; + std::vector _values; + mutable std::mutex _mutex; + IRobot * _robot; +}; + +#endif diff --git a/watch.sh b/watch.sh new file mode 100755 index 0000000..b1cf392 --- /dev/null +++ b/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'