GHOST_NDOFManagerUnix::GHOST_NDOFManagerUnix(GHOST_System& sys) : GHOST_NDOFManager(sys), m_available(false) { if (spnav_open() != -1) { m_available = true; /* determine exactly which device (if any) is plugged in */ #define MAX_LINE_LENGTH 100 /* look for USB devices with Logitech or 3Dconnexion's vendor ID */ FILE *command_output = popen("lsusb | grep '046d:\\|256f:'", "r"); if (command_output) { char line[MAX_LINE_LENGTH] = {0}; while (fgets(line, MAX_LINE_LENGTH, command_output)) { unsigned short vendor_id = 0, product_id = 0; if (sscanf(line, "Bus %*d Device %*d: ID %hx:%hx", &vendor_id, &product_id) == 2) if (setDevice(vendor_id, product_id)) { break; /* stop looking once the first 3D mouse is found */ } } pclose(command_output); } } else { #ifdef DEBUG /* annoying for official builds, just adds noise and most people don't own these */ puts("ndof: spacenavd not found"); /* This isn't a hard error, just means the user doesn't have a 3D mouse. */ #endif } }
int main(void) { #if defined(BUILD_X11) Display *dpy; Window win; unsigned long bpix; #endif spnav_event sev; signal(SIGINT, sig); #if defined(BUILD_X11) if(!(dpy = XOpenDisplay(0))) { fprintf(stderr, "failed to connect to the X server\n"); return 1; } bpix = BlackPixel(dpy, DefaultScreen(dpy)); win = XCreateSimpleWindow(dpy, DefaultRootWindow(dpy), 0, 0, 1, 1, 0, bpix, bpix); /* This actually registers our window with the driver for receiving * motion/button events through the 3dxsrv-compatible X11 protocol. */ if(spnav_x11_open(dpy, win) == -1) { fprintf(stderr, "failed to connect to the space navigator daemon\n"); return 1; } #elif defined(BUILD_AF_UNIX) if(spnav_open()==-1) { fprintf(stderr, "failed to connect to the space navigator daemon\n"); return 1; } #else #error Unknown build type! #endif /* spnav_wait_event() and spnav_poll_event(), will silently ignore any non-spnav X11 events. * * If you need to handle other X11 events you will have to use a regular XNextEvent() loop, * and pass any ClientMessage events to spnav_x11_event, which will return the event type or * zero if it's not an spnav event (see spnav.h). */ while(spnav_wait_event(&sev)) { if(sev.type == SPNAV_EVENT_MOTION) { printf("got motion event: t(%d, %d, %d) ", sev.motion.x, sev.motion.y, sev.motion.z); printf("r(%d, %d, %d)\n", sev.motion.rx, sev.motion.ry, sev.motion.rz); } else { /* SPNAV_EVENT_BUTTON */ printf("got button %s event b(%d)\n", sev.button.press ? "press" : "release", sev.button.bnum); } } spnav_close(); return 0; }
events() { static k3d::bool_t initialized = false; if(!initialized) { initialized = true; if(-1 == spnav_open()) { k3d::log() << error << "Error opening connection to spnav daemon." << std::endl; return; } if(!Glib::thread_supported()) Glib::thread_init(); g_dispatcher.reset(new Glib::Dispatcher()); g_dispatcher->connect(sigc::ptr_fun(handle_event)); g_mutex.reset(new Glib::Mutex()); Glib::Thread::create(sigc::ptr_fun(&event_loop), false); } }
int main(int argc, char **argv) { ros::init(argc, argv); ros::node node("spacenav"); node.advertise<std_msgs::Vector3>("/spacenav/offset", 2); node.advertise<std_msgs::Vector3>("/spacenav/rot_offset", 2); node.advertise<joy::Joy>("/spacenav/joy", 2); if (spnav_open() == -1) { fprintf(stderr, "Error: Could not open the space navigator device\n"); fprintf(stderr, "Did you remember to run spacenavd (as root)?\n"); ros::fini(); return 1; } spnav_event sev; int ret; int no_motion_count = 0; joy::Joy joystick_msg; joystick_msg.axes.resize(6); joystick_msg.buttons.resize(2); while (node.ok()) { ret = spnav_poll_event(&sev); spnav_remove_events(SPNAV_EVENT_MOTION); if (ret == 0) { if (++no_motion_count > 30) { no_motion_count = 0; std_msgs::Vector3 offset_msg; offset_msg.x = offset_msg.y = offset_msg.z = 0; node.publish("/spacenav/offset", offset_msg); std_msgs::Vector3 rot_offset_msg; rot_offset_msg.x = rot_offset_msg.y = rot_offset_msg.z = 0; node.publish("/spacenav/rot_offset", rot_offset_msg); joystick_msg.axes[0] = offset_msg.x / FULL_SCALE; joystick_msg.axes[1] = offset_msg.y / FULL_SCALE; joystick_msg.axes[2] = offset_msg.z / FULL_SCALE; joystick_msg.axes[3] = rot_offset_msg.x / FULL_SCALE; joystick_msg.axes[4] = rot_offset_msg.y / FULL_SCALE; joystick_msg.axes[5] = rot_offset_msg.z / FULL_SCALE; } } if (sev.type == SPNAV_EVENT_MOTION) { std_msgs::Vector3 offset_msg; offset_msg.x = sev.motion.z; offset_msg.y = -sev.motion.x; offset_msg.z = sev.motion.y; node.publish("/spacenav/offset", offset_msg); std_msgs::Vector3 rot_offset_msg; rot_offset_msg.x = sev.motion.rz; rot_offset_msg.y = -sev.motion.rx; rot_offset_msg.z = sev.motion.ry; //printf("%lf %lf %lf\n", rot_offset_msg.x, rot_offset_msg.y, rot_offset_msg.z); node.publish("/spacenav/rot_offset", rot_offset_msg); joystick_msg.axes[0] = offset_msg.x / FULL_SCALE; joystick_msg.axes[1] = offset_msg.y / FULL_SCALE; joystick_msg.axes[2] = offset_msg.z / FULL_SCALE; joystick_msg.axes[3] = rot_offset_msg.x / FULL_SCALE; joystick_msg.axes[4] = rot_offset_msg.y / FULL_SCALE; joystick_msg.axes[5] = rot_offset_msg.z / FULL_SCALE; no_motion_count = 0; } else if (sev.type == SPNAV_EVENT_BUTTON) { //printf("type, press, bnum = <%d, %d, %d>\n", sev.button.type, sev.button.press, sev.button.bnum); joystick_msg.buttons[sev.button.bnum] = sev.button.press; } node.publish("/spacenav/joy", joystick_msg); usleep(1000); } node.unadvertise("/spacenav/offset"); ros::fini(); return 0; }
QSpaceNavigator::QSpaceNavigator() { if(spnav_open() != -1) { spaceSock = new QSocketNotifier(spnav_fd(),QSocketNotifier::Read, this); connect(spaceSock, SIGNAL(activated(int)), this, SLOT(poll())); }
int main(int argc, char **argv) { ros::init(argc, argv, "ardrone3dnav"); ros::NodeHandle node_handle; // ros::Publisher offset_pub = // node_handle.advertise<geometry_msgs::Vector3>("spacenav/offset", 2); // ros::Publisher rot_offset_pub = // node_handle.advertise<geometry_msgs::Vector3>("spacenav/rot_offset", 2); ros::Publisher twist_pub = node_handle.advertise<geometry_msgs::Twist>("cmd_vel", 2); // ros::Publisher joy_pub = node_handle.advertise<joy::Joy>("spacenav/joy", // 2); ros::Publisher land_pub = node_handle.advertise<std_msgs::Empty>("ardrone/land", 2); ros::Publisher takeoff_pub = node_handle.advertise<std_msgs::Empty>("ardrone/takeoff", 2); ros::Publisher reset_pub = node_handle.advertise<std_msgs::Empty>("ardrone/reset", 2); if (spnav_open() == -1) { ROS_ERROR("Could not open the space navigator device. Did you remember to " "run spacenavd (as root)?"); return 1; } else ROS_INFO("Properly connected to the space navigator device."); sensor_msgs::Joy joystick_msg; joystick_msg.axes.resize(6); joystick_msg.buttons.resize(2); spnav_event sev; int no_motion_count = 0; bool motion_stale = false; geometry_msgs::Vector3 offset_msg; geometry_msgs::Vector3 rot_offset_msg; geometry_msgs::Twist twist_msg; std_msgs::Empty empty_msg; while (node_handle.ok()) { bool joy_stale = false; bool queue_empty = false; // Sleep when the queue is empty. // If the queue is empty 30 times in a row output zeros. // Output changes each time a button event happens, or when a motion // event happens and the queue is empty. switch (spnav_poll_event(&sev)) { case 0: queue_empty = true; if (++no_motion_count > 30) { offset_msg.x = offset_msg.y = offset_msg.z = 0; rot_offset_msg.x = rot_offset_msg.y = rot_offset_msg.z = 0; no_motion_count = 0; motion_stale = true; } break; case SPNAV_EVENT_MOTION: offset_msg.x = sev.motion.z / FULL_SCALE; offset_msg.y = -sev.motion.x / FULL_SCALE; offset_msg.z = sev.motion.y / FULL_SCALE; rot_offset_msg.x = sev.motion.rz / FULL_SCALE; rot_offset_msg.y = -sev.motion.rx / FULL_SCALE; rot_offset_msg.z = sev.motion.ry / FULL_SCALE; // printf("%lf %lf %lf\n", rot_offset_msg.x, rot_offset_msg.y, // rot_offset_msg.z); motion_stale = true; break; case SPNAV_EVENT_BUTTON: // printf("type, press, bnum = <%d, %d, %d>\n", sev.button.type, // sev.button.press, sev.button.bnum); // joystick_msg.buttons[sev.button.bnum] = sev.button.press; if (sev.button.press == 1) { switch (sev.button.bnum) { case 0: takeoff_pub.publish(empty_msg); ROS_INFO("The drone is taking off!"); break; case 1: land_pub.publish(empty_msg); ROS_INFO("The drone is landing"); break; case 6: reset_pub.publish(empty_msg); break; } } joy_stale = true; break; default: ROS_WARN("Unknown message type in spacenav. This should never happen."); break; } if (motion_stale && (queue_empty || joy_stale)) { // offset_pub.publish(offset_msg); // rot_offset_pub.publish(rot_offset_msg); twist_msg.linear = offset_msg; twist_msg.angular = rot_offset_msg; twist_pub.publish(twist_msg); // joystick_msg.axes[0] = offset_msg.x / FULL_SCALE; // joystick_msg.axes[1] = offset_msg.y / FULL_SCALE; // joystick_msg.axes[2] = offset_msg.z / FULL_SCALE; // joystick_msg.axes[3] = rot_offset_msg.x / FULL_SCALE; // joystick_msg.axes[4] = rot_offset_msg.y / FULL_SCALE; // joystick_msg.axes[5] = rot_offset_msg.z / FULL_SCALE; no_motion_count = 0; motion_stale = false; joy_stale = true; } // if (joy_stale) //{ // joy_pub.publish(joystick_msg); //} if (queue_empty) usleep(1000); } return 0; }
int main(int argc, char **argv) { ros::init(argc, argv, "spacenav"); ros::NodeHandle node_handle; ros::Publisher offset_pub = node_handle.advertise<geometry_msgs::Vector3>("spacenav/offset", 2); ros::Publisher rot_offset_pub = node_handle.advertise<geometry_msgs::Vector3>("spacenav/rot_offset", 2); ros::Publisher twist_pub = node_handle.advertise<geometry_msgs::Twist>("spacenav/twist", 2); ros::Publisher joy_pub = node_handle.advertise<sensor_msgs::Joy>("spacenav/joy", 2); if (spnav_open() == -1) { ROS_ERROR("Could not open the space navigator device. Did you remember to run spacenavd (as root)?"); return 1; } // Parameters // The number of polls needed to be done before the device is considered "static" int static_count_threshold = 30; ros::param::get("~/static_count_threshold",static_count_threshold); // If true, the node will zero the output when the device is "static" bool zero_when_static = true; ros::param::get("~/zero_when_static",zero_when_static); // If the device is considered "static" and each trans, rot component is // below the deadband, it will output zeros in either rotation, translation, // or both double static_trans_deadband = 50, static_rot_deadband = 50; ros::param::get("~/static_trans_deadband",static_trans_deadband); ros::param::get("~/static_rot_deadband",static_rot_deadband); sensor_msgs::Joy joystick_msg; joystick_msg.axes.resize(6); joystick_msg.buttons.resize(2); spnav_event sev; int no_motion_count = 0; bool motion_stale = false; geometry_msgs::Vector3 offset_msg; geometry_msgs::Vector3 rot_offset_msg; geometry_msgs::Twist twist_msg; while (node_handle.ok()) { bool joy_stale = false; bool queue_empty = false; // Sleep when the queue is empty. // If the queue is empty 30 times in a row output zeros. // Output changes each time a button event happens, or when a motion // event happens and the queue is empty. joystick_msg.header.stamp = ros::Time().now(); switch (spnav_poll_event(&sev)) { case 0: queue_empty = true; if (++no_motion_count > static_count_threshold) { if ( zero_when_static || ( fabs(offset_msg.x) < static_trans_deadband && fabs(offset_msg.y) < static_trans_deadband && fabs(offset_msg.z) < static_trans_deadband) ) { offset_msg.x = offset_msg.y = offset_msg.z = 0; } if ( zero_when_static || ( fabs(rot_offset_msg.x) < static_rot_deadband && fabs(rot_offset_msg.y) < static_rot_deadband && fabs(rot_offset_msg.z) < static_rot_deadband ) ) { rot_offset_msg.x = rot_offset_msg.y = rot_offset_msg.z = 0; } no_motion_count = 0; motion_stale = true; } break; case SPNAV_EVENT_MOTION: offset_msg.x = sev.motion.z; offset_msg.y = -sev.motion.x; offset_msg.z = sev.motion.y; rot_offset_msg.x = sev.motion.rz; rot_offset_msg.y = -sev.motion.rx; rot_offset_msg.z = sev.motion.ry; motion_stale = true; break; case SPNAV_EVENT_BUTTON: //printf("type, press, bnum = <%d, %d, %d>\n", sev.button.type, sev.button.press, sev.button.bnum); joystick_msg.buttons[sev.button.bnum] = sev.button.press; joy_stale = true; break; default: ROS_WARN("Unknown message type in spacenav. This should never happen."); break; } if (motion_stale && (queue_empty || joy_stale)) { offset_pub.publish(offset_msg); rot_offset_pub.publish(rot_offset_msg); twist_msg.linear = offset_msg; twist_msg.angular = rot_offset_msg; twist_pub.publish(twist_msg); joystick_msg.axes[0] = offset_msg.x / FULL_SCALE; joystick_msg.axes[1] = offset_msg.y / FULL_SCALE; joystick_msg.axes[2] = offset_msg.z / FULL_SCALE; joystick_msg.axes[3] = rot_offset_msg.x / FULL_SCALE; joystick_msg.axes[4] = rot_offset_msg.y / FULL_SCALE; joystick_msg.axes[5] = rot_offset_msg.z / FULL_SCALE; no_motion_count = 0; motion_stale = false; joy_stale = true; } if (joy_stale) { joy_pub.publish(joystick_msg); } if (queue_empty) { usleep(1000); } } spnav_close(); return 0; }
void SpaceNavigatorPollingThread::run() { m_stopped = false; if(spnav_open() == -1) return; qreal posfactor = 0.1; int currX = 0, currY = 0, currZ = 0; int currRX = 0, currRY = 0, currRZ = 0; Qt::MouseButtons buttons = Qt::NoButton; kDebug() << "started spacenavigator polling thread"; while( ! m_stopped ) { spnav_event event; if( spnav_poll_event( &event ) ) { if( event.type == SPNAV_EVENT_MOTION ) { /* * The coordinate system of the space navigator is like the following: * x-axis : from left to right * y-axis : from down to up * z-axis : from front to back * We probably want to make sure that the x- and y-axis match Qt widget * coordinate system: * x-axis : from left to right * y-axis : from back to front * The z-axis would then go from up to down in a right handed coordinate system. * z-axis : from up to down */ //kDebug() << "got motion event: t("<< event.motion.x << event.motion.y << event.motion.z << ") r(" << event.motion.rx << event.motion.ry << event.motion.rz << ")"; currX = static_cast<int>( posfactor * event.motion.x ); currY = -static_cast<int>( posfactor * event.motion.z ); currZ = -static_cast<int>( posfactor * event.motion.y ); currRX = static_cast<int>( posfactor * event.motion.rx ); currRY = static_cast<int>( -posfactor * event.motion.rz ); currRZ = static_cast<int>( -posfactor * event.motion.ry ); emit moveEvent( currX, currY, currZ, currRX, currRY, currRZ, buttons ); } else { /* SPNAV_EVENT_BUTTON */ Qt::MouseButton button = event.button.bnum == 0 ? Qt::LeftButton : Qt::RightButton; KoDeviceEvent::Type type; if( event.button.press ) { //kDebug() << "got button press event b(" << event.button.bnum << ")"; buttons |= button; type = KoDeviceEvent::ButtonPressed; } else { //kDebug() << "got button release event b(" << event.button.bnum << ")"; buttons &= ~button; type = KoDeviceEvent::ButtonReleased; } emit buttonEvent( currX, currY, currZ, currRX, currRY, currRZ, buttons, button, type ); } spnav_remove_events( event.type ); } msleep(10); } kDebug() << "finished spacenavigator polling thread"; }
int main(int argc, char **argv) { ros::init(argc, argv, "spacenav"); ros::NodeHandle node_handle; ros::Publisher offset_pub = node_handle.advertise<geometry_msgs::Vector3>("spacenav/offset", 2); ros::Publisher rot_offset_pub = node_handle.advertise<geometry_msgs::Vector3>("spacenav/rot_offset", 2); ros::Publisher twist_pub = node_handle.advertise<geometry_msgs::Twist>("spacenav/twist", 2); ros::Publisher joy_pub = node_handle.advertise<sensor_msgs::Joy>("spacenav/joy", 2); if (spnav_open() == -1) { ROS_ERROR("Could not open the space navigator device. Did you remember to run spacenavd (as root)?"); return 1; } sensor_msgs::Joy joystick_msg; joystick_msg.axes.resize(6); joystick_msg.buttons.resize(2); spnav_event sev; int no_motion_count = 0; bool motion_stale = false; geometry_msgs::Vector3 offset_msg; geometry_msgs::Vector3 rot_offset_msg; geometry_msgs::Twist twist_msg; while (node_handle.ok()) { bool joy_stale = false; bool queue_empty = false; // Sleep when the queue is empty. // If the queue is empty 30 times in a row output zeros. // Output changes each time a button event happens, or when a motion // event happens and the queue is empty. joystick_msg.header.stamp = ros::Time().now(); switch (spnav_poll_event(&sev)) { case 0: queue_empty = true; if (++no_motion_count > 30) { offset_msg.x = offset_msg.y = offset_msg.z = 0; rot_offset_msg.x = rot_offset_msg.y = rot_offset_msg.z = 0; no_motion_count = 0; motion_stale = true; } break; case SPNAV_EVENT_MOTION: offset_msg.x = sev.motion.z; offset_msg.y = -sev.motion.x; offset_msg.z = sev.motion.y; rot_offset_msg.x = sev.motion.rz; rot_offset_msg.y = -sev.motion.rx; rot_offset_msg.z = sev.motion.ry; //printf("%lf %lf %lf\n", rot_offset_msg.x, rot_offset_msg.y, rot_offset_msg.z); motion_stale = true; break; case SPNAV_EVENT_BUTTON: //printf("type, press, bnum = <%d, %d, %d>\n", sev.button.type, sev.button.press, sev.button.bnum); joystick_msg.buttons[sev.button.bnum] = sev.button.press; joy_stale = true; break; default: ROS_WARN("Unknown message type in spacenav. This should never happen."); break; } if (motion_stale && (queue_empty || joy_stale)) { offset_pub.publish(offset_msg); rot_offset_pub.publish(rot_offset_msg); twist_msg.linear = offset_msg; twist_msg.angular = rot_offset_msg; twist_pub.publish(twist_msg); joystick_msg.axes[0] = offset_msg.x / FULL_SCALE; joystick_msg.axes[1] = offset_msg.y / FULL_SCALE; joystick_msg.axes[2] = offset_msg.z / FULL_SCALE; joystick_msg.axes[3] = rot_offset_msg.x / FULL_SCALE; joystick_msg.axes[4] = rot_offset_msg.y / FULL_SCALE; joystick_msg.axes[5] = rot_offset_msg.z / FULL_SCALE; no_motion_count = 0; motion_stale = false; joy_stale = true; } if (joy_stale) { joy_pub.publish(joystick_msg); } if (queue_empty) usleep(1000); } return 0; }