ControllerNode Class Reference

#include <ControllerNode.hpp>

Inheritance diagram for ControllerNode:

Collaboration diagram for ControllerNode:

Public Member Functions
	ControllerNode ()

Private Member Functions
void	loop ()
void	send_debug_messages ()
void	send_simulation_messages ()
void	calculate_erpm ()
void	calculate_turn_angle ()
void	publish_control_values (ControlMotionType::UniquePtr msg)
void	on_measurement_callback (MeasurementsType::SharedPtr msg)
void	on_pathpolynom_callback (PathpolynomType::SharedPtr msg)
void	on_controller_reference_callback (ControllerReferenceType::SharedPtr msg)
void	on_map_debug_image_callback (const sensor_msgs::msg::Image::ConstSharedPtr &msg)
rcl_interfaces::msg::SetParametersResult	new_parameters_callback (const std::vector< rclcpp::Parameter > &parameters)

Private Attributes
bool	m_debugMode = false
bool	m_simulationMode = false
std::atomic< bool >	m_disableController
std::atomic< bool >	m_stop_ControllerNode = false
std::atomic< bool >	m_start_ParkingNode = false
rclcpp::Publisher< ControlMotionType >::SharedPtr	m_controlValuesPubSPtr
rclcpp::Publisher< ControlLightsType >::SharedPtr	m_controlLightsPubSPtr
rclcpp::Publisher< DebugControllerType >::SharedPtr	m_debugControllerPubSPtr
rclcpp::Publisher< SimulationVehicleControlType >::SharedPtr	m_simulationVehicleControlPubSPtr
rclcpp::Publisher< std_msgs::msg::UInt8 >::SharedPtr	m_lightsStdPublisherSPtr
rclcpp::Subscription< PathpolynomType >::SharedPtr	m_pathPolynomSubSPtr
rclcpp::Subscription< ControllerReferenceType >::SharedPtr	m_controllerRefSubSPtr
rclcpp::Subscription< MeasurementsType >::SharedPtr	m_measurementSubSPtr
image_transport::Subscriber	m_localMapDebugImgSubscription
image_transport::Publisher	m_localMapDebugTrajectoryImgPublisher
MeasurementsType::SharedPtr	m_lastReceivedMeasurementSPtr
std::mutex	m_measurementMtx
std::shared_ptr< ControlLightsType >	m_lastSentControlLights
PathpolynomType::SharedPtr	m_lastReceivedPathpolynomSPtr
std::shared_ptr< PolySpline >	m_currentReferenceSplineSPtr
std::shared_ptr< galaxis::line_markings_reference_t >	m_currentLineMarkingReferenceSPtr
std::mutex	m_pathPolynomMtx
std::atomic< int64_t >	m_lastPolynomUpdateTimestamp
std::shared_ptr< galaxis::control::TwoAndAHalfCarrots >	m_controlTwoAndAHalfCarrotsSPtr
std::shared_ptr< std::vector< float > >	m_turnAnglesSPtr
std::mutex	m_controlMtx
std::atomic< float >	m_currentVelocity = 0.f
std::atomic< float >	m_currentVelocityApplied = 0.f
std::atomic< float >	m_currentVelocityReference = 0.f
std::tuple< float, float >	m_currentTurnAngles = std::make_tuple(0.f, 0.f)
std::tuple< std::atomic< bool >, std::atomic< bool > >	m_currentIndicatorState = std::make_tuple(false, false)
std::atomic< float >	m_minVelocity = 0.4f
std::atomic< float >	m_velocityRollingAverageFactorForNewVelocity = 0.01f

Additional Inherited Members
Protected Member Functions inherited from GalaxisNode
	GalaxisNode ()=delete
	GalaxisNode (std::string p_name, unsigned int p_heartbeatTimePeriod)
	GalaxisNode (std::string p_name)
