tracking sever

3_PointCloud/SteamVrTrackingServer.cpp

+#include <chrono>
+#include <csignal>
+#include <iostream>
+#include <thread>
+#include <sstream>
+
+#include <boost/program_options.hpp>
+#include <openvr.h>
+#include <SimpleAmqpClient/SimpleAmqpClient.h>
+
+
+struct Config
+{
+	std::string hostname = "127.0.0.1";
+	int port = 5672;
+	std::string exchange = "irpose";
+	bool debug = false;
+	int messages_per_second = 60;
+} config_;
+
+
+bool running_ = true;
+
+AmqpClient::Channel::ptr_t connection_ = nullptr;
+vr::IVRSystem *vr_system_ = nullptr;
+vr::TrackedDevicePose_t tracked_device_poses_[vr::k_unMaxTrackedDeviceCount];
+
+void signalHandler(int signum)
+{
+	running_ = false;
+}
+
+bool init()
+{
+	// Connect to Amqp server
+	try
+	{
+		std::cout << "Connecting to host " << config_.hostname << std::endl;
+		connection_ = AmqpClient::Channel::Create(config_.hostname);
+		std::cout << "Declaring exchange " << config_.exchange << std::endl;
+		connection_->DeclareExchange(config_.exchange, AmqpClient::Channel::EXCHANGE_TYPE_FANOUT);
+	}
+	catch (const AmqpClient::ChannelErrorException &ex)
+	{
+		std::cerr << "ChannelErrorException: " << ex.reply_code() << " - " << ex.reply_text() << std::endl;
+	}
+	catch (const std::exception &ex)
+	{
+		std::cerr << "Could not connect to AMQP server: " << ex.what() << std::endl;
+		std::cout << "Tracking without publishing AMQP messages..." << std::endl;
+	}
+
+	if (connection_)
+	{
+		std::cout << "Connected to AMQP server. Publishing about " << config_.messages_per_second << " messages per second." << std::endl;
+	}
+
+	// Loading the SteamVR Runtime
+	vr::EVRInitError eError = vr::VRInitError_None;
+	vr_system_ = vr::VR_Init(&eError, vr::VRApplication_Overlay);
+
+	if (eError != vr::VRInitError_None)
+	{
+		vr_system_ = nullptr;
+		char buf[1024];
+		sprintf_s(buf, sizeof(buf), "Unable to init VR runtime: %s", vr::VR_GetVRInitErrorAsEnglishDescription(eError));
+		std::cerr << buf << std::endl;
+		return false;
+	}
+
+
+	//m_pRenderModels = (vr::IVRRenderModels *)vr::VR_GetGenericInterface(vr::IVRRenderModels_Version, &eError);
+	//if (!m_pRenderModels)
+	//{
+	//	m_pHMD = NULL;
+	//	vr::VR_Shutdown();
+
+	//	char buf[1024];
+	//	sprintf_s(buf, sizeof(buf), "Unable to get render model interface: %s", vr::VR_GetVRInitErrorAsEnglishDescription(eError));
+	//	SDL_ShowSimpleMessageBox(SDL_MESSAGEBOX_ERROR, "VR_Init Failed", buf, NULL);
+	//	return false;
+	//}
+
+	return true;
+}
+
+void shutdown()
+{
+	if (vr_system_)
+	{
+		vr::VR_Shutdown();
+		vr_system_ = nullptr;
+	}
+
+	//for (std::vector< CGLRenderModel * >::iterator i = m_vecRenderModels.begin(); i != m_vecRenderModels.end(); i++)
+	//{
+	//	delete (*i);
+	//}
+	//m_vecRenderModels.clear();
+}
+
+//-----------------------------------------------------------------------------
+// Purpose: Processes a single VR event
+//-----------------------------------------------------------------------------
+void ProcessVREvent(const vr::VREvent_t & event)
+{
+	switch (event.eventType)
+	{
+	case vr::VREvent_TrackedDeviceActivated:
+	{
+		printf("Device %u attached.\n", event.trackedDeviceIndex);
+	}
+	break;
+	case vr::VREvent_TrackedDeviceDeactivated:
+	{
+		printf("Device %u detached.\n", event.trackedDeviceIndex);
+	}
+	break;
+	case vr::VREvent_TrackedDeviceUpdated:
+	{
+		printf("Device %u updated.\n", event.trackedDeviceIndex);
+	}
+	break;
+	}
+}
+
+void handleInput()
+{
+	// Process SteamVR events
+	vr::VREvent_t event;
+	while (vr_system_->PollNextEvent(&event, sizeof(event)))
+	{
+		ProcessVREvent(event);
+	}
+}
+
+void printMatrix(const vr::HmdMatrix34_t &htm)
+{
+	for (int rows = 0; rows < 4; ++rows)
+	{
+		std::cout << (rows == 0 ? "[" : "") << "\t";
+
+		if (rows == 3)
+		{
+			for (int cols = 0; cols < 4; ++cols)
+			{
+				printf("%.3f\t", (cols == 3 ? 1.0f : 0.0f));
+				//std::cout << (cols == 3 ? 1 : 0) << "\t";
+			}
+		}
+		else
+		{
+			for (int cols = 0; cols < 4; ++cols)
+			{
+				printf("%.3f\t", htm.m[rows][cols]);
+				//std::cout << htm.m[rows][cols] << "\t";
+			}
+		}
+
+		std::cout << (rows == 3 ? "]" : "") << std::endl;
+	}
+}
+
+int main(int argc, char *argv[])
+{
+	boost::program_options::options_description desc("Allowed options");
+	desc.add_options()
+		("help,h", "Show this help message")
+		("list,l", "Show connected VR devices")
+		("host", boost::program_options::value<std::string>(&config_.hostname)->default_value("127.0.0.1"), "AMQP server")
+		("port,p", boost::program_options::value<int>(&config_.port)->default_value(5672), "AMQP port")
+		("exchange,e", boost::program_options::value<std::string>(&config_.exchange), "Exchange on the AMQP server")
+		("messages_per_second,m", boost::program_options::value<int>(&config_.messages_per_second)->default_value(60), "Maximum number of messages published to AMQP server (<= 0 means unlimited)")
+		("verbose,v", boost::program_options::bool_switch(&config_.debug)->default_value(false), "Show verbose debug output")
+		;
+
+	boost::program_options::variables_map vm;
+	try
+	{
+		boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
+		boost::program_options::notify(vm);
+	}
+	catch (boost::program_options::error &e)
+	{
+		std::cerr << "Error parsing command line arguments: " << e.what() << std::endl;
+		return 1;
+	}
+
+	if (vm.count("help"))
+	{
+		std::cout << desc << std::endl;
+		return 0;
+	}
+
+	if (config_.debug)
+	{
+		std::cout << "Debug output enabled" << std::endl;
+	}
+
+	if (!init())
+	{
+		shutdown();
+		return 1;
+	}
+
+	if (vm.count("list"))
+	{
+		std::cout << "List of available VR devices:" << std::endl;
+		for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
+		{
+			vr::ETrackedDeviceClass device_class = vr_system_->GetTrackedDeviceClass(nDevice);
+			if (device_class == vr::TrackedDeviceClass_Invalid)
+			{
+				continue;
+			}
+
+			std::string type;
+			switch (device_class)
+			{
+			case vr::TrackedDeviceClass_Controller:        type = "Controller"; break;
+			case vr::TrackedDeviceClass_HMD:               type = "HMD"; break;
+			case vr::TrackedDeviceClass_Invalid:           type = "Invalid"; break;
+			case vr::TrackedDeviceClass_GenericTracker:    type = "GenericTracker"; break;
+			case vr::TrackedDeviceClass_TrackingReference: type = "TrackingReference"; break;
+			default:                                       type = "Unknown"; break;
+			}
+
+			const uint32_t size = 1024;
+			char serial[size];
+			vr_system_->GetStringTrackedDeviceProperty(nDevice, vr::ETrackedDeviceProperty::Prop_SerialNumber_String, serial, size);
+
+			std::cout << "\tDevice " << nDevice << ":" << std::endl;
+			std::cout << "\t\tType: " << type << std::endl;
+			std::cout << "\t\tSerial: " << serial << std::endl;
+		}
+
+		shutdown();
+		return 0;
+	}
+
+	signal(SIGINT, signalHandler);
+	std::cout << "Tracking started. Cancel with Ctrl-C" << std::endl;
+
+	std::chrono::duration<double, std::milli> cycle_time;
+	if (config_.messages_per_second > 0)
+	{
+		cycle_time = std::chrono::duration<double, std::milli>(1000.0 / config_.messages_per_second);
+	}
+
+	while (running_)
+	{
+		auto start = std::chrono::steady_clock::now();
+
+		handleInput();
+
+		// Get device poses
+		vr_system_->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0, tracked_device_poses_, vr::k_unMaxTrackedDeviceCount);
+
+		// TODO: remove
+		vr::HmdMatrix34_t raw_to_standing_htm = vr_system_->GetRawZeroPoseToStandingAbsoluteTrackingPose();
+		std::cout << "RawZeroPoseToStandingAbsoluteTrackingPose:" << std::endl;
+		printMatrix(raw_to_standing_htm);
+
+		for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
+		{
+			if (!tracked_device_poses_[nDevice].bPoseIsValid)
+			{
+				continue;
+			}
+
+			vr::ETrackedDeviceClass device_class = vr_system_->GetTrackedDeviceClass(nDevice);
+
+			if (device_class == vr::TrackedDeviceClass_Invalid)
+			{
+				continue;
+			}
+
+			vr::HmdMatrix34_t htm = tracked_device_poses_[nDevice].mDeviceToAbsoluteTracking;
+
+			std::string type;
+			std::string model_type;
+			switch (device_class)
+			{
+			case vr::TrackedDeviceClass_Controller:
+				type = "Controller";
+				model_type = "vive_controller";
+				break;
+			case vr::TrackedDeviceClass_HMD:
+				type = "HMD";
+				model_type = "vive_hmd";
+				break;
+			case vr::TrackedDeviceClass_Invalid:
+				type = "Invalid";
+				break;
+			case vr::TrackedDeviceClass_GenericTracker:
+				type = "GenericTracker";
+				break;
+			case vr::TrackedDeviceClass_TrackingReference:
+				type = "TrackingReference";
+				model_type = "vive_base_station";
+				break;
+			default:
+				type = "Unknown";
+				break;
+			}
+
+			if (config_.debug)
+			{
+				std::cout << "Pose of device " << nDevice << " (" << type << ")" << std::endl;
+				printMatrix(htm);
+			}
+
+			const uint32_t size = 1024;
+			char serial[size];
+			vr_system_->GetStringTrackedDeviceProperty(nDevice, vr::ETrackedDeviceProperty::Prop_SerialNumber_String, serial, size);
+
+			auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+
+			std::ostringstream message;
+			message << "%YAML:1.0\n";
+			message << "object_type: \"" << model_type << "\"\n";
+			message << "serial: \"" << serial << "\"\n";
+			message << "timestamp: \"" << std::to_string(timestamp) << "\"\n";
+			message << "pose: !!opencv-matrix\n   rows: 4\n   cols: 4\n   dt: f\n   data: [ ";
+			message << htm.m[0][0] << ", ";
+			message << htm.m[0][1] << ", ";
+			message << htm.m[0][2] << ", ";
+			message << htm.m[0][3] << ", ";
+			message << htm.m[1][0] << ", ";
+			message << htm.m[1][1] << ", ";
+			message << htm.m[1][2] << ", ";
+			message << htm.m[1][3] << ", ";
+			message << htm.m[2][0] << ", ";
+			message << htm.m[2][1] << ", ";
+			message << htm.m[2][2] << ", ";
+			message << htm.m[2][3] << ", ";
+			message << "0.0, 0.0, 0.0, 1.0 ]\n";
+			message << "source_type: \"steam_vr\"\n";
+			message << "source_serial: \"steam_vr_1\"\n";
+
+			if (config_.debug)
+			{
+				std::cout << message.str() << std::endl;
+			}
+
+			if (connection_)
+			{
+				AmqpClient::BasicMessage::ptr_t test = AmqpClient::BasicMessage::Create(message.str());
+				connection_->BasicPublish(config_.exchange, "", test);
+			}
+		}
+
+		if (config_.messages_per_second > 0)
+		{
+			std::this_thread::sleep_until(start + cycle_time);
+		}
+	}
+
+	shutdown();
+
+	return 0;
+}