RemoteControl.cpp

Go to the documentation of this file.

#include "remotecontrol/RemoteControl.hpp"
#include <galaxis/node_ros_pubsub_wrapper.hpp>
 
#include <iostream>
#include <stdio.h>
 
#include <utils/logger.hpp>
#include <unistd.h>
 
using namespace galaxis::ros;
using namespace galaxis::utils;
 
RemoteControl::RemoteControl() :
    rclcpp::Node("SimulationRemoteControl") {
  rclcpp::QoS qos_params = get_qos_profile(QOS_BEST_EFFORT);
  m_publisherSPtr        = this->create_publisher<SimulationVehicleControlType>("galaxis/simulation/remotecontrol", qos_params);
  log("Vehicle Control Message Publisher created.");
 
  loop();
}
 
RemoteControl::~RemoteControl() {
  system("stty cooked");
}
 
void RemoteControl::loop() {
  log("Start RemoteControl Loop");
  log("Press 'w a d 1 2 3 4 e' for drive and ESC/q for quit");
 
  // get raw input from terminal
  system("stty raw");
 
  log("Waiting for input...");
 
  int index = 0;
  while(true) {
    char c;
    c = getchar();
 
    // DO NOT REMOVE THIS LINE!
    if(c == 'q' || c == 27) {
      m_speed = 0.f;
      sendControlInputs();
      break;
    }
 
    switch(c) {
      case 'w':
        m_yaw = 0.f;
        break;
      case 'a':
        m_yaw += 0.15;
        break;
      case 'd':
        m_yaw += -0.15;
        break;
      case '1':
        m_speed = 0.f;
        break;
      case '2':
        m_speed = 0.3f;
        break;
      case '3':
        m_speed = 1.f;
        break;
      case '4':
        m_speed = 2.f;
        break;
      case 'r':
        m_reset = true;
        break;
      case 'e':
        m_remoteEnabled = m_remoteEnabled ? false : true;
        m_speed = 0.f;
        break;
      default:
        log(" No Command. [", c, "] ");
        break;
    }
 
    log_nonewline("\r[", ++index, "] Update VehicleControl Inputs. Speed: ", m_speed, " [m/s] Turning Angle: ", m_yaw, " ");
    sendControlInputs();
 
    usleep(100000);
  }
  system("stty cooked");
 
  log("\nRemoteControl exited.");
  exit(0);
}
 
void RemoteControl::sendControlInputs() {
  auto message = std::make_unique<SimulationVehicleControlType>();
 
  message->car_speed       = m_speed;
  message->turnangle_front = m_yaw;
 
  message->reset = m_reset;
  m_reset        = false;
 
  message->remote_control = m_remoteEnabled ? 1 : 0;
 
  m_publisherSPtr->publish(std::move(message));
}