ControllerNode.cpp

Go to the documentation of this file.

#include <cstdio>
#include "ControllerNode.hpp"
 
#include <galaxis/AutopilotSettings.hpp>
#include <galaxis/motorSettings.hpp>
#include <galaxis/controllerSettings.hpp>
#include <galaxis/node_ros_pubsub_wrapper.hpp>
#include <utils/file.hpp>
#include <utils/settings.hpp>
 
#include <unistd.h> // TODO REMOVE
 
using namespace galaxis::control;
using namespace galaxis::ros;
using namespace galaxis::utils;
 
using std::placeholders::_1;
 
ControllerNode::ControllerNode() :
    GalaxisNode("ControllerNode") {
 
  log("Starting ControllerNode...");
 
  galaxis::ros::create_subscription(this,
                                    m_measurementSubSPtr,
                                    std::bind(&ControllerNode::on_measurement_callback, this, _1));
  galaxis::ros::create_subscription(this,
                                    m_pathPolynomSubSPtr,
                                    std::bind(&ControllerNode::on_pathpolynom_callback, this, _1));
 
  galaxis::ros::create_subscription(this,
                                    m_controllerRefSubSPtr,
                                    std::bind(&ControllerNode::on_controller_reference_callback, this, _1));
 
  galaxis::ros::create_publisher(this, m_controlValuesPubSPtr);
  galaxis::ros::create_publisher(this, m_controlLightsPubSPtr);
  galaxis::ros::create_publisher(this, m_debugControllerPubSPtr);
 
 
  rclcpp::QoS qosParamsR             = get_qos_profile(QOS_RELIABLE);
  m_lightsStdPublisherSPtr = this->create_publisher<std_msgs::msg::UInt8>("/lights/std_msg", qosParamsR);
 
  //Debug MapDrawTrajectory
  m_localMapDebugTrajectoryImgPublisher = image_transport::create_publisher(this, TOPIC_AP_CONTROLLER_DATA_DEBUG);
 
  // Simulation Adapter Publisher
  rclcpp::QoS qosParams             = get_qos_profile(QOS_BEST_EFFORT);
  m_simulationVehicleControlPubSPtr = this->create_publisher<SimulationVehicleControlType>("galaxis/simulation/remotecontrol", qosParams);
 
  // Configuration
  // m_turnAnglesSPtr                   = galaxis::utils::read_from_file_to_float("settings/onstartup_control_turn_angles.csv");
  m_turnAnglesSPtr = std::make_shared<vector<float>>();
  m_turnAnglesSPtr->push_back(-0.35);
  m_turnAnglesSPtr->push_back(-0.30);
  m_turnAnglesSPtr->push_back(-0.24);
  m_turnAnglesSPtr->push_back(-0.20);
  m_turnAnglesSPtr->push_back(-0.16);
  m_turnAnglesSPtr->push_back(-0.12);
  m_turnAnglesSPtr->push_back(-0.08);
  m_turnAnglesSPtr->push_back(-0.04);
  m_turnAnglesSPtr->push_back(-0.02);
  m_turnAnglesSPtr->push_back(0);
  m_turnAnglesSPtr->push_back(0.02);
  m_turnAnglesSPtr->push_back(0.04);
  m_turnAnglesSPtr->push_back(0.08);
  m_turnAnglesSPtr->push_back(0.12);
  m_turnAnglesSPtr->push_back(0.16);
  m_turnAnglesSPtr->push_back(0.20);
  m_turnAnglesSPtr->push_back(0.24);
  m_turnAnglesSPtr->push_back(0.30);
  m_turnAnglesSPtr->push_back(0.35);
 
  float horizonStepSize              = 0.5f;
  int errorAccuracyStepsPerHorizon   = 4;
  float carWheelbase                 = galaxis::controllerSettings::carWheelbase;
  float errorIncreaseForFirstHorizon = 4;
 
  // Further Config from file
  std::string configPath = "settings/onstartup_controller.json";
  auto settingsJsonSPtr = read_json_file(configPath);
  json& settingsJson = (*settingsJsonSPtr)["settings"];
 
  if(this->get_global_settings()["simulation_mode"]) {
    m_simulationMode = true;
    carWheelbase = galaxis::controllerSettings::carWheelbaseSimulationDrDrift;
  }
  m_debugMode = settingsJson["debug_mode"];
 
  log("Configuration:\n - SimulationMode: ", m_simulationMode ? "ENABLED" : "DISABLED");
  log(" - DebugMode: ", m_debugMode ? "ENABLED" : "DISABLED");
  log("CarConfiguration:\n - CarWheelbase: ", carWheelbase);
 
  if (m_simulationMode || m_debugMode) sleep(1); // to view settings
 
  m_controlTwoAndAHalfCarrotsSPtr = std::make_shared<TwoAndAHalfCarrots>(TwoAndAHalfCarrots(m_turnAnglesSPtr, horizonStepSize, 2 /* horizon */, errorAccuracyStepsPerHorizon, carWheelbase, errorIncreaseForFirstHorizon));
 
  m_currentReferenceSplineSPtr = std::make_shared<PolySpline>();
  m_currentReferenceSplineSPtr->add_polynom(-100, 100, galaxis::polynom{ 0, 0, 0, 0 });
 
  m_currentVelocity = 0.0f;
  m_currentVelocityApplied = 0.f;
 
  calculate_turn_angle();
 
 
  // debug subscription
  if(settingsJson["publish_map_trajectory_merged_debug_msg"] == true) {
    m_localMapDebugImgSubscription = image_transport::create_subscription(
      this,
      galaxis::ros::TOPIC_AP_DATA_IMAGE_DEBUG,
      std::bind(&ControllerNode::on_map_debug_image_callback, this, _1),
      "raw");
  }
 
  this->enable_parameters(std::bind(&ControllerNode::new_parameters_callback, this, std::placeholders::_1));
 
  // start controller loop
  m_disableController = false;
  std::thread(&ControllerNode::loop, this).detach();
}
 
