compAIR/compLIB
compAIR/compLIB
Archived
4
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Add complib client and TCP communication

Browse source
This commit is contained in:
Konstantin Lampalzer 2022-05-22 23:00:57 +02:00
parent 1a033c8b03
commit a484bc2137
35 changed files with 815 additions and 232 deletions
Download patch file Download diff file Expand all files Collapse all files

10
server_v2/src/communication/MessageBuilder.cpp
View file

@ -1,3 +1,5 @@
#include <cstdint>
#include "include/communication/MessageBuilder.hpp"
using namespace CompLib;
@ -36,17 +38,17 @@ CompLib::EncoderReadPositionsResponse *MessageBuilder::encoder_read_positions_re
response->set_allocated_header(header(EncoderReadPositionsResponse::descriptor()->full_name()));
response->set_allocated_status(default_successful_status());
for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
response->set_positions(i, positions.at(i));
response->add_positions(positions.at(i));
}
return response;
}
CompLib::EncoderReadVelocitiesResponse *MessageBuilder::encoder_read_velocities_response(std::array<double, ROBOT_MOTOR_COUNT> velocities) {
auto response = new EncoderReadVelocitiesResponse();
response->set_allocated_header(header(EncoderReadPositionsResponse::descriptor()->full_name()));
response->set_allocated_header(header(EncoderReadVelocitiesResponse::descriptor()->full_name()));
response->set_allocated_status(default_successful_status());
for (int i = 0; i < ROBOT_MOTOR_COUNT; ++i) {
response->set_velocities(i, velocities.at(i));
response->add_velocities(velocities.at(i));
}
return response;
}
@ -56,7 +58,7 @@ CompLib::IRSensorsReadAllResponse *MessageBuilder::ir_sensors_read_all_response(
response->set_allocated_header(header(IRSensorsReadAllResponse::descriptor()->full_name()));
response->set_allocated_status(default_successful_status());
for (int i = 0; i < ROBOT_IR_SENSOR_COUNT; ++i) {
response->set_data(i, data.at(i));
response->add_data(data.at(i));
}
return response;
}

1
server_v2/src/communication/MessageProcessor.cpp
View file

