57 lines
2.6 KiB
C++
57 lines
2.6 KiB
C++
#include <spdlog/spdlog.h>
|
|
|
|
#include "include/communication/MessageProcessor.hpp"
|
|
#include "include/communication/MessageBuilder.hpp"
|
|
|
|
#include "include/Encoders.hpp"
|
|
#include "include/IRSensors.hpp"
|
|
#include "include/ClosedLoopMotorController.hpp"
|
|
|
|
using namespace CompLib;
|
|
|
|
google::protobuf::Message *MessageProcessor::process_message(const std::string &serializedMessage) {
|
|
CompLib::GenericRequest message;
|
|
message.ParseFromString(serializedMessage);
|
|
auto messageTypeName = message.header().message_type();
|
|
|
|
try {
|
|
// Encoder
|
|
if (messageTypeName == EncoderReadPositionsRequest::GetDescriptor()->full_name()) {
|
|
return MessageBuilder::encoder_read_positions_response(Encoders::getInstance().get_positions());
|
|
} else if (messageTypeName == EncoderReadVelocitiesRequest::GetDescriptor()->full_name()) {
|
|
return MessageBuilder::encoder_read_velocities_response(Encoders::getInstance().get_velocities_rpm());
|
|
}
|
|
|
|
// IR Sensors
|
|
if (messageTypeName == IRSensorsEnableRequest::GetDescriptor()->full_name()) {
|
|
IRSensors::enable();
|
|
return MessageBuilder::default_successful_generic_response();
|
|
} else if (messageTypeName == IRSensorsDisableRequest::GetDescriptor()->full_name()) {
|
|
IRSensors::disable();
|
|
return MessageBuilder::default_successful_generic_response();
|
|
} else if (messageTypeName == IRSensorsReadAllRequest::GetDescriptor()->full_name()) {
|
|
return MessageBuilder::ir_sensors_read_all_response(IRSensors::getInstance().read());
|
|
}
|
|
|
|
// Motors
|
|
if (messageTypeName == MotorsSetPowerRequest::GetDescriptor()->full_name()) {
|
|
MotorsSetPowerRequest request;
|
|
request.ParseFromString(serializedMessage);
|
|
ClosedLoopMotorController::getInstance().set_power(request.port(), request.power());
|
|
return MessageBuilder::default_successful_generic_response();
|
|
} else if (messageTypeName == MotorsSetSpeedRequest::GetDescriptor()->full_name()) {
|
|
MotorsSetSpeedRequest request;
|
|
request.ParseFromString(serializedMessage);
|
|
ClosedLoopMotorController::getInstance().set_speed(request.port(), request.speed());
|
|
return MessageBuilder::default_successful_generic_response();
|
|
}
|
|
} catch (const std::exception &ex) {
|
|
spdlog::error("Error when processing message with header {}: {}", messageTypeName, ex.what());
|
|
google::protobuf::Message *returnMessage = MessageBuilder::generic_response(false, ex.what());
|
|
return returnMessage;
|
|
}
|
|
|
|
spdlog::error("Message {} unknown!", messageTypeName);
|
|
google::protobuf::Message *returnMessage = MessageBuilder::generic_response(false, ERROR_MESSAGE_HEADER_UNKNOWN);
|
|
return returnMessage;
|
|
}
|