Chordata_communicator.cpp 10.6 KB
Newer Older
Bruno Laurencich's avatar
Bruno Laurencich committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46
/**
 * @file Chordata_communicator.cpp
 * Definitions for the communicator namespace
 *
 * @author Bruno Laurencich
 * @version 0.1.0 
 * @date 2018/08/20
 *
 * Notochord  
 * -- Hub program for the Chordata Open Source motion capture system
 *
 * http://chordata.cc
 * contact@chordata.cc
 *
 *
 * Copyright 2018 Bruno Laurencich
 *
 * This file is part of Notochord.
 *
 * Notochord 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.
 *
 * Notochord 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 Notochord. If not, see <https://www.gnu.org/licenses/>.
 *
 * This program uses code from various sources, the default license is GNU GPLv3
 * for all code, the dependencies where originally distributed as follows:
 * -LSM9DS# libraries: Copyright 2014,2015 Jim Lindblom @ SparkFun Electronics, as beerware.
 * -fmt library: Copyright (c) 2012 - present, Victor Zverovich, Under BSD 2-clause License.
 * -spdlog library: Copyright(c) 2015 Gabi Melman, under MIT license.
 * -args library: Copyright (c) 2016 Taylor C. Richberger, under MIT license.
 * -bcm2835 library: Copyright 2011-2013 Mike McCaule, Under GNU-GPLv2 License.
 * -oscpack library: Copyright (c) 2004-2013 Ross Bencina, under MIT license.
 * -Sensor fusion algorithms: Copyright 2011 SOH Madgwick, Under GNU-GPLv3 License.
 * -tinyxml2 library: Copyright <unknowk year> Lee Thomason, under zlib License.
 * -pstreams library: Copyright (C) 2001 - 2017 Jonathan Wakely, under Boost Software License
 *
 */

Bruno Laurencich's avatar
Bruno Laurencich committed
47 48
#include <tuple>

49
#include "fmt/core.h"
Bruno Laurencich's avatar
Bruno Laurencich committed
50 51
#include "spdlog/spdlog.h"
#include "spdlog/sinks/null_sink.h"
52 53
#include "spdlog/sinks/ansicolor_sink.h"
#include "spdlog/sinks/daily_file_sink.h"
Bruno Laurencich's avatar
Bruno Laurencich committed
54 55 56 57 58 59 60 61 62

#include "Chordata_communicator.h"
#include "Chordata_error.h"

using namespace Chordata::Communicator;
using namespace std;
namespace spd = spdlog;

comm_fn_t Chordata::Communicator::output_handler,
63
		Chordata::Communicator::trace_handler,
Bruno Laurencich's avatar
Bruno Laurencich committed
64 65 66 67 68 69 70 71 72 73 74
		Chordata::Communicator::debug_handler,
		Chordata::Communicator::info_handler,
		Chordata::Communicator::warn_handler,
		Chordata::Communicator::err_handler;

transmit_fn_t Chordata::Communicator::transmit_handler;

Chordata::Timekeeper Chordata::Communicator::timekeeper;

long Chordata::Communicator::log_attemps = 0;