template<typename MsgSPtr >
void	tickSub (std::map< galaxis::serviceStates::State, std::function< void(const MsgSPtr)>> const *p_functionMapPtr, MsgSPtr const p_message)
template<typename... Args>
void	tickPub (std::map< galaxis::serviceStates::State, std::function< void(Args...)>> const *p_functionMapPtr, Args... p_args)
void	register_callback (std::function< void(galaxis::serviceStates::State)> p_function)
void	service_state_callback (ServiceStateType::SharedPtr const p_msgSPtr)
galaxis::serviceStates::State	get_current_service_state () const
void	enable_parameters (std::function< rcl_interfaces::msg::SetParametersResult(const std::vector< rclcpp::Parameter > &)> callbackFunc)
void	create_parameters (std::shared_ptr< json > configJsonSPtr)
json const &	get_global_settings ()

Detailed Description

Definition at line 22 of file ControllerNode.hpp.

Constructor & Destructor Documentation

ControllerNode()

ControllerNode::ControllerNode

(

)

Definition at line 19 of file ControllerNode.cpp.

                               :
    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();
}

References calculate_turn_angle(), galaxis::controllerSettings::carWheelbase, galaxis::controllerSettings::carWheelbaseSimulationDrDrift, GalaxisNode::enable_parameters(), GalaxisNode::get_global_settings(), galaxis::utils::log(), loop(), m_controllerRefSubSPtr, m_controlLightsPubSPtr, m_controlTwoAndAHalfCarrotsSPtr, m_controlValuesPubSPtr, m_currentReferenceSplineSPtr, m_currentVelocity, m_currentVelocityApplied, m_debugControllerPubSPtr, m_debugMode, m_disableController, m_lightsStdPublisherSPtr, m_localMapDebugImgSubscription, m_localMapDebugTrajectoryImgPublisher, m_measurementSubSPtr, m_pathPolynomSubSPtr, m_simulationMode, m_simulationVehicleControlPubSPtr, m_turnAnglesSPtr, new_parameters_callback(), on_controller_reference_callback(), on_map_debug_image_callback(), on_measurement_callback(), on_pathpolynom_callback(), and galaxis::utils::read_json_file().

Here is the call graph for this function:

Member Function Documentation

calculate_erpm()

void ControllerNode::calculate_erpm ( )

private

calculate_turn_angle()

void ControllerNode::calculate_turn_angle ( )

private

Definition at line 259 of file ControllerNode.cpp.

                                          {
  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();
}

References galaxis::utils::log(), m_controlMtx, m_controlTwoAndAHalfCarrotsSPtr, m_currentLineMarkingReferenceSPtr, m_currentReferenceSplineSPtr, m_currentTurnAngles, m_currentVelocity, m_currentVelocityApplied, m_currentVelocityReference, m_minVelocity, m_pathPolynomMtx, m_simulationMode, and m_velocityRollingAverageFactorForNewVelocity.

Referenced by ControllerNode(), and on_pathpolynom_callback().

Here is the call graph for this function:

Here is the caller graph for this function:

loop()

void ControllerNode::loop ( )

private

Definition at line 122 of file ControllerNode.cpp.

                          {
  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
  }
}

References galaxis::servoSettings::angleToServoSetting(), galaxis::utils::log_warning(), m_controlLightsPubSPtr, m_currentIndicatorState, m_currentTurnAngles, m_currentVelocity, m_debugMode, m_lastPolynomUpdateTimestamp, m_lastSentControlLights, m_lightsStdPublisherSPtr, m_simulationMode, m_stop_ControllerNode, publish_control_values(), send_debug_messages(), send_simulation_messages(), and galaxis::motorSettings::velocityToErpm().

Referenced by ControllerNode().

Here is the call graph for this function:

Here is the caller graph for this function:

new_parameters_callback()

rcl_interfaces::msg::SetParametersResult ControllerNode::new_parameters_callback ( const std::vector< rclcpp::Parameter > &  parameters )

private

Definition at line 431 of file ControllerNode.cpp.

                                                                                                                         {
  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;
}

References galaxis::utils::log(), galaxis::utils::log_warning(), m_controlTwoAndAHalfCarrotsSPtr, m_minVelocity, m_start_ParkingNode, m_stop_ControllerNode, and m_velocityRollingAverageFactorForNewVelocity.

