UWBDataPublisher.cpp

//    ROS node that publishes the localization data from sensor fusion of the crazyflie, using uwb for coordinates
//    Copyright (C) 2017  Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard
//
//    This program 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.
//
//    This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.

#define DEG2RAD	0.01745329251

#include <stdlib.h>
#include <string.h>

#include <ros/package.h>
#include <rosbag/bag.h>

#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/Anchors.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h"

#include "IndoorNewton.h"

#include "UWBDataPublisher.h"

using namespace d_fall_pps;

// This is a test comment to check wtf git does

// the clientID defines namespace and identifier in the CentralManagerService
int clientID;

CrazyflieContext context;
rosbag::Bag bag;

ros::ServiceClient centralManager;
ros::ServiceClient uwbManager;
ros::Publisher UWBDataPublisher;

double rollPitchYaw[3];

UWBAnchorArray anchors;
double anchorPositions[3*UWB_NUM_ANCHORS];

double prevDistances[UWB_NUM_ANCHORS] = {0};

KalmanState xState;
KalmanState yState;
KalmanState zState;

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "UWBDataPublisher");
    ros::NodeHandle nodeHandle("~");
    ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();

    if(!loadParameters(nodeHandle))
    	return -1;

    centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
	loadCrazyflieContext();
	ros::Subscriber dbChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);

	uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
	ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, uwbChangedCallback);

	// Subscribe to the anchor positions and all relevant Data from crazyradio
	ros::Subscriber cfRPYSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/RPY",10,cfRPYCallback);
	ros::Subscriber uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/AnchorDistances", 10, cfDistancesCallback);

	UWBDataPublisher = nodeHandle.advertise<CrazyflieData>("UWBData", 10);

    std::string package_path;
	package_path = ros::package::getPath("d_fall_pps") + "/";
	std::string record_file = package_path + "log/UWBDataPublisherLog.bag";
    bag.open(record_file, rosbag::bagmode::Write);

    initKalmanStates();

    ROS_INFO_STREAM("[UWBDataPublisher] started!");

    ros::spin();
    bag.close();
    
    return 0;
}

void d_fall_pps::dbChangedCallback(const std_msgs::Int32& msg)
{
	d_fall_pps::loadCrazyflieContext();
}

void d_fall_pps::uwbChangedCallback(const std_msgs::Int32& msg)
{
	d_fall_pps::loadUWBSettings();
}

void d_fall_pps::loadUWBSettings()
{
	Anchors a;

	if(uwbManager.call(a))
	{
		anchors = a.response.anchorArray;
	}
	else
	{
		ROS_ERROR("[UWBDataPublisher] Could not update UWB Settings!");
	}

	for(int i = 0; i < UWB_NUM_ANCHORS; ++i)
	{
		anchorPositions[3*i] = anchors.data[i].x;
		anchorPositions[3*i + 1] = anchors.data[i].y;
		anchorPositions[3*i + 2] = anchors.data[i].z;
	}
}

//Callback function to handle roll pitch yaw information from CF
void d_fall_pps::cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY)
{
	rollPitchYaw[0] = arrRPY.data[0] * DEG2RAD;
	rollPitchYaw[1] = arrRPY.data[1] * DEG2RAD;
	rollPitchYaw[2] = arrRPY.data[2] * DEG2RAD;
}

// Callback functions to handle times information from CF
void d_fall_pps::cfDistancesCallback(const std_msgs::UInt32MultiArray &distances)
{
	CrazyflieData data;
    
    double arrXYZ[3] = {0};
    calculateXYZ(distances, arrXYZ);
	
	data.crazyflieName = context.crazyflieName;
	data.x = arrXYZ[0];
	data.y = arrXYZ[1];
	data.z = arrXYZ[2];
	data.roll = rollPitchYaw[0];
	data.pitch = rollPitchYaw[1];
	data.yaw = rollPitchYaw[2];
	data.occluded = false;
	data.acquiringTime = 0; //???
	
	UWBDataPublisher.publish(data);
}

//calculates position from anchor locations and distances -> forces algorithm?
void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, double(&xyz)[3])
{
	double dist[UWB_NUM_ANCHORS];
	double result[3];

	for(unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
	{
		dist[i] = distances.data[i] / 100.0; //in dm

		if(distances.data[i] == 0)
		{
			dist[i] = prevDistances[i];
			ROS_WARN("[UWBDataPublisher] bad reading from Anchor %i", i);
		}
		else
			prevDistances[i] = dist[i];
	}

	IndoorNewton(anchorPositions, dist, result, 50);

	updateKalmanState(xState, result[0] / 10.0);
	updateKalmanState(yState, result[1] / 10.0);
	updateKalmanState(zState, result[2] / 10.0);

	
	// Use this for Kalmanfiltered position
	//xyz[0] = xState.x;
	//xyz[1] = yState.x;
	//xyz[2] = zState.x;

	// Use this for unfiltered position
	xyz[0] = result[0] / 10.0;
	xyz[1] = result[1] / 10.0;
	xyz[2] = result[2] / 10.0;
}

void d_fall_pps::initKalmanStates()
{
	// Init x state
	// TODO Update
	xState.x = 0;
	xState.q = 0.005;
	xState.r = 6;
	xState.p = 0;

	// Init y state
	// TODO Update
	yState.x = 0;
	yState.q = 0.005;
	yState.r = 6;
	yState.p = 0;

	// Init z state
	// TODO Update
	zState.x = 0;
	zState.q = 0.005;
	zState.r = 6;
	zState.p = 0;
}

void d_fall_pps::updateKalmanState(KalmanState& state, double measurment)
{
	state.p += state.q;
	state.k = state.p / (state.p + state.r);
	state.x += state.k * (measurment - state.x);
	state.p *= (1 - state.k);
}

// Loads the current context of the client
void d_fall_pps::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = clientID;

	centralManager.waitForExistence(ros::Duration(-1));

	if(centralManager.call(contextCall))
	{
		context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("[UWBDataPublisher] context updated");
	}
}

// Loads the parameters of the node handle
bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{
	if(!nodeHandle.getParam("clientID", clientID))
	{
		ROS_ERROR("Failed to get clientID!");
		return false;
	}

	return true;
}

// calculatse distances from passed microseconds
float d_fall_pps::tiagosAwesomeFormula(uint32_t ticks)
{
	return ticks * SPEED_OF_LIGHT / LOCO_DECK_FRQ;
	//return (time * 75.0 * (1.0 / (128.0 * 499.2)));
}