ECUNode.cpp

Go to the documentation of this file.

#include "ECUNode.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/qos.hpp"
 
#include <sensor_msgs/msg/range.hpp>
 
#include "ECUHandler.hpp"
#include "util/Packets.hpp"
#include "galaxis/motorSettings.hpp"
#include <galaxis/node_ros_pubsub_wrapper.hpp>
#include <algorithm>
#include <utils/logger.hpp>
#include <memory> // for Shared/Unique Ptr
#include <utility> // for std::move
#include <algorithm>
 
using namespace galaxis::utils;
 
void ECUNode::init(std::shared_ptr<ECUHandler> p_ecuHandlerSPtr) {
  /* for create_publisher and create_subscriber */
  using std::placeholders::_1;
 
  galaxis::ros::create_publisher(this, m_measurementPubSPtr);
  
  rclcpp::QoS qos = galaxis::ros::get_qos_profile(galaxis::ros::QoS_Profile_t::QOS_RELIABLE);
  // m_measurementPubSPtr2 = this->create_publisher<MeasurementsType>("/measurements/lowfrequency", qos);
 
  galaxis::ros::create_publisher(this, m_buttonStatePubSPtr);
 
  galaxis::ros::create_subscription(this,
                                    m_controlValuesSubSPtr,
                                    std::bind(&ECUNode::motion_callback, this, _1));
 
  galaxis::ros::create_subscription(this,
                                    m_settingsValuesSubSPtr,
                                    std::bind(&ECUNode::settingsvalues_callback, this, _1));
 
  galaxis::ros::create_subscription(this,
                                    m_lightsSubSPtr,
                                  std::bind(&ECUNode::lights_callback, this, _1));
 
    
  
  m_velocityPubSPtr = this->create_publisher<std_msgs::msg::Float32>("/measurements/speed", qos);
 
  // m_TOFRearPubSPtr = this->create_publisher<sensor_msgs::msg::Range>("/measurements/rear", qos);
  // m_TOFRightPubSPtr = this->create_publisher<sensor_msgs::msg::Range>("/measurements/right", qos);
 
  m_ecuHandlerSPtr = p_ecuHandlerSPtr;
 
  log("successfully initialised...");
}
 
void ECUNode::motion_callback(const ControlMotionType::SharedPtr p_msgSPtr) {
  log_debug("send control values via ECU handler");
 
  ComputerPacket packet = {};
 
  packet.motor_erpm    = p_msgSPtr->set_motor_erpm;
  packet.motor_current = p_msgSPtr->set_motor_current;
  log_debug("motor current ", p_msgSPtr->set_motor_current);
  packet.servo_front = std::min(int(p_msgSPtr->set_servo_front) + 3, 127);
  packet.servo_rear  = p_msgSPtr->set_servo_rear;
  packet.brake  = p_msgSPtr->set_brakes ? 1 : 0;
 
  uint8_t headlight    = 1;
  uint8_t blinkerLeft  = 0;
  uint8_t blinkerRight = 0;
  uint8_t brakelight   = packet.brake;
 
  if(m_lastLightsSPtr != nullptr && this->get_clock()->now() - m_lastLightsSPtr->header.stamp < rclcpp::Duration(2,0)) { // skip if light msg older than 2seconds
    m_lightsMtx.lock();
    headlight    = m_lastLightsSPtr->set_headlight;
    // 0x0 off, 0x1 on, 0x2 blink, 0x3 NOT set
    blinkerLeft  = m_lastLightsSPtr->set_blinker_left ? 0x2 : 0;  // TODO before it was 0x3 but ecu crashed
    blinkerRight = m_lastLightsSPtr->set_blinker_right ? 0x2 : 0; // TODO before it was 0x3 but ecu crashed
    brakelight   = m_lastLightsSPtr->set_brake_light;
    m_lightsMtx.unlock();
  }
 
  galaxis::serviceStates::State state = get_current_service_state();
  packet.lightState =
      convertLightStatesIntoFinalState(
          headlight,
          brakelight,
          blinkerLeft,
          blinkerRight,
          0,
          0,
          0);
 
  log("preamble " , packet.preamble);
  log("packet_id " , packet.packet_id);
  log("motor_erpm " , packet.motor_erpm);
  log("motor_current " , packet.motor_current);
  log("servo_front " , packet.servo_front);
  log("servo_rear " , packet.servo_rear);
  log("lightState " , packet.lightState);
  log("crc " , packet.crc);
 
  m_ecuHandlerSPtr->TransmitPacket(packet);
}
 