@ -13,6 +13,7 @@ google::protobuf::Message *MessageProcessor::process_message(const std::string &
CompLib::GenericRequest message;
message.ParseFromString(serializedMessage);
auto messageTypeName = message.header().message_type();
spdlog::debug("Got request with type {}", messageTypeName);
try {
// Encoder

74
server_v2/src/communication/TCPSocketServer.cpp Normal file
View file

@ -0,0 +1,74 @@
#include <sys/un.h>
#include <sys/socket.h>
#include <zconf.h>
#include <netinet/in.h>
#include <spdlog/spdlog.h>
#include "include/communication/TCPSocketServer.hpp"
#include "include/communication/MessageProcessor.hpp"
#define SERVER_PORT 9090
#define TCP_LISTEN_BACKLOG 5
TCPSocketServer::TCPSocketServer() {
server_thread = std::thread(&TCPSocketServer::server_loop, this);
server_thread.detach();
}
[[noreturn]] void TCPSocketServer::server_loop() {
struct sockaddr_in address;
int socket_file_descriptor = socket(AF_INET, SOCK_STREAM, 0);
if (socket_file_descriptor == 0) {
spdlog::error("TCP socket initialization failed!");
exit(EXIT_FAILURE);
}
int opt = 1;
if (int err = setsockopt(socket_file_descriptor, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt))) {
spdlog::error("TCP setsockopt failed! Err: {}", err);
exit(EXIT_FAILURE);
}
address.sin_family = AF_INET;
address.sin_addr.s_addr = INADDR_ANY;
address.sin_port = htons(SERVER_PORT);
if (bind(socket_file_descriptor, (struct sockaddr *) &address, sizeof(address)) < 0) {
spdlog::error("TCP bind failed!");
exit(EXIT_FAILURE);
}
if (listen(socket_file_descriptor, TCP_LISTEN_BACKLOG) < 0) {
spdlog::error("TCP listen failed!");
exit(EXIT_FAILURE);
}
char read_buffer[SOCKET_BUFFER_SIZE];
char write_buffer[SOCKET_BUFFER_SIZE];
for (;;) {
int client_file_descriptor = accept(socket_file_descriptor, NULL, NULL);
if (client_file_descriptor < 0) {
spdlog::error("TCP accept failed!");
continue;
}
read(client_file_descriptor, read_buffer, 1);
uint8_t message_size = read_buffer[0];
read(client_file_descriptor, read_buffer, read_buffer[0]);
std::string string_message;
for (int i{}; i < message_size; i++) {
string_message += read_buffer[i];
}
auto response = MessageProcessor::process_message(string_message);
uint8_t response_size = response->ByteSizeLong();
write_buffer[0] = response_size;
write(client_file_descriptor, write_buffer, 1);
response->SerializeToArray(write_buffer, SOCKET_BUFFER_SIZE);
write(client_file_descriptor, write_buffer, response_size);
close(client_file_descriptor);
}
}

25
server_v2/src/main.cpp
View file

@ -1,24 +1,31 @@
#include <spdlog/spdlog.h>
#include <unistd.h>
#include "include/reset.hpp"
#include "include/Encoders.hpp"
#include "include/OdometryController.hpp"
#include "include/ClosedLoopMotorController.hpp"
#include "include/communication/UnixSocketServer.hpp"
#include "include/communication/TCPSocketServer.hpp"
using namespace std;
int main() {
Reset::reset_robot();
spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
spdlog::set_level(spdlog::level::debug);
ClosedLoopMotorController::getInstance().set_speed(0, -125);
ClosedLoopMotorController::getInstance().set_speed(3, 125);
OdometryController::getInstance().enable();
UnixSocketServer unixSocketServer;
TCPSocketServer tcpSocketServer;
for (int i = 0; i < 10000; ++i) {
auto odom = OdometryController::getInstance().get();
spdlog::info("X {} Y {} W {}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation());
usleep(1000);
}
// ClosedLoopMotorController::getInstance().set_speed(0, -125);
// ClosedLoopMotorController::getInstance().set_speed(3, 125);
// OdometryController::getInstance().enable();
// for (int i = 0; i < 10000; ++i) {
// auto odom = OdometryController::getInstance().get();
// spdlog::info("X {} Y {} W {}", odom.get_x_position(), odom.get_y_position(), odom.get_angular_orientation());
// usleep(1000);
// }
std::this_thread::sleep_for(std::chrono::hours(12));
return 0;
}

191
server_v2/src/spi.cpp
View file

@ -1,8 +1,3 @@
#include <fcntl.h> //Needed for SPI port
#include <sys/ioctl.h> //Needed for SPI port
#include <linux/spi/spidev.h> //Needed for SPI port
#include <unistd.h> //Needed for SPI port
#include <iostream>
#include <string.h>
#include <spdlog/spdlog.h>
@ -10,104 +5,146 @@
#include "include/mathUtils.hpp"
void check_for_error(int error_code, std::string error_message) {
if (error_code < 0) {
spdlog::error(error_message);
exit(1);
}
}
Spi::Spi() {
int spi_mode = SPI_MODE_0;
int spi_bits_per_workd = SPI_BITS_PER_WORD;
int spi_speed = SPI_SPEED;
check_for_error(spi_file_descriptor = open("/dev/spidev1.2", O_RDWR), "Could not open SPI device");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MODE, &spi_mode), "Could not set SPI_IOC_WR_MODE");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MODE, &spi_mode), "Could not set SPI_IOC_RD_MODE");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_WR_BITS_PER_WORD");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_RD_BITS_PER_WORD");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed), "Could not set SPI_IOC_WR_MAX_SPEED_HZ");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MAX_SPEED_HZ, &spi_mode), "Could not set SPI_IOC_RD_MAX_SPEED_HZ");
if (error_code < 0) {
spdlog::error(error_message);
exit(1);
}
}
void Spi::clear_buffers() {
memset(tx_buffer, 0, SPI_BUFFER_SIZE);
memset(rx_buffer, 0, SPI_BUFFER_SIZE);
memset(tx_buffer, 0, SPI_BUFFER_SIZE);
memset(rx_buffer, 0, SPI_BUFFER_SIZE);
}
uint8_t Spi::calculate_checksum(uint8_t* data, uint8_t length) {
int sum = 0;
for (int i = 0; i < length; i++) {
sum += data[i];
}
return sum % 256;
uint8_t Spi::calculate_checksum(uint8_t *data, uint8_t length) {
int sum = 0;
for (int i = 0; i < length; i++) {
sum += data[i];
}
return sum % 256;
}
#ifdef IS_RASPI
#include <fcntl.h> //Needed for SPI port
#include <sys/ioctl.h> //Needed for SPI port
#include <linux/spi/spidev.h> //Needed for SPI port
#include <unistd.h> //Needed for SPI port
Spi::Spi() {
int spi_mode = SPI_MODE_0;
int spi_bits_per_workd = SPI_BITS_PER_WORD;
int spi_speed = SPI_SPEED;
check_for_error(spi_file_descriptor = open("/dev/spidev1.2", O_RDWR), "Could not open SPI device");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MODE, &spi_mode), "Could not set SPI_IOC_WR_MODE");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MODE, &spi_mode), "Could not set SPI_IOC_RD_MODE");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_WR_BITS_PER_WORD");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_RD_BITS_PER_WORD");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed), "Could not set SPI_IOC_WR_MAX_SPEED_HZ");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MAX_SPEED_HZ, &spi_mode), "Could not set SPI_IOC_RD_MAX_SPEED_HZ");
}
int Spi::read(uint8_t reg, uint8_t length) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
uint8_t read_buffer[SPI_BUFFER_SIZE] = {0};
read_array(reg, length, read_buffer);
uint8_t read_buffer[SPI_BUFFER_SIZE] = {0};
read_array(reg, length, read_buffer);
return mathUtils::int_from_bytes(read_buffer, length);
return mathUtils::int_from_bytes(read_buffer, length);
}
void Spi::read_array(uint8_t reg, uint8_t length, uint8_t* data) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
void Spi::read_array(uint8_t reg, uint8_t length, uint8_t *data) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
clear_buffers();
tx_buffer[0] = 0;
tx_buffer[1] = reg;
tx_buffer[2] = length;
clear_buffers();
tx_buffer[0] = 0;
tx_buffer[1] = reg;
tx_buffer[2] = length;
transfer();
transfer();
uint8_t checksum = calculate_checksum(rx_buffer, length + 3);
if (checksum != rx_buffer[length + 3]) {
spdlog::error("Received invalid checksum {}. Should be {}", rx_buffer[length +3], checksum);
}
uint8_t checksum = calculate_checksum(rx_buffer, length + 3);
if (checksum != rx_buffer[length + 3]) {
spdlog::error("Received invalid checksum {}. Should be {}", rx_buffer[length + 3], checksum);
}
memcpy(data, rx_buffer + 2, length);
memcpy(data, rx_buffer + 2, length);
}
void Spi::write(uint8_t reg, uint8_t length, int value) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
uint8_t write_buffer[SPI_BUFFER_SIZE] = {0};
mathUtils::bytes_from_int(value, length, write_buffer);
write_array(reg, length, write_buffer);
uint8_t write_buffer[SPI_BUFFER_SIZE] = {0};
mathUtils::bytes_from_int(value, length, write_buffer);
write_array(reg, length, write_buffer);
}
void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t* data) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t *data) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
clear_buffers();
tx_buffer[0] = 1;
tx_buffer[1] = reg;
tx_buffer[2] = length;
memcpy(tx_buffer + 3, data, length);
clear_buffers();
tx_buffer[0] = 1;
tx_buffer[1] = reg;
tx_buffer[2] = length;
memcpy(tx_buffer + 3, data, length);
transfer();
transfer();
}
void Spi::transfer() {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
struct spi_ioc_transfer spi;
memset(&spi, 0, sizeof(spi));
spi.tx_buf = (unsigned long) tx_buffer;
spi.rx_buf = (unsigned long) rx_buffer;
spi.len = SPI_BUFFER_SIZE;
spi.delay_usecs = 0;
spi.speed_hz = SPI_SPEED;
spi.bits_per_word = SPI_BITS_PER_WORD;
spi.cs_change = 0;
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data");
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
if (tx_buffer[1] != rx_buffer[1]) {
spdlog::error("SPI error during read/write of register {}. Got reg {} instead!", tx_buffer[1], rx_buffer[1]);
}
struct spi_ioc_transfer spi;
memset(&spi, 0, sizeof(spi));
spi.tx_buf = (unsigned long) tx_buffer;
spi.rx_buf = (unsigned long) rx_buffer;
spi.len = SPI_BUFFER_SIZE;
spi.delay_usecs = 0;
spi.speed_hz = SPI_SPEED;
spi.bits_per_word = SPI_BITS_PER_WORD;
spi.cs_change = 0;
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data");
if (tx_buffer[1] != rx_buffer[1]) {
spdlog::error("SPI error during read/write of register {}. Got reg {} instead!", tx_buffer[1], rx_buffer[1]);
}
}
#else
Spi::Spi() {}
int Spi::read(uint8_t reg, uint8_t length) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
spdlog::warn("Calling SPI without actual interface.");
return 0;
}
void Spi::read_array(uint8_t reg, uint8_t length, uint8_t *data) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
spdlog::warn("Calling SPI without actual interface.");
clear_buffers();
memcpy(data, rx_buffer, length);
}
void Spi::write(uint8_t reg, uint8_t length, int value) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
spdlog::warn("Calling SPI without actual interface.");
}
void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t *data) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
spdlog::warn("Calling SPI without actual interface.");
}
void Spi::transfer() {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
spdlog::warn("Calling SPI without actual interface.");
}
#endif