Branch data     Line data    Source code

       1                 :             : #include "remoteMove.hpp"
       2                 :             : 
       3                 :             : #include <chrono>
       4                 :             : #include <cstring>
       5                 :             : #include <stdexcept>
       6                 :             : 
       7                 :           0 : RemoteMove::RemoteMove(CarMove& car_instance, std::string bind_address)
       8                 :           0 :     : context(1),
       9                 :           0 :       socket(context, zmq::socket_type::rep),
      10                 :           0 :       address(std::move(bind_address)),
      11                 :           0 :       car(car_instance),
      12                 :           0 :       running(false) {
      13                 :           0 :     socket.bind(address);
      14                 :           0 :     std::cout << "Remote control server started at " << address << "\n";
      15                 :           0 : }
      16                 :             : 
      17                 :           0 : RemoteMove::~RemoteMove() { stop(); }
      18                 :             : 
      19                 :           0 : void RemoteMove::start() {
      20                 :           0 :     if (!running) {
      21                 :           0 :         running = true;
      22                 :           0 :         listener_thread = std::thread(&RemoteMove::listenerLoop, this);
      23                 :           0 :         std::cout << "Remote control listener started" << "\n";
      24                 :             :     }
      25                 :           0 : }
      26                 :             : 
      27                 :           0 : void RemoteMove::stop() {
      28                 :           0 :     if (running) {
      29                 :           0 :         running = false;
      30                 :           0 :         if (listener_thread.joinable()) {
      31                 :           0 :             listener_thread.join();
      32                 :             :         }
      33                 :           0 :         std::cout << "Remote control listener stopped" << "\n";
      34                 :             :     }
      35                 :           0 : }
      36                 :             : 
      37                 :           0 : void RemoteMove::listenerLoop() {
      38                 :           0 :     while (running) {
      39                 :           0 :         zmq::message_t request;
      40                 :           0 :         if (socket.recv(request, zmq::recv_flags::dontwait)) {
      41                 :           0 :             if (request.size() == 8) {
      42                 :           0 :                 auto* data = static_cast<float*>(request.data());
      43                 :           0 :                 float acceleration = data[0];
      44                 :           0 :                 float steering = data[1];
      45                 :           0 :                 acceleration = std::max(-1.0F, std::min(1.0F, acceleration));
      46                 :           0 :                 steering = std::max(-1.0F, std::min(1.0F, steering));
      47                 :           0 :                 processCommand(acceleration, steering);
      48                 :           0 :                 zmq::message_t reply(2);
      49                 :           0 :                 memcpy(reply.data(), "OK", 2);
      50                 :           0 :                 socket.send(reply, zmq::send_flags::none);
      51                 :           0 :             } else {
      52                 :           0 :                 std::cout << "Received invalid message size: " << request.size() << " bytes"
      53                 :           0 :                           << "\n";
      54                 :           0 :                 zmq::message_t reply(5);
      55                 :           0 :                 memcpy(reply.data(), "ERROR", 5);
      56                 :           0 :                 socket.send(reply, zmq::send_flags::none);
      57                 :           0 :             }
      58                 :             :         }
      59                 :           0 :         std::this_thread::sleep_for(std::chrono::milliseconds(10));
      60                 :           0 :     }
      61                 :           0 : }
      62                 :             : 
      63                 :           0 : void RemoteMove::processCommand(float acceleration, float steering) {
      64                 :             :     // Convert acceleration (-1 to 1) to motor speed (-100 to 100)
      65                 :           0 :     int motorspeed = static_cast<int>(acceleration * 100);
      66                 :             : 
      67                 :             :     // Convert steering (-1 to 1) to servo angle (-MAX_ANGLE to MAX_ANGLE)
      68                 :           0 :     int servo_angle = static_cast<int>(steering * 45);
      69                 :           0 :     std::cout << "Command received - Acceleration: " << acceleration << " (" << motorspeed
      70                 :           0 :               << "%), Steering: " << steering << " (" << servo_angle << "°)!n";
      71                 :             :     // Apply the commands to the car
      72                 :           0 :     car.setMotorSpeed(motorspeed);
      73                 :           0 :     car.setServoAngle(servo_angle);
      74                 :           0 : }