void ControllerNode::loop() {
  while(1) {
    //TODO wait for measurements...
    // if(m_disableController == true) { // controller idle
    //   std::this_thread::sleep_for(std::chrono::milliseconds(10));
    //   continue;
    // }
 
    bool apOffline = false;
    // Stop Car if more than 0.4 seconds no poly updates from AP
    if(std::abs(this->get_clock()->now().nanoseconds() - m_lastPolynomUpdateTimestamp) > 4e8) { 
      log_warning("Stopping car. No Polynom update within 400ms! (Is the AP online?)");
      m_currentVelocity = 0.f;
      apOffline = true;
    }
 
    auto msgUPtr               = std::make_unique<ControlMotionType>();
    msgUPtr->set_motor_erpm    = galaxis::motorSettings::velocityToErpm(m_currentVelocity);
    msgUPtr->set_motor_current = 0;
    msgUPtr->set_servo_front   = galaxis::servoSettings::angleToServoSetting(std::get<0>(m_currentTurnAngles));
    msgUPtr->set_servo_rear    = 0; //std::get<1>(m_currentTurnAngles);
    msgUPtr->set_brakes        = ((m_currentVelocity == 0.f) && !apOffline);
    
    // parking mode
    if(m_stop_ControllerNode == true) {
      auto set_start_ParkingNode_result = this->set_parameter(rclcpp::Parameter("CarOS.start_ParkingNode", 1));
      if (!set_start_ParkingNode_result.successful) {
        RCLCPP_ERROR(this->get_logger(), "Failed to set CarOS.start_ParkingNode: %s", set_start_ParkingNode_result.reason.c_str());
      }
      log_warning("Real World: In parking mode. Controller Node is in idle mode.");
    } else {
      publish_control_values(std::move(msgUPtr));
 
      // send lights only if changed
      // PROBLEM! with ECU ? maybe due to flash?
      if(m_lastSentControlLights == nullptr || (this->get_clock()->now() - m_lastSentControlLights->header.stamp) > rclcpp::Duration(0,100000000)) { // every 100ms
        m_lastSentControlLights = std::make_shared<ControlLightsType>();
        m_lastSentControlLights->header.stamp = this->get_clock()->now();
        m_lastSentControlLights->set_blinker_left  = std::get<0>(m_currentIndicatorState);
        m_lastSentControlLights->set_blinker_right = std::get<1>(m_currentIndicatorState);
        m_lastSentControlLights->set_headlight     = true;
        m_lastSentControlLights->set_brake_light   = ((m_currentVelocity == 0.f) && !apOffline); // TODO
        m_controlLightsPubSPtr->publish(*m_lastSentControlLights);
        
 
        // for VideoCockpit
        auto lightUPtr = std::make_unique<std_msgs::msg::UInt8>();
        lightUPtr->data = 0; // DDDDDBRL (D:=DontCare, B:=Brake, R:=Right, L:=Left)
        lightUPtr->data |= std::get<0>(m_currentIndicatorState) ? 1 : 0;
        lightUPtr->data |= (std::get<1>(m_currentIndicatorState) ? 1 : 0) << 1;
        lightUPtr->data |= (((m_currentVelocity == 0.f) && !apOffline) ? 1 : 0) << 2;
        m_lightsStdPublisherSPtr->publish(std::move(lightUPtr));
      }
    }
    //phth end
 
 
 
    // send control debug
    if(m_debugMode == true) {
      send_debug_messages();
    }
 
    // send Simulation Message
    if(m_simulationMode == true) {
      //phth
      if(m_stop_ControllerNode == true) {
        auto set_start_ParkingNode_result = this->set_parameter(rclcpp::Parameter("CarOS.start_ParkingNode", 1));
        if (!set_start_ParkingNode_result.successful) {
          RCLCPP_ERROR(this->get_logger(), "Failed to set CarOS.start_ParkingNode: %s", set_start_ParkingNode_result.reason.c_str());
        }
        log_warning("Simulation: In parking mode. Controller Node is in idle mode.");
      } else {
        send_simulation_messages();
      }
      //phth end
    }
 
    std::this_thread::sleep_for(std::chrono::milliseconds(2)); // TODO: maybe remove
  }
}
 
void ControllerNode::send_debug_messages() {
  if(m_lastReceivedPathpolynomSPtr == nullptr) {
    log("[SKIP] PathPolynom is nullptr. Skip sending Debug Message.");
    return;
  }
 
  if(m_controlMtx.try_lock()) {
    if(m_pathPolynomMtx.try_lock()) {
      DebugControllerType::UniquePtr debugMessage = std::make_unique<DebugControllerType>();
 
      debugMessage->polynom         = PathpolynomType(*m_lastReceivedPathpolynomSPtr);
      debugMessage->turnangle_front = std::get<0>(m_currentTurnAngles);
      debugMessage->turnangle_rear  = std::get<1>(m_currentTurnAngles);
      debugMessage->car_speed       = m_currentVelocity;
 
      std::shared_ptr<galaxis::control::CarPoseSeries const> firstPoseSeries  = m_controlTwoAndAHalfCarrotsSPtr->get_last_first_poseseries();
      std::shared_ptr<galaxis::control::CarPoseSeries const> secondPoseSeries = m_controlTwoAndAHalfCarrotsSPtr->get_last_second_poseseries();
 
      for(auto const pose: *firstPoseSeries) {
        Vector2fType vec;
        vec.x = pose->position[0];
        vec.y = pose->position[1];
        debugMessage->carpose_series.push_back(vec);
      }
 
      for(auto const pose: *secondPoseSeries) {
        Vector2fType vec;
        vec.x = pose->position[0];
        vec.y = pose->position[1];
        debugMessage->carpose_series.push_back(vec);
      }
 
      m_debugControllerPubSPtr->publish(std::move(debugMessage));
      log("[PUBLISH] DebugController message.");
 
      m_pathPolynomMtx.unlock();
    } else {
      log("[SKIP] PathPolynom mutexed. Skip sending Debug Message.");
    }
    m_controlMtx.unlock();
  } else {
    log("[SKIP] Control mutexed. Skip sending Debug Message.");
  }
}
 
void ControllerNode::send_simulation_messages() {
  auto message = std::make_unique<SimulationVehicleControlType>();
 
  message->car_speed       = m_currentVelocity;
  message->turnangle_front = std::get<0>(m_currentTurnAngles);
  message->reset           = false;
 
  m_simulationVehicleControlPubSPtr->publish(std::move(message));
}
 
void ControllerNode::calculate_turn_angle() {
  m_controlMtx.lock();
  m_pathPolynomMtx.lock();
 
  // TODO: set current Pose (maybe not neccessary)
  // CarPose currentPose();
  // currentPose.orientation = ...;
  if(m_currentVelocityReference >= 0.01f) {
    std::get<0>(m_currentTurnAngles) = m_controlTwoAndAHalfCarrotsSPtr->calculate_best_yaw_rate(m_currentReferenceSplineSPtr, m_currentLineMarkingReferenceSPtr, m_currentVelocity); // front turn angle
    std::get<1>(m_currentTurnAngles) = 0; // rear turn angle
  }
 
  // decrease velocity for steering
  float veloFactorForDecrease = 1 - std::abs(std::get<0>(m_currentTurnAngles)) * 1.5f;
  float desiredVelocity = m_currentVelocityReference * veloFactorForDecrease;
 
  m_currentVelocity = desiredVelocity * m_velocityRollingAverageFactorForNewVelocity + (1 - m_velocityRollingAverageFactorForNewVelocity) * m_currentVelocityApplied;
 
  if(m_currentVelocity <= m_minVelocity) {
    float min = m_minVelocity < m_currentVelocityReference ? m_minVelocity : m_currentVelocityReference;
    m_currentVelocity = min;
  }
 
  if(m_currentVelocityReference < m_currentVelocity) {
    float ref = m_currentVelocityReference;
    m_currentVelocity = ref;
  }
 
  if(m_currentVelocityReference < 0.01f) {
    m_currentVelocity = 0;
  }
 
  if(m_simulationMode == true) {
    float currVelo = m_currentVelocity;
    m_currentVelocityApplied = currVelo;
  }
 
  log("[CALCULATION] TurnAngle [Rad]: ", std::get<0>(m_currentTurnAngles), "  Velocity [m/s]: ", m_currentVelocity);
 
  m_pathPolynomMtx.unlock();
  m_controlMtx.unlock();
}
 
void ControllerNode::on_measurement_callback(const MeasurementsType::SharedPtr msg) {
  log("#New Measurements arrived.");
  if(m_measurementMtx.try_lock()) {
    m_lastReceivedMeasurementSPtr = msg;
    m_currentVelocityApplied = galaxis::motorSettings::erpmToVelocity(msg->motor_erpm);
    m_measurementMtx.unlock();
  } else {
    log_warning("ControllerNode::on_measurement_callback: MeasurementsPtr mutexed. Could not update Measurements");
  }
}
 