Referenced by ControllerNode().

Here is the call graph for this function:

Here is the caller graph for this function:

on_controller_reference_callback()

void ControllerNode::on_controller_reference_callback ( ControllerReferenceType::SharedPtr  msg )

private

Definition at line 353 of file ControllerNode.cpp.

                                                                                                {
  
  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));
}

References galaxis::utils::log(), m_currentIndicatorState, m_currentVelocityReference, and m_disableController.

Referenced by ControllerNode().

Here is the call graph for this function:

Here is the caller graph for this function:

on_map_debug_image_callback()

void ControllerNode::on_map_debug_image_callback ( const sensor_msgs::msg::Image::ConstSharedPtr &  msg )

private

Definition at line 363 of file ControllerNode.cpp.

                                                                                             {
  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");
}

References galaxis::utils::log(), galaxis::autopilot::lookupLookBehindCarCM, m_controlMtx, m_controlTwoAndAHalfCarrotsSPtr, and m_localMapDebugTrajectoryImgPublisher.

Referenced by ControllerNode().

Here is the call graph for this function:

Here is the caller graph for this function:

on_measurement_callback()

void ControllerNode::on_measurement_callback ( MeasurementsType::SharedPtr  msg )

private

Definition at line 302 of file ControllerNode.cpp.

                                                                                {
  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");
  }
}

References galaxis::motorSettings::erpmToVelocity(), galaxis::utils::log(), galaxis::utils::log_warning(), m_currentVelocityApplied, m_lastReceivedMeasurementSPtr, and m_measurementMtx.

Referenced by ControllerNode().

Here is the call graph for this function:

Here is the caller graph for this function:

on_pathpolynom_callback()

void ControllerNode::on_pathpolynom_callback ( PathpolynomType::SharedPtr  msg )

private

Definition at line 313 of file ControllerNode.cpp.

                                                                               {
  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();
}

References calculate_turn_angle(), galaxis::utils::log(), m_currentLineMarkingReferenceSPtr, m_currentReferenceSplineSPtr, m_lastPolynomUpdateTimestamp, m_lastReceivedPathpolynomSPtr, and m_pathPolynomMtx.

Referenced by ControllerNode().

Here is the call graph for this function:

Here is the caller graph for this function:

publish_control_values()

void ControllerNode::publish_control_values ( ControlMotionType::UniquePtr  msg )

private

Definition at line 343 of file ControllerNode.cpp.

                                                                          {
  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));
}

References galaxis::utils::log(), and m_controlValuesPubSPtr.

Referenced by loop().

Here is the call graph for this function:

Here is the caller graph for this function:

send_debug_messages()

void ControllerNode::send_debug_messages ( )

private

Definition at line 204 of file ControllerNode.cpp.

                                         {
  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.");
  }
}

References galaxis::utils::log(), m_controlMtx, m_controlTwoAndAHalfCarrotsSPtr, m_currentTurnAngles, m_currentVelocity, m_debugControllerPubSPtr, m_lastReceivedPathpolynomSPtr, and m_pathPolynomMtx.

Referenced by loop().

Here is the call graph for this function:

Here is the caller graph for this function:

send_simulation_messages()

void ControllerNode::send_simulation_messages ( )

private

Definition at line 249 of file ControllerNode.cpp.

                                              {
  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));
}

References m_currentTurnAngles, m_currentVelocity, and m_simulationVehicleControlPubSPtr.

Referenced by loop().

Here is the caller graph for this function:

Member Data Documentation

m_controllerRefSubSPtr

rclcpp::Subscription<ControllerReferenceType>::SharedPtr ControllerNode::m_controllerRefSubSPtr

private

Definition at line 40 of file ControllerNode.hpp.

Referenced by ControllerNode().

m_controlLightsPubSPtr

rclcpp::Publisher<ControlLightsType>::SharedPtr ControllerNode::m_controlLightsPubSPtr

private