void ECUNode::settingsvalues_callback(const SettingsValuesType::SharedPtr p_msgSPtr) {
  log("New settings callback");
  SettingsPacket packet = {};
 
  packet.firmware.motor_current_max = p_msgSPtr->firmware.motor_current_max;
  log("motor_current_max: ", std::to_string(p_msgSPtr->firmware.motor_current_max));
  packet.firmware.erpm_max = p_msgSPtr->firmware.erpm_max;
  log("motor_current_max: ", std::to_string(p_msgSPtr->firmware.erpm_max));
  packet.firmware.max_brake_current = p_msgSPtr->firmware.max_brake_current;
  log("motor_current_max: ", std::to_string(p_msgSPtr->firmware.max_brake_current));
 
  packet.firmware.max_servo_offset.front = p_msgSPtr->firmware.max_servo_offset.front;
  log("motor_current_max: ", std::to_string(p_msgSPtr->firmware.max_servo_offset.front));
  packet.firmware.max_servo_offset.rear = p_msgSPtr->firmware.max_servo_offset.rear;
  log("motor_current_max: ", std::to_string(p_msgSPtr->firmware.max_servo_offset.rear));
 
  packet.firmware.manual_override_difference = p_msgSPtr->firmware.manual_override_difference;
  log("motor_current_max: ", std::to_string(p_msgSPtr->firmware.manual_override_difference));
  packet.firmware.use_rc_to_steer = p_msgSPtr->firmware.use_rc_to_steer;
  log("motor_current_max: ", std::to_string(p_msgSPtr->firmware.use_rc_to_steer));
  packet.firmware.rc_use_erpm = p_msgSPtr->firmware.rc_use_erpm;
  log("motor_current_max: ", std::to_string(p_msgSPtr->firmware.rc_use_erpm));
 
  packet.firmware.servo_offset.front = p_msgSPtr->firmware.servo_offset.front;
  log("motor_current_max: ", std::to_string(p_msgSPtr->firmware.servo_offset.front));
  packet.firmware.servo_offset.rear = p_msgSPtr->firmware.servo_offset.rear;
  log("motor_current_max: ", std::to_string(p_msgSPtr->firmware.servo_offset.rear));
 
  packet.firmware.motor_current_max_rc = p_msgSPtr->firmware.motor_current_max_rc;
  log("motor_current_max: ", std::to_string(p_msgSPtr->firmware.motor_current_max_rc));
  packet.firmware.erpm_max_rc = p_msgSPtr->firmware.erpm_max_rc;
  log("motor_current_max: ", std::to_string(p_msgSPtr->firmware.erpm_max_rc));
 
  m_ecuHandlerSPtr->TransmitPacket(packet);
}
 
void ECUNode::publishMeasurements(const EcuPacket* const packet, const std::array<std::chrono::duration<double, std::milli>, 10>& diff_ms) {
  //log_debug("publish measurements");
  if(packet == nullptr) {
    log_warning("nullptr as argument");
    return;
  }
 
  std::unique_ptr<MeasurementsType> msgUPtr = std::make_unique<MeasurementsType>();
 
  msgUPtr->header.stamp          = this->get_clock()->now();
  msgUPtr->rc_active             = packet->rc_active;
  msgUPtr->motor_erpm            = packet->motor_erpm;
  msgUPtr->motor_duty_cycle      = packet->motor_duty_cycle;
  msgUPtr->linear_acceleration_x = packet->linear_acceleration_x;
  msgUPtr->linear_acceleration_y = packet->linear_acceleration_y;
  msgUPtr->linear_acceleration_z = packet->linear_acceleration_z;
  msgUPtr->gyro_x                = packet->gyro_x;
  
  // Sometimes if the tof sensor sees no obstacle the distance becomes 0, to fix 
  // this we set it to max
  if(packet->tof_right == 0){
    msgUPtr->time_of_flight_right  = 2.f;
  }else{
    msgUPtr->time_of_flight_right  = float(packet->tof_right) / 1000.f;
  }
  if(packet->tof_rear == 0){
    msgUPtr->time_of_flight_rear  = 2.f;
  }else{
    msgUPtr->time_of_flight_rear  = float(packet->tof_rear) / 1000.f;
  }
 
  msgUPtr->orientation           = packet->orientation;
  msgUPtr->button_state          = packet->buttonState;
  msgUPtr->battery_level         = packet->batteryLevel;
  msgUPtr->applied_servo_front   = packet->applied_servo_front;
  msgUPtr->applied_servo_rear    = packet->applied_servo_rear;
  msgUPtr->applied_motor_current = packet->applied_motor_current;
  msgUPtr->applied_motor_erpm    = packet->applied_motor_erpm;
  msgUPtr->applied_motor_brake   = packet->applied_motor_brake;
  for(size_t i = 0; i < 3; i++) {
    msgUPtr->battery_cell_level[i] = packet->batteryCellLevel[i];
  }
 
  static int count = 0;
  count++;
  if(count == 10) {
    std::array<double, 10> temp_array;
    for(size_t i = 0; i < temp_array.size(); ++i) {
      temp_array[i] = diff_ms.at(i).count();
    }
    std::sort(temp_array.begin(), temp_array.end());
    // log("Time Diff to last packet: [,,,,,,,,,]",
    //             temp_array[0],
    //             temp_array[1],
    //             temp_array[2],
    //             temp_array[3],
    //             temp_array[4],
    //             temp_array[5],
    //             temp_array[6],
    //             temp_array[7],
    //             temp_array[8],
    //             temp_array[9]);
    log("Max difference: ", temp_array[9]);
    log("Min difference: ", temp_array[0]);
    uint32_t sum_differences = 0;
    for(size_t i = 0; i < 10; i++) {
      sum_differences += diff_ms.at(i).count();
    }
    float average_differences = (float)sum_differences / 10.0;
    log("average differences: ", average_differences);
    log("rc_active: ", packet->rc_active);
    log("Motor ERPM: ", packet->motor_erpm);
    float rpm = packet->motor_erpm / 7;
    log("Motor RPM: ", rpm);
    float velocity = galaxis::motorSettings::erpmToVelocity(packet->motor_erpm);
    log("Velocity: ", velocity);
    log("Duty Cycle: ", packet->motor_duty_cycle);
    if(packet->tof_right == 0){
      log("tof_right: 2.0");
    }else{
      log("tof_right: ", packet->tof_right / 1000.f);
    }
    if(packet->tof_rear == 0){
      log("tof_rear: 2.0");
    }else{
      log("tof_rear: ", packet->tof_rear / 1000.f);
    }
    
    
    log("orientation: ", packet->orientation);
    log("linear_acc_x: ", packet->linear_acceleration_x);
    log("linear_acc_y: ", packet->linear_acceleration_y);
    log("linear_acc_z: ", packet->linear_acceleration_z);
    log("gyro_x: ", packet->gyro_x);
    log("buttonState: ", packet->buttonState);
    log("batteryLevel: ", packet->batteryLevel);
    for(size_t i = 0; i < 3; i++) {
      log("batteryCellLevel[]: ", i, packet->batteryCellLevel[i]);
    }
    log("applied servo front: ", packet->applied_servo_front);
    log("applied servo rear: ", packet->applied_servo_rear);
    log("Applied motor current: ", packet->applied_motor_current);
    log("Applied motor erpm: ", packet->applied_motor_erpm);
    log("Applied motor brake: ", packet->applied_motor_brake);
    log("\n");
  }
 
  //TODO split measurements into different messages
  // if(packet->buttonState != m_lastButtonState) {
  //   auto buttonStateMsgUPtr = std::make_unique<ButtonStateMessageType>();
  //   log("Sending new buttonstate ", packet->buttonState);
  //   buttonStateMsgUPtr->pressed_button_1 = (packet->buttonState >> 0) & 1;
  //   buttonStateMsgUPtr->pressed_button_2 = (packet->buttonState >> 1) & 1;
  //   buttonStateMsgUPtr->pressed_button_3 = (packet->buttonState >> 2) & 1;
 
  //   m_buttonStatePubSPtr->publish(std::move(buttonStateMsgUPtr));
  // }
 
  m_measurementPubSPtr->publish(std::move(msgUPtr));
 
  if(count == 10) {
    std::unique_ptr<std_msgs::msg::Float32> velocityUPtr = std::make_unique<std_msgs::msg::Float32>();
    velocityUPtr->data = galaxis::motorSettings::erpmToVelocity(packet->motor_erpm);
    m_velocityPubSPtr->publish(std::move(velocityUPtr));
  }
    
 
  // ############### ToF redundant separate msg ###############
  // std::unique_ptr<sensor_msgs::msg::Range> range_msg = std::make_unique<sensor_msgs::msg::Range>();
  // range_msg->max_range = 2.f;
  // range_msg->min_range = 0.f;
 
  // if(packet->tof_rear == 0){
  //   range_msg->range  = 2.f;
  // }else{
  //   range_msg->range  = float(packet->tof_rear) / 1000.f;
  // }
 
  // m_TOFRearPubSPtr->publish(std::move(range_msg));
 
  // std::unique_ptr<sensor_msgs::msg::Range> range_msg2 = std::make_unique<sensor_msgs::msg::Range>();
  // range_msg2->max_range = 2.f;
  // range_msg2->min_range = 0.f;
 
  // if(packet->tof_right == 0){
  //   range_msg2->range  = 2.f;
  // }else{
  //   range_msg2->range  = float(packet->tof_right) / 1000.f;
  // }
 
  // m_TOFRightPubSPtr->publish(std::move(range_msg2));
 
 
  m_lastButtonState = packet->buttonState;
 
  if(count >= 10) {
    count = 0;
  }
}
 
void ECUNode::lights_callback(const ControlLightsType::SharedPtr p_msgSPtr) {
  std::unique_lock<std::mutex> lock(m_lightsMtx);
  m_lastLightsSPtr = p_msgSPtr;
}