int mtev_json_object_to_fd(int fd, struct mtev_json_object *obj)
{
const char *mtev_json_str;
int ret;
unsigned int wpos, wsize;
if(!obj) {
MC_ERROR("mtev_json_object_to_fd: object is null%s\n", "");
return -1;
}
if(!(mtev_json_str = mtev_json_object_to_json_string(obj))) {
return -1;
}
wsize = (unsigned int)(strlen(mtev_json_str) & UINT_MAX); /* CAW: probably unnecessary, but the most 64bit safe */
wpos = 0;
while(wpos < wsize) {
if((ret = write(fd, mtev_json_str + wpos, wsize-wpos)) < 0) {
MC_ERROR("mtev_json_object_to_fd: error writing fd %d: %s\n",
fd, strerror(errno));
return -1;
}
/* because of the above check for ret < 0, we can safely cast and add */
wpos += (unsigned int)ret;
}
return 0;
}
struct json_object* json_object_from_file(char *filename)
{
struct printbuf *pb;
struct json_object *obj;
char buf[JSON_FILE_BUF_SIZE];
int fd, ret;
if((fd = open(filename, O_RDONLY)) < 0) {
MC_ERROR("json_object_from_file: error reading file %s: %s\n",
filename, strerror(errno));
return error_ptr(-1);
}
if(!(pb = printbuf_new())) {
MC_ERROR("json_object_from_file: printbuf_new failed\n");
return error_ptr(-1);
}
while((ret = read(fd, buf, JSON_FILE_BUF_SIZE)) > 0) {
printbuf_memappend(pb, buf, ret);
}
close(fd);
if(ret < 0) {
MC_ABORT("json_object_from_file: error reading file %s: %s\n",
filename, strerror(errno));
printbuf_free(pb);
return error_ptr(-1);
}
obj = json_tokener_parse(pb->buf);
printbuf_free(pb);
return obj;
}
struct json_object* json_object_from_file(const char *filename)
{
struct printbuf *pb;
struct json_object *obj;
char buf[JSON_FILE_BUF_SIZE];
int ret;
FILE * fd;
if((fd = fopen(filename, "R")) < 0) {
MC_ERROR("json_object_from_file: error reading file %s: %s\n",
filename, strerror(errno));
return (struct json_object*)error_ptr(-1);
}
if(!(pb = printbuf_new())) {
fclose(fd);
MC_ERROR("json_object_from_file: printbuf_new failed\n");
return (struct json_object*)error_ptr(-1);
}
while((ret = fread(buf,JSON_FILE_BUF_SIZE, sizeof(char), fd)) > 0) {
//while((ret = fread(fd, buf, JSON_FILE_BUF_SIZE)) > 0) {
printbuf_memappend(pb, buf, ret);
}
fclose(fd);
if(ret < 0) {
MC_ABORT("json_object_from_file: error reading file %s: %s\n",
filename, strerror(errno));
printbuf_free(pb);
return (struct json_object*)error_ptr(-1);
}
obj = json_tokener_parse(pb->buf);
printbuf_free(pb);
return obj;
}
const char *json_type_to_name(enum json_type o_type)
{
if (o_type < 0 || o_type >= NELEM(json_type_name))
{
MC_ERROR("json_type_to_name: type %d is out of range [0,%d]\n", o_type, NELEM(json_type_name));
return NULL;
}
return json_type_name[o_type];
}
void vrpn_3DMicroscribe::report_changes(vrpn_uint32 class_of_service)
{
vrpn_Button::timestamp = timestamp;
vrpn_Tracker::timestamp = timestamp;
vrpn_Button::report_changes();
if (d_connection) {
char msgbuf[1000];
int len = vrpn_Tracker::encode_to(msgbuf);
if (d_connection->pack_message(len, timestamp,
position_m_id, d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
MC_ERROR("Tracker: cannot write message: tossing\n");
}
} else {
MC_ERROR("Tracker: No valid connection\n");
}
}
int json_object_to_file(char *filename, struct json_object *obj)
{
const char *json_str;
int ret;
unsigned int wpos, wsize;
FILE * fd;
if(!obj) {
MC_ERROR("json_object_to_file: object is null\n");
return -1;
}
if((fd = fopen(filename, "W")) < 0) {
MC_ERROR("json_object_to_file: error opening file %s: %s\n",
filename, strerror(errno));
return -1;
}
if(!(json_str = json_object_to_json_string(obj))) {
fclose(fd);
return -1;
}
wsize = (unsigned int)(strlen(json_str) & UINT_MAX); /* CAW: probably unnecessary, but the most 64bit safe */
wpos = 0;
while(wpos < wsize) {
if((ret = fwrite(json_str + wpos, wsize-wpos, sizeof(char), fd)) < 0) {
// if((ret = write(fd, json_str + wpos, wsize-wpos)) < 0) {
fclose(fd);
MC_ERROR("json_object_to_file: error writing file %s: %s\n",
filename, strerror(errno));
return -1;
}
/* because of the above check for ret < 0, we can safely cast and add */
wpos += (unsigned int)ret;
}
fclose(fd);
return 0;
}
// This routine will reset the 3DMicroscribe, zeroing the Origin position,
// mode.
int vrpn_3DMicroscribe::reset(void)
{
#ifdef VRPN_USE_MICROSCRIBE
int iResult;
// ARM_FULL: ArmDll32 calculates and updates the Cartesian position and orientation of the stylus tip.
iResult = ArmSetUpdate(ARM_FULL);
if(iResult != ARM_SUCCESS)
{
//error setting the update type, disconnect and end the thread
MC_ERROR( "Unable to set the update type for the MicroScribe." );
return -1;
}
//use mm instead of inches
ArmSetLengthUnits(ARM_MM);
//use radians instead of degrees
//ArmSetAngleUnits(ARM_RADIANS);
//get the position of the tip
length_3D tipPosition;
angle_3D tipVector;
iResult = ArmGetTipPosition(&tipPosition); //retrieves the current stylus tip position in Cartesian coordinates
iResult = ArmGetTipOrientationUnitVector(&tipVector); //retrieves the current stylus tip's unit vector orientation
if(iResult == ARM_NOT_CONNECTED)
{
//error connecting
MC_ERROR( "MicroScribe connection lost!" );
return -1;
}
#endif
// We're now waiting for a response from the box
status = STATUS_SYNCING;
vrpn_gettimeofday(×tamp, NULL); // Set watchdog now
return 0;
}
int json_object_to_file_ext(const char *filename, struct json_object *obj, int flags)
{
const char *json_str;
int fd, ret;
unsigned int wpos, wsize;
if(!obj) {
MC_ERROR("json_object_to_file: object is null\n");
return -1;
}
if((fd = open(filename, O_WRONLY | O_TRUNC | O_CREAT, 0644)) < 0) {
MC_ERROR("json_object_to_file: error opening file %s: %s\n",
filename, strerror(errno));
return -1;
}
if(!(json_str = json_object_to_json_string_ext(obj,flags))) {
close(fd);
return -1;
}
wsize = (unsigned int)(strlen(json_str) & UINT_MAX); /* CAW: probably unnecessary, but the most 64bit safe */
wpos = 0;
while(wpos < wsize) {
if((ret = write(fd, json_str + wpos, wsize-wpos)) < 0) {
close(fd);
MC_ERROR("json_object_to_file: error writing file %s: %s\n",
filename, strerror(errno));
return -1;
}
/* because of the above check for ret < 0, we can safely cast and add */
wpos += (unsigned int)ret;
}
close(fd);
return 0;
}
struct json_object* json_object_from_file(const char *filename)
{
struct json_object *obj;
int fd;
if((fd = open(filename, O_RDONLY)) < 0) {
MC_ERROR("json_object_from_file: error opening file %s: %s\n",
filename, strerror(errno));
return NULL;
}
obj = json_object_from_fd(fd);
close(fd);
return obj;
}
/*
* Create a JSON object from already opened file descriptor.
*
* This function can be helpful, when you opened the file already,
* e.g. when you have a temp file.
* Note, that the fd must be readable at the actual position, i.e.
* use lseek(fd, 0, SEEK_SET) before.
*/
struct json_object* json_object_from_fd(int fd)
{
struct printbuf *pb;
struct json_object *obj;
char buf[JSON_FILE_BUF_SIZE];
int ret;
if(!(pb = printbuf_new())) {
MC_ERROR("json_object_from_file: printbuf_new failed\n");
return NULL;
}
while((ret = read(fd, buf, JSON_FILE_BUF_SIZE)) > 0) {
printbuf_memappend(pb, buf, ret);
}
if(ret < 0) {
MC_ERROR("json_object_from_fd: error reading fd %d: %s\n", fd, strerror(errno));
printbuf_free(pb);
return NULL;
}
obj = json_tokener_parse(pb->buf);
printbuf_free(pb);
return obj;
}
struct mtev_json_object* mtev_json_object_from_file(char *filename)
{
struct mtev_json_object *obj;
int fd;
if((fd = open(filename, O_RDONLY)) < 0) {
MC_ERROR("mtev_json_object_from_file: error reading file %s: %s\n",
filename, strerror(errno));
return (struct mtev_json_object*)error_ptr(-1);
}
obj = mtev_json_object_from_fd(fd);
close(fd);
return obj;
}
int mtev_json_object_to_file(char *filename, struct mtev_json_object *obj)
{
int fd, ret;
if((fd = open(filename, O_WRONLY | O_TRUNC | O_CREAT, 0644)) < 0) {
MC_ERROR("mtev_json_object_to_file: error opening file %s: %s\n",
filename, strerror(errno));
return -1;
}
ret = mtev_json_object_to_fd(fd, obj);
close(fd);
return ret;
}
int vrpn_3DMicroscribe::get_report(void)
{
#ifdef VRPN_USE_MICROSCRIBE
length_3D tipPosition;
angle_3D tipOri;
DWORD buts;
int iResult = ArmGetTipPosition(&tipPosition); //retrieves the current stylus tip position in Cartesian coordinates
iResult = ArmGetTipOrientation(&tipOri); //retrieves the current stylus tip's unit vector orientation
iResult = ArmGetButtonsState(&buts);
if(iResult == ARM_NOT_CONNECTED)
{
//error connecting
MC_ERROR( "MicroScribe connection lost!" );
return 0;
}
//set the position, considering the scale, offset and origin matrix
pos[0] = (tipPosition.y * m_Scale + m_OffSet[0])* MM_TO_METERS ;
pos[1] = (tipPosition.z * m_Scale + m_OffSet[1])* MM_TO_METERS;
pos[2] = (tipPosition.x * m_Scale + m_OffSet[2])* MM_TO_METERS;
//vPosition = m_Matrix * vPosition + m_vPlaneOffset; extending the microscribe onto a plane
//set the orientation, considering the origin matrix
float ori[3]={tipOri.y, tipOri.z, tipOri.x};
ConvertOriToQuat(ori);
status = STATUS_READING; // ready to process event packet
vrpn_gettimeofday(×tamp, NULL); // set timestamp of this event
buttons[0] = ((buts & 0x02) != 0); // button 1
buttons[1] = ((buts & 0x01) != 0); // button 2
#endif
report_changes(); // Report updates to VRPN
return 0;
}
struct mtev_json_object *mtev_json_object_from_fd(int fd)
{
struct jl_printbuf *pb;
struct mtev_json_object *obj;
char buf[JSON_FILE_BUF_SIZE];
int ret;
if(!(pb = jl_printbuf_new())) {
MC_ERROR("mtev_json_object_from_fd: jl_printbuf_new failed%s\n", "");
return (struct mtev_json_object*)error_ptr(-1);
}
while((ret = read(fd, buf, JSON_FILE_BUF_SIZE)) > 0) {
jl_printbuf_memappend(pb, buf, ret);
}
if(ret < 0) {
MC_ABORT("mtev_json_object_from_fd: error reading fd %d: %s\n",
fd, strerror(errno));
jl_printbuf_free(pb);
return (struct mtev_json_object*)error_ptr(-1);
}
obj = mtev_json_tokener_parse(pb->buf);
jl_printbuf_free(pb);
return obj;
}
// This creates a vrpn_3DMicroscribe and sets it to reset mode.
vrpn_3DMicroscribe::vrpn_3DMicroscribe (const char * name, vrpn_Connection * c,
const char * Port, long int BaudRate,
float OffsetX/* = 0.0f*/,
float OffsetY/* = 0.0f*/,
float OffsetZ/* = 0.0f*/,
float Scale/*=1.0f*/):
vrpn_Tracker(name, c),
vrpn_Button_Filter(name, c),
_numbuttons(2)
{
// Set the parameters in the parent classes
vrpn_Button::num_buttons = _numbuttons;
if(!strcmp(Port, "COM1") )
m_PortNumber=1;
else if(!strcmp(Port, "COM2") )
m_PortNumber=2;
else if(!strcmp(Port, "COM3") )
m_PortNumber=3;
else if(!strcmp(Port, "COM4") )
m_PortNumber=4;
m_BaudRate=BaudRate;
m_OffSet[0]=OffsetX; m_OffSet[1]=OffsetY; m_OffSet[2]=OffsetZ;
m_Scale=Scale;
// Set the status of the buttons and analogs to 0 to start
clear_values();
#ifdef VRPN_USE_MICROSCRIBE
int iResult;
iResult=ArmStart(NULL);
if(ARM_SUCCESS != iResult)
{
//error starting the MicroScribe drivers
MC_ERROR( "Unable to start MicroScribe ArmDll32." );
return;
}
//don't use error handlers
iResult = ArmSetErrorHandlerFunction(NO_HCI_HANDLER, NULL);
iResult = ArmSetErrorHandlerFunction(BAD_PORT_HANDLER, NULL);
iResult = ArmSetErrorHandlerFunction(CANT_OPEN_HANDLER, NULL);
iResult = ArmSetErrorHandlerFunction(CANT_BEGIN_HANDLER, NULL);
//connect to the correct port
switch(m_PortNumber)
{
case 1:
iResult = ArmConnect(1, m_BaudRate);
break;
case 2:
iResult = ArmConnect(2, m_BaudRate);
break;
case 3:
iResult = ArmConnect(3, m_BaudRate);
break;
case 4:
iResult = ArmConnect(4, m_BaudRate);
break;
default:
iResult = ArmConnect(0, 0); //try all available ports and baud rates
break;
}
if(ARM_SUCCESS != iResult)
{
//error connecting, end the thread
ArmEnd();
MC_ERROR( "Unable to connect to the MicroScribe." );
return;
}
#endif
// Set the mode to reset
status = STATUS_RESETTING;
}