Definition at line 33 of file ControllerNode.hpp.

Referenced by ControllerNode(), and loop().

m_controlMtx

std::mutex ControllerNode::m_controlMtx

private

Definition at line 64 of file ControllerNode.hpp.

Referenced by calculate_turn_angle(), on_map_debug_image_callback(), and send_debug_messages().

m_controlTwoAndAHalfCarrotsSPtr

std::shared_ptr<galaxis::control::TwoAndAHalfCarrots> ControllerNode::m_controlTwoAndAHalfCarrotsSPtr

private

Definition at line 62 of file ControllerNode.hpp.

Referenced by calculate_turn_angle(), ControllerNode(), new_parameters_callback(), on_map_debug_image_callback(), and send_debug_messages().

m_controlValuesPubSPtr

rclcpp::Publisher<ControlMotionType>::SharedPtr ControllerNode::m_controlValuesPubSPtr

private

Definition at line 32 of file ControllerNode.hpp.

Referenced by ControllerNode(), and publish_control_values().

m_currentIndicatorState

std::tuple<std::atomic<bool>, std::atomic<bool> > ControllerNode::m_currentIndicatorState = std::make_tuple(false, false)

private

Definition at line 71 of file ControllerNode.hpp.

Referenced by loop(), and on_controller_reference_callback().

m_currentLineMarkingReferenceSPtr

std::shared_ptr<galaxis::line_markings_reference_t> ControllerNode::m_currentLineMarkingReferenceSPtr

private

Definition at line 56 of file ControllerNode.hpp.

Referenced by calculate_turn_angle(), and on_pathpolynom_callback().

m_currentReferenceSplineSPtr

std::shared_ptr<PolySpline> ControllerNode::m_currentReferenceSplineSPtr

private

Definition at line 55 of file ControllerNode.hpp.

Referenced by calculate_turn_angle(), ControllerNode(), and on_pathpolynom_callback().

m_currentTurnAngles

std::tuple<float, float> ControllerNode::m_currentTurnAngles = std::make_tuple(0.f, 0.f)

private

Definition at line 70 of file ControllerNode.hpp.

Referenced by calculate_turn_angle(), loop(), send_debug_messages(), and send_simulation_messages().

m_currentVelocity

std::atomic<float> ControllerNode::m_currentVelocity = 0.f

private

Definition at line 67 of file ControllerNode.hpp.

Referenced by calculate_turn_angle(), ControllerNode(), loop(), send_debug_messages(), and send_simulation_messages().

m_currentVelocityApplied

std::atomic<float> ControllerNode::m_currentVelocityApplied = 0.f

private

Definition at line 68 of file ControllerNode.hpp.

Referenced by calculate_turn_angle(), ControllerNode(), and on_measurement_callback().

m_currentVelocityReference

std::atomic<float> ControllerNode::m_currentVelocityReference = 0.f

private

Definition at line 69 of file ControllerNode.hpp.

Referenced by calculate_turn_angle(), and on_controller_reference_callback().

m_debugControllerPubSPtr

rclcpp::Publisher<DebugControllerType>::SharedPtr ControllerNode::m_debugControllerPubSPtr

private

Definition at line 34 of file ControllerNode.hpp.

Referenced by ControllerNode(), and send_debug_messages().

m_debugMode

bool ControllerNode::m_debugMode = false

private

Definition at line 24 of file ControllerNode.hpp.

Referenced by ControllerNode(), and loop().

m_disableController

std::atomic<bool> ControllerNode::m_disableController

private

Definition at line 26 of file ControllerNode.hpp.

Referenced by ControllerNode(), and on_controller_reference_callback().

m_lastPolynomUpdateTimestamp

std::atomic<int64_t> ControllerNode::m_lastPolynomUpdateTimestamp

private

Definition at line 59 of file ControllerNode.hpp.

Referenced by loop(), and on_pathpolynom_callback().

m_lastReceivedMeasurementSPtr

MeasurementsType::SharedPtr ControllerNode::m_lastReceivedMeasurementSPtr

