Galaxis Codebase: ECUNode Class Reference

#include <ECUNode.hpp>

Inheritance diagram for ECUNode:

Collaboration diagram for ECUNode:

[legend]

Public Member Functions
	ECUNode ()

void	init (std::shared_ptr< ECUHandler > _handler)
	Initialises the ECUNode and saves the pointer to the ECU Handler. More...

void	publishMeasurements (const EcuPacket *const packet, const std::array< std::chrono::duration< double, std::milli >, 10 > &diff_ms)
	Publishes the new Measurements from the ECU on the apropriate Publishers. More...

Private Member Functions
void	motion_callback (const ControlMotionType::SharedPtr p_msgSPtr)
	Callback function on new message. Sends the Data to the ECUHandler to send to the ECU. More...

void	lights_callback (const ControlLightsType::SharedPtr p_msgSPtr)

void	settingsvalues_callback (const SettingsValuesType::SharedPtr p_msgSPtr)
	Callback function on new message. Sends the Data to the ECUHandler to send to the ECU. More...

Private Attributes
rclcpp::Publisher< ButtonStateMessageType >::SharedPtr	m_buttonStatePubSPtr = nullptr
	Publisher object for buttonStates. More...

rclcpp::Publisher< MeasurementsType >::SharedPtr	m_measurementPubSPtr = nullptr
	Publisher object for measurements. More...

rclcpp::Publisher< sensor_msgs::msg::Range >::SharedPtr	m_TOFRearPubSPtr

rclcpp::Publisher< sensor_msgs::msg::Range >::SharedPtr	m_TOFRightPubSPtr

rclcpp::Publisher< std_msgs::msg::Float32 >::SharedPtr	m_velocityPubSPtr

rclcpp::Subscription< ControlMotionType >::SharedPtr	m_controlValuesSubSPtr = nullptr
	Subscriber object for control values from controller node. More...

rclcpp::Subscription< SettingsValuesType >::SharedPtr	m_settingsValuesSubSPtr = nullptr
	Subscriber object for control values from controller node. More...

rclcpp::Subscription< ControlLightsType >::SharedPtr	m_lightsSubSPtr = nullptr

std::shared_ptr< ECUHandler >	m_ecuHandlerSPtr = nullptr
	Handler for ECU communication. More...

ControlLightsType::SharedPtr	m_lastLightsSPtr = nullptr

uint8_t	m_lastButtonState = 0

std::mutex	m_lightsMtx

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 15 of file ECUNode.hpp.

Constructor & Destructor Documentation

◆ ECUNode()

ECUNode::ECUNode ( )

inline

Definition at line 17 of file ECUNode.hpp.

17 :

18 GalaxisNode("ecu_node") {}

Member Function Documentation

◆ init()

void ECUNode::init ( std::shared_ptr< ECUHandler > _handler )

Initialises the ECUNode and saves the pointer to the ECU Handler.

Parameters

_handler An ECU Handler object

Definition at line 19 of file ECUNode.cpp.

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

References lights_callback(), galaxis::utils::log(), motion_callback(), and settingsvalues_callback().

Here is the call graph for this function:

◆ lights_callback()

void ECUNode::lights_callback ( const ControlLightsType::SharedPtr p_msgSPtr )

private

Definition at line 300 of file ECUNode.cpp.

                                                                         {
   std::unique_lock<std::mutex> lock(m_lightsMtx);
   m_lastLightsSPtr = p_msgSPtr;
 }

Referenced by init().

Here is the caller graph for this function:

◆ motion_callback()

void ECUNode::motion_callback ( const ControlMotionType::SharedPtr p_msgSPtr )

private

Callback function on new message. Sends the Data to the ECUHandler to send to the ECU.

Parameters

p_msgSPtr

Definition at line 54 of file ECUNode.cpp.

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

References ComputerPacket, convertLightStatesIntoFinalState(), galaxis::utils::log(), and galaxis::utils::log_debug().

Referenced by init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ publishMeasurements()