75 76 77
Comm_Substrate::Comm_Substrate(const Chordata::Configuration_Data& global_config){
	Chordata::Configuration_Data config = global_config;

78 79 80 81 82
	switch (global_config.comm.log_level){
		case 1: log_level = spd::level::debug; break;
		case 2: log_level = spd::level::trace; break;
		default:log_level = spd::level::info;
		}
Bruno Laurencich's avatar
Bruno Laurencich committed
83 84 85 86 87 88 89

	err_level = (global_config.comm.log_level == 0)?
		spd::level::err : spd::level::warn;	

	if ( 0 != create_dir( fmt::format("{}/{}", global_config.exe_path ,_CHORDATA_FILEOUT_DIR).c_str()))
		throw Chordata::System_Error(errno);
	
90
	auto push_redirect = [&config] (const auto source, auto& dest, auto level)
Bruno Laurencich's avatar
Bruno Laurencich committed
91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107
		{
		for (unsigned i =0,mask; i<=_Output_Redirect_keywords_n; ++i ){
			mask = 1 << i-1;
			switch (source & mask){
				case Chordata::NONE:
				break;
				case Chordata::STDOUT:
					dest.push_back(make_shared<spdlog::sinks::ansicolor_stdout_sink_mt>());
					// cout<< "stdout"<< endl;
				break;
				case Chordata::STDERR:
					dest.push_back(make_shared<spdlog::sinks::ansicolor_stderr_sink_mt>());
					// cout<< "STDERR"<< endl;
				break;
				case Chordata::FILE:
					dest.push_back(make_shared
									<spdlog::sinks::daily_file_sink_mt>
108
										(config.comm.filename, 0, 0)
Bruno Laurencich's avatar
Bruno Laurencich committed
109 110 111 112
									);
					// cout<< "FILE"<< endl;
				break;
				case Chordata::OSC:
113
					dest.push_back(make_shared<OscOut>(config));
Bruno Laurencich's avatar
Bruno Laurencich committed
114 115 116 117 118
					// cout<< "OSC"<< endl;
				break;
			}
		}
	};
119
	
Bruno Laurencich's avatar
Bruno Laurencich committed
120 121
	push_redirect(global_config.comm.transmit, transmit, log_level);

122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141
	
	Chordata::Output_Redirect log_redirect = global_config.comm.log;
	Chordata::Output_Redirect err_redirect = global_config.comm.error;


	if (global_config.runner_mode){
		Chordata::Output_Redirect remove_stdouts = ___CAST_REDIR_(Chordata::STDOUT|Chordata::STDERR);
		log_redirect &= ~remove_stdouts;
		err_redirect &= ~remove_stdouts;

		log_redirect |= Chordata::OSC;
		err_redirect |= Chordata::OSC;

		config.comm.ip = "localhost";
		config.comm.port = 50419;
	}

	push_redirect(log_redirect, log, log_level);
	push_redirect(err_redirect, error, err_level);

Bruno Laurencich's avatar
Bruno Laurencich committed
142 143 144
}

#define _PATTERN_FORMAT "[{{:>{}}}] {{}}"
145
#define _LOGGING_PATTERN "%^%l~%$%v"
Bruno Laurencich's avatar
Bruno Laurencich committed
146 147 148

using spdlevel = spdlog::level::level_enum;

149 150 151
#define TRANSMIT_OSC(S,Q) osc_sink->transmit(S,Q)/*; osc_sink->flush()*/
#define TRANSMIT_OTHERS(S,Q) logger.log(which, "{},{:d},{:f},{:f},{:f},{:f}", S, timekeeper.ellapsedMillis(), Q.w, Q.x, Q.y, Q.z);/*\
							logger.flush()*/
152

