RigidBody(
            ros::NodeHandle& nh,
            const std::string& ip,
            int port,
            const std::string& frame_id,
            const std::string& child_frame_id,
            const tf::Transform& transform,
            const tf::Vector3& scaling)
            : m_br()
            , m_connection()
            , m_tracker()
            , m_target()
            , m_transform(transform)
            , m_scaling(scaling)
        {
            std::string connec_nm = ip + ":" + boost::lexical_cast<std::string>(port);
            m_connection = vrpn_get_connection_by_name(connec_nm.c_str());

            // child_frame_id is the same as remote object name
            m_tracker = new vrpn_Tracker_Remote(child_frame_id.c_str(), m_connection);
            m_tracker->register_change_handler(this, track_target);

            m_target.header.frame_id = frame_id;
            m_target.child_frame_id = child_frame_id;
        }
Exemplo n.º 2
0
vrpn_Mutex_Remote::vrpn_Mutex_Remote(const char *name, vrpn_Connection *c)
    : vrpn_Mutex(name, c ? c : ((strcmp(name, "null") == 0)
                                    ? (vrpn_Connection *)NULL
                                    : vrpn_get_connection_by_name(name)))
    , d_state(AVAILABLE)
    , d_myIndex(-1)
    , d_requestBeforeInit(vrpn_FALSE)
    , d_reqGrantedCB(NULL)
    , d_reqDeniedCB(NULL)
    , d_takeCB(NULL)
    , d_releaseCB(NULL)
{

    if (d_connection) {
        d_connection->register_handler(d_grantRequest_type, handle_grantRequest,
                                       this);
        d_connection->register_handler(d_denyRequest_type, handle_denyRequest,
                                       this);
        d_connection->register_handler(d_releaseNotification_type,
                                       handle_releaseNotification, this);
        d_connection->register_handler(d_initialize_type, handle_initialize,
                                       this);
        if (d_connection->connected()) {
            requestIndex();
        }
        vrpn_int32 got =
            d_connection->register_message_type(vrpn_got_connection);
        d_connection->register_handler(got, handle_gotConnection, this);
    }
}
Exemplo n.º 3
0
vrpn_Connection *vrpn_Imager_Stream_Buffer::open_new_log_connection(
    const char *local_in_logfile_name, const char *local_out_logfile_name,
    const char *remote_in_logfile_name, const char *remote_out_logfile_name)
{
    vrpn_Connection *ret = NULL;

    // Find the relevant part of the name (skip past last '@'
    // if there is one); also find the port number.
    const char *cname = d_imager_server_name;
    const char *where_at; // Part of name past last '@'
    if ((where_at = strrchr(cname, '@')) != NULL) {
        cname = where_at + 1; // Chop off the front of the name
    }

    // Pass "true" to force_connection so that it will open a new
    // connection even if we already have one with that name.
    ret = vrpn_get_connection_by_name(
        where_at, local_in_logfile_name, local_out_logfile_name,
        remote_in_logfile_name, remote_out_logfile_name, NULL, true);
    if (!ret || !ret->doing_okay()) {
        struct timeval now;
        vrpn_gettimeofday(&now, NULL);
        fprintf(stderr, "vrpn_Imager_Stream_Buffer::open_new_log_connection: "
                        "Could not create connection (files already exist?)");
        if (ret) {
            delete ret;
            return NULL;
        }
    }

    return ret;
}
Exemplo n.º 4
0
vrpn_BaseClass::vrpn_BaseClass (const char * name, vrpn_Connection * c)
{
    bool firstTimeCalled = (d_connection==NULL);  // has the constructor been called before?
    // note that this might also be true if it was called once before but failed.

    if (firstTimeCalled)
    {
        // Get the connection for this object established. If the user passed in a
        // NULL connection object, then we determine the connection from the name of
        // the object itself (for example, [email protected] will make a
        // connection to the machine mumble on the standard VRPN port).
        //
        // The vrpn_BassClassUnique destructor handles telling the connection we
        // are no longer referring to it.  Since we only add the reference once
        // here (when d_connection is NULL), it is okay for the unique destructor
        // to remove the reference.
        if (c) {	// using existing connection.
            d_connection = c;
            d_connection->addReference();
        } else {
            // This will implicitly add the reference to the connection.
            d_connection = vrpn_get_connection_by_name(name);
        }

        // Get the part of the name for this device that does not include the connection.
        // The vrpn_BassClassUnique destructor handles the deletion of the space.
        d_servicename = vrpn_copy_service_name(name);
    }
}
Exemplo n.º 5
0
viconObjTracker::viconObjTracker(const std::string host_name, const std::string obj_name, double self_loop_freq):
  ArtTimer(self_loop_freq),
  x(0),
  y(0),
  z(0),
  qw(1),
  qx(0),
  qy(0),
  qz(0)