private

Definition at line 47 of file ControllerNode.hpp.

Referenced by on_measurement_callback().

m_lastReceivedPathpolynomSPtr

PathpolynomType::SharedPtr ControllerNode::m_lastReceivedPathpolynomSPtr

private

Definition at line 54 of file ControllerNode.hpp.

Referenced by on_pathpolynom_callback(), and send_debug_messages().

m_lastSentControlLights

std::shared_ptr<ControlLightsType> ControllerNode::m_lastSentControlLights

private

Definition at line 51 of file ControllerNode.hpp.

Referenced by loop().

m_lightsStdPublisherSPtr

rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr ControllerNode::m_lightsStdPublisherSPtr

private

Definition at line 36 of file ControllerNode.hpp.

Referenced by ControllerNode(), and loop().

m_localMapDebugImgSubscription

image_transport::Subscriber ControllerNode::m_localMapDebugImgSubscription

private

Definition at line 43 of file ControllerNode.hpp.

Referenced by ControllerNode().

m_localMapDebugTrajectoryImgPublisher

image_transport::Publisher ControllerNode::m_localMapDebugTrajectoryImgPublisher

private

Definition at line 44 of file ControllerNode.hpp.

Referenced by ControllerNode(), and on_map_debug_image_callback().

m_measurementMtx

std::mutex ControllerNode::m_measurementMtx

private

Definition at line 48 of file ControllerNode.hpp.

Referenced by on_measurement_callback().

m_measurementSubSPtr

rclcpp::Subscription<MeasurementsType>::SharedPtr ControllerNode::m_measurementSubSPtr

private

Definition at line 41 of file ControllerNode.hpp.

Referenced by ControllerNode().

m_minVelocity

std::atomic<float> ControllerNode::m_minVelocity = 0.4f

private

Definition at line 72 of file ControllerNode.hpp.

Referenced by calculate_turn_angle(), and new_parameters_callback().

m_pathPolynomMtx

std::mutex ControllerNode::m_pathPolynomMtx

private

Definition at line 58 of file ControllerNode.hpp.

Referenced by calculate_turn_angle(), on_pathpolynom_callback(), and send_debug_messages().

m_pathPolynomSubSPtr

rclcpp::Subscription<PathpolynomType>::SharedPtr ControllerNode::m_pathPolynomSubSPtr

private

Definition at line 39 of file ControllerNode.hpp.

Referenced by ControllerNode().

m_simulationMode

bool ControllerNode::m_simulationMode = false

private

Definition at line 25 of file ControllerNode.hpp.

Referenced by calculate_turn_angle(), ControllerNode(), and loop().

m_simulationVehicleControlPubSPtr

rclcpp::Publisher<SimulationVehicleControlType>::SharedPtr ControllerNode::m_simulationVehicleControlPubSPtr

private

Definition at line 35 of file ControllerNode.hpp.

Referenced by ControllerNode(), and send_simulation_messages().

m_start_ParkingNode

std::atomic<bool> ControllerNode::m_start_ParkingNode = false

private

Definition at line 29 of file ControllerNode.hpp.

Referenced by new_parameters_callback().

m_stop_ControllerNode

std::atomic<bool> ControllerNode::m_stop_ControllerNode = false

private

Definition at line 28 of file ControllerNode.hpp.

Referenced by loop(), and new_parameters_callback().

m_turnAnglesSPtr

std::shared_ptr<std::vector<float> > ControllerNode::m_turnAnglesSPtr

private

Definition at line 63 of file ControllerNode.hpp.

Referenced by ControllerNode().

m_velocityRollingAverageFactorForNewVelocity

std::atomic<float> ControllerNode::m_velocityRollingAverageFactorForNewVelocity = 0.01f

private

Definition at line 73 of file ControllerNode.hpp.

Referenced by calculate_turn_angle(), and new_parameters_callback().

---

The documentation for this class was generated from the following files:

• nodes/ControllerNode/include/ControllerNode.hpp
• nodes/ControllerNode/src/ControllerNode.cpp