void ECUNode::publishMeasurements	(	const EcuPacket *const	packet,
		const std::array< std::chrono::duration< double, std::milli >, 10 > &	diff_ms
	)

Publishes the new Measurements from the ECU on the apropriate Publishers.

Definition at line 140 of file ECUNode.cpp.

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

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

Referenced by main().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ settingsvalues_callback()

void ECUNode::settingsvalues_callback ( const SettingsValuesType::SharedPtr p_msgSPtr )

private

Callback function on new message. Sends the Data to the ECUHandler to send to the ECU.

Parameters

p_msgSPtr

Definition at line 104 of file ECUNode.cpp.

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

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

Referenced by init().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ m_buttonStatePubSPtr

rclcpp::Publisher<ButtonStateMessageType>::SharedPtr ECUNode::m_buttonStatePubSPtr = nullptr

private

Publisher object for buttonStates.

Definition at line 40 of file ECUNode.hpp.

◆ m_controlValuesSubSPtr

rclcpp::Subscription<ControlMotionType>::SharedPtr ECUNode::m_controlValuesSubSPtr = nullptr

private

Subscriber object for control values from controller node.

Definition at line 60 of file ECUNode.hpp.

◆ m_ecuHandlerSPtr

std::shared_ptr<ECUHandler> ECUNode::m_ecuHandlerSPtr = nullptr

private

Handler for ECU communication.

Definition at line 73 of file ECUNode.hpp.

◆ m_lastButtonState

uint8_t ECUNode::m_lastButtonState = 0

private

Definition at line 77 of file ECUNode.hpp.

◆ m_lastLightsSPtr

ControlLightsType::SharedPtr ECUNode::m_lastLightsSPtr = nullptr

private

Definition at line 75 of file ECUNode.hpp.

◆ m_lightsMtx

std::mutex ECUNode::m_lightsMtx

private

Definition at line 79 of file ECUNode.hpp.

◆ m_lightsSubSPtr

rclcpp::Subscription<ControlLightsType>::SharedPtr ECUNode::m_lightsSubSPtr = nullptr

private

Definition at line 69 of file ECUNode.hpp.

◆ m_measurementPubSPtr

rclcpp::Publisher<MeasurementsType>::SharedPtr ECUNode::m_measurementPubSPtr = nullptr

private

Publisher object for measurements.

Definition at line 46 of file ECUNode.hpp.

◆ m_settingsValuesSubSPtr

rclcpp::Subscription<SettingsValuesType>::SharedPtr ECUNode::m_settingsValuesSubSPtr = nullptr

private

Subscriber object for control values from controller node.

Definition at line 66 of file ECUNode.hpp.

◆ m_TOFRearPubSPtr

rclcpp::Publisher<sensor_msgs::msg::Range>::SharedPtr ECUNode::m_TOFRearPubSPtr

private

Definition at line 51 of file ECUNode.hpp.

◆ m_TOFRightPubSPtr

rclcpp::Publisher<sensor_msgs::msg::Range>::SharedPtr ECUNode::m_TOFRightPubSPtr

private

Definition at line 52 of file ECUNode.hpp.

◆ m_velocityPubSPtr

rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr ECUNode::m_velocityPubSPtr

private

Definition at line 53 of file ECUNode.hpp.

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

nodes/ECUNode/include/ECUNode.hpp
nodes/ECUNode/src/ECUNode.cpp

Public Member Functions

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ ECUNode()

Member Function Documentation

◆ init()

◆ lights_callback()

◆ motion_callback()

◆ publishMeasurements()

◆ settingsvalues_callback()

Member Data Documentation

◆ m_buttonStatePubSPtr

◆ m_controlValuesSubSPtr

◆ m_ecuHandlerSPtr

◆ m_lastButtonState

◆ m_lastLightsSPtr

◆ m_lightsMtx

◆ m_lightsSubSPtr

◆ m_measurementPubSPtr

◆ m_settingsValuesSubSPtr

◆ m_TOFRearPubSPtr

◆ m_TOFRightPubSPtr

◆ m_velocityPubSPtr