{
  // Since if connection is not established it will not complain there is not much point in checking anything here
  vicon_connection = vrpn_get_connection_by_name( host_name.c_str() );

  if(vicon_connection == NULL)
  {
    fprintf(stderr, "ERROR: vicon connection was not established. NULL pointer returned ...\n");
    return;
  }

  // child_frame_id is the same as remote object name
  obj_tracker = new vrpn_Tracker_Remote(obj_name.c_str(), vicon_connection);
  obj_tracker->register_change_handler(this, vicon_track_obj);

  if(self_loop_freq > 0)
  {
    startFreq(self_loop_freq);
  }

}
  VrpnClientRos::VrpnClientRos(ros::NodeHandle nh, ros::NodeHandle private_nh)
  {
    output_nh_ = private_nh;

    host_ = getHostStringFromParams(private_nh);

    ROS_INFO_STREAM("Connecting to VRPN server at " << host_);
    connection_ = std::shared_ptr<vrpn_Connection>(vrpn_get_connection_by_name(host_.c_str()));
    ROS_INFO("Connection established");

    double update_frequency;
    private_nh.param<double>("update_frequency", update_frequency, 100.0);
    mainloop_timer = nh.createTimer(ros::Duration(1 / update_frequency), boost::bind(&VrpnClientRos::mainloop, this));

    double refresh_tracker_frequency;
    private_nh.param<double>("refresh_tracker_frequency", refresh_tracker_frequency, 0.0);

    if (refresh_tracker_frequency > 0.0)
    {
      refresh_tracker_timer_ = nh.createTimer(ros::Duration(1 / refresh_tracker_frequency),
                                              boost::bind(&VrpnClientRos::updateTrackers, this));
    }

    std::vector<std::string> param_tracker_names_;
    if (private_nh.getParam("trackers", param_tracker_names_))
    {
      for (std::vector<std::string>::iterator it = param_tracker_names_.begin();
           it != param_tracker_names_.end(); ++it)
      {
        trackers_.insert(std::make_pair(*it, std::make_shared<VrpnTrackerRos>(*it, connection_, output_nh_)));
      }
    }
  }
void _DLLExport setUp(char* trackerName,int port){
	char connectionName[128];
	sprintf(connectionName,"localhost:%d",port);
	connection = vrpn_get_connection_by_name(connectionName);

	tracker = new vrpn_Tracker_Remote(trackerName,connection);
	tracker->register_change_handler(NULL,handle_pos);
}
Exemplo n.º 8
0
 Rigid_Body(ros::NodeHandle& nh, std::string server_ip,
            int port) 
 {
     target_pub = nh.advertise<geometry_msgs::TransformStamped>("pose", 100);
     std::string connec_nm = server_ip + ":" + boost::lexical_cast<std::string>(port);
     connection = vrpn_get_connection_by_name(connec_nm.c_str());
     std::string target_name = nh.getNamespace().substr(1);
     tracker = new vrpn_Tracker_Remote(target_name.c_str(), connection);
     this->tracker->register_change_handler(NULL, track_target);
 }
