ControllerNode.hpp

Go to the documentation of this file.

#ifndef _CONTROLLERNODE_HPP_
#define _CONTROLLERNODE_HPP_
 
#include <memory>
#include <mutex>
#include <atomic>
 
#include "rclcpp/rclcpp.hpp"
#include <galaxis/GalaxisNode.hpp>
#include "std_msgs/msg/string.hpp"
#include <galaxis/messages.hpp>
#include <galaxis/Errors.hpp>
 
#include <control/control.hpp>
#include <utils/logger.hpp>
#include <utils/poly.hpp>
#include <std_msgs/msg/u_int8.hpp>
 
#include <image_transport/image_transport.h>
 
 
class ControllerNode : public GalaxisNode {
private:
  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;
 
  // Publishers
  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;
 
  //Subscribers
  rclcpp::Subscription<PathpolynomType>::SharedPtr m_pathPolynomSubSPtr;
  rclcpp::Subscription<ControllerReferenceType>::SharedPtr m_controllerRefSubSPtr;
  rclcpp::Subscription<MeasurementsType>::SharedPtr m_measurementSubSPtr;
  //Debug
  image_transport::Subscriber m_localMapDebugImgSubscription;
  image_transport::Publisher m_localMapDebugTrajectoryImgPublisher;
 
  // Measurements
  MeasurementsType::SharedPtr m_lastReceivedMeasurementSPtr;
  std::mutex m_measurementMtx;
 
  //ControlLights
  std::shared_ptr<ControlLightsType> m_lastSentControlLights;
 
  // Pathpolynom
  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;
 
  // Control
  std::shared_ptr<galaxis::control::TwoAndAHalfCarrots> m_controlTwoAndAHalfCarrotsSPtr;
  std::shared_ptr<std::vector<float>> m_turnAnglesSPtr;
  std::mutex m_controlMtx;
 
  // - Current settings
  std::atomic<float> m_currentVelocity                      = 0.f; // in m/s
  std::atomic<float> m_currentVelocityApplied               = 0.f; // in m/s
  std::atomic<float> m_currentVelocityReference             = 0.f; // in m/s
  std::tuple<float, float> m_currentTurnAngles = std::make_tuple(0.f, 0.f); // in radiance
  std::tuple<std::atomic<bool>, std::atomic<bool>> m_currentIndicatorState = std::make_tuple(false, false); // in radiance
  std::atomic<float> m_minVelocity                                       = 0.4f; // min Velocity for steering in m/s // 45deg = m_minVelocity
  std::atomic<float> m_velocityRollingAverageFactorForNewVelocity        = 0.01f; 
 
public:
  ControllerNode();
 
private:
  void loop();
  void send_debug_messages();
  void send_simulation_messages();
 
  void calculate_erpm();
  void calculate_turn_angle();
 
  // publish
  void publish_control_values(ControlMotionType::UniquePtr msg);
 
  // subscribe
  void on_measurement_callback(MeasurementsType::SharedPtr msg);
  void on_pathpolynom_callback(PathpolynomType::SharedPtr msg);
  void on_controller_reference_callback(ControllerReferenceType::SharedPtr msg);
  // debug
  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);
};
 
#endif // _CONTROLLERNODE_HPP_