void ControllerNode::on_pathpolynom_callback(const PathpolynomType::SharedPtr msg) {
  log("#New Pathpolynom received.");
  m_pathPolynomMtx.lock();
  m_lastReceivedPathpolynomSPtr = msg;
 
  m_currentReferenceSplineSPtr->clear();
  m_currentReferenceSplineSPtr->add_polynom(-100, 100, galaxis::polynom{ 
                                                                        static_cast<float>(msg->c[0]),
                                                                        static_cast<float>(msg->c[1]),
                                                                        static_cast<float>(msg->c[2]),
                                                                        static_cast<float>(msg->c[3]) 
                                                                       }); // TODO Spline?
 
  m_currentLineMarkingReferenceSPtr = std::make_shared<galaxis::line_markings_reference_t>();
  m_currentLineMarkingReferenceSPtr->center = PolySpline(msg->c_m, msg->max_center, msg->min_center);
  m_currentLineMarkingReferenceSPtr->right = PolySpline(msg->c_r, msg->max_right, msg->min_right);
  m_currentLineMarkingReferenceSPtr->left = PolySpline(msg->c_l, msg->max_left, msg->min_left);
  m_currentLineMarkingReferenceSPtr->isCenterEnabled = msg->center_enabled;
  m_currentLineMarkingReferenceSPtr->isRightEnabled = msg->right_enabled;
  m_currentLineMarkingReferenceSPtr->isLeftEnabled = msg->left_enabled;
 
 
  m_lastPolynomUpdateTimestamp = this->get_clock()->now().nanoseconds();
 
  m_pathPolynomMtx.unlock();
 
  //log("->calculate_turn_angle");
  calculate_turn_angle();
}
 
void ControllerNode::publish_control_values(ControlMotionType::UniquePtr msg) {
  assert(msg != nullptr);
  assert(m_controlValuesPubSPtr != nullptr);
 
  log("[PUBLISH] ControlValues\n - ERPM: ", msg->set_motor_erpm, "\n - Motor Current: ", msg->set_motor_current, "\n - Servo Front: ",
      static_cast<int>(msg->set_servo_front), "\n - Servo Back: ", static_cast<int>(msg->set_servo_rear), "\n - Brakes: ", msg->set_brakes, "\n");
 
  m_controlValuesPubSPtr->publish(std::move(msg));
}
 
void ControllerNode::on_controller_reference_callback(const ControllerReferenceType::SharedPtr msg) {
  
  m_disableController = (msg->controller_enable == false);
  m_currentVelocityReference = msg->velocity_reference;
  std::get<0>(m_currentIndicatorState) = msg->indicator_left;
  std::get<1>(m_currentIndicatorState) = msg->indicator_right;
 
  log("#New Controller Reference received: \n - VelocityRef [m/s]: ", m_currentVelocityReference, "\n - Indicators (L,R): ", std::get<0>(m_currentIndicatorState), ",", std::get<1>(m_currentIndicatorState));
}
 
void ControllerNode::on_map_debug_image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) {
  if(m_localMapDebugTrajectoryImgPublisher.getNumSubscribers() == 0) {
    log("[on_map_debug_image_callback] Skip sending Map-Trajectory-Debug-Image. Reason: NO Subscriber on the corresponding topic.");
    return;
  }
 
  std::shared_ptr<sensor_msgs::msg::Image> imageMsg = std::make_shared<sensor_msgs::msg::Image>();
 
  imageMsg->header.stamp = msg->header.stamp;
  imageMsg->width = msg->width;
  imageMsg->height = msg->height;
  imageMsg->encoding = msg->encoding;
  imageMsg->is_bigendian = msg->is_bigendian;
  imageMsg->step = msg->step;
 
  imageMsg->data = msg->data;
 
  // draw trajectory
 
  // m_pathPolynomMtx.lock();
  // float orientation = m_lastReceivedPathpolynomSPtr->pose.orientation;
  // Eigen::Rotation2Df rotationMat(orientation);
  // m_pathPolynomMtx.unlock();
 
  m_controlMtx.lock();
  auto poseSeries = m_controlTwoAndAHalfCarrotsSPtr->get_last_first_poseseries();
 
  float trajectoryPointPositionX = 0.f;
  float trajectoryPointPositionY = 0.f;
  for(int i = 0; i < 2; ++i) {
    for(auto const& pose : *poseSeries) {
      trajectoryPointPositionX = - pose->position[1] * 100;
      trajectoryPointPositionY = galaxis::autopilot::lookupLookBehindCarCM + pose->position[0] * 100; 
 
      // draw circle
      int radiusCM = 2;
      for(int y = -radiusCM; y <= radiusCM; y++) {
        for(int x = -radiusCM; x <= radiusCM; x++) {
          if(x*x+y*y <= radiusCM*radiusCM) {
            int imageX = trajectoryPointPositionX + x + imageMsg->width / 2;
            int imageY = imageMsg->height - trajectoryPointPositionY + y;
            int index = imageY * imageMsg->step + imageX * 3;
 
            //skip if not on image
            if (imageX < 0 || imageY < 0 || imageX >= imageMsg->width || imageY >= imageMsg->height) {
              continue;
            }   
 
            if(index >= 0 && index + 2 <= static_cast<int>(imageMsg->data.size())) {
              imageMsg->data[index] = 255;
              imageMsg->data[index + 1] = 242;
              imageMsg->data[index + 2] = 167;
              // Yellow: https://www.color-hex.com/color/fff2a7
            }
          }
        }
      }
    }
 
    poseSeries = m_controlTwoAndAHalfCarrotsSPtr->get_last_second_poseseries();
  }
 
  m_controlMtx.unlock();
 
  m_localMapDebugTrajectoryImgPublisher.publish(imageMsg);
  log("[DEBUG] [PUBLISH] Map-Trajectory-Debug-Image");
}
 
