DataListener.cpp

//    Copyright (C) 2018, ETH Zurich, D-ITET
//          Yvan Bosshard           byvan       @ee.ethz.ch
//          Michael Rogenmoser      michaero    @ee.ethz.ch
//          Tiago Salzmann          tiagos      @ee.ethz.ch
//
//    This file is part of D-FaLL-System.
//    
//    D-FaLL-System is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//    
//    D-FaLL-System is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//    
//    You should have received a copy of the GNU General Public License
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
//    
//
//    ----------------------------------------------------------------------------------
//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
//
//
//    DESCRIPTION:
//    Handles connection to the UWB basestation and anchor settings.
//
//    ----------------------------------------------------------------------------------


#include "DataListener.h"

#include "IndoorNewton.h"

#include <string>
#include <vector>

#include "ros/ros.h"
#include <ros/package.h>

bool compare_anchor_data(const anchor_raw_data& a, const anchor_raw_data& b)
{
	if ((a.from == b.from && a.to < b.to) || a.from < b.from)
		return true;

	return false;
}

DataListener::DataListener()
{
}

DataListener::~DataListener()
{
}

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;

	_writeToYAML();
}

void DataListener::_LOC_Debugger(std::string value)
{
	unsigned int source, dest, distance;
	sscanf(value.c_str(), "$%u/%u/%u", &source, &dest, &distance);
	distance -= 500;

	anchors_raw.push_back(anchor_raw_data(source, dest, distance));

	//printf("--- LOC: \t%u \t%u \t%u\n", source, dest, distance);

	//Reset Timer
	locTimer = std::chrono::high_resolution_clock::now();
}

bool DataListener::_check_data_validity()
{
	if (anchors_raw.size() == 0)
		return false;

	for (unsigned int i = 0; i < 5; i++)
	{
		for (unsigned int j = i + 1; j < 6; j++)
		{
			if (_get_distance_average(i, j) == -1)
				return false;
		}
	}

	return true;
}

double DataListener::_get_distance_average(unsigned int anchor_from, unsigned int anchor_to)
{
	unsigned int sum = 0;
	unsigned int count = 0;
	std::list<anchor_raw_data>::iterator it;
	for (it = anchors_raw.begin(); it != anchors_raw.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)
		return -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::_calculateAnchorPosition()
{
	if (!_check_data_validity())
		return false;

	anchors_raw.sort(compare_anchor_data);

	double d01, d02, d12;
	d01 = _get_distance_average(0, 1);
	d02 = _get_distance_average(0, 2);
	d12 = _get_distance_average(1, 2);

	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 d03, d23;
	d03 = _get_distance_average(0,3);
	d23 = _get_distance_average(2,3);*/


	/*double a01, a21;
	a01 = asin(-12.8/d01);
	a21 = asin(-12.8/d12);

	d01 *= cos(a01);
	d12 *= cos(a21);*/
	/*

	d01 *= cos(a01);
	d12 *= cos(a21);*/

	/*double a03, a23;
	a03 = asin(-6/d03);
	a23 = asin(-6/d23);

	d03 *= cos(a03);
	d23 *= cos(a23);

	double x3 = (d03 * d03 + d02 * d02 - d23 * d23) / (2 * d02);
	double y3 = d03 * sin(acos((d03 * d03 + d02 * d02 - d23 * d23) / (2 * d02 * d03)));


	

	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, \
	                               x3, -y3, -6, \
	                               d02, 0, 0
	                              };

	ROS_WARN("x: %f, y: %f", x, -y);*/


	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]));
	//anchor_position.push_back(anchor_position_data(3, anchor_positions[12], anchor_positions[13], anchor_positions[14]));

	//For anchor n we only need d0n, d1n, d2n
	for (unsigned int n = 3; n < 6; n++)
	{
		double anchor_distances[6] = { _get_distance_average(0, n), _get_distance_average(1, n), _get_distance_average(2, n), \
		                               _get_distance_average(0, n), _get_distance_average(1, n), _get_distance_average(2, n)
		                             };


		//Calculate position of anchor n using IndoorNewton
		double result[3];
		IndoorNewton(anchor_positions, anchor_distances, result, 100);

		anchor_position.push_back(anchor_position_data(n, result[0], result[1], 0.0));//result[2])); //result[2]
	}

	//Save anchor positions to yaml file
	_writeToYAML();

	finishedLoc = true;
	return true;
}

void DataListener::_resetLocalisation()
{
	serial.closeConnection();

	anchors_raw.clear();
	anchor_position.clear();

	offset.x = 0;
	offset.y = 0;
	offset.z = 0;

	finishedLoc = false;
}

bool DataListener::_startSerialCommunication()
{
	if (!serial.startConnection("/dev/ttyACM0")) return false;
	if (!serial.writeSerial("loc100\r")) return false;

	locTimer = std::chrono::high_resolution_clock::now();
	while (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - locTimer).count() < 2000) {
		if (!serial.manageSerial())
			return false;

		std::string value = serial.readSerial();
		while (value != "") {
			if (value.at(0) == '$')
				_LOC_Debugger(value);

			value = serial.readSerial();
		}
	}

	return true;
}

bool DataListener::startAnchorLocalistaion()
{
	_resetLocalisation();

	for (unsigned int i = 0; i < 5 && !finishedLoc; i++)
	{
		if (i != 0)
			ROS_WARN("[Selflocalisation] Looks like something went wrong... %u tries left\n", 5 - i);

		_resetLocalisation();
		if (!_startSerialCommunication())
			continue;
		if (!_calculateAnchorPosition()) {
			ROS_ERROR("[Selflocalisation] One or more distances are missing");
			continue;
		}
	}
	serial.closeConnection();

	if (finishedLoc)
		return true;

	return false;
}

void DataListener::invertAnchor(unsigned int id)
{
	anchor_position[id].z *= -1;

	_writeToYAML();
}

void DataListener::_writeToYAML()
{
	ros::NodeHandle nodeHandle("~");

	//std::vector<float> anchorArr;
	std::vector<double> anchorArr;

	for (unsigned int i = 0; i < anchor_position.size(); i++)
	{
		//printf("anchor%i (%f, %f, %f)\n", i, anchor_position[i].x, anchor_position[i].y, anchor_position[i].z);
		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);
	}
}