Commit 68dd4b93 authored by michaero's avatar michaero
Browse files

Added structure and code for Anchor calibration (self-localization)

parent 57db1ad2
#pragma once
#include "SerialCommon.h"
#include <list>
#include <vector>
#include <string>
#include <stdio.h>
#include <cmath>
#include <ctime>
struct anchor_data
{
unsigned int from, to, distance;
anchor_data(unsigned int f, unsigned int t, unsigned int d) : from(f), to(t), distance(d) {}
};
struct anchor_position_data
{
unsigned int id;
double x, y, z;
anchor_position_data(){}
anchor_position_data(unsigned int _id, double _x, double _y, double _z) : id(_id), x(_x), y(_y), z(_z) {}
};
class DataListener
{
public:
DataListener();
~DataListener();
bool checkAndReadData(std::string value);
bool calulateAnchorPosition();
void setOffset(double x, double y, double z);
private:
double _distances[8];
std::list<anchor_data> anchors;
std::vector<anchor_position_data> anchor_position;
anchor_position_data offset;
unsigned int elapsedTime;
std::clock_t locTimer;
double _get_distance_average(unsigned int anchor_from, unsigned int anchor_to, int& flag);
void _CF_Debugger(std::string value);
void _LOC_Debugger(std::string value);
};
\ No newline at end of file
#pragma once
//SERIAL COMMUNICATION PARAMETERS
#define MAX_SERIAL_BUFFER_SIZE 180
#define SERIAL_BAUD_RATE B921600
#define SERIAL_BYTE_SIZE 8
#define SERIAL_STOP_BITS 0
#define SERIAL_PARITY 0
#define SERIAL_COMMAND_END '\n'
#define SERIAL_READY_FLAG "-7\n"
#define SERIAL_FLAG_TIMEOUT 1000
#define K_WARNING "\x1B[33m"
#define K_ERROR "\x1B[31m"
#define K_SUCCESS "\x1B[32m"
#define K_NORMAL "\x1B[0m"
#define KBLU "\x1B[34m"
#define KMAG "\x1B[35m"
#define KCYN "\x1B[36m"
#define KWHT "\x1B[37m"
\ No newline at end of file
#pragma once
#include "SerialCommon.h"
#include <errno.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h> //Timer
#include <fcntl.h> //Device settings
#include <termios.h> //For serial comm
#include <sys/ioctl.h> //For ioctl
#include <string.h> // needed for memset
#include <string>
#include <vector>
#include <ctime>
class Serial
{
public:
Serial();
void init();
int startConnection(const char* port);
void closeConnection();
bool isConnected(const char* port = "");
void writeSerial(const char* buffer);
int manageSerial();
std::string readSerial();
private:
int tty_fd;
struct termios tio;
std::vector<std::string> _commands;
std::string _cur_command;
std::clock_t timer;
std::string _port;
bool _ready;
bool _isConnected;
};
\ No newline at end of file
...@@ -38,6 +38,9 @@ namespace d_fall_pps ...@@ -38,6 +38,9 @@ namespace d_fall_pps
// Callback to update anchor Positions // Callback to update anchor Positions
void updateCallback(const std_msgs::Int32 &message); void updateCallback(const std_msgs::Int32 &message);
// Callback to recalibrate anchor Positions
void calibrateAnchors();
} }
#endif // UWBMANAGERSERVICE_H included #endif // UWBMANAGERSERVICE_H included
#include "DataListener.h"
#include "IndoorNewton.h"
#include <string>
#include <vector>
#include "ros/ros.h"
#include <ros/package.h>
bool compare_anchor_data(const anchor_data& a, const anchor_data& b)
{
if ((a.from == b.from && a.to < b.to) || a.from < b.from)
return true;
return false;
}
DataListener::DataListener()
{
for (unsigned int j = 0; j < 8; j++) {
_distances[j] = 0;
}
offset.x = 0;
offset.y = 0;
offset.z = 0;
elapsedTime = 0;
remove("AnchorPos.yaml");
remove("data.txt");
}
DataListener::~DataListener()
{
}
bool DataListener::checkAndReadData(std::string value)
{
switch (value.at(0))
{
case '#':
_CF_Debugger(value);
return 1;
break;
case '$':
_LOC_Debugger(value);
return 1;
break;
default:
break;
}
return 0;
}
void DataListener::setOffset(double x, double y, double z)
{
printf("Offset now is (%.2lf, %.2lf, %.2lf)\n", x, y, z);
offset.x = x;
offset.y = y;
offset.z = z;
}
void DataListener::_CF_Debugger(std::string value)
{
unsigned int distances[8];
sscanf(value.c_str(), "#%u/%u/%u/%u/%u/%u/%u/%u/", &distances[0], &distances[1], &distances[2], &distances[3], &distances[4], &distances[5], &distances[6], &distances[7]);
printf("--- DISTANCE: \t%u \t%u \t%u \t%u \t%u \t%u \t%u \t%u\n", distances[0], distances[1], distances[2], distances[3], \
distances[4], distances[5], distances[6], distances[7]);
/*FILE* file = fopen("data.txt", "a");
elapsedTime += distances[7];
fprintf(file, "%u\n", elapsedTime);
fclose(file);*/
}
void DataListener::_LOC_Debugger(std::string value)
{
unsigned int source, dest, distance;
sscanf(value.c_str(), "$%u/%u/%u", &source, &dest, &distance);
distance -= 500; //500;
anchors.push_back(anchor_data(source, dest, distance));
printf("--- LOC: \t%u \t%u \t%u\n", source, dest, distance);
//Reset Timer
locTimer = std::clock();
}
double DataListener::_get_distance_average(unsigned int anchor_from, unsigned int anchor_to, int& flag)
{
unsigned int sum = 0;
unsigned int count = 0;
std::list<anchor_data>::iterator it;
for (it = anchors.begin(); it != anchors.end(); it++)
{
//Not a measurement from and to
if (it->from != anchor_from || it->to != anchor_to)
continue;
if (it->distance < 10000) //Simple filter -> if the distance is less than 10m = 10'000mm
{
sum += it->distance;
count++;
}
}
//FAILED
if (count == 0 || sum == 0)
flag = -1;
//Divide by 100 to get it in dm
return ((double)sum / (double)count) / 100.0;
}
////////////////////////////////////////////////////////////
/*
I KNOW, THIS NEWTON ALGORITHM IS MADE FOR 6 ANCHORS. NOT THE BEST SOLUTION WHEN USING ONLY 3 TO DETERMINE
THE POSITION OF THE OTHER ANCHORS, BUT HEY, IT WORKS xD
*/
bool DataListener::calulateAnchorPosition()
{
//List is not empty and there is a 5s delay after the last received distance
if (anchors.size() == 0 || (std::clock() - locTimer) / ((double)CLOCKS_PER_SEC) < 2)
return false;
anchors.sort(compare_anchor_data);
int flag = 1;
double d01, d02, d12;
d01 = _get_distance_average(0, 1, flag);
d02 = _get_distance_average(0, 2, flag);
d12 = _get_distance_average(1, 2, flag);
double x = (d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02);
double y = d01 * sin(acos((d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02 * d01)));
double anchor_positions[18] = {0, 0, 0, \
x, -y, 0, \
d02, 0, 0, \
0, 0, 0, \
x, -y, 0, \
d02, 0, 0
};
anchor_position.push_back(anchor_position_data(0, anchor_positions[0], anchor_positions[1], anchor_positions[2]));
anchor_position.push_back(anchor_position_data(1, anchor_positions[3], anchor_positions[4], anchor_positions[5]));
anchor_position.push_back(anchor_position_data(2, anchor_positions[6], anchor_positions[7], anchor_positions[8]));
//Error with one of the preset values
if (flag == -1)
return false;
//For anchor n we only need d0n, d1n, d2n
for (unsigned int n = 3; n < 6; n++)
{
int flag = 1;
double anchor_distances[6] = { _get_distance_average(0, n, flag), _get_distance_average(1, n, flag), _get_distance_average(2, n, flag), \
_get_distance_average(0, n, flag), _get_distance_average(1, n, flag), _get_distance_average(2, n, flag)
};
if (flag != -1) {
//Calculate position of anchor n using IndoorNewton
double result[3];
IndoorNewton(anchor_positions, anchor_distances, result, 200);
anchor_position.push_back(anchor_position_data(n, result[0], result[1], result[2]));
}
}
//Save anchor positions to yaml file
ros::NodeHandle nodeHandle("~");
std::vector<float> anchorArr;
for (unsigned int i = 0; i < anchor_position.size(); i++)
{
anchorArr = {anchor_position[i].x - offset.x, anchor_position[i].y - offset.y, anchor_position[i].z - offset.z};
nodeHandle.setParam("anchor" + std::to_string(i), anchorArr);
}
// FILE* file = fopen("AnchorPos.yaml", "w");
// fprintf(file, "anchorLength: 6 # Update this number after adding/removing anchors!!!\n\n");
// fprintf(file, "# Structure: anchorID [x, y, z] (ID is always from 0 to length-1)\n\n");
// printf("%s\n---- ANCHOR POSITIONS ----\n", K_NORMAL);
// printf("---- Without offset\n");
// for (unsigned int i = 0; i < anchor_position.size(); i++)
// printf("anchor%u: [%.2lf, %.2lf, %.2lf]\n", i, anchor_position[i].x, anchor_position[i].y, anchor_position[i].z);
// printf("\n");
// printf("---- With offset: (%.2lf, %.2lf, %.2lf)\n", offset.x, offset.y, offset.z);
// for (unsigned int i = 0; i < anchor_position.size(); i++)
// {
// fprintf(file, "anchor%u: [%.2lf, %.2lf, %.2lf]\n", i, anchor_position[i].x - offset.x, anchor_position[i].y - offset.y, anchor_position[i].z - offset.z);
// printf("%sanchor%u: [%.2lf, %.2lf, %.2lf]\n", K_WARNING, i, anchor_position[i].x - offset.x, anchor_position[i].y - offset.y, anchor_position[i].z - offset.z);
// }
// printf("%s---- Saved anchor positions to AnchorPos.yaml\n", K_NORMAL);
// printf("%s--------------------------\n\n", K_NORMAL);
// fclose(file);
// anchors.clear();
return true;
}
\ No newline at end of file
#include "SerialUnix.h"
Serial::Serial()
{
_isConnected = false;
_port = "";
_ready = true;
_cur_command = "";
}
void Serial::init()
{
startConnection("/dev/ttyACM0");
usleep(500000);
}
int Serial::startConnection(const char* port)
{
printf("Attempting to connect to Serial port %s...\n", port);
_port = "";
_isConnected = false;
tio.c_iflag = 0;
tio.c_oflag = 0;
tio.c_cflag = CS8 | CREAD | CLOCAL;
tio.c_lflag = 0;
tio.c_cc[VMIN] = 1;
tio.c_cc[VTIME] = 5;
tty_fd = open(port, O_RDWR | O_NONBLOCK | O_SYNC);
if (tty_fd < 0) {
printf("%sError opening port: %s\n", K_ERROR, strerror(errno));
return -1;
}
cfsetospeed(&tio, SERIAL_BAUD_RATE);
cfsetispeed(&tio, SERIAL_BAUD_RATE);
if (tcsetattr(tty_fd, TCSANOW, &tio) != 0)
{
printf("%sError from tcsetattr: %s\n", K_ERROR, strerror(errno));
return -1;
}
_port = port;
_ready = true;
_isConnected = true;
printf("%sConnected\n", K_WARNING);
printf("%s", K_NORMAL);
return 1;
}
void Serial::closeConnection()
{
if (_port != "" && _isConnected)
close(tty_fd); // Port release
}
bool Serial::isConnected(const char* port)
{
if (port != "")
return (_isConnected && _port == port);
return _isConnected;
}
std::string Serial::readSerial()
{
if (_commands.empty())
return "";
std::string ret = _commands.front();
_commands.erase(_commands.begin());
return ret;
}
void Serial::writeSerial(const char* buffer)
{
if(!_isConnected)
return;
printf("Sending packet\n");
for (unsigned int i = 0; i < strlen(buffer); i++)
{
write(tty_fd, &buffer[i], 1);
}
bool timeout = true;
_ready = false;
timer = std::clock();
while (!_ready && timeout) {
timeout = (std::clock() - timer) / ((double)CLOCKS_PER_SEC / 1000) < SERIAL_FLAG_TIMEOUT;
manageSerial();
}
if (!timeout) printf("%s---- BASE STATION ACK TIMEOUT ----\n", K_ERROR); //We got a timeout
}
int Serial::manageSerial()
{
if(!_isConnected)
return 0;
int bytes = 0;
ioctl(tty_fd, FIONREAD, &bytes);
//go through all bytes (read them separately)
if (bytes) { //There are bytes on the buffer
char value;
for (unsigned int i = 0; i < bytes; i++)
{
if (read(tty_fd, &value, 1) > 0) {
//Append character to current command
_cur_command.push_back(value);
//End of command -> push
if (value == SERIAL_COMMAND_END || _cur_command.size() >= MAX_SERIAL_BUFFER_SIZE) {
//check if it is the ready flag
if (_cur_command == SERIAL_READY_FLAG)
_ready = true;
else
_commands.push_back(_cur_command);
_cur_command = "";
}
}
else {
printf("%sFailed to read\n", K_ERROR);
return 0;
}
}
}
return 1;
}
\ No newline at end of file
...@@ -23,9 +23,13 @@ ...@@ -23,9 +23,13 @@
#include "UWBManagerService.h" #include "UWBManagerService.h"
#include "SerialUnix.h"
#include "DataListener.h"
#define UWB_UPDATE_DISABLE 0 #define UWB_UPDATE_DISABLE 0
#define UWB_UPDATE_ENABLE 1 #define UWB_UPDATE_ENABLE 1
#define UWB_UPDATE_ANCHORS 5 #define UWB_UPDATE_ANCHORS 5
#define UWB_CALIBRATE_ANCHORS 7
using namespace d_fall_pps; using namespace d_fall_pps;
...@@ -105,6 +109,8 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message) ...@@ -105,6 +109,8 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
case UWB_UPDATE_DISABLE: case UWB_UPDATE_DISABLE:
enableUWB = false; enableUWB = false;
break; break;
case UWB_CALIBRATE_ANCHORS:
calibrateAnchors();
case UWB_UPDATE_ENABLE: case UWB_UPDATE_ENABLE:
enableUWB = true; enableUWB = true;
case UWB_UPDATE_ANCHORS: case UWB_UPDATE_ANCHORS:
...@@ -116,3 +122,43 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message) ...@@ -116,3 +122,43 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
ROS_WARN("[UWBManagerService] Updated UWB Settings!"); ROS_WARN("[UWBManagerService] Updated UWB Settings!");
} }
void d_fall_pps::calibrateAnchors()
{
Serial serial;
DataListener interpreter;
bool useLocalisation = false;
serial.init();
bool running = true;
interpreter.setOffset(6.75, -24.5, -6.0);
useLocalisation = true;
serial.writeSerial("loc100\r");
while (running)
{
serial.manageSerial();
/*if (!serial.manageSerial())
break;*/
std::string value = serial.readSerial();
while (value != "") {
//printf("\tRECEIVED: %s", value.c_str());
interpreter.checkAndReadData(value);
value = serial.readSerial();
}
if(useLocalisation)
interpreter.calulateAnchorPosition();
}
serial.closeConnection();
//readYaml();
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment