zmq example

cf_grab.py

#!/usr/bin/env python3
import zmq
import cflib.crtp
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie

from autonomousSequence import reset_estimator

uri = "radio://0/70/2M"
initial_position = [0, 0, 0.5]


def vec_sub(a, b):
    return [a[0]-b[0], a[1]-b[1], a[2]-b[2]]


def vec_add(a, b):
    return [a[0]+b[0], a[1]+b[1], a[2]+b[2]]


def main():
    cflib.crtp.init_drivers(enable_debug_driver=False)

    crazyflie_position = initial_position

    # Prepare our context and publisher
    context = zmq.Context()
    subscriber = context.socket(zmq.PULL)
    subscriber.connect("tcp://10.0.4.154:1234")

    with SyncCrazyflie(uri) as scf:
        reset_estimator(scf)

        cf = scf.cf

        cf.param.set_value('flightmode.posSet', '1')

        trigger_was_pressed = False
        controller_origin = [0, 0, 0]
        crazyflie_origin = [0, 0, 0.5]

        while True:
            msg = subscriber.recv_json()

            controller_position = [msg['pos'][0], -1*msg['pos'][2], msg['pos'][1]]
            trigger_pressed = msg['trigger'] > 0.95
            print(msg)

            if trigger_pressed and not trigger_was_pressed:
                controller_origin = controller_position
                crazyflie_origin = crazyflie_position

            if trigger_pressed:
                crazyflie_position = vec_add(crazyflie_origin,
                                             vec_sub(controller_position,
                                                     controller_origin))

            print(crazyflie_position)

            cf.commander.send_setpoint(crazyflie_position[1], crazyflie_position[0],
                                       0,
                                       int((crazyflie_position[2]+0.5) * 1000))

            trigger_was_pressed = trigger_pressed

    # We never get here but clean up anyhow
    subscriber.close()
    context.term()


if __name__ == "__main__":
    main()

vive-diy-position-sensor-geometry-getter.cpp

// vive-diy-position-sensor-geometry-getter.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <chrono>
#include <thread>

#include "zmq.h"

using namespace std;
using namespace std::literals;


int main()
{

	void *context = zmq_init(1);
	void *socket = zmq_socket(context, ZMQ_PUSH);
	zmq_bind(socket, "tcp://0.0.0.0:1234");

	void *insocket = zmq_socket(context, ZMQ_PULL);

	zmq_bind(insocket, "tcp://0.0.0.0:1235");

	char buffer[1024];
	float trigger[vr::k_unMaxTrackedDeviceCount];

	bool pressed[vr::k_unMaxTrackedDeviceCount] = { false };
	int pulse[vr::k_unMaxTrackedDeviceCount] = { 0 };

	char inputBuffer[1024];

	vr::HmdError err;
	vr::IVRSystem *vrSystem = vr::VR_Init(&err, vr::EVRApplicationType::VRApplication_Scene);

	vr::TrackedDevicePose_t pose_array[vr::k_unMaxTrackedDeviceCount];
	vr::VRControllerState_t controller_state;

	for (;;) {
		vrSystem->GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseOrigin::TrackingUniverseStanding, 0.5f, pose_array, vr::k_unMaxTrackedDeviceCount);

		int length = 1;
		
		do {
			length = zmq_recv(insocket, inputBuffer, 1024, ZMQ_DONTWAIT);
			if (length > 0) {
				inputBuffer[length] = 0;
				int inputId, inputPulse;
				sscanf(inputBuffer, "%d %d", &inputId, &inputPulse);

				if (inputId < vr::k_unMaxTrackedDeviceCount) {
					pulse[inputId] = inputPulse;
				}
			}
		} while (length >= 0);
		

		int idx = 0;
		for (vr::TrackedDeviceIndex_t i = 0; i < vr::k_unMaxTrackedDeviceCount; i++) {
			vr::TrackedDevicePose_t pose = pose_array[i];
			if (pose.bPoseIsValid && vrSystem->GetTrackedDeviceClass(i) == vr::TrackedDeviceClass::TrackedDeviceClass_Controller) {
				vr::HmdMatrix34_t &m = pose.mDeviceToAbsoluteTracking;
				printf("c%d origin %f %f %f matrix %f %f %f %f %f %f %f %f %f\n", i,
					m.m[0][3], m.m[1][3], m.m[2][3],
					m.m[0][0], m.m[0][1], m.m[0][2], m.m[1][0], m.m[1][1], m.m[1][2], m.m[2][0], m.m[2][1], m.m[2][2]);
			
				vrSystem->GetControllerState(i, &controller_state, sizeof(controller_state));

				for (uint32_t axis = 0; axis < vr::k_unControllerStateAxisCount; axis++) {
					vr::EVRControllerAxisType type = (vr::EVRControllerAxisType) vrSystem->GetInt32TrackedDeviceProperty(i, (vr::TrackedDeviceProperty)(vr::Prop_Axis0Type_Int32 + axis));
					if (type == vr::EVRControllerAxisType::k_eControllerAxis_Trigger) {
						printf("Trigger state: %f\n", controller_state.rAxis[axis].x);
						trigger[i] = controller_state.rAxis[axis].x;
					}
				}

				sprintf(buffer, "{\"id\": %d, \"pos\":[%f, %f, %f], \"trigger\": %f}", i, m.m[0][3], m.m[1][3], m.m[2][3], trigger[i]);
				zmq_send(socket, buffer, strlen(buffer), ZMQ_NOBLOCK);

				if (pulse[i] > 0) {
					vrSystem->TriggerHapticPulse(i, 0, pulse[i]);
					pulse[i] = 0;
				}

				if (trigger[i] > 0.50 && !pressed[i]) {
					pressed[i] = true;
					vrSystem->TriggerHapticPulse(i, 0, 2000);
				}
				if (trigger[i] < 0.40 && pressed[i]) {
					vrSystem->TriggerHapticPulse(i, 0, 2000);
					pressed[i] = false;
				}

				idx++;
			}
		}

		

		std::this_thread::sleep_for(10ms);
	}
	

	system("pause");

	zmq_close(socket);
	vr::VR_Shutdown();
    return 0;
}