Bruno Laurencich's avatar
Bruno Laurencich committed
153 154 155 156 157
auto Chordata::Communicator::initializers::transmit_fn(spdlevel which, Comm_Substrate::redirects&& v, spdlevel global_level){
	static auto transmitRedirects = std::move(v);

	static std::shared_ptr<OscOut> osc_sink;

158 159 160 161 162 163 164 165 166 167 168 169 170 171
	// for (auto& r:transmitRedirects){
	// 	if (auto o = dynamic_pointer_cast<OscOut>(r))
	// 		osc_sink = std::move(o);
	// }

	auto r = std::begin(transmitRedirects);
	while (r != std::end(transmitRedirects)) {
	    // Do some stuff
	    if (auto o = dynamic_pointer_cast<OscOut>(*r)){
	    	osc_sink = std::move(o);
	        r = transmitRedirects.erase(r);
	    }
	    else
	        ++r;
Bruno Laurencich's avatar
Bruno Laurencich committed
172 173 174 175 176 177 178
	}

	if (transmitRedirects.size() == 0)
		transmitRedirects.push_back(make_shared<spdlog::sinks::null_sink_mt>());
	
	static spdlog::logger logger("transmit_logger", transmitRedirects.begin(), transmitRedirects.end());
	logger.set_level(global_level);
179
	logger.set_pattern("%v");
Bruno Laurencich's avatar
Bruno Laurencich committed
180 181 182 183 184 185 186 187 188 189 190

	static auto pattern = fmt::format(_PATTERN_FORMAT,  _CHORDATA_PRINT_MILLIS_PADDING );

	comm_fn_t outAction = [ which, pattern=pattern.c_str() ](const std::string& s){ 
		logger.log(which, pattern, timekeeper.ellapsedMillis(), s);
		logger.flush();

	};

	transmit_fn_t transmitAction;

191 192 193 194
	if (osc_sink && transmitRedirects.size() > 0){
		transmitAction = [which](const std::string& s, const Quaternion& q){
			TRANSMIT_OSC(s,q); 	TRANSMIT_OTHERS(s,q); };
	} else if  (osc_sink){
Bruno Laurencich's avatar
Bruno Laurencich committed
195
		transmitAction = [](const std::string& s, const Quaternion& q){
196
			TRANSMIT_OSC(s,q); };
Bruno Laurencich's avatar
Bruno Laurencich committed
197
	} else {
198 199
		transmitAction = [which](const std::string& s, const Quaternion& q){
			TRANSMIT_OTHERS(s,q); };
Bruno Laurencich's avatar
Bruno Laurencich committed
200 201 202 203 204 205 206 207 208 209 210 211 212 213 214
	}

	return std::make_tuple(outAction,transmitAction);
}


comm_fn_t Chordata::Communicator::initializers::log_fn(spdlevel which, Comm_Substrate::redirects&& v, spdlevel global_level){
	static auto outRedirects= std::move(v);

	if (outRedirects.size() == 0)
		outRedirects.push_back(make_shared<spdlog::sinks::null_sink_mt>());
	
	static spdlog::logger logger("output_logger", outRedirects.begin(), outRedirects.end());
	logger.set_level(global_level);
	logger.set_pattern(_LOGGING_PATTERN);
215
	// logger.flush_on(spd::level::err);
Bruno Laurencich's avatar
Bruno Laurencich committed
216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234

	static auto pattern = fmt::format(_PATTERN_FORMAT,  _CHORDATA_PRINT_MILLIS_PADDING );

	comm_fn_t outAction = [ which, pattern=pattern.c_str() ](const std::string& s){ 
		logger.log(which, pattern, timekeeper.ellapsedMillis(), s); 
	};

	return outAction;
}

comm_fn_t Chordata::Communicator::initializers::error_fn(spdlevel which, Comm_Substrate::redirects&& v, spdlevel global_level){
	static auto errRedirects= std::move(v);
	
	if (errRedirects.size() == 0)
		errRedirects.push_back(make_shared<spdlog::sinks::null_sink_mt>());
	
	static spdlog::logger logger("error_logger", errRedirects.begin(), errRedirects.end());
	logger.set_level(global_level);
	logger.set_pattern(_LOGGING_PATTERN);
235
	logger.flush_on(spd::level::warn);
Bruno Laurencich's avatar
Bruno Laurencich committed
236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259

	static auto pattern = fmt::format(_PATTERN_FORMAT,  _CHORDATA_PRINT_MILLIS_PADDING );

	comm_fn_t outAction = [ which, pattern=pattern.c_str() ](const std::string& s){ 
		logger.log(which, pattern, timekeeper.ellapsedMillis(), s); 
	};

	return outAction;
}

void Chordata::Communicator::init_communicator(const Chordata::Configuration_Data& global_config){
	Chordata::Communicator::init_communicator(Chordata::Communicator::Comm_Substrate(global_config));	
}


