LocalizationServer.cpp

//    This ROS node is subscribed to the ViconDataPublisher as well as
//	  to the UWBDataPublisher. It is used to relay the data from the
//	  desired localisation source to the controller and the GUI.
//
//    Copyright (C) 2018
//          Yvan Bosshard           byvan       @ee.ethz.ch
//          Michael Rogenmoser      michaero    @ee.ethz.ch
//          Tiago Salzmann          tiagos      @ee.ethz.ch
//
//    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/>.

#include <chrono>
#include <stdlib.h>
#include <std_msgs/String.h>
#include <ros/package.h>

#include "LocalizationServer.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 "std_msgs/Float32.h"

using namespace d_fall_pps;

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

// Self explanatory variables
ros::ServiceClient centralManager;
ros::ServiceClient uwbManager;
ros::Publisher localizationPublisher;
CrazyflieContext context;

bool enableUWB = false;
bool useUWB = false;

// store resent localisation updates
CrazyflieData CFData_vicon;
CrazyflieData CFData_uwb;

/************************************************************
 *	Main			  										*
 ***********************************************************/
int main(int argc, char* argv[])
{
	// Initialize the node and it's handles
	ros::init(argc, argv, "LocalizationServer");
	ros::NodeHandle nodeHandle("~");
	ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();

	// load context parameters
	if(!loadParameters(nodeHandle))
		return -1;

	// connect to the central manager service to receive CF Context and subscribe to CF Context updates of the DB
	centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
	loadCrazyflieContext();
	ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);

	uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);

	ros::Subscriber enableUWBSubscriber = namespaceNodeHandle.subscribe("student_GUI/uwbChanged", 1, uwbChangedCallback);
	loadUWBSettings();

	// subscribe to the global vicon publisher and local uwb publisher
	ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
	ROS_INFO_STREAM("[LocalizationServer] subscribed to Vicon");
	ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData", 100, UWBDataCallback);
	ROS_INFO_STREAM("[LocalizationServer] subscribed to UWB");

	// initialize the position data publisher
	localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1);

	ROS_INFO_STREAM("[LocalizationServer] started!");

	ros::spin();

	return 0;
}


/************************************************************
 *	Functions			  									*
 ***********************************************************/
bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{
	if(!nodeHandle.getParam("clientID", clientID))
	{
		ROS_ERROR("[LocalizationServer] Failed to get clientID!");
		return false;
	}

	return true;
}

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("[LocalizationServer] context updated");
	}
}

void d_fall_pps::loadUWBSettings()
{
	Anchors a;

	if(uwbManager.call(a))
	{
		enableUWB = a.response.enableUWB;
	}
	else
	{
		enableUWB = false;
		ROS_ERROR("[LocalizationServer] Could not update UWB Settings!");
	}

	if(enableUWB)
		ROS_WARN("[LocalizationServer] Use of UWB activated!");
	else
		ROS_WARN("[LocalizationServer] Use of UWB deactivated!");
}


/************************************************************
 *	Callbacks		  										*
 ***********************************************************/
void d_fall_pps::viconDataCallback(const ViconData &viconData)
{
	for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
	{
		CrazyflieData data = *it;

		if(data.crazyflieName == context.crazyflieName)
		{
			// if desired, publish Vicon data
			if(! (enableUWB && useUWB))
				localizationPublisher.publish(data);

			CFData_vicon = data;

			// leave the for-loop so not every vector element has to be compared, once the matching context is found
			break;
		}
	}
}

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

void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
{
	// override UWB Data
	CFData_uwb = data;

	// For the sake of nostalgia, the following lines
	// were kept in the code

	// Uncomment the lines to be overriden by vicon when using UWB
	//CFData_uwb.x = CFData_vicon.x;
	//CFData_uwb.y = CFData_vicon.y;
	//CFData_uwb.z = CFData_vicon.z;
	//CFData_uwb.roll = CFData_vicon.roll;
	//CFData_uwb.pitch = CFData_vicon.pitch;
	//CFData_uwb.pitch *= -1; // (deprecated)
	//CFData_uwb.yaw = CFData_vicon.yaw;

	// if desired, publish UWB data
	if(enableUWB && useUWB)
	{
		localizationPublisher.publish(CFData_uwb);
	}
}

void d_fall_pps::uwbChangedCallback(const std_msgs::Int32 &msg)
{
	useUWB = msg.data;

	if(useUWB)
		ROS_WARN("[LocalizationServer] UWB activated!");
	else
		ROS_WARN("[LocalizationServer] UWB deactivated!");
}