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;
}
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);
}
}
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;
}
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);
}
}
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);
}
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);
}
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));
}
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;
}
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;
}
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;
}
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;
}
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;
}
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;
}
/** 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
}
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;
}
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();
}
}
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 */