Exemplo n.º 9
0
int main (int argc, char ** argv) {

  char ib [1000];
  char sb [75], tb [75];
  char * qp;
  char * source_location;
  int port;
  int len;

  if (argc != 2) Usage(argv[0]);

  source_location = argv[1];

  connection = vrpn_get_connection_by_name (source_location);

  controller = new vrpn_Forwarder_Controller (connection);

  printf("Commands:\n"
         "  quit\n"
         "  open <port>\n"
         "  forward <port> \"<service name>\" \"<message type>\"\n");
  do {
    if (fgets(ib, 1000, stdin) == NULL) {
	perror("Could not read line");
	return -1;
    }
    fprintf(stderr, "Got:  >%s<\n", ib);
    if (!strncmp(ib, "open", 4)) {
      port = atoi(ib + 5);
      controller->start_remote_forwarding(port);
      fprintf(stderr, "Opening %d.\n", port);
    }
    if (!strncmp(ib, "forward", 7)) {
      port = atoi(ib + 8);
      qp = strchr(ib, '\"');  // start of service name
      qp++;
      len = strcspn(qp, "\"");  fprintf(stderr, "Prefix len %d.\n", len);
      strncpy(sb, qp, len);
      sb[len] = '\0';
      qp = strchr(qp, '\"');  // end of service name
      qp++;
      qp = strchr(qp, '\"');  // start of message type name
      qp++;
      len = strcspn(qp, "\"");  fprintf(stderr, "Prefix len %d.\n", len);
      strncpy(tb, qp, len);
      tb[len] = '\0';
      qp = strchr(qp, '\"');  // end of message type name

      controller->forward_message_type(port, sb, tb);
fprintf(stderr, "Forwarding %s of %s on %d.\n", tb, sb, port);
    }
    connection->mainloop();
  } while (strncmp(ib, "quit", 4));
  
}
Exemplo n.º 10
0
void vrpn_PeerMutex::addPeer(const char *stationName)
{
    vrpn_Connection **newc;
    peerData *newg;
    losePeerData *d;
    int i;

    // complex
    while (d_numPeers >= d_numConnectionsAllocated) {

        // reallocate arrays
        d_numConnectionsAllocated = 2 * (d_numConnectionsAllocated + 1);
        newc = new vrpn_Connection *[d_numConnectionsAllocated];
        newg = new peerData[d_numConnectionsAllocated];
        if (!newc || !newg) {
            fprintf(stderr, "vrpn_PeerMutex::addPeer:  Out of memory.\n");
            return;
        }
        for (i = 0; i < d_numPeers; i++) {
            newc[i] = d_peer[i];
            newg[i] = d_peerData[i];
        }
        if (d_peer) {
            delete[] d_peer;
        }
        if (d_peerData) {
            delete[] d_peerData;
        }
        d_peer = newc;
        d_peerData = newg;
    }
    d_peer[d_numPeers] = vrpn_get_connection_by_name(stationName);
    // d_peerData[d_numPeers].grantedLock = vrpn_false;

    d = new losePeerData;
    if (!d) {
        fprintf(stderr, "vrpn_PeerMutex::addPeer:  Out of memory.\n");
        return;
    }
    d->connection = d_peer[d_numPeers];
    d->mutex = this;
    vrpn_int32 control;
    vrpn_int32 drop;
    control = d_peer[d_numPeers]->register_sender(vrpn_CONTROL);
    drop = d_peer[d_numPeers]->register_message_type(vrpn_dropped_connection);
    d_peer[d_numPeers]->register_handler(drop, handle_losePeer, d, control);

#ifdef VERBOSE
    fprintf(stderr, "vrpn_PeerMutex::addPeer:  added peer named %s.\n",
            stationName);
#endif

    d_numPeers++;
}
    vrpn_ConnectionPtr
    VRPNConnectionCollection::getConnection(std::string const &device,
                                            std::string const &host) {
        auto connMap = *m_connMap;
        auto existing = connMap.find(host);
        if (existing != end(connMap)) {
            return existing->second;
        }
        auto fullName = device + "@" + host;

        vrpn_ConnectionPtr newConn(
            vrpn_get_connection_by_name(fullName.c_str(), nullptr, nullptr,
                                        nullptr, nullptr, nullptr, true));
        connMap[host] = newConn;
        newConn->removeReference(); // Remove extra reference.
        return newConn;
    }