rcl_interfaces::msg::SetParametersResult ControllerNode::new_parameters_callback(const std::vector<rclcpp::Parameter>& parameters) {
  log("[ControllerNode] NEW PARAMETERS");
  
  rcl_interfaces::msg::SetParametersResult result;
  result.successful = true;
  result.reason = "";
 
  for (const auto &param: parameters) {
    if (param.get_name() == "settings.horizon_step_time_sec") {
      if(check_parameter<double>(param, result, 0.1, 2.0)) {
        m_controlTwoAndAHalfCarrotsSPtr->set_horizon_step_time(param.as_double());
      }
    }
    else if (param.get_name() == "settings.error_accuracy_steps_per_horizon") {
      if(check_parameter<int>(param, result, 5, 100)) {
        m_controlTwoAndAHalfCarrotsSPtr->set_error_accuracy_steps_per_horizon(param.as_int());
      }
    }
    else if (param.get_name() == "settings.error_factor_first_horizon") {
      if(check_parameter<double>(param, result, -5.0, 5.0)) {
        m_controlTwoAndAHalfCarrotsSPtr->set_error_factor_first_horizon(param.as_double());
      }
    }
    else if (param.get_name() == "settings.max_decreased_velocity_for_steering") {
      if(check_parameter<double>(param, result, 0.3)) {
        m_minVelocity = param.as_double();
      }
    }
    else if (param.get_name() == "settings.velocity_rolling_average_factor_to_new_velocity") {
      if(check_parameter<double>(param, result, 0.000001, 1)) {
        m_velocityRollingAverageFactorForNewVelocity = param.as_double();
      }
    }
    //phth
    else if (param.get_name() == "CarOS.stop_ControllerNode") {
      if(check_parameter<int>(param, result, 0)) {
        m_stop_ControllerNode = (param.as_int() == 1);
      }
    }
    else if (param.get_name() == "CarOS.start_ParkingNode") {
      if(check_parameter<int>(param, result, 0)) {
        m_start_ParkingNode = (param.as_int() == 1);
      }
    }
 
    //phth end
    else if (param.get_name() == "controlmetrics.cross_angle_error_weight") {
      if(check_parameter<double>(param, result, 0)) {
        m_controlTwoAndAHalfCarrotsSPtr->set_cross_angle_error_weight( param.as_double());
      }
    }
    else if (param.get_name() == "controlmetrics.cross_line_marking_error_weight") {
      if(check_parameter<double>(param, result, 0)) {
        m_controlTwoAndAHalfCarrotsSPtr->set_cross_line_marking_error_weight( param.as_double());
      }
    }
    else {
      result.successful = false;
      result.reason += "[" + param.get_name() + "] UNKNOWN param or NOT IMPLEMENTED; ";
    }
  }
 
  if(result.successful == false) {
    log_warning("[AutopilotNode] Failed to set some parameters. Reasons: ", result.reason);
  }
 
  return result;
}