Skip to content

Instantly share code, notes, and snippets.

@ataffanel ataffanel/cf_grab.py Secret
Created Dec 7, 2017

Embed
What would you like to do?
zmq example
#!/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 : 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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.