Exemplo n.º 12
0
int main (int argc, char * argv[]) {

  if (argc != 3) {
    fprintf(stderr, "Usage: %s hostname port\n", argv[0]);
    return -1;
  }

  char * hostname = argv[1];
  char * port = argv[2];
  int length = strlen(hostname) + strlen(port) + 1; //+ 1 for ':' 

  char result[length];
  bzero(result, length);

  strcpy(result, hostname);
  strcat(result, ":");
  strcat(result, port);

  const char * TRACKER_NAME = "Tracker0";
  int CONNECTION_PORT = vrpn_DEFAULT_LISTEN_PORT_NO;

  vrpn_Connection * connection = vrpn_get_connection_by_name(result);
  
  //create a dummy server
  vrpn_Tracker_NULL * null_tracker = new vrpn_Tracker_NULL(TRACKER_NAME, connection, 1, 60.0);

  vrpn_Tracker_Remote * tracker = new vrpn_Tracker_Remote(TRACKER_NAME, connection); 

  tracker->register_change_handler(NULL, handle_data);

  while (1) {

    null_tracker->mainloop();
    tracker->mainloop();
    connection->mainloop();
    vrpn_SleepMsecs(1000);
  }

  disconn(socketfd);

  return 0;
}
Exemplo n.º 13
0
int open_client_connection_and_loop(const char *local_in, const char *local_out,
                                    const char *remote_in, const char *remote_out)
{
  //---------------------------------------------------------------------
  // Open a client connection to the server, asking for the logging files
  // requested in the parameters.
  char name[512];
  sprintf(name, "%s@localhost:%d", CLIENT_TEXT_NAME, CONNECTION_PORT);
  client_connection = vrpn_get_connection_by_name(name, local_in, local_out,
    remote_in, remote_out);
  if (client_connection == NULL) {
    fprintf(stderr,"Cannot open client connection\n");
    return -2;
  }

  //---------------------------------------------------------------------
  // Open the client-side text sender and receivers.
  create_and_link_text_remote();

  //---------------------------------------------------------------------
  // Go for a few seconds to let messages get sent back and forth.

  struct timeval now, start;
  vrpn_gettimeofday(&now, NULL);
  start = now;
  while ( now.tv_sec - start.tv_sec < 3 ) {
    send_text_once_per_second();
    server_connection->mainloop();
    client_connection->mainloop();
    vrpn_gettimeofday(&now, NULL);
  }

  //---------------------------------------------------------------------
  // Delete the client-side objects and get rid of the client connection
  // by reducing its reference count.
  delete client_text_receiver;
  delete client_text_sender;
  client_connection->removeReference();

  return 0;
}
Exemplo n.º 14
0
DWORD WINAPI hacerTracking(LPVOID lpParam)
{
	 
		vrpn_Connection *connection;

		char connectionName[128];
		int  port = 3883;

		sprintf(connectionName,"localhost:%d", port);
        
		connection = vrpn_get_connection_by_name(connectionName);
    
		vrpn_Tracker_Remote *tracker = new vrpn_Tracker_Remote("Pared", connection);

  		tracker->register_change_handler(NULL, handle_pos);	
		while(!kbhit())
		{
			tracker->mainloop();
			connection->mainloop();
			Sleep(5);
		}
     return 0;
}
Exemplo n.º 15
0
JNIEXPORT jboolean JNICALL 
Java_vrpn_AnalogRemote_init( JNIEnv *env, jobject jobj, jstring jname, 
							 jstring jlocalInLogfileName, jstring jlocalOutLogfileName,
							 jstring jremoteInLogfileName, jstring jremoteOutLogfileName )
{
  // make a global reference to the Java AnalogRemote
  // object, so that it can be referenced in the function
  // handle_analog_change(...)
  jobj = env->NewGlobalRef( jobj );

  // create the analog
  const char* name = env->GetStringUTFChars( jname, NULL );
  const char* local_in_logfile_name = jlocalInLogfileName == NULL ? NULL :
	  env->GetStringUTFChars( jlocalInLogfileName, NULL );
  const char* local_out_logfile_name = jlocalOutLogfileName == NULL ? NULL :
	  env->GetStringUTFChars( jlocalOutLogfileName, NULL );
  const char* remote_in_logfile_name = jremoteInLogfileName == NULL ? NULL :
	  env->GetStringUTFChars( jremoteInLogfileName, NULL );
  const char* remote_out_logfile_name = jremoteOutLogfileName == NULL ? NULL :
	  env->GetStringUTFChars( jremoteOutLogfileName, NULL );
  vrpn_Connection* conn 
	  = vrpn_get_connection_by_name( name, local_in_logfile_name, local_out_logfile_name,
									 remote_in_logfile_name, remote_out_logfile_name );
  vrpn_Analog_Remote* a = new vrpn_Analog_Remote( name, conn );
  a->register_change_handler( jobj, handle_analog_change );
  env->ReleaseStringUTFChars( jname, name );
  env->ReleaseStringUTFChars( jlocalInLogfileName, local_in_logfile_name );
  env->ReleaseStringUTFChars( jlocalOutLogfileName, local_out_logfile_name );
  env->ReleaseStringUTFChars( jremoteInLogfileName, remote_in_logfile_name );
  env->ReleaseStringUTFChars( jremoteOutLogfileName, remote_out_logfile_name );
 
  // now stash 'a' in the jobj's 'native_device' field
  jlong ja = (jlong) a;
  env->SetLongField( jobj, jfid_vrpn_VRPNDevice_native_device, ja );
  
  return true;
}
Exemplo n.º 16
0
void main(int argc, char **argv) {

HWND hWin;
vrpn_Sound_Server_A3D * soundServer = NULL;
vrpn_Tracker_Remote   * tracker_connection;
char                    tracker_device[512];
char                    tracker_name[512];
vrpn_Connection       * connection;
vrpn_Connection       * trackerCon;
int                     got_report;

int USE_TRACKER;

	char	* config_file_name = "vrpn.cfg";
	FILE	* config_file;
	char 	* client_name   = NULL;
	int	    client_port   = 4150;
	int	    bail_on_error = 1;
	int	    verbose       = 1;
	int	    auto_quit     = 0;
	int	    realparams    = 0;
	int 	  loop          = 0;
	int	    port          = vrpn_DEFAULT_LISTEN_PORT_NO;

	connection = new vrpn_Synchronized_Connection (port);
	
	// Open the configuration file
	if (verbose) printf("Reading from config file %s\n", config_file_name);
	
	if ( (config_file = fopen(config_file_name, "r")) == NULL) 
	{
		perror("Cannot open config file");
		printf("  (filename %s)\n", config_file_name);
		return;
	}
		
	// Read the configuration file, creating a device for each entry.
	// Each entry is on one line, which starts with the name of the
	//   class of the object that is to be created.
	// If we fail to open a certain device, print a message and decide
	//  whether we should bail.
	{	
		char	line[512];	// Line read from the input file
		char *pch;
		char    scrap[512], s2[512];
	
		// Read lines from the file until we run out
		while ( fgets(line, sizeof(line), config_file) != NULL ) 
		{
			
			// Make sure the line wasn't too long
			if (strlen(line) >= sizeof(line)-1) 
			{
				printf("Line too long in config file: %s\n",line);
				if (bail_on_error) { return; }
				else { continue; }	// Skip this line
			}
			
			if ((strlen(line)<3)||(line[0]=='#')) 
			{
				// comment or empty line -- ignore
				continue;
			}
			
			// copy for strtok work
			strncpy(scrap, line, sizeof(line) - 1);
			// Figure out the device from the name and handle appropriately
			
			// WARNING: SUBSTRINGS WILL MATCH THE EARLIER STRING, SO 
			// ADD AN EMPTY SPACE TO THE END OF STATIC STRINGS!!!!
			
			//	  #define isit(s) !strncmp(line,s,strlen(s))
#define isit(s) !strcmp(pch=strtok(scrap," \t"),s)
#define next() pch += strlen(pch) + 1
	
			#ifdef _WIN32

			if(isit("vrpn_Sound_Server"))
			{
				printf("%s\n",pch); 
				next();
				printf("%s\n",pch);
				if (sscanf(pch,"%511s\t%d\t%511s\t%511s",s2,&USE_TRACKER,tracker_name, tracker_device) != 4) 
				{
					printf("Bad vrpn_Server_Sound line: %s\n",line);
					if (bail_on_error) 
					{ 
						return; 
					}
					else 
					{ 
						continue; 
					}	// Skip this line
				}

				hWin = GetConsoleHwnd();

				printf("Begin initializing A3D Sound Server\n");	
				soundServer = NULL;
				soundServer = new vrpn_Sound_Server_A3D(s2, connection,hWin);
				if (soundServer == NULL) 
					printf("Can't create sound server\n");
        printf("End A3D Sound Server initialization\n");	
				
			}
#endif
		}
	}

	fclose(config_file);

	// Open remote tracker if we are to use one

	if (USE_TRACKER) {
		
		char newname[1024];
		sprintf(newname,"%s@%s",(const char*)tracker_device, (const char*)tracker_name);
		printf("Using tracker: %s\n",newname);
		trackerCon = vrpn_get_connection_by_name(tracker_name);
		tracker_connection = new vrpn_Tracker_Remote((const char *) newname);
		// SET UP TRACKER HANDLER
		if (trackerCon->doing_okay()) {
			printf( "TC OK.\n");
		} else {
			printf( "TC Not OK.\n");
		}
	}
	else printf("Not using tracker\n");

	loop = 0;
	
	if (client_name) 
	{
		printf( "vrpn_serv: connecting to client: %s:%d\n",
			client_name, client_port);
		if (connection->connect_to_client(client_name, client_port))
		{
			printf( "server: could not connect to client %s:%d\n", client_name, client_port);
		}
	}
	
		
// ********************************************************************
// **                                                                **
// **                MAIN LOOP                                       **
// **                                                                **
// ********************************************************************
float fPrevTime = 0.0f;
float fFrameTime;
float fTime;
int counter = 0;
int stopNow = 0;
int numconnections = 0;
char buf[1024];

	printf("Begin main loop\n");

	while (!stopNow && 	!_kbhit()) {

    soundServer->GetLastError(buf);

    if (!strncmp(buf,"ERROR",5)) {
		  printf("%s", buf);
    }
      counter++;

  	  // record time since last frame 
      if (counter==NUM_SPIN) {
	    fTime = (float)timeGetTime();
	    counter = 0;

	    fFrameTime = (fTime - fPrevTime) * 0.001f;
	  
	    printf("Running at %4.2f Hz\n", (float) NUM_SPIN/fFrameTime);

        fPrevTime = fTime;
      }

	  soundServer->mainloop();
						
		// ensure we get a new report!
		if (USE_TRACKER) {
		  tracker_connection->mainloop();
		  got_report = 0;
		  if (trackerCon->doing_okay())
		    while (!got_report) 
			  tracker_connection->mainloop(); 
		}

		// Send and receive all messages
		connection->mainloop();
		if (numconnections==0 && connection->connected())
           numconnections++;
	
		if (((numconnections!=0) & (!connection->connected())) | !connection->doing_okay())  {
			soundServer->shutDown();
		  numconnections=0;
		}
	}

	printf("about to shutdown\n");
//	delete connection;
   delete soundServer;
}
Exemplo n.º 17
0
/** Uses the VRPN library to get the position and orientation of a
 * tracked object.
 *
 * @param object The name of the object being tracked.
 *
 * @param hostname The IP address or hostname of the VRPN server or
 * tracking system computer. If hostname is set to NULL, the
 * ~/.vrpn-server file is consulted.
 *
 * @param pos An array to be filled in with the position information
 * for the tracked object. If we are unable to track the object, a
 * message may be printed and pos will be set to a fixed value.
 *
 * @param orient An array to be filled in with the orientation matrix
 * for the tracked object. The orientation matrix is in row-major
 * order can be used with OpenGL. If the tracking system is moving an
 * object around on the screen, this matrix can be used directly. If
 * the tracking system is moving the OpenGL camera, this matrix may
 * need to be inverted. If we are unable to track the object, a
 * message may be printed and orient will be set to the identity
 * matrix.
 *
 * @return 1 if we returned data from the tracker. 0 if there was
 * problems connecting to the tracker.
 */