void Chordata::Communicator::init_communicator(Comm_Substrate&& c){
	static bool done = false;
	if (!done){
		done = true;
	} else {
		warn("Only the first init_communicator() call has effect");
		return;
	}

260 261 262 263
	// size_t q_size = _CHORDATA_LOG_QUEUE_SIZE;
 //    spdlog::set_async_mode(q_size, spdlog::async_overflow_policy::block_retry,
 //                       nullptr,
 //                       std::chrono::seconds(_CHORDATA_LOG_QUEUE_SIZE));
Bruno Laurencich's avatar
Bruno Laurencich committed
264 265 266 267 268 269 270

    auto outputs = initializers::transmit_fn( spd::level::info, std::move(c.transmit), c.log_level );

	Chordata::Communicator::output_handler = std::get<0>(outputs);

	Chordata::Communicator::transmit_handler = std::get<1>(outputs);

271 272 273
	Chordata::Communicator::trace_handler =
			initializers::log_fn( spd::level::trace, std::move(c.log), c.log_level );

Bruno Laurencich's avatar
Bruno Laurencich committed
274 275 276 277 278 279 280 281 282 283 284 285
	Chordata::Communicator::debug_handler =
			initializers::log_fn( spd::level::debug, std::move(c.log), c.log_level );

	Chordata::Communicator::info_handler =
			initializers::log_fn( spd::level::info, std::move(c.log), c.log_level );

	Chordata::Communicator::warn_handler =
			initializers::error_fn( spd::level::warn, std::move(c.error), c.err_level );

	Chordata::Communicator::err_handler =
			initializers::error_fn( spd::level::err, std::move(c.error), c.err_level );

286

287

Bruno Laurencich's avatar
Bruno Laurencich committed
288
	if (Chordata::Communicator::log_attemps > 0){
289
		Chordata::Communicator::warn("{} loging attemps made, before the communicator functions were initialized", 
Bruno Laurencich's avatar
Bruno Laurencich committed
290 291 292 293 294
			Chordata::Communicator::log_attemps);
	}
	
}

Bruno Laurencich's avatar
Bruno Laurencich committed
295 296 297 298 299 300
// osc::OutboundPacketStream& operator<<(osc::OutboundPacketStream& stream, const uint64_t& num)
// {
//     // write obj to stream
//     return os;
// }

Bruno Laurencich's avatar
Bruno Laurencich committed
301
void Chordata::Communicator::OscOut::transmit(const std::string& s, const Quaternion& q){
Bruno Laurencich's avatar
Bruno Laurencich committed
302 303 304
	p << osc::BeginMessage( s.c_str() )
    // << static_cast<int64_t>(timekeeper.ellapsedMicros()) 
    << q.w << q.x << q.y << q.z << osc::EndMessage;
305 306

    flush_();
Bruno Laurencich's avatar
Bruno Laurencich committed
307 308
}

309
void Chordata::Communicator::OscOut::sink_it_(const spdlog::details::log_msg& msg){
Bruno Laurencich's avatar
Bruno Laurencich committed
310
	p << osc::BeginMessage( (msg.level < spdlog::level::warn)? OscOut::comm_address.c_str() : OscOut::error_address.c_str() )
311
    << fmt::to_string(msg.raw).c_str() << osc::EndMessage;
312

313
    flush_();
Bruno Laurencich's avatar
Bruno Laurencich committed
314 315
}

316
void Chordata::Communicator::OscOut::flush_(){
Bruno Laurencich's avatar
Bruno Laurencich committed
317 318 319 320 321 322 323 324 325
	transmitSocket.Send( p.Data(), p.Size() );
	p.Clear();
}

#define print_with_millis(stream, text) \
	stream << "["; \
	stream.width( _CHORDATA_PRINT_MILLIS_PADDING ); \
	stream << timekeeper.ellapsedMillis(); \
	stream << "] " << text << std::endl;