int vrpn_get(const char *object, const char *hostname, float pos[3], float orient[16])
{
	/* Set to default values */
	vec3f_set(pos, 10000,10000,10000);
	mat4f_identity(orient);
#ifdef MISSING_VRPN
	printf("You are missing VRPN support.\n");
	return 0;
#else
	if(object == NULL || strlen(object) == 0)
	{
		msg(WARNING, "Empty or NULL object name was passed into this function.\n");
		return 0;
	}
	if(hostname != NULL && strlen(hostname) == 0)
	{
		msg(WARNING, "Hostname is an empty string.\n");
		return 0;
	}
	
	/* Construct an object@hostname string. */
	std::string hostnamecpp;
	std::string objectcpp;
	if(hostname == NULL)
	{
		char *hostnameInFile = vrpn_default_host();
		if(hostnameInFile)
			hostnamecpp = hostnameInFile;
		else
		{
			msg(ERROR, "Failed to find hostname of VRPN server.\n");
			exit(EXIT_FAILURE);
		}
		
	}
	else
		hostnamecpp = hostname;

	objectcpp = object;
	std::string fullname = objectcpp + "@" + hostnamecpp;

	/* Check if we have a tracker object for that string in our map. */
	if(nameToTracker.count(fullname))
	{
		/* If we already have a tracker object, ask it to run the main
		 * loop (and therefore call our handle_tracker() function if
		 * there is new data). */
		nameToTracker[fullname]->mainloop();

		/* If our callback has been called, get the callback object
		 * and get the data out of it. */
		if(nameToCallbackData.count(fullname))
		{
			vrpn_TRACKERCB t = nameToCallbackData[fullname];
			float pos4[4];
			for(int i=0; i<3; i++)
				pos4[i] = t.pos[i];
			pos4[3]=1;

			double orientd[16];
			// Convert quaternion into orientation matrix.
			q_to_ogl_matrix(orientd, t.quat);
			for(int i=0; i<16; i++)
				orient[i] = (float) orientd[i];

			/* VICON in the MTU IVS lab is typically calibrated so that:
			 * X = points to the right (while facing screen)
			 * Y = points into the screen
			 * Z = up
			 * (left-handed coordinate system)
			 *
			 * PPT is typically calibrated so that:
			 * X = the points to the wall that has two closets at both corners
			 * Y = up
			 * Z = points to the door
			 * (right-handed coordinate system)
			 *
			 * By default, OpenGL assumes that:
			 * X = points to the right (while facing screen in the IVS lab)
			 * Y = up
			 * Z = points OUT of the screen (i.e., -Z points into the screen in te IVS lab)
			 * (right-handed coordinate system)
			 *
			 * Below, we convert the position and orientation
			 * information into the OpenGL convention.
			 */
			if(strlen(hostnamecpp.c_str()) > 14 && strncmp(hostnamecpp.c_str(), "tcp://141.219.", 14) == 0) // MTU vicon tracker
			{
				float viconTransform[16] = { 1,0,0,0,  // column major order!
				                             0,0,-1,0,
				                             0,1,0,0,
				                             0,0,0,1 };
				mat4f_mult_mat4f_new(orient, viconTransform, orient);
				mat4f_mult_vec4f_new(pos4, viconTransform, pos4);
				vec3f_copy(pos,pos4);
				return 1; // we successfully collected some data
			}
			else // Non-Vicon tracker
			{
				/* Don't transform other tracking systems */
				// orient is already filled in
				vec3f_copy(pos, pos4);
				return 1; // we successfully collected some data
			}
		}
	}
	else
	{
		/* If this is our first time, create a tracker for the object@hostname string, register the callback handler. */
		msg(INFO, "Connecting to VRPN server: %s\n", hostnamecpp.c_str());
		// If we are making a TCP connection and the server isn't up, the following function call may hang for a long time
		vrpn_Connection *connection = vrpn_get_connection_by_name(hostnamecpp.c_str());

		/* Wait for a bit to see if we can connect. Sometimes we don't immediately connect! */
		for(int i=0; i<1000 && !connection->connected(); i++)
		{
		    usleep(1000); // 1000 microseconds * 1000 = up to 1 second of waiting.
		    connection->mainloop();
		}
		/* If connection failed, exit. */
		if(!connection->connected())
		{
		    delete connection;
		    msg(ERROR, "Failed to connect to tracker: %s\n", fullname.c_str());
		    return 0;
		}
		vrpn_Tracker_Remote *tkr = new vrpn_Tracker_Remote(fullname.c_str(), connection);
		nameToTracker[fullname] = tkr;
		tkr->register_change_handler((void*) fullname.c_str(), handle_tracker);
		kuhl_getfps_init(&fps_state);
		kalman_initialize(&kalman, 0.1, 0.1);
	}
	return 0;
#endif
}
Exemplo n.º 18
0
int main(int argc, char **argv)
{
    char  default_imager[] = "TestImage@localhost";
    char	*device_name = default_imager;
    char	*logfile_name = NULL;
    int i;

    // Parse the command line.  If there is one argument, it is the device
    // name.  If there is a second, it is a logfile name.
    if (argc >= 2) {
        device_name = argv[1];
    }
    if (argc >= 3) {
        logfile_name = argv[2];
    }
    if (argc > 3) {
        fprintf(stderr, "Usage: %s [device_name [logfile_name]]\n", argv[0]);
        exit(-1);
    }

    // In case we end up loading an video from a file, make sure we
    // neither pre-load nor accumulate the images.
    vrpn_FILE_CONNECTIONS_SHOULD_PRELOAD = false;
    vrpn_FILE_CONNECTIONS_SHOULD_ACCUMULATE = false;

    // Say that we've posted a redisplay so that the callback handler
    // for endframe doesn't try to post one before glut is open.
    g_already_posted = true;

    // Create a log file of the video if we've been asked to do logging.
    // This has the side effect of having the imager also use this same
    // connection, because VRPN maps the same connection name to the
    // same one rather than creating a new one.
    if (logfile_name) {
        g_connection = vrpn_get_connection_by_name(device_name, logfile_name);
    }

    // Open the Imager client and set the callback
    // for new data and for information about the size of
    // the image.
    printf("Opening %s\n", device_name);
    g_imager = new vrpn_Imager_Remote(device_name);
    g_imager->register_description_handler(NULL, handle_description_message);
    g_imager->register_region_handler(g_imager, handle_region_change);
    g_imager->register_discarded_frames_handler(NULL, handle_discarded_frames);
    g_imager->register_end_frame_handler(g_imager, handle_end_of_frame);

    printf("Waiting to hear the image dimensions...\n");
    while (!g_got_dimensions) {
        g_imager->mainloop();
        vrpn_SleepMsecs(1);
    }

    // Because the vrpn_Imager server doesn't follow "The VRPN Way" in that
    // it will continue to attempt to flood the network with more data than
    // can be sent, we need to tell the client's connection to stop handling
    // incoming messages after some finite number, to avoid getting stuck down
    // in the imager's mainloop() and never returning control to the main
    // program.  This strange-named function does this for us.  If the camera
    // is not sending too many messages for the network, this should not have
    // any effect.
    g_imager->connectionPtr()->Jane_stop_this_crazy_thing(50);

    // Allocate memory for the image and clear it, so that things that
    // don't get filled in will be black.
    if ( (g_image = new unsigned char[g_Xdim * g_Ydim * 3]) == NULL) {
        fprintf(stderr,"Out of memory when allocating image!\n");
        return -1;
    }
    for (i = 0; i < g_Xdim * g_Ydim * 3; i++) {
        g_image[i] = 0;
    }
    g_ready_for_region = true;
    printf("Receiving images at size %dx%d\n", g_Xdim, g_Ydim);
    printf("Press '0'-'9' in OpenGL window to throttle incoming images.\n");
    printf("Press '-' to disable throttling.\n");
    printf("Press 'a' to enable/disable autoscaling of brightness.\n");
    printf("Press 'q' or 'Q' or ESC to quit.\n");

    // Initialize GLUT and create the window that will display the
    // video -- name the window after the device that has been
    // opened in VRPN.
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);
    glutInitWindowSize(g_Xdim, g_Ydim);
    glutInitWindowPosition(100, 100);
    glutCreateWindow(vrpn_copy_service_name(device_name));

    // Set the display function and idle function for GLUT (they
    // will do all the work) and then give control over to GLUT.
    glutDisplayFunc(myDisplayFunc);
    glutIdleFunc(myIdleFunc);
    glutKeyboardFunc(myKeyboardFunc);
    glutMainLoop();

    // Clean up objects and return.  This code is never called because
    // glutMainLoop() never returns.
    return 0;
}
Exemplo n.º 19
0
int main (int argc, char ** argv) {

  char * source_location;
  char * source_name;
  char * client_name;
  int port = 4510;
  int retval;

  if ((argc < 3) || (argc > 4))
    Usage(argv[0]);

  // Expect that source_name and client_name are both ???
  // Port is the port that our client will connect to us on

  source_location = argv[1];
  source_name = argv[2];
  client_name = argv[2];
  if (argc == 4)
    port = atoi(argv[3]);

  // Connect to the server.

  server_connection = vrpn_get_connection_by_name (source_location);

  // Open a port for our client to connect to us.

  client_connection = vrpn_create_server_connection(port);

  // Put a forwarder on that port.

  forwarder = new vrpn_StreamForwarder
    (server_connection, source_name, client_connection, client_name);

  // Tell the forwarder to send Tracker Pos/Quat messages through,
  // using the same name on both sides.

  retval = forwarder->forward("Tracker Pos/Quat", "Tracker Pos/Quat");

  if (retval)
    fprintf(stderr, "forwarder->forward(\"Tracker Pos/Quat\") failed.\n");
  else
    fprintf(stderr, "forwarder->forward(\"Tracker Pos/Quat\") succeeded.\n");

  // Set up a signal handler to shut down cleanly on control-C.

  install_handler();

  // Do the dirty work.

  while (1) {

    // Get any received messages and queue them up for transmission
    // to the client.

    server_connection->mainloop();

    // Send them.

    client_connection->mainloop();

  }

}
Exemplo n.º 20
0
void init (const char * station_name, 
           const char * local_in_logfile, const char * local_out_logfile,
           const char * remote_in_logfile, const char * remote_out_logfile,
           const char * NIC)
{
	char devicename [1000];
	//char * hn;

  vrpn_int32 gotConn_type;

	// explicitly open up connections with the proper logging parameters
	// these will be entered in the table and found by the
	// vrpn_get_connection_by_name() inside vrpn_Tracker and vrpn_Button

	sprintf(devicename, "Tracker0@%s", station_name);
	if (!strncmp(station_name, "file:", 5)) {
fprintf(stderr, "Opening file %s.\n", station_name);
	  c = new vrpn_File_Connection (station_name);  // now unnecessary!
          if (local_in_logfile || local_out_logfile ||
              remote_in_logfile || remote_out_logfile) {
            fprintf(stderr, "Warning:  Reading from file, so not logging.\n");
          }
	} else {
fprintf(stderr, "Connecting to host %s.\n", station_name);
	  c = vrpn_get_connection_by_name
                    (station_name,
		    local_in_logfile, local_out_logfile,
		    remote_in_logfile, remote_out_logfile,
                    NIC);
          if (delayTime > 0.0) {
            //((vrpn_DelayedConnection *) c)->setDelay
                              //(vrpn_MsecsTimeval(delayTime * 1000.0));
            //((vrpn_DelayedConnection *) c)->delayAllTypes(vrpn_TRUE);
          }
	}

	fc = new vrpn_File_Controller (c);

	if (beRedundant) {
	    rc = new vrpn_RedundantRemote (c);
	}

fprintf(stderr, "Tracker's name is %s.\n", devicename);
	tkr = new vrpn_Tracker_Remote (devicename);


	sprintf(devicename, "Button0@%s", station_name);
fprintf(stderr, "Button's name is %s.\n", devicename);
	btn = new vrpn_Button_Remote (devicename);
	sprintf(devicename, "Button1@%s", station_name);
fprintf(stderr, "Button 2's name is %s.\n", devicename);
	btn2 = new vrpn_Button_Remote (devicename);


	// Set up the tracker callback handler
	printf("Tracker update: '.' = pos, '/' = vel, '~' = acc\n");
	tkr->register_change_handler(NULL, handle_pos);
	tkr->register_change_handler(NULL, handle_vel);
	tkr->register_change_handler(NULL, handle_acc);

	// Set up the button callback handler
	printf("Button update: B<number> is <newstate>\n");
	btn->register_change_handler(NULL, handle_button);
	btn2->register_change_handler(NULL, handle_button);

  gotConn_type = c->register_message_type(vrpn_got_connection);

  c->register_handler(gotConn_type, handle_gotConnection, NULL);

}	/* init */