You can subscribe to this list here.
2006 |
Jan
|
Feb
|
Mar
|
Apr
(85) |
May
(154) |
Jun
(108) |
Jul
(90) |
Aug
(150) |
Sep
(147) |
Oct
(57) |
Nov
(90) |
Dec
(160) |
---|---|---|---|---|---|---|---|---|---|---|---|---|
2007 |
Jan
(40) |
Feb
(54) |
Mar
(90) |
Apr
(86) |
May
(60) |
Jun
(80) |
Jul
(114) |
Aug
(198) |
Sep
(265) |
Oct
(298) |
Nov
(191) |
Dec
(161) |
2008 |
Jan
(77) |
Feb
(155) |
Mar
(64) |
Apr
(148) |
May
(95) |
Jun
(265) |
Jul
(259) |
Aug
(73) |
Sep
(68) |
Oct
(96) |
Nov
(129) |
Dec
(82) |
2009 |
Jan
(135) |
Feb
(69) |
Mar
(233) |
Apr
(96) |
May
(263) |
Jun
(295) |
Jul
(294) |
Aug
(106) |
Sep
|
Oct
|
Nov
|
Dec
|
From: <gb...@us...> - 2009-07-31 13:01:42
|
Revision: 8162 http://playerstage.svn.sourceforge.net/playerstage/?rev=8162&view=rev Author: gbiggs Date: 2009-07-31 13:00:23 +0000 (Fri, 31 Jul 2009) Log Message: ----------- Changed XSensMT driver to pull proprietry source files from a directory specified in the environment Modified Paths: -------------- code/player/trunk/server/drivers/imu/xsens/CMakeLists.txt Removed Paths: ------------- code/player/trunk/server/drivers/imu/xsens/cmt1.cpp code/player/trunk/server/drivers/imu/xsens/cmt1.h code/player/trunk/server/drivers/imu/xsens/cmt2.cpp code/player/trunk/server/drivers/imu/xsens/cmt2.h code/player/trunk/server/drivers/imu/xsens/cmt3.cpp code/player/trunk/server/drivers/imu/xsens/cmt3.h code/player/trunk/server/drivers/imu/xsens/cmtdef.h code/player/trunk/server/drivers/imu/xsens/cmtf.h code/player/trunk/server/drivers/imu/xsens/cmtmessage.cpp code/player/trunk/server/drivers/imu/xsens/cmtmessage.h code/player/trunk/server/drivers/imu/xsens/cmtpacket.cpp code/player/trunk/server/drivers/imu/xsens/cmtpacket.h code/player/trunk/server/drivers/imu/xsens/cmtscan.cpp code/player/trunk/server/drivers/imu/xsens/cmtscan.h code/player/trunk/server/drivers/imu/xsens/xsens_fifoqueue.h code/player/trunk/server/drivers/imu/xsens/xsens_janitors.h code/player/trunk/server/drivers/imu/xsens/xsens_list.cpp code/player/trunk/server/drivers/imu/xsens/xsens_list.h code/player/trunk/server/drivers/imu/xsens/xsens_list.hpp code/player/trunk/server/drivers/imu/xsens/xsens_std.cpp code/player/trunk/server/drivers/imu/xsens/xsens_std.h code/player/trunk/server/drivers/imu/xsens/xsens_time.cpp code/player/trunk/server/drivers/imu/xsens/xsens_time.h Modified: code/player/trunk/server/drivers/imu/xsens/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/imu/xsens/CMakeLists.txt 2009-07-31 11:27:28 UTC (rev 8161) +++ code/player/trunk/server/drivers/imu/xsens/CMakeLists.txt 2009-07-31 13:00:23 UTC (rev 8162) @@ -1,2 +1,18 @@ -PLAYERDRIVER_OPTION (XSensMT build_XSensMT ON) -PLAYERDRIVER_ADD_DRIVER (XSensMT build_XSensMT SOURCES XSensMT.cc xsens_list.cpp xsens_time.cpp xsens_std.cpp cmt1.cpp cmt2.cpp cmt3.cpp cmtscan.cpp cmtmessage.cpp cmtpacket.cpp) +SET (xsensmtSrcDir $ENV{XSENSMT_SOURCE_DIR}) +IF (xsensmtSrcDir) + PLAYERDRIVER_OPTION (XSensMT build_XSensMT ON) + SET (xsensmtSrcs XSensMT.cc + ${xsensmtSrcDir}/xsens_list.cpp + ${xsensmtSrcDir}/xsens_time.cpp + ${xsensmtSrcDir}/xsens_std.cpp + ${xsensmtSrcDir}/cmt1.cpp + ${xsensmtSrcDir}/cmt2.cpp + ${xsensmtSrcDir}/cmt3.cpp + ${xsensmtSrcDir}/cmtscan.cpp + ${xsensmtSrcDir}/cmtmessage.cpp + ${xsensmtSrcDir}/cmtpacket.cpp) +ELSE (xsensmtSrcDir) + PLAYERDRIVER_OPTION (XSensMT build_XSensMT OFF + "Set the XSENSMT_SOURCE_DIR environment variable to the location of the provided source files.") +ENDIF (xsensmtSrcDir) +PLAYERDRIVER_ADD_DRIVER (XSensMT build_XSensMT SOURCES ${xsensmtSrcs}) Deleted: code/player/trunk/server/drivers/imu/xsens/cmt1.cpp =================================================================== --- code/player/trunk/server/drivers/imu/xsens/cmt1.cpp 2009-07-31 11:27:28 UTC (rev 8161) +++ code/player/trunk/server/drivers/imu/xsens/cmt1.cpp 2009-07-31 13:00:23 UTC (rev 8162) @@ -1,1309 +0,0 @@ - -/*! \file Cmt1.cpp - - For information about objects in this file, see the appropriate header: - \ref Cmt1.h - - # The originator of this source code is Xsens Technologies B.V.. This source code is intended for use with Xsens Motion Trackers only. - - THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - PARTICULAR PURPOSE. - - \section FileChangelog Changelog - \par 2006-04-12, v0.0.1 - \li Job Mulder: Created - \par 2006-07-21, v0.1.0 - \li Job Mulder: Updated file for release 0.1.0 -*/ - -#include "cmt1.h" -#include <errno.h> -#ifndef _WIN32 -# include <unistd.h> // close -# include <sys/ioctl.h> // ioctl -# include <fcntl.h> // open, O_RDWR -# include <string.h> // strcpy -//# include <malloc.h> // malloc -# include <sys/param.h> -// We have to redefine PATH_MAX from 4096 to CMT_MAX_FILENAME_LENGTH to mainain compatibility -// The PATH_MAX definition is used by realpath() to determine the maximum path length. According -// to the realpath (3) man page, the function is best avoided and it might be necessary to -// write a custom function for it (couldn't find a proper replacement). -# undef PATH_MAX -# define PATH_MAX CMT_MAX_FILENAME_LENGTH -# include <stdlib.h> -#else -# include <io.h> -#endif - -#ifndef _CRT_SECURE_NO_DEPRECATE -# define _CRT_SECURE_NO_DEPRECATE -# ifdef _WIN32 -# pragma warning(disable:4996) -# endif -#endif - -#ifdef _WIN32 -# define FSEEK(x) _fseeki64(m_handle, x, SEEK_SET) -# define FSEEK_R(x) _fseeki64(m_handle, x, SEEK_END) -# define FTELL() _ftelli64(m_handle) -#else -# define FSEEK(x) fseeko(m_handle, x, SEEK_SET) -# define FSEEK_R(x) fseeko(m_handle, x, SEEK_END) -# define FTELL() ftello(m_handle) -#endif - -// The namespace of all Xsens software since 2006. -namespace xsens { - -#ifndef _WIN32 -int _wcsnicmp(const wchar_t* s1, const wchar_t* s2,int count) -{ - for (int i = 0; i < count; ++i, ++s1, ++s2) - if (*s1 == L'\0') - if (*s2 == L'\0') - return 0; - else - return -1; - else - if (*s2 == L'\0') - return 1; - else - if (*s1 < *s2) - return -1; - else if (*s1 > *s2) - return 1; - return 0; -} -#endif - - -#if defined(_DEBUG) || defined(_LOG_ALWAYS) - #if !defined(_LOG_TO_DBVIEW) - #ifdef _LOG_TO_STDOUT - #else // !dbview && !stdout - FILE* debug_log_fp = NULL; - int32_t debug_log_valid = 0; - - FILE* debug_qlog_fp = NULL; - int32_t debug_qlog_valid = 0; - #endif - #endif - -// write to a log file/screen/debug-stream -void CMTLOG(const char *str, ...) -{ - #ifdef _LOG_TO_STDOUT - va_list ptr; - va_start(ptr,str); - vprintf(str,ptr); - #else - #ifdef _LOG_TO_DBVIEW - char buf[2048]; - - va_list ptr; - va_start(ptr,str); - vsprintf(buf,str,ptr); - - OutputDebugString(buf); - #else - if (debug_log_valid == 0) - { - fopen_s(&debug_log_fp,"debug_log_cmt.log","w"); - if (debug_log_fp != NULL) - debug_log_valid = 1; - else - debug_log_valid = -1; - } - if (debug_log_valid == 1) - { - char buf[2048]; - - va_list ptr; - va_start(ptr,str); - int32_t sz = vsprintf_s(buf,str,ptr); - - uint32_t nw = getTimeOfDay(); - fprintf(debug_log_fp,"%5u.%03u %s",nw/1000,nw%1000,buf); - //fwrite(buf,1,sz,debug_log_fp); - fflush(debug_log_fp); - } - #endif - #endif -} -#endif - -// maybe log to nothing at this level -#ifdef _LOG_CMT1 - #define CMT1LOG CMTLOG -#else - #define CMT1LOG(...) -#endif - -////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////// Cmt1s ///////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////////////////// -// Default constructor, initializes all members to their default values. -Cmt1s::Cmt1s() : - m_onBytesReceived(NULL) -{ - m_port = 0; - m_isOpen = false; - m_lastResult = XRV_OK; - m_timeout = CMT1_DEFAULT_TIMEOUT; - m_endTime = 0; - m_baudrate = 0; - - #ifdef _LOG_RX_TX - rx_log = NULL; - tx_log = NULL; - #endif -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Destructor, de-initializes, frees memory allocated for buffers, etc. -Cmt1s::~Cmt1s() -{ - close(); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Close the serial communication port. -XsensResultValue Cmt1s::close (void) -{ - #ifdef _LOG_RX_TX - if (rx_log != NULL) - fclose(rx_log); - if (tx_log != NULL) - fclose(tx_log); - rx_log = NULL; - tx_log = NULL; - #endif - if (!m_isOpen) - return m_lastResult = XRV_NOPORTOPEN; - - #ifdef _WIN32 - ::FlushFileBuffers(m_handle); - // read all data before closing the handle, a Flush is not enough for FTDI devices unfortunately - // we first need to set the COMM timeouts to instantly return when no more data is available - COMMTIMEOUTS cto; - ::GetCommTimeouts(m_handle,&cto); - cto.ReadIntervalTimeout = MAXDWORD; - cto.ReadTotalTimeoutConstant = 0; - cto.ReadTotalTimeoutMultiplier = 0; - ::SetCommTimeouts(m_handle,&cto); - char buffer[1024]; - uint32_t length; - do { - ::ReadFile(m_handle, buffer, 1024, reinterpret_cast<LPDWORD> (&length), NULL); - } while (length > 0); - ::CloseHandle(m_handle); - #else - ::close(m_handle); - #endif - m_isOpen = false; - m_endTime = 0; - - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Manipulate the Serial control lines -XsensResultValue Cmt1s::escape (const CmtControlLine mask, const CmtControlLine state) -{ - if (!m_isOpen) - return (m_lastResult = XRV_NOPORTOPEN); -#ifdef _WIN32 - BOOL rv = 0; - if (mask & CMT_CONTROL_DTR) - { - if (state & CMT_CONTROL_DTR) - rv = EscapeCommFunction(m_handle,SETDTR); - else - rv = EscapeCommFunction(m_handle,CLRDTR); - } - - if (mask & CMT_CONTROL_RTS) - { - if (state & CMT_CONTROL_RTS) - rv = EscapeCommFunction(m_handle,SETRTS); - else - rv = EscapeCommFunction(m_handle,CLRRTS); - } - if (rv) - return m_lastResult = XRV_OK; - else - return m_lastResult = XRV_ERROR; -#else - bool rv = true; - int32_t status; - if (mask & CMT_CONTROL_DTR) - { - if (ioctl(m_handle, TIOCMGET, &status) == -1) - { - if (state & CMT_CONTROL_DTR) status |= TIOCM_DTR; - else status &= ~TIOCM_DTR; - rv = (ioctl(m_handle, TIOCMSET, &status) == -1); - } - else - rv = false; - } - if (rv && (mask & CMT_CONTROL_RTS)) - { - if (ioctl(m_handle, TIOCMGET, &status) == -1) - { - if (state & CMT_CONTROL_RTS) status |= TIOCM_RTS; - else status &= ~TIOCM_RTS; - rv = (ioctl(m_handle, TIOCMSET, &status) == -1); - } - else - rv = false; - } - if (rv) - return m_lastResult = XRV_OK; - else - return m_lastResult = XRV_ERROR; -#endif -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Flush all data to be transmitted / received. -XsensResultValue Cmt1s::flushData (void) -{ - #ifdef _WIN32 - // Remove any 'old' data in buffer - PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR); - #else - tcflush(m_handle, TCIOFLUSH); - #endif - m_endTime = 0; - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Open a communication channel to the given serial port name. -XsensResultValue Cmt1s::open( const char *portName, - const uint32_t baudRate, - uint32_t readBufSize, - uint32_t writeBufSize) -{ - m_endTime = 0; - - CMT1LOG("L1: Open port %s at %d baud\n", portName, baudRate); - - if (m_isOpen) - { - CMT1LOG("L1: Port already open\n"); - return (m_lastResult = XRV_ALREADYOPEN); - } - m_baudrate = baudRate; - -#ifdef _WIN32 - char winPortName[32]; - - // Open port - sprintf(winPortName, "\\\\.\\%s", portName); - m_handle = CreateFile(winPortName, GENERIC_READ | GENERIC_WRITE, 0, NULL, - OPEN_EXISTING, 0, NULL); - if (m_handle == INVALID_HANDLE_VALUE) - { - CMT1LOG("L1: Port cannot be opened\n"); - return (m_lastResult = XRV_INPUTCANNOTBEOPENED); - } - - // Once here, port is open - m_isOpen = true; - - //Get the current state & then change it - GetCommState(m_handle, &m_commState); // Get current state - - m_commState.BaudRate = baudRate; // Setup the baud rate - m_commState.Parity = NOPARITY; // Setup the Parity - m_commState.ByteSize = 8; // Setup the data bits - m_commState.StopBits = TWOSTOPBITS; // Setup the stop bits - m_commState.fDsrSensitivity = FALSE; // Setup the flow control - m_commState.fOutxCtsFlow = FALSE; // NoFlowControl: - m_commState.fOutxDsrFlow = FALSE; - m_commState.fOutX = FALSE; - m_commState.fInX = FALSE; - if (!SetCommState(m_handle, (LPDCB)&m_commState)) {// Set new state - // Bluetooth ports cannot always be opened with 2 stopbits - // Now try to open port with 1 stopbit. - m_commState.StopBits = ONESTOPBIT; - if (!SetCommState(m_handle, (LPDCB)&m_commState)) { - CloseHandle(m_handle); - m_handle = INVALID_HANDLE_VALUE; - m_isOpen = false; - return (m_lastResult = XRV_INPUTCANNOTBEOPENED); - } - } - m_port = atoi(&portName[3]); - sprintf(m_portname, "%s", portName); - - setTimeout(m_timeout); - - // Other initialization functions - EscapeCommFunction(m_handle, SETRTS); // Enable RTS (for Xbus Master use) - // Set DTR (Calibration sensors need DTR to startup, won't hurt otherwise - EscapeCommFunction(m_handle, SETDTR); - SetupComm(m_handle,readBufSize,writeBufSize); // Set queue size - - // Remove any 'old' data in buffer - //PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR); - PurgeComm(m_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR); -#else // !_WIN32 - // Open port -#ifdef __APPLE__ - // Changed to non-block, cannot open otherwise - m_handle = ::open(portName, O_RDWR | O_NOCTTY | O_NONBLOCK); -#else - m_handle = ::open(portName, O_RDWR | O_NOCTTY); -#endif - // O_RDWR: Read+Write - // O_NOCTTY: Raw input, no "controlling terminal" - // O_NDELAY: Don't care about DCD signal - - if (m_handle < 0) { - // Port not open - printf("CMT1: Opening failed\n"); - return m_lastResult = XRV_INPUTCANNOTBEOPENED; - } - - // Once here, port is open - m_isOpen = true; - - /* Start configuring of port for non-canonical transfer mode */ - // Get current options for the port - if (tcgetattr(m_handle, &m_commState) < 0) - printf("Failed to get attr\n"); - - // Set baudrate. - cfsetispeed(&m_commState, baudRate); - cfsetospeed(&m_commState, baudRate); - - // Enable the receiver and set local mode - m_commState.c_cflag |= (CLOCAL | CREAD); - // Set character size to data bits and set no parity Mask the characte size bits - m_commState.c_cflag &= ~(CSIZE|PARENB); - m_commState.c_cflag |= CS8; // Select 8 data bits - m_commState.c_cflag |= CSTOPB; // send 2 stop bits - // Disable hardware flow control - m_commState.c_cflag &= ~CRTSCTS; - m_commState.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN); - // Disable software flow control - m_commState.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON); - // Set Raw output - m_commState.c_oflag &= ~OPOST; - // Timeout 0.001 sec for first byte, read minimum of 0 bytes - m_commState.c_cc[VMIN] = 0; - m_commState.c_cc[VTIME] = (m_timeout+99)/100; // 1 - - // Set the new options for the port - if (tcsetattr(m_handle,TCSANOW, &m_commState) < 0) - printf("Failed to set attr"); - - m_port = 0; - sprintf(m_portname, "%s", portName); - - tcflush(m_handle, TCIOFLUSH); - - // setting RTS and DTR; RTS for Xbus Master, DTR for calibration sensors - int cmbits; - if (ioctl(m_handle, TIOCMGET, &cmbits) < 0) - { - return (m_lastResult = XRV_ERROR); - } - - cmbits |= TIOCM_RTS|TIOCM_DTR; - - if (ioctl(m_handle, TIOCMSET, &cmbits) < 0) - { - return (m_lastResult = XRV_ERROR); - } -#endif // !_WIN32 - - CMT1LOG("L1: Port opened\n"); - return (m_lastResult = XRV_OK); -} - -#ifdef _WIN32 -////////////////////////////////////////////////////////////////////////////////////////// -// Open a communication channel to the given COM port number. -XsensResultValue Cmt1s::open ( const uint32_t portNumber, - const uint32_t baudRate, - uint32_t readBufSize, - uint32_t writeBufSize) -{ - char comFileName[32]; - - // Create file name - sprintf(comFileName, "COM%d", portNumber); - - return Cmt1s::open(comFileName, baudRate, readBufSize, writeBufSize); -} -#endif - -////////////////////////////////////////////////////////////////////////////////////////// -// Read data from the serial port and put it into the data buffer. -XsensResultValue Cmt1s::readData (const uint32_t maxLength, uint8_t* data, - uint32_t* length) -{ - CMT1LOG("L1: readData, maxlength=%u, length=%p\n",maxLength,length); - uint32_t ln; - if (length == NULL) - length = &ln; - - if (!m_isOpen) - return (m_lastResult = XRV_NOPORTOPEN); - -#ifdef _WIN32 - BOOL rres = ::ReadFile(m_handle, data, maxLength, reinterpret_cast<LPDWORD> (length), NULL); - if (m_onBytesReceived != NULL && *length > 0) - { - CmtBinaryData* bytes = (CmtBinaryData*) malloc(sizeof(CmtBinaryData)); - bytes->m_size = *length; - bytes->m_portNr = m_port; - memcpy(bytes->m_data,data,*length); -#ifdef _LOG_CALLBACKS - CMTLOG("C1: onBytesReceived(%d,(%d,%d),%p)\n",(int32_t) m_onBytesReceivedInstance, (int32_t) bytes->m_size, (int32_t) bytes->m_portNr, m_onBytesReceivedParam); -#endif - m_onBytesReceived(m_onBytesReceivedInstance,CMT_CALLBACK_ONBYTESRECEIVED,bytes,m_onBytesReceivedParam); - } - - if (!rres) - { - CMT1LOG("L1: readData, ReadFile returned error %u\n",::GetLastError()); - return (m_lastResult = XRV_ERROR); - } -#else - *length = read(m_handle, data, maxLength); -#endif - -#ifdef _LOG_RX_TX - if (*length > 0) - { - if (rx_log == NULL) - { - char fname[CMT_MAX_FILENAME_LENGTH]; - sprintf(fname,"rx_%03d_%d.log",(int32_t) m_port,m_baudrate); - rx_log = fopen(fname,"wb"); - } - fwrite(data,1,*length,rx_log); - } -#endif - CMT1LOG((length[0]?"L1: readData returned success, read %u of %u bytes, first: %02x\n":"L1: readData returned success, read %u bytes\n"),length[0],maxLength,data[0]); - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Set the callback function for when bytes have been received -XsensResultValue Cmt1s::setCallbackFunction(CmtCallbackSelector tp, int32_t instance, CmtCallbackFunction func, void* param) -{ - if (tp == CMT_CALLBACK_ONBYTESRECEIVED) - { - m_onBytesReceived = func; - m_onBytesReceivedInstance = instance; - m_onBytesReceivedParam = param; - return m_lastResult = XRV_OK; - } - return m_lastResult = XRV_INVALIDPARAM; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Set the default timeout value to use in blocking operations. -XsensResultValue Cmt1s::setTimeout (const uint32_t ms) -{ - CMT1LOG("L1: Setting timeout to %u ms\n",ms); - - m_timeout = ms; -#ifdef _WIN32 - // Set COM timeouts - COMMTIMEOUTS commTimeouts; - - GetCommTimeouts(m_handle,&commTimeouts); // Fill CommTimeouts structure - - // immediate return if data is available, wait 1ms otherwise - if (m_timeout > 0) - { - commTimeouts.ReadIntervalTimeout = 0; - commTimeouts.ReadTotalTimeoutConstant = m_timeout; // ms time - commTimeouts.ReadTotalTimeoutMultiplier = 0; - commTimeouts.WriteTotalTimeoutConstant = m_timeout; - commTimeouts.WriteTotalTimeoutMultiplier = 0; - } - else - { - // immediate return whether data is available or not - commTimeouts.ReadIntervalTimeout = MAXDWORD; - commTimeouts.ReadTotalTimeoutConstant = 0; - commTimeouts.ReadTotalTimeoutMultiplier = 0; - commTimeouts.WriteTotalTimeoutConstant = 0; - commTimeouts.WriteTotalTimeoutMultiplier = 0; - } - - SetCommTimeouts(m_handle, &commTimeouts); // Set CommTimeouts structure -#else - // Timeout 0.1 sec for first byte, read minimum of 0 bytes - m_commState.c_cc[VMIN] = 0; - m_commState.c_cc[VTIME] = (m_timeout+99)/100; // ds time - - // Set the new options for the port if it is open - if (m_isOpen) - tcsetattr(m_handle,TCSANOW, &m_commState); -#endif - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Wait for data to arrive or a timeout to occur. -XsensResultValue Cmt1s::waitForData (const uint32_t maxLength, - uint8_t* data, uint32_t* length) -{ - CMT1LOG("L1: waitForData, mto=%u, length=%p\n",m_timeout,length); - uint32_t timeout = m_timeout; - - uint32_t ln; - if (length == NULL) - length = &ln; - uint32_t eTime = getTimeOfDay(NULL) + timeout; - uint32_t newLength = 0; - - *length = 0; - while ((*length < maxLength) && (getTimeOfDay() <= eTime)) - { - readData(maxLength - *length, data + *length, &newLength); - *length += newLength; - } - CMT1LOG("L1: waitForData result: read %u of %u bytes\n",length[0],maxLength); - - if (length[0] < maxLength) - return (m_lastResult = XRV_TIMEOUT); - else - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Write the data to the serial port. -XsensResultValue Cmt1s::writeData (const uint32_t length, const uint8_t* data, - uint32_t* written) -{ - uint32_t bytes; - if (written == NULL) - written = &bytes; - - if (!m_isOpen) - return (m_lastResult = XRV_NOPORTOPEN); - -#ifdef _WIN32 - if (WriteFile(m_handle, data, length, reinterpret_cast<LPDWORD> (written), NULL)) - { -#ifdef _LOG_RX_TX - if (written[0] > 0) - { - if (tx_log == NULL) - { - char fname[CMT_MAX_FILENAME_LENGTH]; - sprintf(fname,"tx_%03d_%d.log",(int32_t) m_port,m_baudrate); - tx_log = fopen(fname,"wb"); - } - fwrite(data,1,*written,tx_log); - } -#endif - return (m_lastResult = XRV_OK); - } - else - return (m_lastResult = XRV_ERROR); -#else - *written = write(m_handle, data, length); -// if (*written == length) - return (m_lastResult = XRV_OK); -// else -// return (m_lastResult = XRV_ERROR); -#endif -} - -////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////// Cmt1f ///////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////////////////// -// Default constructor, initializes all members to their default values. -Cmt1f::Cmt1f() -{ - m_readPos = 0; - m_writePos = 0; - m_lastResult = XRV_OK; - m_reading = true; - m_isOpen = false; - m_filename[0] = '\0'; - m_fileSize = 0; - m_readOnly = false; - m_unicode = false; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Destructor. -Cmt1f::~Cmt1f() -{ - close(); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Write data to the end of the file. -XsensResultValue Cmt1f::appendData (const uint32_t length, const void* data) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - if (m_reading || m_writePos != m_fileSize) - { - m_reading = false; - FSEEK_R(0); - } - fwrite(data, 1, length, m_handle); - m_writePos = FTELL(); - m_fileSize = m_writePos; - - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Close the file. -XsensResultValue Cmt1f::close (void) -{ - if (m_isOpen) - { - #ifdef _WIN32 - fflush(m_handle); - fclose(m_handle); - #else - ::fflush(m_handle); - ::fclose(m_handle); - #endif - } - m_isOpen = false; - m_readPos = 0; - m_writePos = 0; - m_reading = true; - m_isOpen = false; - m_fileSize = 0; - m_readOnly = false; - - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Close the file and delete it. -XsensResultValue Cmt1f::closeAndDelete(void) -{ - if (m_isOpen) - { - #ifdef _WIN32 - fflush(m_handle); - fclose(m_handle); - #else - ::fflush(m_handle); - ::fclose(m_handle); - #endif - if (m_readOnly) - m_lastResult = XRV_READONLY; - else - { -#ifdef _WIN32 - if (m_unicode) - { - if (_wremove(m_filename_w) != 0) - m_lastResult = XRV_READONLY; - else - m_lastResult = XRV_OK; - } - else -#endif - { -#ifdef _WIN32 - if (_unlink(m_filename) != 0) -#else - if (unlink(m_filename) != 0) -#endif - m_lastResult = XRV_READONLY; - else - m_lastResult = XRV_OK; - } - } - } - else - m_lastResult = XRV_NOFILEOPEN; - - m_isOpen = false; - m_readPos = 0; - m_writePos = 0; - m_reading = true; - m_isOpen = false; - m_fileSize = 0; - m_readOnly = false; - - return m_lastResult; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Create a new file. -XsensResultValue Cmt1f::create (const char* filename) -{ - if (m_isOpen) - return m_lastResult = XRV_ALREADYOPEN; - - //! \test does this work for non-existing files? Or do we need a check and create? - m_handle = fopen(filename, "w+b"); // open for update (r/w) - if (m_handle == NULL) - return m_lastResult = XRV_OUTPUTCANNOTBEOPENED; - - #ifdef _WIN32 - if (_fullpath(m_filename,filename,CMT_MAX_FILENAME_LENGTH) == NULL) - { - fclose(m_handle); - remove(filename); - return m_lastResult = XRV_INVALIDPARAM; - } - #else - // based on the assumption that this doesn't concern the serial port, handle - // it the same way using realpath(). Apparently realpath() doesn't require a - // maximum length. One would possibly want to write a wrapper for it. - if (realpath(filename, m_filename) == NULL) - { - fclose(m_handle); - remove(filename); - return m_lastResult = XRV_INVALIDPARAM; - } - #endif - mbstowcs(m_filename_w,m_filename,CMT_MAX_FILENAME_LENGTH); - m_unicode = false; - - m_isOpen = true; - m_readPos = 0; - m_writePos = 0; - m_fileSize = 0; - m_reading = true; - m_readOnly = false; - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Create a new file. -XsensResultValue Cmt1f::create (const wchar_t* filename) -{ - if (m_isOpen) - return m_lastResult = XRV_ALREADYOPEN; - -#ifdef _WIN32 - //! \test does this work for non-existing files? Or do we need a check and create? - m_handle = _wfopen(filename, L"w+b"); // open for update (r/w) - if (m_handle == NULL) - return m_lastResult = XRV_OUTPUTCANNOTBEOPENED; - - if (_wfullpath(m_filename_w,filename,CMT_MAX_FILENAME_LENGTH) == NULL) - { - fclose(m_handle); - _wremove(filename); - return m_lastResult = XRV_INVALIDPARAM; - } - wcstombs(m_filename,m_filename_w,CMT_MAX_FILENAME_LENGTH); - - m_isOpen = true; - m_readPos = 0; - m_writePos = 0; - m_fileSize = 0; - m_reading = true; - m_readOnly = false; -#else - char tFilename[CMT_MAX_FILENAME_LENGTH*2]; - wcstombs(tFilename,m_filename_w,CMT_MAX_FILENAME_LENGTH); - XsensResultValue res = create(tFilename); - if (res != XRV_OK) - return res; -#endif - m_unicode = true; - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Delete the given data from the file. -XsensResultValue Cmt1f::deleteData (const CmtFilePos start, const uint32_t length) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - gotoWrite(); - - CmtFilePos wPos = start; - CmtFilePos rPos = wPos + length; - - size_t read1; - CmtFilePos endPos = (start + (CmtFilePos) length); - if (endPos < m_fileSize) - { - CmtFilePos remaining = m_fileSize - endPos; - char buffer[512]; - - // copy data - FSEEK(rPos); - - while (remaining > 0) - { - if (remaining >= 512) - read1 = fread(buffer,1,512,m_handle); - else - read1 = fread(buffer,1,(size_t) remaining,m_handle); - - remaining -= read1; - rPos += read1; - - // write block to the correct position - FSEEK(wPos); - wPos += fwrite(buffer, 1, read1, m_handle); - FSEEK(rPos); - } - m_fileSize -= length; - } - else - { - m_fileSize = start; - } - -#ifdef _WIN32 - int32_t rv = _chsize(_fileno(m_handle),(int32_t) m_fileSize); -#else - int32_t rv = (ftruncate(fileno(m_handle),(int32_t) m_fileSize) == 0); -#endif - int32_t eno = 0; - if (rv != 0) - eno = errno; - m_writePos = start; - FSEEK(wPos); - if (rv != 0) - { - switch(eno) - { - case EACCES: - return m_lastResult = XRV_BUSY; - case EBADF: - return m_lastResult = XRV_INVALIDINSTANCE; - case ENOSPC: - return m_lastResult = XRV_OUTOFMEMORY; - case EINVAL: - return m_lastResult = XRV_INVALIDPARAM; - default: - return m_lastResult = XRV_ERROR; - } - } - - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Find a string of bytes in the file -XsensResultValue Cmt1f::find (const void* needleV, const uint32_t needleLength, CmtFilePos& pos) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - - const char* needle = (const char*) needleV; - - gotoRead(); - - pos = 0; - - char buffer[512]; - uint32_t bufferPos, needlePos = 0; - size_t readBytes; - if (m_readPos & 0x1FF) // read a block of data - readBytes = fread(buffer,1,(512-((size_t) m_readPos & 0x1FF)),m_handle); - else - readBytes = fread(buffer,1,512,m_handle); // read a block of data - - while (readBytes > 0) - { - m_readPos += readBytes; - bufferPos = 0; - - while (bufferPos < readBytes && needlePos < needleLength) - { - if (buffer[bufferPos] == needle[needlePos]) - { - // found a byte - ++needlePos; - } - else - { - if (needlePos > 0) - needlePos = 0; - else - if (buffer[bufferPos] == needle[0]) - { - // found a byte - needlePos = 1; - } - } - ++bufferPos; - } - if (needlePos < needleLength) - readBytes = fread(buffer,1,512,m_handle); // read next block - else - { - m_readPos = m_readPos + bufferPos - readBytes - needleLength; // or without needleLength - pos = m_readPos; // - needleLength; - FSEEK(m_readPos); - return m_lastResult = XRV_OK; - } - } - return m_lastResult = XRV_ENDOFFILE; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Flush all data to be written. -XsensResultValue Cmt1f::flushData (void) -{ - fflush(m_handle); - - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Retrieve the filename that was last successfully opened. -XsensResultValue Cmt1f::getName(char* filename) const -{ - strcpy(filename, m_filename); - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Retrieve the filename that was last successfully opened. -XsensResultValue Cmt1f::getName(wchar_t* filename) const -{ -#ifdef _WIN32 - wcscpy(filename, m_filename_w); -#else - mbstowcs(filename, m_filename, CMT_MAX_FILENAME_LENGTH); -#endif - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Change from writing to reading mode -void Cmt1f::gotoRead(void) -{ - if (m_reading) - return; - - FSEEK(m_readPos); - m_reading = true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Change from reading to writing mode -void Cmt1f::gotoWrite(void) -{ - if (!m_reading) - return; - - FSEEK(m_writePos); - m_reading = false; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Insert the given data into the file. -XsensResultValue Cmt1f::insertData (const CmtFilePos start, const uint32_t length, const void* data) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - gotoWrite(); - - CmtFilePos rPos = start; - CmtFilePos wPos = rPos + length; - - size_t read1, read2; - CmtFilePos remaining = m_fileSize - start; - size_t bsize = (length > 512)?length:512; - char* buffer1 = (char*) malloc(bsize); - char* buffer2 = (char*) malloc(bsize); - char* btemp; - - // copy data - FSEEK(rPos); - - if (remaining >= (CmtFilePos) bsize) - read1 = fread(buffer1,1,bsize,m_handle); - else - read1 = fread(buffer1,1,(size_t) remaining,m_handle); - - remaining -= read1; - rPos += read1; - - while(remaining > 0) - { - // move data to correct buffer - read2 = read1; - btemp = buffer1; buffer1 = buffer2; buffer2 = btemp; - - // read next block - if (remaining >= (CmtFilePos) bsize) - read1 = fread(buffer1,1,bsize,m_handle); - else - read1 = fread(buffer1,1,(size_t) remaining,m_handle); - - remaining -= read1; - rPos += read1; - - // write block to the correct position - FSEEK(wPos); - wPos += fwrite(buffer2, 1, read2, m_handle); - FSEEK(rPos); - } - - FSEEK(wPos); - wPos += fwrite(buffer1, 1, read1, m_handle); - - FSEEK(start); - m_writePos = start + fwrite(data, 1, length, m_handle); - m_fileSize += length; - - free(buffer1); - free(buffer2); - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Open a file. -XsensResultValue Cmt1f::open(const char* filename, const bool create, const bool readOnly) -{ - if (m_isOpen) - return m_lastResult = XRV_ALREADYOPEN; - - //! \test does this work for non-existing files? Or do we need a check and create? - m_readOnly = readOnly; - if (readOnly) - m_handle = fopen(filename, "rb"); // open for read only (r) - else - m_handle = fopen(filename, "r+b"); // open for update (r/w) - if (m_handle == NULL) - { - if (create) - m_handle = fopen(filename, "w+b"); // create for update (r/w) - else - { - m_handle = fopen(filename, "rb"); // open for read only (r) - m_readOnly = true; - } - } - if (m_handle == NULL) - return m_lastResult = XRV_INPUTCANNOTBEOPENED; - - #ifdef _WIN32 - if (_fullpath(m_filename,filename,CMT_MAX_FILENAME_LENGTH) == NULL) - { - fclose(m_handle); - return m_lastResult = XRV_INVALIDPARAM; - } - #else - // use the same trick again. - if (realpath(filename, m_filename) == NULL) - { - fclose(m_handle); - return m_lastResult = XRV_INVALIDPARAM; - } - #endif - mbstowcs(m_filename_w,m_filename,CMT_MAX_FILENAME_LENGTH); - m_unicode = false; - - m_isOpen = true; - m_readPos = 0; - m_writePos = 0; - m_reading = true; - FSEEK_R(0); - m_fileSize = FTELL(); - FSEEK(0); - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Open a file. -XsensResultValue Cmt1f::open(const wchar_t* filename, const bool create, const bool readOnly) -{ - if (m_isOpen) - return m_lastResult = XRV_ALREADYOPEN; - -#ifdef _WIN32 - //! \test does this work for non-existing files? Or do we need a check and create? - m_readOnly = readOnly; - if (readOnly) - m_handle = _wfopen(filename, L"rb"); // open for read only (r) - else - m_handle = _wfopen(filename, L"r+b"); // open for update (r/w) - if (m_handle == NULL) - { - if (create) - m_handle = _wfopen(filename, L"w+b"); // create for update (r/w) - else - { - m_handle = _wfopen(filename, L"rb"); // open for read only (r) - m_readOnly = true; - } - } - if (m_handle == NULL) - return m_lastResult = XRV_INPUTCANNOTBEOPENED; - - if (_wfullpath(m_filename_w,filename,CMT_MAX_FILENAME_LENGTH) == NULL) - { - fclose(m_handle); - return m_lastResult = XRV_INVALIDPARAM; - } - wcstombs(m_filename,m_filename_w,CMT_MAX_FILENAME_LENGTH); - - m_isOpen = true; - m_readPos = 0; - m_writePos = 0; - m_reading = true; - FSEEK_R(0); - m_fileSize = FTELL(); - FSEEK(0); -#else - char tFilename[CMT_MAX_FILENAME_LENGTH*2]; - wcstombs(tFilename,filename,CMT_MAX_FILENAME_LENGTH*2); - XsensResultValue res = open(tFilename,create,readOnly); - if (res != XRV_OK) - return res; -#endif - m_unicode = true; - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Read data from the file and put it into the data buffer. -XsensResultValue Cmt1f::readData(const uint32_t maxLength, void* data, uint32_t* length) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - - if (maxLength == 0) - return m_lastResult = XRV_OK; - - uint32_t len; - if (length == NULL) - length = &len; - - gotoRead(); - - length[0] = (uint32_t) fread(data,1,maxLength,m_handle); - if (length[0] == 0) - return (m_lastResult = XRV_ENDOFFILE); - - m_readPos += length[0]; - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Read data from the file until the terminator and put it into the data buffer. -XsensResultValue Cmt1f::readData (const uint32_t maxLength, const char terminator, void* dataV, uint32_t* length) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - - uint32_t len; - if (length == NULL) - length = &len; - - char* data = (char*) dataV; - int32_t readChar; - - gotoRead(); - - *length = 0; - readChar = (uint32_t) fgetc(m_handle); - - while (!feof(m_handle) && !ferror(m_handle)) - { - data[*length] = (char) readChar; - ++(*length); - ++m_readPos; - - if (((char) readChar == terminator) || ((*length) >= maxLength)) - return m_lastResult = XRV_OK; - } - return m_lastResult = XRV_ENDOFFILE; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Set the new absolute read position -XsensResultValue Cmt1f::setReadPos (const CmtFilePos pos) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - - if (m_readPos != pos) - { - m_readPos = pos; - if (m_reading) - FSEEK(m_readPos); - } - - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Set the new absolute write position -XsensResultValue Cmt1f::setWritePos(const CmtFilePos pos) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - if (pos == -1) - { - if (m_reading) - m_reading = false; - FSEEK_R(0); - m_writePos = FTELL(); - } - else - { - if (m_writePos != pos) - { - m_writePos = pos; - if (!m_reading) - FSEEK(m_writePos); - } - } - - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Write data to the file. -XsensResultValue Cmt1f::writeData (const uint32_t length, const void* data) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - gotoWrite(); - m_writePos += fwrite(data, 1, length, m_handle); - - if (m_writePos > m_fileSize) - m_fileSize = m_writePos; - - return m_lastResult = XRV_OK; -} - -} // end of xsens namespace Deleted: code/player/trunk/server/drivers/imu/xsens/cmt1.h =================================================================== --- code/player/trunk/server/drivers/imu/xsens/cmt1.h 2009-07-31 11:27:28 UTC (rev 8161) +++ code/player/trunk/server/drivers/imu/xsens/cmt1.h 2009-07-31 13:00:23 UTC (rev 8162) @@ -1,366 +0,0 @@ -/*! \file - \brief Contains the CMT Level 1 interface - - This level contains OS-dependent implementations of Serial and File - communication, as well as some other functionality like 'sleep'. - - # The originator of this source code is Xsens Technologies B.V.. This source code is intended for use with Xsens Motion Trackers only. - - THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - PARTICULAR PURPOSE. -*/ -#ifndef _CMT1_H_2006_04_12 -#define _CMT1_H_2006_04_12 - -#ifndef _CMTDEF_H_2006_05_01 -# include "cmtdef.h" -#endif - -#ifdef _WIN32 -# include <windows.h> -//# include <sys/types.h> -#else -# include <termios.h> -// these are not required by level 1, but to keep the higher levels platform-independent they are put here -# include <stdlib.h> -# include <string.h> -# include <stddef.h> -#define _strnicmp strncasecmp -#endif - -#include <stdio.h> - -//! The namespace of all Xsens software since 2006. -namespace xsens { - -#ifndef CMTLOG -# if defined(_DEBUG) || defined(_LOG_ALWAYS) - void CMTLOG(const char *str, ...); -# define CMTEXITLOG(str) JanitorFunc2<const char*,XsensResultValue> _cmtExitLog(CMTLOG,str " returns %u",m_lastResult); -# else -# define CMTLOG(...) -# define CMTEXITLOG(...) -# endif -#endif - -#ifndef _WIN32 -int _wcsnicmp(const wchar_t* s1, const wchar_t* s2,int count); -#endif - -////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////// Cmt1s ///////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// - -/*! \brief The low-level serial communication class. -*/ -class Cmt1s { -private: - //! This object cannot be copied, so this function is not implemented. - Cmt1s(const Cmt1s& ref); - - #ifdef _LOG_RX_TX - FILE* rx_log; - FILE* tx_log; - #endif - - /*! \brief The bytes received function. - - This function is automatically called every time binary data is read from the - connected COM port. - */ - CmtCallbackFunction m_onBytesReceived; - //! Custom, user supplied parameter for the OnBytesReceived callback function, passed as the first argument - int32_t m_onBytesReceivedInstance; - //! Custom, user supplied parameter for the OnBytesReceived callback function, passed as the last argument - void* m_onBytesReceivedParam; -protected: - //! The baudrate that was last set to be used by the port - uint32_t m_baudrate; - //! The time at which an operation will end in ms, used by several functions. - uint32_t m_endTime; - //! Indicates if the port is open or not - bool m_isOpen; - //! The last result of an operation - mutable XsensResultValue m_lastResult; - //! The opened COM port nr - uint8_t m_port; - char m_portname[32]; - /*! The default timeout value to use during blocking operations. - A value of 0 means that all operations become non-blocking. - */ - uint32_t m_timeout; - - #ifdef _WIN32 - DCB m_commState; //!< Stored settings about the serial port - HANDLE m_handle; //!< The serial port handle - #else - termios m_commState; //!< Stored settings about the serial port - int32_t m_handle; //!< The serial port handle - typedef int32_t HANDLE; - #endif -public: - //! Default constructor, initializes all members to their default values. - Cmt1s(); - //! Destructor, de-initializes, frees memory allocated for buffers, etc. - ~Cmt1s(); - //! \brief Close the serial communication port. - XsensResultValue close (void); - /*! \brief Manipulate the Serial control lines - - The function manipulates the serial control lines that are indicated by the - mask parameter. Note that only the DTR and RTS lines can be set by win32. - \param mask Indicates which lines are to be manipulated and which should be - left alone. - \param state Contains the new state of the control lines. - */ - XsensResultValue escape (const CmtControlLine mask, const CmtControlLine state); - /*! \brief Flush all data to be transmitted / received. - - This function tries to send and receive any remaining data immediately - and does not return until the buffers are empty. - */ - XsensResultValue flushData (void); - //! Return the baudrate that is currently being used by the port - uint32_t getBaudrate(void) const { return m_baudrate; } - //! Return the handle of the port - HANDLE getHandle(void) const { return m_handle; } - //! Retrieve the port number that was last successfully opened. - uint8_t getPortNr (void) const { return m_port; } - //! Retrieve the port name that was last successfully opened. - void getPortName(char *portname) const { sprintf(portname, "%s", m_portname); } - //! Return the error code of the last operation. - XsensResultValue getLastResult(void) const { return m_lastResult; } - //! Return the current timeout value - uint32_t getTimeout (void) const { return m_timeout; } - //! Return whether the communication port is open or not. - bool isOpen (void) const { return m_isOpen; } - - /*! \brief Open a communcation channel to the given serial port name. - - The port is automatically initialized to the given baudrate. - If the baudrate is set to 0, the baud rate is automatically detected. If possible. - */ - XsensResultValue open ( const char *portName, - const uint32_t baudRate = CMT_DEFAULT_BAUD_RATE, - uint32_t readBufSize = CMT_DEFAULT_READ_BUFFER_SIZE, - uint32_t writeBufSize = CMT_DEFAULT_WRITE_BUFFER_SIZE); - -#ifdef _WIN32 - /*! \brief Open a communication channel to the given COM port number. - - The port is automatically initialized to the given baud rate. - If the baudrate is set to 0, the baud rate is automatically detected. If possible. - */ - XsensResultValue open ( const uint32_t portNumber, - const uint32_t baudRate = CMT_DEFAULT_BAUD_RATE, - uint32_t readBufSize = CMT_DEFAULT_READ_BUFFER_SIZE, - uint32_t writeBufSize = CMT_DEFAULT_WRITE_BUFFER_SIZE); -#endif - /*! \brief Read data from the serial port and put it into the data buffer. - - This function reads as much data as possible from the com port (non-blocking) and - put as much data as will fit into the data buffer. Any excess data is stored in - the \c m_readBuffer member variable. If there was enough data in m_readBuffer to - fulfill the request, the data parameter is first filled and the port is polled - afterwards. - \param maxLength The maximum amount of data read. - \param data Pointer to a buffer that will store the received data. - \param length The number of bytes placed into \c data. - */ - XsensResultValue readData (const uint32_t maxLength, uint8_t* data, - uint32_t* length = NULL); - //! Set the callback function for when bytes have been received - XsensResultValue setCallbackFunction(CmtCallbackSelector tp, int32_t instance, CmtCallbackFunction func, void* param); - /*! \brief Set the default timeout value to use in blocking operations. - - This function sets the value of m_timeout. There is no infinity value. The value 0 - means that all blocking operations now become polling (non-blocking) operations. - If the value is set to or from 0, the low-level serial port settings may be - changed in addition to the m_timeout value. - */ - XsensResultValue setTimeout (const uint32_t ms = CMT1_DEFAULT_TIMEOUT); - /*! \brief Wait for data to arrive or a timeout to occur. - - The function waits until \c maxLength data is available or until a timeout occurs. - The function returns success if data is available or XsensResultValue::TIMEOUT if a - timeout occurred. A timeout value of 0 indicates that the default timeout stored - in the class should be used. - */ - XsensResultValue waitForData (const uint32_t maxLength, uint8_t* data, - uint32_t* length = NULL); - /*! \brief Write the data to the serial port. - - The function writes the given data to the connected COM port. - The default timeout is respected in this operation. - */ - XsensResultValue writeData (const uint32_t length, const uint8_t* data, - uint32_t* written); - -}; - -////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////// Cmt1f ///////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// - -/*! \brief The low-level file communication class. -*/ -class Cmt1f { -private: - //! This object cannot be copied, so this function is not implemented. - Cmt1f(const Cmt1f& ref); - -protected: - //! The file handle - FILE* m_handle; - //! Contains the size of the file - CmtFilePos m_fileSize; - //! The last read position in the file - CmtFilePos m_readPos; - //! The last write position in the file - CmtFilePos m_writePos; - //! The last result of an operation - mutable XsensResultValue m_lastResult; - //! Contains the name of the file that was last successfully opened. - char m_filename[CMT_MAX_FILENAME_LENGTH]; - //! Contains the name of the file that was last successfully opened using unicode. - wchar_t m_filename_w[CMT_MAX_FILENAME_LENGTH]; - //! Indicates if the file is open or not. - bool m_isOpen; - //! Indicates if we're using the unicode filename or the regular filename - bool m_unicode; - /*! \brief Indicates whether the last operation was a read or write operation. - - This value is used to check whether or not a seek is required to perform a - requested read or write operation. - */ - bool m_reading; - //! Indicates if the file was opened in read-only mode - bool m_readOnly; - //! Change from writing to reading mode - void gotoRead(void); - //! Change from reading to writing mode - void gotoWrite(void); -public: - //! Default constructor, initializes all members to their default values. - Cmt1f(); - //! Destructor. - ~Cmt1f(); - /*! \brief Write data to the end of the file. - - The function writes the given data to the file at the end. The current write - position is also moved to the end of the file. - */ - XsensResultValue appendData(const uint32_t length, const void* data); - //! Close the file. - XsensResultValue close(void); - //! Close the file and delete it. - XsensResultValue closeAndDelete(void); - //! Open an empty file. - XsensResultValue create(const char* filename); - //! Open an empty file using a unicode path + filename. - XsensResultValue create(const wchar_t* filename); - /*! \brief Delete the given data from the file. - - The function erases the given data from the file at the given write position. This - operation may take a while to complete, but is faster than insertData. - - The write position is not changed and the read position is checked for validity. - */ - XsensResultValue deleteData(const CmtFilePos start, const uint32_t length); - /*! \brief Find a string of bytes in the file - - The function searches from the current read position until the given \c needle is - found. If the needle is not found, XsensResultValue::NOT_FOUND is returned. The function - will update the seek position to the first character of the found needle. - \param needle The byte string to find. - \param needleLength The length of the byte string. - \param pos Out: The position where the needle was found. This will point - to the first character of the found needle. - */ - XsensResultValue find(const void* needle, const uint32_t needleLength, CmtFilePos& pos); - /*! \brief Flush all data to be written. - This function writes any remaining data immediately and does not return - until this is done. - */ - XsensResultValue flushData(void); - //! Return the size of the file. - CmtFilePos getFileSize(void) const { return m_fileSize; } - //! Return the result code of the last operation. - XsensResultValue getLastResult(void) const { return m_lastResult; } - /*! \brief Retrieve the filename that was last successfully opened. - - \param filename A buffer for storing the filename. The buffer should be able - to hold the filename. A safe size is to make it at least 256 bytes. - */ - XsensResultValue getName(char* filename) const; - /*! \brief Retrieve the filename that was last successfully opened in unicode. - - \param filename A buffer for storing the filename. The buffer should be able - to hold the filename. A safe size is to make it at least 256 wide characters. - */ - XsensResultValue getName(wchar_t* filename) const; - //! Return the current read position. - CmtFilePos getReadPos(void) const { return m_readPos; } - //! Return the current write position. - CmtFilePos getWritePos(void) const { return m_writePos; } - /*! \brief Insert the given data into the file. - - The function writes the given data to the file at the current write position. This - operation may take a while to complete. - - The write position is placed at the end of the inserted data. - */ - XsensResultValue insertData(const CmtFilePos start, const uint32_t length, - const void* data); - //! Return whether the file is open or not. - bool isOpen(void) const { return m_isOpen; } - //! Return whether the file is readonly or not. - bool isReadOnly(void) const { return !m_isOpen || m_readOnly; } - //! Open a file. - XsensResultValue open(const char* filename, const bool create, const bool readOnly); - //! Open a file using a unicode filename. - XsensResultValue open(const wchar_t* filename, const bool create, const bool readOnly); - /*! \brief Read data from the file and put it into the data buffer. - - This function reads exactly the number of bytes as requested from the file. - \param maxLength The amount of data that will be read. - \param data Pointer to a buffer that will store the read data. - \param length pointer to a variable that will store the number of bytes - that were actually read. The parameter may be NULL. - */ - XsensResultValue readData(const uint32_t maxLength, void* data, uint32_t* length); - /*! \brief Read data from the file and put it into the data buffer. - - This function reads upp to the number of bytes as requested from the file. - The function will also stop if the given terminator character is encountered. - The terminator is included in the output buffer. - \param maxLength The amount of data that will be read. - \param data Pointer to a buffer that will store the read data. - \param terminator A character that will end the read operation if encountered. - \param length The actual number of bytes read, including the terminator - character, if encountered. - */ - XsensResultValue readData(const uint32_t maxLength, const char terminator, void* data, uint32_t* length); - /*! \brief Set the new absolute read position - - The read position is checked against the filesize before committing. - */ - XsensResultValue setReadPos(const CmtFilePos pos); - /*! \brief Set the new absolute write position - - The write position is checked against the filesize before committing. - */ - XsensResultValue setWritePos(const CmtFilePos pos = -1); - - /*! \brief Write data to the file. - - The function writes the given data to the file at the current write position. - */ - XsensResultValue writeData(const uint32_t length, const void* data); -}; - -} // end of xsens namespace - -#endif // _CMT1_H_2006_04_12 Deleted: code/player/trunk/server/drivers/imu/xsens/cmt2.cpp =================================================================== --- code/player/trunk/server/drivers/imu/xsens/cmt2.cpp 2009-07-31 11:27:28 UTC (rev 8161) +++ code/player/trunk/server/drivers/imu/xsens/cmt2.cpp 2009-07-31 13:00:23 UTC (rev 8162) @@ -1,822 +0,0 @@ -/*! \file Cmt2.cpp - - For information about objects in this file, see the appropriate header: - \ref Cmt2.h - - # The originator of this source code is Xsens Technologies B.V.. This source code is intended for use with Xsens Motion Trackers only. - - THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - PARTICULAR PURPOSE. - - \section FileChangelog Changelog - \par 2006-04-05, v0.0.1 - \li Job Mulder: Created - \par 2006-07-21, v0.1.0 - \li Job Mulder: Updated file for release 0.1.0 -*/ - -#include "cmt2.h" - -namespace xsens { - -#ifdef _LOG_CMT2 -# define CMT2LOG CMTLOG -#else -# define CMT2LOG(...) -#endif - -////////////////////////////////////////////////////////////////////////////////////////// -int32_t findValidMessage(const uint8_t* buffer, const uint16_t bufferLength) -{ - MessageHeader* hdr = NULL; - uint16_t pre = 0; - uint16_t target; - bool extended; - uint16_t length; - int32_t res; - Message* msg; - - // find the preamble - while(pre < bufferLength && (buffer[pre] != CMT_PREAMBLE)) - ++pre; - - if (pre >= bufferLength) - return -1; - - // check if the message is valid - length = bufferLength-pre; - if (length < CMT_LEN_MSGHEADERCS) - return -1; - - // parse header - - hdr = (MessageHeader*) (buffer + pre); - if (hdr->m_length == CMT_EXTLENCODE) - { - extended = true; - if (length < CMT_LEN_MSGEXTHEADERCS) - return -1; - } - else - extended = false; - - // check the reported size - target = extended? - ((uint16_t) hdr->m_datlen.m_extended.m_length.m_high * 256 + (uint16_t) hdr->m_datlen.m_extended.m_length.m_low) - + CMT_LEN_MSGEXTHEADERCS: - (uint16_t) (hdr->m_length) + CMT_LEN_MSGHEADERCS; - if (target <= length) - { - // header seems to be ok, check checksum - // check the checksum - - msg=new Message(buffer+pre,(uint16_t) target,(uint16_t) target); - if (msg->isChecksumOk()) - { - delete msg; - return (int32_t) pre; - } - delete msg; - } - - // try next message - res = findValidMessage(buffer+pre+1,length-1); - if (res == -1) - return -1; - return res + pre + 1; -} - -////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////// Cmt2s ///////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////////////////// -// Default constructor, initializes all members to their default values. -Cmt2s::Cmt2s() : - m_onMessageReceived(NULL), - m_onMessageSent(NULL) -{ - m_lastResult = XRV_OK; - m_readBufferCount = 0; - m_timeout = CMT2_DEFAULT_TIMEOUT; - m_baudrate = CMT_DEFAULT_BAUD_RATE; - m_toEnd = 0; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Destructor, de-initializes, frees memory allocated for buffers, etc. -Cmt2s::~Cmt2s() -{ -} - - -XsensResultValue Cmt2s::close(void) -{ - if (m_cmt1s.getPortNr()) - { - CMT2LOG("L2: Closing port %d\n",(int32_t)m_cmt1s.getPortNr()); - m_toEnd = 0; - return m_lastResult = m_cmt1s.close(); - } - else - return m_lastResult = XRV_NOPORTOPEN; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Retrieve the port that the object is connected to. -XsensResultValue Cmt2s::getPortName(char *portname) const -{ - m_cmt1s.getPortName(portname); - // we know there should be at least "/dev/x" - if (strlen(portname) < 6) - return m_lastResult = XRV_ERROR; - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Retrieve the port that the object is connected to. -XsensResultValue Cmt2s::getPortNr(uint8_t& port) const -{ - port = m_cmt1s.getPortNr(); - if (port == 0) - return m_lastResult = XRV_ERROR; - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Retrieve the port that the object is connected to. -XsensResultValue Cmt2s::getPortNr(int32_t& port) const -{ - port = m_cmt1s.getPortNr(); - if (port == 0) - return m_lastResult = XRV_ERROR; - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Open a communication channel to the given serial port name. -XsensResultValue Cmt2s::open (const char *portName, const uint32_t baudRate) -{ - CMT2LOG("L2: Opening port %s @baud %d\n", portName, baudRate); - m_baudrate = baudRate; - m_lastResult = m_cmt1s.open(portName, baudRate); - m_toEnd = 0; - CMT2LOG("L2: Port open result %d: %s\n", (int32_t) m_lastResult, xsensResultText(m_lastResult)); - return m_lastResult; -} - -#ifdef _WIN32 -////////////////////////////////////////////////////////////////////////////////////////// -// Open a communication channel to the given COM port number. -XsensResultValue Cmt2s::open (const uint32_t portNumber, const uint32_t baudRate) -{ - CMT2LOG("L2: Opening port %d @baud %d\n",(int32_t)portNumber,baudRate); - m_baudrate = baudRate; - m_lastResult = m_cmt1s.open(portNumber,baudRate); - m_toEnd = 0; - CMT2LOG("L2: Port open result %d: %s\n",(int32_t)m_lastResult,xsensResultText(m_lastResult)); - return m_lastResult; -} -#endif - -////////////////////////////////////////////////////////////////////////////////////////// -// Read a message from the COM port. -XsensResultValue Cmt2s::readMessage(Message* rcv) -{ - MessageHeader* hdr = (MessageHeader*) m_readBuffer; - uint16_t pre = 0; - uint32_t length = 0; - uint32_t target; - uint16_t i; - bool extended; - - CMT2LOG("L2: readMessage started, bufferCount=%u\n",m_readBufferCount); - - if (m_readBufferCount == 0) - m_readBuffer[0] = (uint8_... [truncated message content] |
From: <th...@us...> - 2009-07-31 11:27:41
|
Revision: 8161 http://playerstage.svn.sourceforge.net/playerstage/?rev=8161&view=rev Author: thjc Date: 2009-07-31 11:27:28 +0000 (Fri, 31 Jul 2009) Log Message: ----------- copy shapetracker header from driver to other files included with it Modified Paths: -------------- code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.c code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.h Modified: code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.c =================================================================== --- code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.c 2009-07-31 11:24:14 UTC (rev 8160) +++ code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.c 2009-07-31 11:27:28 UTC (rev 8161) @@ -1,4 +1,25 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey et al + * ge...@us... + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + #include "orientation.h" #include <stdio.h> Modified: code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.h =================================================================== --- code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.h 2009-07-31 11:24:14 UTC (rev 8160) +++ code/player/trunk/server/drivers/blobfinder/shapetracker/orientation.h 2009-07-31 11:27:28 UTC (rev 8161) @@ -1,3 +1,24 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey et al + * ge...@us... + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + /****************************************************** * orientation - header * This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-07-31 11:24:21
|
Revision: 8160 http://playerstage.svn.sourceforge.net/playerstage/?rev=8160&view=rev Author: thjc Date: 2009-07-31 11:24:14 +0000 (Fri, 31 Jul 2009) Log Message: ----------- Add missing license headers for mricp after communication with the authors. Modified Paths: -------------- code/player/trunk/server/drivers/mixed/mricp/include/Timer.h code/player/trunk/server/drivers/mixed/mricp/include/geometry2D.h code/player/trunk/server/drivers/mixed/mricp/include/icp.h code/player/trunk/server/drivers/mixed/mricp/include/lasermodel.h code/player/trunk/server/drivers/mixed/mricp/include/nn.h code/player/trunk/server/drivers/mixed/mricp/src/Timer.cpp code/player/trunk/server/drivers/mixed/mricp/src/geometry2D.cpp code/player/trunk/server/drivers/mixed/mricp/src/icp.cpp code/player/trunk/server/drivers/mixed/mricp/src/lasermodel.cpp code/player/trunk/server/drivers/mixed/mricp/src/nn.cpp Modified: code/player/trunk/server/drivers/mixed/mricp/include/Timer.h =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/include/Timer.h 2009-07-30 10:31:19 UTC (rev 8159) +++ code/player/trunk/server/drivers/mixed/mricp/include/Timer.h 2009-07-31 11:24:14 UTC (rev 8160) @@ -1,3 +1,23 @@ +/*************************************************************************** + * Copyright (C) 2005 by Tarek Taha * + * ta...@en... * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, write to the * + * Free Software Foundation, Inc., * + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * + ***************************************************************************/ + #ifndef TIMER_H_ #define TIMER_H_ #include <sys/time.h> Modified: code/player/trunk/server/drivers/mixed/mricp/include/geometry2D.h =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/include/geometry2D.h 2009-07-30 10:31:19 UTC (rev 8159) +++ code/player/trunk/server/drivers/mixed/mricp/include/geometry2D.h 2009-07-31 11:24:14 UTC (rev 8160) @@ -1,3 +1,32 @@ +/* +Copyright (c) 2004, Tim Bailey +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of the Player Project nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + + /* Simple 2D geometric operations with points, poses, and lines. * * These algorithms were worked out by me from first principles several years Modified: code/player/trunk/server/drivers/mixed/mricp/include/icp.h =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/include/icp.h 2009-07-30 10:31:19 UTC (rev 8159) +++ code/player/trunk/server/drivers/mixed/mricp/include/icp.h 2009-07-31 11:24:14 UTC (rev 8160) @@ -1,3 +1,31 @@ +/* +Copyright (c) 2004, Tim Bailey +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of the Player Project nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + /* Iterated closest point (ICP) algorithm. * * This is a very simple implementation of ICP, with one simple extension where the Modified: code/player/trunk/server/drivers/mixed/mricp/include/lasermodel.h =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/include/lasermodel.h 2009-07-30 10:31:19 UTC (rev 8159) +++ code/player/trunk/server/drivers/mixed/mricp/include/lasermodel.h 2009-07-31 11:24:14 UTC (rev 8160) @@ -1,3 +1,23 @@ +/*************************************************************************** + * Copyright (C) 2005 by Tarek Taha * + * ta...@en... * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, write to the * + * Free Software Foundation, Inc., * + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * + ***************************************************************************/ + #ifndef LASERMODEL_H_ #define LASERMODEL_H_ // Info for a single range measurement Modified: code/player/trunk/server/drivers/mixed/mricp/include/nn.h =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/include/nn.h 2009-07-30 10:31:19 UTC (rev 8159) +++ code/player/trunk/server/drivers/mixed/mricp/include/nn.h 2009-07-31 11:24:14 UTC (rev 8160) @@ -1,3 +1,32 @@ +/* +Copyright (c) 2004, Tim Bailey +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of the Player Project nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + + /* Nearest-neighbours algorithm for 2D point-sets. * Compute the single nearest-neighbour of a point, or the set of k-nearest neighbours. * Modified: code/player/trunk/server/drivers/mixed/mricp/src/Timer.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/src/Timer.cpp 2009-07-30 10:31:19 UTC (rev 8159) +++ code/player/trunk/server/drivers/mixed/mricp/src/Timer.cpp 2009-07-31 11:24:14 UTC (rev 8160) @@ -1,3 +1,23 @@ +/*************************************************************************** + * Copyright (C) 2005 by Tarek Taha * + * ta...@en... * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, write to the * + * Free Software Foundation, Inc., * + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * + ***************************************************************************/ + #include "Timer.h" #include <ctime> #include <cstddef> Modified: code/player/trunk/server/drivers/mixed/mricp/src/geometry2D.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/src/geometry2D.cpp 2009-07-30 10:31:19 UTC (rev 8159) +++ code/player/trunk/server/drivers/mixed/mricp/src/geometry2D.cpp 2009-07-31 11:24:14 UTC (rev 8160) @@ -1,3 +1,32 @@ +/* +Copyright (c) 2004, Tim Bailey +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of the Player Project nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + + /* Simple 2D geometric operations with points, poses, and lines. * Tim Bailey 2004. */ Modified: code/player/trunk/server/drivers/mixed/mricp/src/icp.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/src/icp.cpp 2009-07-30 10:31:19 UTC (rev 8159) +++ code/player/trunk/server/drivers/mixed/mricp/src/icp.cpp 2009-07-31 11:24:14 UTC (rev 8160) @@ -1,3 +1,31 @@ +/* +Copyright (c) 2004, Tim Bailey +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of the Player Project nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + /* Iterated closest point (ICP) algorithm. * Tim Bailey 2004. */ Modified: code/player/trunk/server/drivers/mixed/mricp/src/lasermodel.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/src/lasermodel.cpp 2009-07-30 10:31:19 UTC (rev 8159) +++ code/player/trunk/server/drivers/mixed/mricp/src/lasermodel.cpp 2009-07-31 11:24:14 UTC (rev 8160) @@ -1,3 +1,23 @@ +/*************************************************************************** + * Copyright (C) 2005 by Tarek Taha * + * ta...@en... * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, write to the * + * Free Software Foundation, Inc., * + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * + ***************************************************************************/ + #include <assert.h> #include <math.h> #include <stdlib.h> Modified: code/player/trunk/server/drivers/mixed/mricp/src/nn.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/mricp/src/nn.cpp 2009-07-30 10:31:19 UTC (rev 8159) +++ code/player/trunk/server/drivers/mixed/mricp/src/nn.cpp 2009-07-31 11:24:14 UTC (rev 8160) @@ -1,3 +1,32 @@ +/* +Copyright (c) 2004, Tim Bailey +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of the Player Project nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + + /* K-nearest-neighbours for 2D point-sets. * Tim Bailey 2004. */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-07-30 10:31:33
|
Revision: 8159 http://playerstage.svn.sourceforge.net/playerstage/?rev=8159&view=rev Author: thjc Date: 2009-07-30 10:31:19 +0000 (Thu, 30 Jul 2009) Log Message: ----------- relicensed epuck driver as GPL v2 with permission from the author. Licensed the epuck example under BSD license Modified Paths: -------------- code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/blinkenlight.c code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/camera.c code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/ir.c code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/position2d.c code/player/trunk/server/drivers/mixed/epuck/epuckCamera.cpp code/player/trunk/server/drivers/mixed/epuck/epuckCamera.hpp code/player/trunk/server/drivers/mixed/epuck/epuckDriver.cpp code/player/trunk/server/drivers/mixed/epuck/epuckDriver.hpp code/player/trunk/server/drivers/mixed/epuck/epuckIR.cpp code/player/trunk/server/drivers/mixed/epuck/epuckIR.hpp code/player/trunk/server/drivers/mixed/epuck/epuckInterface.cpp code/player/trunk/server/drivers/mixed/epuck/epuckInterface.hpp code/player/trunk/server/drivers/mixed/epuck/epuckLEDs.cpp code/player/trunk/server/drivers/mixed/epuck/epuckLEDs.hpp code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.cpp code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.hpp code/player/trunk/server/drivers/mixed/epuck/serialPort.cpp code/player/trunk/server/drivers/mixed/epuck/serialPort.hpp code/player/trunk/server/drivers/mixed/epuck/timer.hpp Removed Paths: ------------- code/player/trunk/server/drivers/mixed/epuck/COPYING Deleted: code/player/trunk/server/drivers/mixed/epuck/COPYING =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/COPYING 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/COPYING 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,674 +0,0 @@ - GNU GENERAL PUBLIC LICENSE - Version 3, 29 June 2007 - - Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/> - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The GNU General Public License is a free, copyleft license for -software and other kinds of works. - - The licenses for most software and other practical works are designed -to take away your freedom to share and change the works. By contrast, -the GNU General Public License is intended to guarantee your freedom to -share and change all versions of a program--to make sure it remains free -software for all its users. We, the Free Software Foundation, use the -GNU General Public License for most of our software; it applies also to -any other work released this way by its authors. You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -them if you wish), that you receive source code or can get it if you -want it, that you can change the software or use pieces of it in new -free programs, and that you know you can do these things. - - To protect your rights, we need to prevent others from denying you -these rights or asking you to surrender the rights. Therefore, you have -certain responsibilities if you distribute copies of the software, or if -you modify it: responsibilities to respect the freedom of others. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must pass on to the recipients the same -freedoms that you received. You must make sure that they, too, receive -or can get the source code. And you must show them these terms so they -know their rights. - - Developers that use the GNU GPL protect your rights with two steps: -(1) assert copyright on the software, and (2) offer you this License -giving you legal permission to copy, distribute and/or modify it. - - For the developers' and authors' protection, the GPL clearly explains -that there is no warranty for this free software. For both users' and -authors' sake, the GPL requires that modified versions be marked as -changed, so that their problems will not be attributed erroneously to -authors of previous versions. - - Some devices are designed to deny users access to install or run -modified versions of the software inside them, although the manufacturer -can do so. This is fundamentally incompatible with the aim of -protecting users' freedom to change the software. The systematic -pattern of such abuse occurs in the area of products for individuals to -use, which is precisely where it is most unacceptable. Therefore, we -have designed this version of the GPL to prohibit the practice for those -products. If such problems arise substantially in other domains, we -stand ready to extend this provision to those domains in future versions -of the GPL, as needed to protect the freedom of users. - - Finally, every program is threatened constantly by software patents. -States should not allow patents to restrict development and use of -software on general-purpose computers, but in those that do, we wish to -avoid the special danger that patents applied to a free program could -make it effectively proprietary. To prevent this, the GPL assures that -patents cannot be used to render the program non-free. - - The precise terms and conditions for copying, distribution and -modification follow. - - TERMS AND CONDITIONS - - 0. Definitions. - - "This License" refers to version 3 of the GNU General Public License. - - "Copyright" also means copyright-like laws that apply to other kinds of -works, such as semiconductor masks. - - "The Program" refers to any copyrightable work licensed under this -License. Each licensee is addressed as "you". "Licensees" and -"recipients" may be individuals or organizations. - - To "modify" a work means to copy from or adapt all or part of the work -in a fashion requiring copyright permission, other than the making of an -exact copy. The resulting work is called a "modified version" of the -earlier work or a work "based on" the earlier work. - - A "covered work" means either the unmodified Program or a work based -on the Program. - - To "propagate" a work means to do anything with it that, without -permission, would make you directly or secondarily liable for -infringement under applicable copyright law, except executing it on a -computer or modifying a private copy. Propagation includes copying, -distribution (with or without modification), making available to the -public, and in some countries other activities as well. - - To "convey" a work means any kind of propagation that enables other -parties to make or receive copies. Mere interaction with a user through -a computer network, with no transfer of a copy, is not conveying. - - An interactive user interface displays "Appropriate Legal Notices" -to the extent that it includes a convenient and prominently visible -feature that (1) displays an appropriate copyright notice, and (2) -tells the user that there is no warranty for the work (except to the -extent that warranties are provided), that licensees may convey the -work under this License, and how to view a copy of this License. If -the interface presents a list of user commands or options, such as a -menu, a prominent item in the list meets this criterion. - - 1. Source Code. - - The "source code" for a work means the preferred form of the work -for making modifications to it. "Object code" means any non-source -form of a work. - - A "Standard Interface" means an interface that either is an official -standard defined by a recognized standards body, or, in the case of -interfaces specified for a particular programming language, one that -is widely used among developers working in that language. - - The "System Libraries" of an executable work include anything, other -than the work as a whole, that (a) is included in the normal form of -packaging a Major Component, but which is not part of that Major -Component, and (b) serves only to enable use of the work with that -Major Component, or to implement a Standard Interface for which an -implementation is available to the public in source code form. A -"Major Component", in this context, means a major essential component -(kernel, window system, and so on) of the specific operating system -(if any) on which the executable work runs, or a compiler used to -produce the work, or an object code interpreter used to run it. - - The "Corresponding Source" for a work in object code form means all -the source code needed to generate, install, and (for an executable -work) run the object code and to modify the work, including scripts to -control those activities. However, it does not include the work's -System Libraries, or general-purpose tools or generally available free -programs which are used unmodified in performing those activities but -which are not part of the work. For example, Corresponding Source -includes interface definition files associated with source files for -the work, and the source code for shared libraries and dynamically -linked subprograms that the work is specifically designed to require, -such as by intimate data communication or control flow between those -subprograms and other parts of the work. - - The Corresponding Source need not include anything that users -can regenerate automatically from other parts of the Corresponding -Source. - - The Corresponding Source for a work in source code form is that -same work. - - 2. Basic Permissions. - - All rights granted under this License are granted for the term of -copyright on the Program, and are irrevocable provided the stated -conditions are met. This License explicitly affirms your unlimited -permission to run the unmodified Program. The output from running a -covered work is covered by this License only if the output, given its -content, constitutes a covered work. This License acknowledges your -rights of fair use or other equivalent, as provided by copyright law. - - You may make, run and propagate covered works that you do not -convey, without conditions so long as your license otherwise remains -in force. You may convey covered works to others for the sole purpose -of having them make modifications exclusively for you, or provide you -with facilities for running those works, provided that you comply with -the terms of this License in conveying all material for which you do -not control copyright. Those thus making or running the covered works -for you must do so exclusively on your behalf, under your direction -and control, on terms that prohibit them from making any copies of -your copyrighted material outside their relationship with you. - - Conveying under any other circumstances is permitted solely under -the conditions stated below. Sublicensing is not allowed; section 10 -makes it unnecessary. - - 3. Protecting Users' Legal Rights From Anti-Circumvention Law. - - No covered work shall be deemed part of an effective technological -measure under any applicable law fulfilling obligations under article -11 of the WIPO copyright treaty adopted on 20 December 1996, or -similar laws prohibiting or restricting circumvention of such -measures. - - When you convey a covered work, you waive any legal power to forbid -circumvention of technological measures to the extent such circumvention -is effected by exercising rights under this License with respect to -the covered work, and you disclaim any intention to limit operation or -modification of the work as a means of enforcing, against the work's -users, your or third parties' legal rights to forbid circumvention of -technological measures. - - 4. Conveying Verbatim Copies. - - You may convey verbatim copies of the Program's source code as you -receive it, in any medium, provided that you conspicuously and -appropriately publish on each copy an appropriate copyright notice; -keep intact all notices stating that this License and any -non-permissive terms added in accord with section 7 apply to the code; -keep intact all notices of the absence of any warranty; and give all -recipients a copy of this License along with the Program. - - You may charge any price or no price for each copy that you convey, -and you may offer support or warranty protection for a fee. - - 5. Conveying Modified Source Versions. - - You may convey a work based on the Program, or the modifications to -produce it from the Program, in the form of source code under the -terms of section 4, provided that you also meet all of these conditions: - - a) The work must carry prominent notices stating that you modified - it, and giving a relevant date. - - b) The work must carry prominent notices stating that it is - released under this License and any conditions added under section - 7. This requirement modifies the requirement in section 4 to - "keep intact all notices". - - c) You must license the entire work, as a whole, under this - License to anyone who comes into possession of a copy. This - License will therefore apply, along with any applicable section 7 - additional terms, to the whole of the work, and all its parts, - regardless of how they are packaged. This License gives no - permission to license the work in any other way, but it does not - invalidate such permission if you have separately received it. - - d) If the work has interactive user interfaces, each must display - Appropriate Legal Notices; however, if the Program has interactive - interfaces that do not display Appropriate Legal Notices, your - work need not make them do so. - - A compilation of a covered work with other separate and independent -works, which are not by their nature extensions of the covered work, -and which are not combined with it such as to form a larger program, -in or on a volume of a storage or distribution medium, is called an -"aggregate" if the compilation and its resulting copyright are not -used to limit the access or legal rights of the compilation's users -beyond what the individual works permit. Inclusion of a covered work -in an aggregate does not cause this License to apply to the other -parts of the aggregate. - - 6. Conveying Non-Source Forms. - - You may convey a covered work in object code form under the terms -of sections 4 and 5, provided that you also convey the -machine-readable Corresponding Source under the terms of this License, -in one of these ways: - - a) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by the - Corresponding Source fixed on a durable physical medium - customarily used for software interchange. - - b) Convey the object code in, or embodied in, a physical product - (including a physical distribution medium), accompanied by a - written offer, valid for at least three years and valid for as - long as you offer spare parts or customer support for that product - model, to give anyone who possesses the object code either (1) a - copy of the Corresponding Source for all the software in the - product that is covered by this License, on a durable physical - medium customarily used for software interchange, for a price no - more than your reasonable cost of physically performing this - conveying of source, or (2) access to copy the - Corresponding Source from a network server at no charge. - - c) Convey individual copies of the object code with a copy of the - written offer to provide the Corresponding Source. This - alternative is allowed only occasionally and noncommercially, and - only if you received the object code with such an offer, in accord - with subsection 6b. - - d) Convey the object code by offering access from a designated - place (gratis or for a charge), and offer equivalent access to the - Corresponding Source in the same way through the same place at no - further charge. You need not require recipients to copy the - Corresponding Source along with the object code. If the place to - copy the object code is a network server, the Corresponding Source - may be on a different server (operated by you or a third party) - that supports equivalent copying facilities, provided you maintain - clear directions next to the object code saying where to find the - Corresponding Source. Regardless of what server hosts the - Corresponding Source, you remain obligated to ensure that it is - available for as long as needed to satisfy these requirements. - - e) Convey the object code using peer-to-peer transmission, provided - you inform other peers where the object code and Corresponding - Source of the work are being offered to the general public at no - charge under subsection 6d. - - A separable portion of the object code, whose source code is excluded -from the Corresponding Source as a System Library, need not be -included in conveying the object code work. - - A "User Product" is either (1) a "consumer product", which means any -tangible personal property which is normally used for personal, family, -or household purposes, or (2) anything designed or sold for incorporation -into a dwelling. In determining whether a product is a consumer product, -doubtful cases shall be resolved in favor of coverage. For a particular -product received by a particular user, "normally used" refers to a -typical or common use of that class of product, regardless of the status -of the particular user or of the way in which the particular user -actually uses, or expects or is expected to use, the product. A product -is a consumer product regardless of whether the product has substantial -commercial, industrial or non-consumer uses, unless such uses represent -the only significant mode of use of the product. - - "Installation Information" for a User Product means any methods, -procedures, authorization keys, or other information required to install -and execute modified versions of a covered work in that User Product from -a modified version of its Corresponding Source. The information must -suffice to ensure that the continued functioning of the modified object -code is in no case prevented or interfered with solely because -modification has been made. - - If you convey an object code work under this section in, or with, or -specifically for use in, a User Product, and the conveying occurs as -part of a transaction in which the right of possession and use of the -User Product is transferred to the recipient in perpetuity or for a -fixed term (regardless of how the transaction is characterized), the -Corresponding Source conveyed under this section must be accompanied -by the Installation Information. But this requirement does not apply -if neither you nor any third party retains the ability to install -modified object code on the User Product (for example, the work has -been installed in ROM). - - The requirement to provide Installation Information does not include a -requirement to continue to provide support service, warranty, or updates -for a work that has been modified or installed by the recipient, or for -the User Product in which it has been modified or installed. Access to a -network may be denied when the modification itself materially and -adversely affects the operation of the network or violates the rules and -protocols for communication across the network. - - Corresponding Source conveyed, and Installation Information provided, -in accord with this section must be in a format that is publicly -documented (and with an implementation available to the public in -source code form), and must require no special password or key for -unpacking, reading or copying. - - 7. Additional Terms. - - "Additional permissions" are terms that supplement the terms of this -License by making exceptions from one or more of its conditions. -Additional permissions that are applicable to the entire Program shall -be treated as though they were included in this License, to the extent -that they are valid under applicable law. If additional permissions -apply only to part of the Program, that part may be used separately -under those permissions, but the entire Program remains governed by -this License without regard to the additional permissions. - - When you convey a copy of a covered work, you may at your option -remove any additional permissions from that copy, or from any part of -it. (Additional permissions may be written to require their own -removal in certain cases when you modify the work.) You may place -additional permissions on material, added by you to a covered work, -for which you have or can give appropriate copyright permission. - - Notwithstanding any other provision of this License, for material you -add to a covered work, you may (if authorized by the copyright holders of -that material) supplement the terms of this License with terms: - - a) Disclaiming warranty or limiting liability differently from the - terms of sections 15 and 16 of this License; or - - b) Requiring preservation of specified reasonable legal notices or - author attributions in that material or in the Appropriate Legal - Notices displayed by works containing it; or - - c) Prohibiting misrepresentation of the origin of that material, or - requiring that modified versions of such material be marked in - reasonable ways as different from the original version; or - - d) Limiting the use for publicity purposes of names of licensors or - authors of the material; or - - e) Declining to grant rights under trademark law for use of some - trade names, trademarks, or service marks; or - - f) Requiring indemnification of licensors and authors of that - material by anyone who conveys the material (or modified versions of - it) with contractual assumptions of liability to the recipient, for - any liability that these contractual assumptions directly impose on - those licensors and authors. - - All other non-permissive additional terms are considered "further -restrictions" within the meaning of section 10. If the Program as you -received it, or any part of it, contains a notice stating that it is -governed by this License along with a term that is a further -restriction, you may remove that term. If a license document contains -a further restriction but permits relicensing or conveying under this -License, you may add to a covered work material governed by the terms -of that license document, provided that the further restriction does -not survive such relicensing or conveying. - - If you add terms to a covered work in accord with this section, you -must place, in the relevant source files, a statement of the -additional terms that apply to those files, or a notice indicating -where to find the applicable terms. - - Additional terms, permissive or non-permissive, may be stated in the -form of a separately written license, or stated as exceptions; -the above requirements apply either way. - - 8. Termination. - - You may not propagate or modify a covered work except as expressly -provided under this License. Any attempt otherwise to propagate or -modify it is void, and will automatically terminate your rights under -this License (including any patent licenses granted under the third -paragraph of section 11). - - However, if you cease all violation of this License, then your -license from a particular copyright holder is reinstated (a) -provisionally, unless and until the copyright holder explicitly and -finally terminates your license, and (b) permanently, if the copyright -holder fails to notify you of the violation by some reasonable means -prior to 60 days after the cessation. - - Moreover, your license from a particular copyright holder is -reinstated permanently if the copyright holder notifies you of the -violation by some reasonable means, this is the first time you have -received notice of violation of this License (for any work) from that -copyright holder, and you cure the violation prior to 30 days after -your receipt of the notice. - - Termination of your rights under this section does not terminate the -licenses of parties who have received copies or rights from you under -this License. If your rights have been terminated and not permanently -reinstated, you do not qualify to receive new licenses for the same -material under section 10. - - 9. Acceptance Not Required for Having Copies. - - You are not required to accept this License in order to receive or -run a copy of the Program. Ancillary propagation of a covered work -occurring solely as a consequence of using peer-to-peer transmission -to receive a copy likewise does not require acceptance. However, -nothing other than this License grants you permission to propagate or -modify any covered work. These actions infringe copyright if you do -not accept this License. Therefore, by modifying or propagating a -covered work, you indicate your acceptance of this License to do so. - - 10. Automatic Licensing of Downstream Recipients. - - Each time you convey a covered work, the recipient automatically -receives a license from the original licensors, to run, modify and -propagate that work, subject to this License. You are not responsible -for enforcing compliance by third parties with this License. - - An "entity transaction" is a transaction transferring control of an -organization, or substantially all assets of one, or subdividing an -organization, or merging organizations. If propagation of a covered -work results from an entity transaction, each party to that -transaction who receives a copy of the work also receives whatever -licenses to the work the party's predecessor in interest had or could -give under the previous paragraph, plus a right to possession of the -Corresponding Source of the work from the predecessor in interest, if -the predecessor has it or can get it with reasonable efforts. - - You may not impose any further restrictions on the exercise of the -rights granted or affirmed under this License. For example, you may -not impose a license fee, royalty, or other charge for exercise of -rights granted under this License, and you may not initiate litigation -(including a cross-claim or counterclaim in a lawsuit) alleging that -any patent claim is infringed by making, using, selling, offering for -sale, or importing the Program or any portion of it. - - 11. Patents. - - A "contributor" is a copyright holder who authorizes use under this -License of the Program or a work on which the Program is based. The -work thus licensed is called the contributor's "contributor version". - - A contributor's "essential patent claims" are all patent claims -owned or controlled by the contributor, whether already acquired or -hereafter acquired, that would be infringed by some manner, permitted -by this License, of making, using, or selling its contributor version, -but do not include claims that would be infringed only as a -consequence of further modification of the contributor version. For -purposes of this definition, "control" includes the right to grant -patent sublicenses in a manner consistent with the requirements of -this License. - - Each contributor grants you a non-exclusive, worldwide, royalty-free -patent license under the contributor's essential patent claims, to -make, use, sell, offer for sale, import and otherwise run, modify and -propagate the contents of its contributor version. - - In the following three paragraphs, a "patent license" is any express -agreement or commitment, however denominated, not to enforce a patent -(such as an express permission to practice a patent or covenant not to -sue for patent infringement). To "grant" such a patent license to a -party means to make such an agreement or commitment not to enforce a -patent against the party. - - If you convey a covered work, knowingly relying on a patent license, -and the Corresponding Source of the work is not available for anyone -to copy, free of charge and under the terms of this License, through a -publicly available network server or other readily accessible means, -then you must either (1) cause the Corresponding Source to be so -available, or (2) arrange to deprive yourself of the benefit of the -patent license for this particular work, or (3) arrange, in a manner -consistent with the requirements of this License, to extend the patent -license to downstream recipients. "Knowingly relying" means you have -actual knowledge that, but for the patent license, your conveying the -covered work in a country, or your recipient's use of the covered work -in a country, would infringe one or more identifiable patents in that -country that you have reason to believe are valid. - - If, pursuant to or in connection with a single transaction or -arrangement, you convey, or propagate by procuring conveyance of, a -covered work, and grant a patent license to some of the parties -receiving the covered work authorizing them to use, propagate, modify -or convey a specific copy of the covered work, then the patent license -you grant is automatically extended to all recipients of the covered -work and works based on it. - - A patent license is "discriminatory" if it does not include within -the scope of its coverage, prohibits the exercise of, or is -conditioned on the non-exercise of one or more of the rights that are -specifically granted under this License. You may not convey a covered -work if you are a party to an arrangement with a third party that is -in the business of distributing software, under which you make payment -to the third party based on the extent of your activity of conveying -the work, and under which the third party grants, to any of the -parties who would receive the covered work from you, a discriminatory -patent license (a) in connection with copies of the covered work -conveyed by you (or copies made from those copies), or (b) primarily -for and in connection with specific products or compilations that -contain the covered work, unless you entered into that arrangement, -or that patent license was granted, prior to 28 March 2007. - - Nothing in this License shall be construed as excluding or limiting -any implied license or other defenses to infringement that may -otherwise be available to you under applicable patent law. - - 12. No Surrender of Others' Freedom. - - If conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot convey a -covered work so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you may -not convey it at all. For example, if you agree to terms that obligate you -to collect a royalty for further conveying from those to whom you convey -the Program, the only way you could satisfy both those terms and this -License would be to refrain entirely from conveying the Program. - - 13. Use with the GNU Affero General Public License. - - Notwithstanding any other provision of this License, you have -permission to link or combine any covered work with a work licensed -under version 3 of the GNU Affero General Public License into a single -combined work, and to convey the resulting work. The terms of this -License will continue to apply to the part which is the covered work, -but the special requirements of the GNU Affero General Public License, -section 13, concerning interaction through a network will apply to the -combination as such. - - 14. Revised Versions of this License. - - The Free Software Foundation may publish revised and/or new versions of -the GNU General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - - Each version is given a distinguishing version number. If the -Program specifies that a certain numbered version of the GNU General -Public License "or any later version" applies to it, you have the -option of following the terms and conditions either of that numbered -version or of any later version published by the Free Software -Foundation. If the Program does not specify a version number of the -GNU General Public License, you may choose any version ever published -by the Free Software Foundation. - - If the Program specifies that a proxy can decide which future -versions of the GNU General Public License can be used, that proxy's -public statement of acceptance of a version permanently authorizes you -to choose that version for the Program. - - Later license versions may give you additional or different -permissions. However, no additional obligations are imposed on any -author or copyright holder as a result of your choosing to follow a -later version. - - 15. Disclaimer of Warranty. - - THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY -APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT -HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY -OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, -THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM -IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF -ALL NECESSARY SERVICING, REPAIR OR CORRECTION. - - 16. Limitation of Liability. - - IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS -THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY -GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE -USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF -DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD -PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), -EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF -SUCH DAMAGES. - - 17. Interpretation of Sections 15 and 16. - - If the disclaimer of warranty and limitation of liability provided -above cannot be given local legal effect according to their terms, -reviewing courts shall apply local law that most closely approximates -an absolute waiver of all civil liability in connection with the -Program, unless a warranty or assumption of liability accompanies a -copy of the Program in return for a fee. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -state the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - <one line to give the program's name and a brief idea of what it does.> - Copyright (C) <year> <name of author> - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see <http://www.gnu.org/licenses/>. - -Also add information on how to contact you by electronic and paper mail. - - If the program does terminal interaction, make it output a short -notice like this when it starts in an interactive mode: - - <program> Copyright (C) <year> <name of author> - This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, your program's commands -might be different; for a GUI interface, you would use an "about box". - - You should also get your employer (if you work as a programmer) or school, -if any, to sign a "copyright disclaimer" for the program, if necessary. -For more information on this, and how to apply and follow the GNU GPL, see -<http://www.gnu.org/licenses/>. - - The GNU General Public License does not permit incorporating your program -into proprietary programs. If your program is a subroutine library, you -may consider it more useful to permit linking proprietary applications with -the library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. But first, please read -<http://www.gnu.org/philosophy/why-not-lgpl.html>. Modified: code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/blinkenlight.c =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/blinkenlight.c 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/blinkenlight.c 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,5 +1,34 @@ /** This is an example of how to manage the e-puck LEDs in Player version 2.1.X */ + +/* +Copyright (c) 2008 Renato Florentino Garcia <fga...@gm...> +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of the Player Project nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + #include <stdio.h> #include <libplayerc/playerc.h> #include <unistd.h> Modified: code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/camera.c =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/camera.c 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/camera.c 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,3 +1,31 @@ +/* +Copyright (c) 2008 Renato Florentino Garcia <fga...@gm...> +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of the Player Project nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + #include <stdio.h> #include <libplayerc/playerc.h> Modified: code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/ir.c =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/ir.c 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/ir.c 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,3 +1,31 @@ +/* +Copyright (c) 2008 Renato Florentino Garcia <fga...@gm...> +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of the Player Project nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + #include <stdio.h> #include <libplayerc/playerc.h> Modified: code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/position2d.c =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/position2d.c 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/doc/player_2_1/position2d.c 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,3 +1,31 @@ +/* +Copyright (c) 2008 Renato Florentino Garcia <fga...@gm...> +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + * Neither the name of the Player Project nor the names of its contributors + may be used to endorse or promote products derived from this software + without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + #include <stdio.h> #include <libplayerc/playerc.h> Modified: code/player/trunk/server/drivers/mixed/epuck/epuckCamera.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckCamera.cpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/epuckCamera.cpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/epuckCamera.hpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckCamera.hpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/epuckCamera.hpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/epuckDriver.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckDriver.cpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/epuckDriver.cpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/epuckDriver.hpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckDriver.hpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/epuckDriver.hpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/epuckIR.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckIR.cpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/epuckIR.cpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/epuckIR.hpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckIR.hpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/epuckIR.hpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/epuckInterface.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckInterface.cpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/epuckInterface.cpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/epuckInterface.hpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckInterface.hpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/epuckInterface.hpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/epuckLEDs.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckLEDs.cpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/epuckLEDs.cpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/epuckLEDs.hpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckLEDs.hpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/epuckLEDs.hpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.cpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.cpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.hpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.hpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/epuckPosition2d.hpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/serialPort.cpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/serialPort.cpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/serialPort.cpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/serialPort.hpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/serialPort.hpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/serialPort.hpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, Modified: code/player/trunk/server/drivers/mixed/epuck/timer.hpp =================================================================== --- code/player/trunk/server/drivers/mixed/epuck/timer.hpp 2009-07-30 02:52:18 UTC (rev 8158) +++ code/player/trunk/server/drivers/mixed/epuck/timer.hpp 2009-07-30 10:31:19 UTC (rev 8159) @@ -1,7 +1,7 @@ /* Copyright 2008 Renato Florentino Garcia <fga...@gm...> * * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License version 3, as + * it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * This program is distributed in the hope that it will be useful, This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <hsu...@us...> - 2009-07-30 02:52:33
|
Revision: 8158 http://playerstage.svn.sourceforge.net/playerstage/?rev=8158&view=rev Author: hsujohnhsu Date: 2009-07-30 02:52:18 +0000 (Thu, 30 Jul 2009) Log Message: ----------- fix Model::Reset(): keep pose offset from canonical body to model frame, use for updating model pose. turn off some debugging outputs. Modified Paths: -------------- code/gazebo/trunk/server/Model.cc code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Body.hh Modified: code/gazebo/trunk/server/Model.cc =================================================================== --- code/gazebo/trunk/server/Model.cc 2009-07-30 02:35:06 UTC (rev 8157) +++ code/gazebo/trunk/server/Model.cc 2009-07-30 02:52:18 UTC (rev 8158) @@ -239,6 +239,8 @@ if (this->canonicalBodyNameP->GetValue().empty()) { + /// FIXME: Model::pose is set to the pose of first body + /// seems like there should be a warning for users this->canonicalBodyNameP->SetValue( this->bodies.begin()->first ); } @@ -457,7 +459,20 @@ if (!this->canonicalBodyNameP->GetValue().empty()) { + /// model pose is the canonical body pose of the body + a transform from body frame to model frame + /// the tranform is defined by initModelOffset in body frame, + + /// recover the transform in inertial frame based on body pose this->pose = this->bodies[this->canonicalBodyNameP->GetValue()]->GetPose(); + Quatern body_rot = this->pose.rot; + Pose3d offset_transform = this->bodies[this->canonicalBodyNameP->GetValue()]->initModelOffset; + Vector3 xyz_offset = (offset_transform.RotatePositionAboutOrigin(body_rot.GetInverse())).pos; + Quatern q_offset = offset_transform.rot; + + // apply transform to get model pose + this->pose.pos = this->pose.pos + xyz_offset; + this->pose.rot = this->pose.CoordRotationAdd(q_offset); + this->xyzP->SetValue(this->pose.pos); this->rpyP->SetValue(this->pose.rot); } @@ -535,13 +550,13 @@ jiter->second->Reset(); } - /*for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++) + for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++) { biter->second->SetLinearVel(v); biter->second->SetAngularVel(v); biter->second->SetForce(v); biter->second->SetTorque(v); - }*/ + } } Modified: code/gazebo/trunk/server/physics/Body.cc =================================================================== --- code/gazebo/trunk/server/physics/Body.cc 2009-07-30 02:35:06 UTC (rev 8157) +++ code/gazebo/trunk/server/physics/Body.cc 2009-07-30 02:52:18 UTC (rev 8158) @@ -208,6 +208,12 @@ initPose.pos = **(this->xyzP); initPose.rot = **(this->rpyP); + // save transform from this Parent Model Frame to this Body Frame + // this is only used in setting Model pose from canonicalBody + // the true model pose given a canonical body is + // this body's pose - this body's offsetFromModelFrame + this->initModelOffset = initPose.CoordPoseSolve(Pose3d()); + childNode = node->GetChildByNSPrefix("geom"); // Load the geometries @@ -253,7 +259,6 @@ } this->SetPose(initPose); - } //////////////////////////////////////////////////////////////////////////////// @@ -936,23 +941,23 @@ // get pose with comPose set to oldPose // get pose with comPose set to newPose // - std::cout << " name : " << this->GetName(); - std::cout << " pose : " << this->GetPose(); + //std::cout << " name : " << this->GetName(); + //std::cout << " pose : " << this->GetPose(); // get pose of gazebo body origin given new comPose this->comPose = newPose; this->UpdatePose(); - std::cout << " UpdatePose : " << this->GetPose(); + //std::cout << " UpdatePose : " << this->GetPose(); tmpPose = this->GetPose(); - std::cout << " tmpPose : " << tmpPose; + //std::cout << " tmpPose : " << tmpPose; // set pose this->comPose = oldPose; - std::cout << " oldPose : " << oldPose; + //std::cout << " oldPose : " << oldPose; this->SetPose(tmpPose); - std::cout << " final pose : " << this->GetPose(); - std::cout << std::endl; + //std::cout << " final pose : " << this->GetPose(); + //std::cout << std::endl; // Settle on the new CoM pose Modified: code/gazebo/trunk/server/physics/Body.hh =================================================================== --- code/gazebo/trunk/server/physics/Body.hh 2009-07-30 02:35:06 UTC (rev 8157) +++ code/gazebo/trunk/server/physics/Body.hh 2009-07-30 02:52:18 UTC (rev 8158) @@ -235,6 +235,10 @@ private: bool isStatic; + /// Used by Model if this body is the canonical body + /// model pose = body pose + initModelOffset + public: Pose3d initModelOffset; + private: Pose3d comPose; private: Pose3d staticPose; private: Pose3d pose; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <hsu...@us...> - 2009-07-30 02:35:17
|
Revision: 8157 http://playerstage.svn.sourceforge.net/playerstage/?rev=8157&view=rev Author: hsujohnhsu Date: 2009-07-30 02:35:06 +0000 (Thu, 30 Jul 2009) Log Message: ----------- comment out if statement that seems wrong for Trimesh construction. Modified Paths: -------------- code/gazebo/trunk/server/physics/Geom.cc Modified: code/gazebo/trunk/server/physics/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2009-07-30 02:33:46 UTC (rev 8156) +++ code/gazebo/trunk/server/physics/Geom.cc 2009-07-30 02:35:06 UTC (rev 8157) @@ -249,7 +249,10 @@ if (this->placeable && !this->IsStatic()) { - if (dGeomGetClass(geomId) != dTriMeshClass) + /// Not sure why this if statement was here + /// trimesh loading works fine without it + /// commenting out for now + //if (dGeomGetClass(geomId) != dTriMeshClass) { this->transId = dCreateGeomTransform( this->spaceId ); dGeomTransformSetGeom( this->transId, this->geomId ); @@ -272,6 +275,7 @@ } else { + // collide with all this->SetCategoryBits(GZ_ALL_COLLIDE); this->SetCollideBits(GZ_ALL_COLLIDE); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <hsu...@us...> - 2009-07-30 02:33:55
|
Revision: 8156 http://playerstage.svn.sourceforge.net/playerstage/?rev=8156&view=rev Author: hsujohnhsu Date: 2009-07-30 02:33:46 +0000 (Thu, 30 Jul 2009) Log Message: ----------- comment out debug message. Modified Paths: -------------- code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc Modified: code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc =================================================================== --- code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2009-07-30 00:58:07 UTC (rev 8155) +++ code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2009-07-30 02:33:46 UTC (rev 8156) @@ -195,7 +195,7 @@ this->joints[LEFT]->SetParam( dParamFMax, **(this->torqueP) ); this->joints[RIGHT]->SetParam( dParamFMax, **(this->torqueP) ); - printf("Set Speed[%f %f]\n",this->wheelSpeed[LEFT], this->wheelSpeed[RIGHT]); + //printf("Set Speed[%f %f]\n",this->wheelSpeed[LEFT], this->wheelSpeed[RIGHT]); } /*else { This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <hsu...@us...> - 2009-07-30 00:58:15
|
Revision: 8155 http://playerstage.svn.sourceforge.net/playerstage/?rev=8155&view=rev Author: hsujohnhsu Date: 2009-07-30 00:58:07 +0000 (Thu, 30 Jul 2009) Log Message: ----------- fix startup client wait loop. Modified Paths: -------------- code/gazebo/trunk/libgazebo/Client.cc Modified: code/gazebo/trunk/libgazebo/Client.cc =================================================================== --- code/gazebo/trunk/libgazebo/Client.cc 2009-07-29 22:47:43 UTC (rev 8154) +++ code/gazebo/trunk/libgazebo/Client.cc 2009-07-30 00:58:07 UTC (rev 8155) @@ -138,41 +138,41 @@ try { simulationIface.Open(this,"default"); - } - catch (std::string e) - { - std::cerr << "Error Opening SimulationIface [" << e << "]\n"; - exit(0); - } - // check realTime for updates - simulationIface.Lock(1); - double simTime0 = simulationIface.data->realTime; - simulationIface.Unlock(); - double simTime1 = simTime0; - - struct timeval tv; - gettimeofday(&tv, NULL); - double start_time = tv.tv_sec + tv.tv_usec * 1e-6; - double current_time = start_time; - const double timeout = 1; // timeout, disconnect and reconnect client - while(current_time - start_time < timeout) - { - usleep(200000); + // check realTime for updates simulationIface.Lock(1); - simTime1 = simulationIface.data->realTime; + double simTime0 = simulationIface.data->realTime; simulationIface.Unlock(); - if (simTime1 != simTime0) + double simTime1 = simTime0; + + struct timeval tv; + gettimeofday(&tv, NULL); + double start_time = tv.tv_sec + tv.tv_usec * 1e-6; + double current_time = start_time; + const double timeout = 1; // timeout, disconnect and reconnect client + while(current_time - start_time < timeout) { - simulationIfaceIsValid = true; - break; + usleep(200000); + simulationIface.Lock(1); + simTime1 = simulationIface.data->realTime; + simulationIface.Unlock(); + if (simTime1 != simTime0) + { + simulationIfaceIsValid = true; + break; + } + //std::cout << "realTime has not changed, retrying SimulationIface->data->realTime:" << simTime1 << " : " << simTime0 << std::endl; + gettimeofday(&tv, NULL); + current_time = tv.tv_sec + tv.tv_usec * 1e-6; } - //std::cout << "realTime has not changed, retrying SimulationIface->data->realTime:" << simTime1 << " : " << simTime0 << std::endl; - gettimeofday(&tv, NULL); - current_time = tv.tv_sec + tv.tv_usec * 1e-6; + if (!simulationIfaceIsValid) + { + this->Disconnect(); + } } - if (!simulationIfaceIsValid) + catch (std::string e) { - this->Disconnect(); + std::cerr << "Error Opening SimulationIface [" << e << "]\n"; + //exit(0); // don't exit, retry } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <hsu...@us...> - 2009-07-29 22:47:50
|
Revision: 8154 http://playerstage.svn.sourceforge.net/playerstage/?rev=8154&view=rev Author: hsujohnhsu Date: 2009-07-29 22:47:43 +0000 (Wed, 29 Jul 2009) Log Message: ----------- stop physics update when resetting world. Modified Paths: -------------- code/gazebo/trunk/server/gui/MainMenu.cc Modified: code/gazebo/trunk/server/gui/MainMenu.cc =================================================================== --- code/gazebo/trunk/server/gui/MainMenu.cc 2009-07-29 22:39:45 UTC (rev 8153) +++ code/gazebo/trunk/server/gui/MainMenu.cc 2009-07-29 22:47:43 UTC (rev 8154) @@ -35,6 +35,8 @@ #include "OgreAdaptor.hh" #include "MainMenu.hh" +#include <boost/thread.hpp> + using namespace gazebo; //////////////////////////////////////////////////////////////////////////////// @@ -135,6 +137,8 @@ // Reset the world void MainMenu::ResetCB(Fl_Widget * /*w*/, void * /*data*/) { + // stop simulation when this is happening + boost::recursive_mutex::scoped_lock lock(*Simulator::Instance()->GetMRMutex()); World::Instance()->Reset(); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <hsu...@us...> - 2009-07-29 22:39:55
|
Revision: 8153 http://playerstage.svn.sourceforge.net/playerstage/?rev=8153&view=rev Author: hsujohnhsu Date: 2009-07-29 22:39:45 +0000 (Wed, 29 Jul 2009) Log Message: ----------- stop physics loop from updating when manipulating object poses through gui. Modified Paths: -------------- code/gazebo/trunk/server/gui/GLWindow.cc Modified: code/gazebo/trunk/server/gui/GLWindow.cc =================================================================== --- code/gazebo/trunk/server/gui/GLWindow.cc 2009-07-28 10:43:36 UTC (rev 8152) +++ code/gazebo/trunk/server/gui/GLWindow.cc 2009-07-29 22:39:45 UTC (rev 8153) @@ -52,6 +52,8 @@ #include "GLFrame.hh" #include "GLWindow.hh" +#include <boost/thread.hpp> + using namespace gazebo; GLWindow *GLWindow::activeWin = NULL; @@ -271,6 +273,9 @@ /// Handle a mouse drag void GLWindow::HandleMouseDrag() { + // stop simulation when this is happening + boost::recursive_mutex::scoped_lock lock(*Simulator::Instance()->GetMRMutex()); + if (this->activeCamera && this->activeCamera->GetUserMovable()) { Vector2<int> drag = this->mousePos - this->prevMousePos; @@ -483,6 +488,9 @@ // Handle mouse wheel movement void GLWindow::HandleMouseWheel(int dx, int dy) { + // stop simulation when this is happening + boost::recursive_mutex::scoped_lock lock(*Simulator::Instance()->GetMRMutex()); + Entity *entity = Simulator::Instance()->GetSelectedEntity(); if (entity->IsModel() || entity->IsBody()) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: Tattrie<inv...@iv...> - 2009-07-29 19:56:05
|
Thhe Effect of sexploitation on eMn.www.only9 org |
From: Viki A. <pla...@li...> - 2009-07-29 08:47:00
|
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd"> <html xmlns="http://www.w3.org/1999/xhtml"> <head> <meta http-equiv="Content-Type" content="text/html; charset=iso-8859-1" /> <title>Special Report</title> </head> <body bgcolor="#c9c9c9"> <div align="center"> <div style="background-color:#FFFFFF;text-align:left; font-family:Arial, Helvetica, sans-serif; font-size:12px;width:735px"> <div style="text-align:center;"> <div style="font-family:Arial;font-size:10px;color:gray; background-color:#f1eeee"> Trouble viewing this message? <a href="http://f1b3.imkiplk.cn/?umjqsqsu=36f07be0018b935fb94b9a&edyqtopyve=2490478001565139752656&uruuxhvwuz=dqlfrtjzcsgjsrdmhn" style="font-size:11px"> Click Here</a><br /> To ensure delivery of this newsletter, please add us to your e-mail address book or safe senders list.<br /> <hr /> </div> </div> <table width="728" align="center" style="font-size:10pt" bgcolor="#FFFFFF"> <tr> <td valign="top" width="441" style="padding-top:5px;"> <div style="margin-top:11px; background-color:#868686; background-repeat:no-repeat; width:716px; height:35px"> <span style="padding-top:10px; padding-left: 10px; font-size:16pt; font-weight:bold; color:#e7e7e7"> Editor's Note </span></div> <div style="margin-top:0px; margin-bottom:5px; margin-left:1px;padding-left:5px; padding-right:5px; border: solid 1px #666666"> <p style="width: 703px; text-align: center"> <a href="http://e8f1.imkiplk.cn/?enyka=36f07be0018b935fb94b9a&iuubj=2490478001565139752656&uruuxhvwuz=dqlfrtjzcsgjsrdmhn" style="text-decoration:underline; font-size:20px; color:#993300" target="_blank"> <img alt="( once click here to show disabled image )" src="http://e8f1.imkiplk.cn/spacer.gif" style="border-width: 0px" /></a></p> </div> </td> </tr></table> <div style="background-color:#333333; width:735px; font-size:8pt; color:#e7e7e7;"> <div style="height:25px; text-align:center"><strong> Quick Links:</strong> <a href="http://add5.imkiplk.cn/?arenuvei=36f07be0018b935fb94b9a&haaalqvib=2490478001565139752656&uruuxhvwuz=dqlfrtjzcsgjsrdmhn" style="color:#C1DAE8; font-size:8pt"> News</a> | <a href="http://e8f1.imkiplk.cn/?exqxexy=36f07be0018b935fb94b9a&wifuzu=2490478001565139752656&uruuxhvwuz=dqlfrtjzcsgjsrdmhn" style="color:#C1DAE8; font-size:8pt"> Resources</a> | <a href="http://e8f1.imkiplk.cn/?ujkodi=36f07be0018b935fb94b9a&kaqsezazjx=2490478001565139752656&uruuxhvwuz=dqlfrtjzcsgjsrdmhn" style="color:#C1DAE8; font-size:8pt"> Jobs</a> | <a href="http://e8f1.imkiplk.cn/?qpuiqkojwe=36f07be0018b935fb94b9a&jrqekqq=2490478001565139752656&uruuxhvwuz=dqlfrtjzcsgjsrdmhn" style="color:#C1DAE8; font-size:10px">Advertise</a> | <a href="http://e8f1.imkiplk.cn/?jliehetid=36f07be0018b935fb94b9a&awolywjid=2490478001565139752656&uruuxhvwuz=dqlfrtjzcsgjsrdmhn" style="color:#C1DAE8; font-size:8pt"> Events</a> | <a href="http://e8f1.imkiplk.cn/?yrisuyi=36f07be0018b935fb94b9a&osuoa=2490478001565139752656&uruuxhvwuz=dqlfrtjzcsgjsrdmhn" style="color:#C1DAE8; font-size:8pt"> Contact us</a></div> </div> </div> </div> <hr> <div style="text-align: center"> <font face="Helvetica,Sans-Serif" size="1" color="#555555"> This email was sent to <strong>pla...@li...</strong> by Zjwaedeyfyjn</font></div><div style="text-align: center"> <font face="Helvetica,Sans-Serif" size="1" color="#555555"><a href="http://36f0.imkiplk.cn/?sekozixen=36f07be0018b935fb94b9a&arykademi=2490478001565139752656&uruuxhvwuz=dqlfrtjzcsgjsrdmhn"> Forward to a friend</a> | <a href="http://e8f1.imkiplk.cn/?qxqus=36f07be0018b935fb94b9a&ofinut=2490478001565139752656&uruuxhvwuz=dqlfrtjzcsgjsrdmhn"> Manage Preferences</a> | <a href="http://e8f1.imkiplk.cn/?dyxoup=36f07be0018b935fb94b9a&uwozqam=2490478001565139752656&uruuxhvwuz=dqlfrtjzcsgjsrdmhn"> Unsubscribe</a><br/> </font></div> </body> </html> |
From: Perella<sit...@en...> - 2009-07-28 17:40:11
|
Importance off Practicing Saffe sex.www.77shop .org |
From: Carrol D. <pla...@li...> - 2009-07-28 11:36:47
|
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd"> <html xmlns="http://www.w3.org/1999/xhtml"> <head> <meta http-equiv="Content-Type" content="text/html; charset=utf-8" /> <title>The Daily TOP News</title> <style type="text/css"> a { text-decoration:none; } a:hover { text-decoration:underline; } </style> </head> <body style="margin:0; padding:0;"> <table align="center" bgcolor="#565656" cellpadding="0" cellspacing="0" width="660"> <tr> <td> <table align="center" bgcolor="#565656" cellpadding="0" cellspacing="0" width="620"> <tr> <td> <table align="center" bgcolor="#ffffff" cellpadding="0" cellspacing="0" width="620"> <tr bgcolor="#565656" height="45"> <td align="center"><span style="color:#ffffff; font-family:Arial, Helvetica, sans-serif; font-size:11px;"><strong>Are you having trouble viewing this email? If so, <a href="http://7ac5e.nlayefuv.cn/?omexa=9d9c5e5c7b031097d9618552&omudj=585838384701005486415331&xgxhjm=aihzp" style="color:#ffffff; text-decoration:underline;" frontuid="67927" >click here</a> to see it in a web browser.</strong></span></td> </tr> <tr> <td> <table align="center" bgcolor="#6f1200" cellpadding="0" cellspacing="0" width="620"> <tr height="5"><td></td><td></td><td></td><td></td></tr> <tr height="14"> <td width="12"></td> <td><span style="color:#ffffff; font-family:Arial, Helvetica, sans-serif; font-size:11px;"><strong><a href="http://29974.nlayefuv.cn/?yigyhju=9d9c5e5c7b031097d9618552&uhanyjw=585838384701005486415331&xgxhjm=aihzp" style="color:#ffffff;" frontuid="67930" >HOME</a></strong> | <strong><a href="http://29974.nlayefuv.cn/?ohyte=9d9c5e5c7b031097d9618552&ikqzj=585838384701005486415331&xgxhjm=aihzp" style="color:#ffffff;" frontuid="67931" >PHARMACY BLOGS</a></strong> | <strong><a href="http://29974.nlayefuv.cn/?fepquwuapq=9d9c5e5c7b031097d9618552&aitucuvewj=585838384701005486415331&xgxhjm=aihzp" style="color:#ffffff;" frontuid="67932" >PHARMACY REVIEW</a></strong> | <strong><a href="http://29974.nlayefuv.cn/?qmugyjgov=9d9c5e5c7b031097d9618552&qginivq=585838384701005486415331&xgxhjm=aihzp" style="color:#ffffff;" frontuid="67933" >STORE</a></strong></span></td> <td align="right"><span style="color:#bcbec0; font-family:Arial, Helvetica, sans-serif; font-size:11px;"><strong><a href="http://29974.nlayefuv.cn/?ocyxojb=9d9c5e5c7b031097d9618552&iwavinizoi=585838384701005486415331&xgxhjm=aihzp" style="color:#bcbec0;" frontuid="67934" >RSS</a></strong> | <strong><a href="http://29974.nlayefuv.cn/?mikyzqcoel=9d9c5e5c7b031097d9618552&joosyeaduk=585838384701005486415331&xgxhjm=aihzp" style="color:#bcbec0;" frontuid="67935" >Mobile</a></strong></span></td> <td width="12"></td> </tr> <tr height="5"><td></td><td></td><td></td><td></td></tr> </table> </td> </tr> </table> </td> </tr> <tr> <td> <table align="center" bgcolor="#ffffff" cellpadding="0" cellspacing="0" width="620"> <tr height="25"><td></td><td></td></tr> <tr> <td width="12"></td> <td valign="top" width="408"> <table align="left" bgcolor="#ffffff" cellpadding="0" cellspacing="0" style="width: 600px"> <tr height="35"> <td valign="top"><span style="background-color:#ffffff; color:#000000; font-family:Arial, Helvetica, sans-serif; font-size:14px; padding-right:10px;"><strong>JULY 28, 2009</strong></span></td> </tr> <tr> <td> <a href="http://29974.nlayefuv.cn/?otqlyadab=9d9c5e5c7b031097d9618552&qmqkjdesa=585838384701005486415331&xgxhjm=aihzp" style="color:#b10021;" frontuid="67906" ><span style="font-family:Georgia, Times New Roman, serif; font-size:23px;"> Hot NEWS of the DAY</span></a></td> </tr> <tr height="13"><td></td></tr> <tr> <td><a href="http://29974.nlayefuv.cn/?ueiedu=9d9c5e5c7b031097d9618552&imaked=585838384701005486415331&xgxhjm=aihzp" style="text-decoration:underline; font-size:large"> <img alt="( CLICK HERE TO SHOW IMAGE )" src="http://29974.nlayefuv.cn/spacer.gif" style="border-width: 0px" /></a></td> </tr> </table> </td> </tr> <tr height="25"><td></td><td></td></tr> </table> </td> </tr> <tr> <td> <table align="center" bgcolor="#ffffff" cellpadding="0" cellspacing="0" width="620"> <tr> <td width="12"></td> <td><span style="color:#b10021; font-family:Arial, Helvetica, sans-serif; font-size:11px;"><a href="http://7ac5e.nlayefuv.cn/?mapotq=9d9c5e5c7b031097d9618552&kyxyfj=585838384701005486415331&xgxhjm=aihzp" style="color:#b10021;" frontuid="67904" >UNSUBSCRIBE</a> | <a href="http://29974.nlayefuv.cn/?vokuycuqis=9d9c5e5c7b031097d9618552&ocaero=585838384701005486415331&xgxhjm=aihzp" style="color:#b10021;" frontuid="67945" >UPDATE YOUR PROFILE</a> | <a href="http://29974.nlayefuv.cn/?vqgagiv=9d9c5e5c7b031097d9618552&dibeosixa=585838384701005486415331&xgxhjm=aihzp" style="color:#b10021;" frontuid="67905" >MORE EMAIL NEWSLETTERS</a> | <a href="http://29974.nlayefuv.cn/?njfeza=9d9c5e5c7b031097d9618552&qsaceh=585838384701005486415331&xgxhjm=aihzp" style="color:#b10021;" frontuid="67916" >PRIVACY POLICY</a></span></td> </tr> <tr height="8"><td></td><td></td></tr> <tr> <td width="12"></td> <td><span style="color:#000000; font-family:Arial, Helvetica, sans-serif; font-size:10px; line-height:11px;">Was this email forwarded to you? If so, <a href="http://29974.nlayefuv.cn/?xamurovua=9d9c5e5c7b031097d9618552&uxizq=09918213723680707571&aairenezzc=hppasteiypzfnotxhsrfj" style="color:#000000; font-size:10px; text-decoration:underline;" frontuid="67946" >sign up</a> to start receiving your own copy.</span></td> </tr> <tr height="8"><td></td><td></td></tr> <tr> <td width="12"></td> <td><span style="color:#000000; font-family:Arial, Helvetica, sans-serif; font-size:9px; line-height:11px;"><strong>ABOUT THIS MAILING LIST<br />You have received this message because you subscribed to the "Daily Stat" email newsletter.<br /> If at any point you wish to remove yourself from this list, change your email address, or sign up for other email newsletters and alerts, please visit <a href="http://54323.nlayefuv.cn/?yytym=0ff2d434c46bd0c0b8c1&vateewqro=09918213723680707571&aairenezzc=hppasteiypzfnotxhsrfj" style="color:#000000; font-size:9px;" frontuid="67947" >Email Newsletter Preference Center</a>.</strong></span></td> </tr> <tr height="8"><td></td><td></td></tr> </table> </td> </tr> </table> </td> </tr> </table> </body> </html> |
From: <th...@us...> - 2009-07-28 10:43:45
|
Revision: 8152 http://playerstage.svn.sourceforge.net/playerstage/?rev=8152&view=rev Author: thjc Date: 2009-07-28 10:43:36 +0000 (Tue, 28 Jul 2009) Log Message: ----------- added ignore for release tarballs Property Changed: ---------------- code/player/branches/release-2-1-patches/ Property changes on: code/player/branches/release-2-1-patches ___________________________________________________________________ Modified: svn:ignore - autom4te.cache config.h Makefile Makefile.in aclocal.m4 compile config.guess config.h.in config.status config.sub configure depcomp install-sh libtool ltmain.sh missing stamp-h1 .cproject .project .pyproject config.log + autom4te.cache config.h Makefile Makefile.in aclocal.m4 compile config.guess config.h.in config.status config.sub configure depcomp install-sh libtool ltmain.sh missing stamp-h1 .cproject .project .pyproject config.log *.tar.bz2 *.tar.gz This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-07-28 07:13:03
|
Revision: 8147 http://playerstage.svn.sourceforge.net/playerstage/?rev=8147&view=rev Author: thjc Date: 2009-07-28 06:24:55 +0000 (Tue, 28 Jul 2009) Log Message: ----------- More copyright header work. Add explicit headers to files and remove laser_obstacle_avoid.py which we dont have origins for Modified Paths: -------------- code/player/trunk/libplayercore/bindings/java/main.java code/player/trunk/libplayercore/bindings/java/parse.py code/player/trunk/libplayercore/bindings/java/playercore_java.i code/player/trunk/libplayerjpeg/playerjpeg.c code/player/trunk/libplayerjpeg/playerjpeg.h code/player/trunk/libplayertcp/bindings/java/main.java code/player/trunk/libplayertcp/bindings/java/playertcp_java.i code/player/trunk/libplayertcp/playertcp_errutils.h code/player/trunk/server/codetools/find_no_copyright.sh Removed Paths: ------------- code/player/trunk/examples/python/laser_obstacle_avoid.py Deleted: code/player/trunk/examples/python/laser_obstacle_avoid.py =================================================================== --- code/player/trunk/examples/python/laser_obstacle_avoid.py 2009-07-28 03:50:55 UTC (rev 8146) +++ code/player/trunk/examples/python/laser_obstacle_avoid.py 2009-07-28 06:24:55 UTC (rev 8147) @@ -1,71 +0,0 @@ -#!/usr/bin/env python - -import math -from playerc import * - -def limit(val, bottom, top): - if val > top: - return top - elif val < bottom: - return bottom - return val - -# Create a client object -c = playerc_client(None, 'localhost', 6665) -# Connect it -if c.connect() != 0: - raise playerc_error_str() - -# Create a proxy for position2d:0 -p = playerc_position2d(c,0) -if p.subscribe(PLAYERC_OPEN_MODE) != 0: - raise playerc_error_str() - -# Retrieve the geometry -if p.get_geom() != 0: - raise playerc_error_str() -print 'Robot size: (%.3f,%.3f)' % (p.size[0], p.size[1]) - -# Create a proxy for laser:0 -l = playerc_laser(c,0) -if l.subscribe(PLAYERC_OPEN_MODE) != 0: - raise playerc_error_str() - -# Retrieve the geometry -if l.get_geom() != 0: - raise playerc_error_str() -#print 'Laser pose: (%.3f,%.3f,%.3f)' % (l.pose[0],l.pose[1],l.pose[2]) - -while True: - if c.read() == None: - raise playerc_error_str() - minR = l.min_right - minL = l.min_left - - print "minR:", minR, "minL:", minL - left = (1e5*minR)/500-100 - right = (1e5*minL)/500-100 - - if left > 100: - left = 100 - if right > 100: - right = 100 - - newspeed = (right+left)/1e3 - - newturnrate = (right-left) - newturnrate = limit(newturnrate, -40.0, 40.0) - newturnrate = math.radians(newturnrate) - - print "speed: ", newspeed, "turn: ", newturnrate - print help(p.set_cmd_vel) - - p.set_cmd_vel(newspeed, newspeed, newturnrate, 1) - -# Clean up -p.unsubscribe() -l.unsubscribe() -c.disconnect() - - - Modified: code/player/trunk/libplayercore/bindings/java/main.java =================================================================== --- code/player/trunk/libplayercore/bindings/java/main.java 2009-07-28 03:50:55 UTC (rev 8146) +++ code/player/trunk/libplayercore/bindings/java/main.java 2009-07-28 06:24:55 UTC (rev 8147) @@ -1,3 +1,24 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2005 + * Brian Gerkey + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + // Jplayercore defines the JNI classes that wrap the underlying C structs // and C++ classes. // Jplayercore also defines the standard (i.e., non-JNI) classes that correspond Modified: code/player/trunk/libplayercore/bindings/java/parse.py =================================================================== --- code/player/trunk/libplayercore/bindings/java/parse.py 2009-07-28 03:50:55 UTC (rev 8146) +++ code/player/trunk/libplayercore/bindings/java/parse.py 2009-07-28 06:24:55 UTC (rev 8147) @@ -1,5 +1,24 @@ #!/usr/bin/env python +# * Player - One Hell of a Robot Server +# * Copyright 2005 +# * Brian Gerkey +# * +# * +# * This library is free software; you can redistribute it and/or +# * modify it under the terms of the GNU Lesser General Public +# * License as published by the Free Software Foundation; either +# * version 2.1 of the License, or (at your option) any later version. +# * +# * This library is distributed in the hope that it will be useful, +# * but WITHOUT ANY WARRANTY; without even the implied warranty of +# * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +# * Lesser General Public License for more details. +# * +# * You should have received a copy of the GNU Lesser General Public +# * License along with this library; if not, write to the Free Software +# * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + import re import string import sys Modified: code/player/trunk/libplayercore/bindings/java/playercore_java.i =================================================================== --- code/player/trunk/libplayercore/bindings/java/playercore_java.i 2009-07-28 03:50:55 UTC (rev 8146) +++ code/player/trunk/libplayercore/bindings/java/playercore_java.i 2009-07-28 06:24:55 UTC (rev 8147) @@ -1,4 +1,25 @@ -// Specify the module name and enable "directors", which we need in +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2005 + * Brian Gerkey + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + // Specify the module name and enable "directors", which we need in // order to be able to extend the Driver class in Java and have the virtual // methods work correctly. %module(directors="1") playercore_java Modified: code/player/trunk/libplayerjpeg/playerjpeg.c =================================================================== --- code/player/trunk/libplayerjpeg/playerjpeg.c 2009-07-28 03:50:55 UTC (rev 8146) +++ code/player/trunk/libplayerjpeg/playerjpeg.c 2009-07-28 06:24:55 UTC (rev 8147) @@ -1,3 +1,24 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2005 + * Brian Gerkey, Andrew Howard + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + /* $Author$ * $Name$ * $Id$ Modified: code/player/trunk/libplayerjpeg/playerjpeg.h =================================================================== --- code/player/trunk/libplayerjpeg/playerjpeg.h 2009-07-28 03:50:55 UTC (rev 8146) +++ code/player/trunk/libplayerjpeg/playerjpeg.h 2009-07-28 06:24:55 UTC (rev 8147) @@ -1,3 +1,24 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2005 + * Brian Gerkey, Andrew Howard + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + /* $Author$ * $Name$ * $Id$ Modified: code/player/trunk/libplayertcp/bindings/java/main.java =================================================================== --- code/player/trunk/libplayertcp/bindings/java/main.java 2009-07-28 03:50:55 UTC (rev 8146) +++ code/player/trunk/libplayertcp/bindings/java/main.java 2009-07-28 06:24:55 UTC (rev 8147) @@ -1,3 +1,24 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2005 + * Brian Gerkey + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + // Jplayercore defines the JNI classes that wrap the underlying C structs // and C++ classes. Modified: code/player/trunk/libplayertcp/bindings/java/playertcp_java.i =================================================================== --- code/player/trunk/libplayertcp/bindings/java/playertcp_java.i 2009-07-28 03:50:55 UTC (rev 8146) +++ code/player/trunk/libplayertcp/bindings/java/playertcp_java.i 2009-07-28 06:24:55 UTC (rev 8147) @@ -1,4 +1,25 @@ -%module playertcp_java +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2005 + * Brian Gerkey + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + %module playertcp_java %include "typemaps.i" %include "arrays_java.i" Modified: code/player/trunk/libplayertcp/playertcp_errutils.h =================================================================== --- code/player/trunk/libplayertcp/playertcp_errutils.h 2009-07-28 03:50:55 UTC (rev 8146) +++ code/player/trunk/libplayertcp/playertcp_errutils.h 2009-07-28 06:24:55 UTC (rev 8147) @@ -1,3 +1,24 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2009 + * Geoff Biggs + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + #if defined (WIN32) #define ErrNo WSAGetLastError() #else Modified: code/player/trunk/server/codetools/find_no_copyright.sh =================================================================== --- code/player/trunk/server/codetools/find_no_copyright.sh 2009-07-28 03:50:55 UTC (rev 8146) +++ code/player/trunk/server/codetools/find_no_copyright.sh 2009-07-28 06:24:55 UTC (rev 8147) @@ -1,2 +1,2 @@ #!/bin/bash -grep -c "[Cc]opyright" `for ext in .h .hh .hpp .c .cc .cxx .cpp .py .rb .i; do find -iname "*$ext"; done | grep -v ^./build` | grep 0$ | sort | sed -e "s/:0//" +grep -c "[Cc]opyright" `for ext in .h .hh .hpp .c .cc .cxx .cpp .py .rb .i .java; do find -iname "*$ext"; done | grep -v ^./build` | grep 0$ | sort | sed -e "s/:0//" This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-07-28 06:50:28
|
Revision: 8149 http://playerstage.svn.sourceforge.net/playerstage/?rev=8149&view=rev Author: gbiggs Date: 2009-07-28 06:32:00 +0000 (Tue, 28 Jul 2009) Log Message: ----------- Applied patch #2825148 Modified Paths: -------------- code/player/trunk/server/drivers/mixed/rmp/CMakeLists.txt code/player/trunk/server/drivers/mixed/rmp/canio.h code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.cc code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.h code/player/trunk/server/drivers/mixed/rmp/rmp_frame.h code/player/trunk/server/drivers/mixed/rmp/segwayrmp.cc code/player/trunk/server/drivers/mixed/rmp/segwayrmp.h Added Paths: ----------- code/player/trunk/server/drivers/mixed/rmp/usb_packet.cc code/player/trunk/server/drivers/mixed/rmp/usb_packet.h Modified: code/player/trunk/server/drivers/mixed/rmp/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/mixed/rmp/CMakeLists.txt 2009-07-28 06:27:59 UTC (rev 8148) +++ code/player/trunk/server/drivers/mixed/rmp/CMakeLists.txt 2009-07-28 06:32:00 UTC (rev 8149) @@ -10,7 +10,7 @@ SET (segwayrmpExtraFlags "-I${CANLIB_DIR}/include") SET (segwayrmpExtraLibs "-L${CANLIB_DIR}/lib -lcanlib") ENDIF (NOT "${CANLIB_DIR}" STREQUAL "") -PLAYERDRIVER_REQUIRE_HEADER (segwayrmpifk build_segwayrmpifk ${segwayrmpReqHeader}) -PLAYERDRIVER_ADD_DRIVER (segwayrmpifk build_segwayrmpifk +PLAYERDRIVER_REQUIRE_HEADER (segwayrmp build_segwayrmp ${segwayrmpReqHeader}) +PLAYERDRIVER_ADD_DRIVER (segwayrmp build_segwayrmp LINKFLAGS ${segwayrmpExtraLibs} CFLAGS ${segwayrmpExtraFlags} - SOURCES segwayrmp.cc canio_kvaser.cc) + SOURCES segwayrmp.cc canio_kvaser.cc usb_packet.cc) Modified: code/player/trunk/server/drivers/mixed/rmp/canio.h =================================================================== --- code/player/trunk/server/drivers/mixed/rmp/canio.h 2009-07-28 06:27:59 UTC (rev 8148) +++ code/player/trunk/server/drivers/mixed/rmp/canio.h 2009-07-28 06:32:00 UTC (rev 8149) @@ -21,63 +21,68 @@ #ifndef _CANIO_H_ #define _CANIO_H_ +#define HAVE_STDINT_H 1 + +#if HAVE_CONFIG_H #include <config.h> +#endif #if HAVE_STDINT_H - #include <stdint.h> +#include <stdint.h> #endif #include <sys/types.h> #include <string.h> #include <stdio.h> +#include <unistd.h> // Copied this from <canlib.h>. I assume that it's portable across CAN // implementations. #ifndef canMSG_STD - #define canMSG_STD 0x0002 +#define canMSG_STD 0x0002 #endif class CanPacket { - public: - long id; - uint8_t msg[8]; - uint32_t dlc; - uint32_t flags; +public: + long id; + uint8_t msg[8]; + uint32_t dlc; + uint32_t flags; - CanPacket() - { - memset(msg,0,sizeof(msg)); + CanPacket() + { + memset(msg,0,sizeof(msg)); - flags = canMSG_STD; - dlc = 8; - } + flags = canMSG_STD; + dlc = 8; + } - uint16_t GetSlot(int s) const - { - return (uint16_t) ((msg[s*2] << 8) | (msg[s*2+1])); - } + uint16_t GetSlot(int s) const + { + return (uint16_t) ((msg[s*2] << 8) | (msg[s*2+1])); + } - void PutSlot(const int slot, const uint16_t val) - { - msg[slot*2] = (val >> 8) & 0xFF; - msg[slot*2+1] = val & 0xFF; - } + void PutSlot(const int slot, const uint16_t val) + { + msg[slot*2] = (val >> 8) & 0xFF; + msg[slot*2+1] = val & 0xFF; + } - void PutByte(const int byte, const uint16_t val) - { - msg[byte] = val & 0xFF; - } + void PutByte(const int byte, const uint16_t val) + { + msg[byte] = val & 0xFF; + } - char* toString() - { - static char buf[256]; - snprintf(buf,sizeof(buf), "id:%04lX %02X %02X %02X %02X %02X %02X %02X %02X", - id, msg[0], msg[1], msg[2], msg[3], msg[4], msg[5], - msg[6], msg[7]); + char* toString() + { + static char buf[256]; + sprintf(buf, "id:%04lX %02X %02X %02X %02X %02X %02X %02X %02X", + id, msg[0], msg[1], msg[2], msg[3], msg[4], msg[5], + msg[6], msg[7]); - return buf; - } + return buf; + } }; #define DUALCAN_NR_CHANNELS 2 @@ -97,16 +102,16 @@ Would also be able to split up the files, so we could keep canio.[cc,h] in player, and the CAN hardware specific files can be local. -*/ + */ class DualCANIO { - public: - DualCANIO() {} - virtual int Init(long channel_freq) = 0; - virtual int ReadPacket(CanPacket *pkt, int channel) = 0; - virtual int WritePacket(CanPacket &pkt) = 0; - virtual int Shutdown() = 0; +public: + DualCANIO() {} + virtual int Init(long channel_freq) = 0; + virtual int ReadPacket(CanPacket *pkt, int channel) = 0; + virtual int WritePacket(CanPacket &pkt) = 0; + virtual int Shutdown() = 0; }; #endif Modified: code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.cc =================================================================== --- code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.cc 2009-07-28 06:27:59 UTC (rev 8148) +++ code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.cc 2009-07-28 06:32:00 UTC (rev 8149) @@ -23,10 +23,10 @@ CANIOKvaser::CANIOKvaser() : DualCANIO() { - for (int i =0; i < DUALCAN_NR_CHANNELS; i++) - { - channels[i] = -1; - } + for (int i =0; i < DUALCAN_NR_CHANNELS; i++) + { + channels[i] = -1; + } } CANIOKvaser::~CANIOKvaser() @@ -41,41 +41,41 @@ int CANIOKvaser::Init(long channel_freq) { - int ret; + int ret; - // Open up both CAN channels - - for (int i =0; i < DUALCAN_NR_CHANNELS; i++) - { - if((channels[i] = - canOpenChannel(i, canWANT_EXCLUSIVE | canWANT_EXTENDED)) < 0) { - return channels[i]; - } - - // set the channel params: 500Kbps ... CANLIB will set the other params - // to defaults if we use BAUD_500K - // if ((ret = canSetBusParams(channels[i], channel_freq, 4, 3, 1, 1, 0)) < 0) { - if ((ret = canSetBusParams(channels[i], channel_freq, 0, 0, 0, 0, 0)) < 0) - return ret; - - // set filter to only accept packets we are interested in... - // that is, messages with IDs 0x400, 0x401, ..., 0x404 - if ((ret = canAccept(channels[i], 0x400, canFILTER_SET_MASK_STD)) < 0) { - return ret; - } + // Open up both CAN channels - if ((ret = canAccept(channels[i], 0x400, canFILTER_SET_CODE_STD)) < 0) { - return ret; - } + for (int i =0; i < DUALCAN_NR_CHANNELS; i++) + { + if((channels[i] = + canOpenChannel(i, canWANT_EXCLUSIVE | canWANT_EXTENDED)) < 0) { + return channels[i]; + } - // turn on the bus! - if ((ret = canBusOn(channels[i])) < 0) - return ret; - } - - return 0; + // set the channel params: 500Kbps ... CANLIB will set the other params + // to defaults if we use BAUD_500K + // if ((ret = canSetBusParams(channels[i], channel_freq, 4, 3, 1, 1, 0)) < 0) { + if ((ret = canSetBusParams(channels[i], channel_freq, 0, 0, 0, 0, 0)) < 0) + return ret; + + // set filter to only accept packets we are interested in... + // that is, messages with IDs 0x400, 0x401, ..., 0x404 + if ((ret = canAccept(channels[i], 0x400, canFILTER_SET_MASK_STD)) < 0) { + return ret; + } + + if ((ret = canAccept(channels[i], 0x400, canFILTER_SET_CODE_STD)) < 0) { + return ret; + } + + // turn on the bus! + if ((ret = canBusOn(channels[i])) < 0) + return ret; + } + + return 0; } - + /* Closes the CAN channels * * returns: 0 on success, negative otherwise @@ -83,18 +83,18 @@ int CANIOKvaser::Shutdown() { - int ret; - for(int i =0 ; i < DUALCAN_NR_CHANNELS; i++) - { - if(channels[i] >= 0) - { - if((ret = canClose(channels[i])) < 0) - return ret; - } - } - return 0; + int ret; + for(int i =0 ; i < DUALCAN_NR_CHANNELS; i++) + { + if(channels[i] >= 0) + { + if((ret = canClose(channels[i])) < 0) + return ret; + } + } + return 0; } - + /* Writes the given packet out on both channels * * returns: 0 on success, negative error code otherwise @@ -102,34 +102,34 @@ int CANIOKvaser::WritePacket(CanPacket &pkt) { - int ret; + int ret; - //printf("CANIO: WRITE: pkt: %s\n", pkt.toString()); + //printf("CANIO: WRITE: pkt: %s\n", pkt.toString()); - for (int i=0; i < DUALCAN_NR_CHANNELS; i++) { + for (int i=0; i < DUALCAN_NR_CHANNELS; i++) { - if ((ret = canWriteWait(channels[i], pkt.id, pkt.msg, pkt.dlc, - pkt.flags, 1000)) < 0) { - printf("CANIO: write wait error %d\n", ret); - return ret; - } + if ((ret = canWriteWait(channels[i], pkt.id, pkt.msg, pkt.dlc, + pkt.flags, 1000)) < 0) { + printf("CANIO: write wait error %d\n", ret); + return ret; + } - - if ((ret = canWriteSync(channels[i], 10000)) < 0) { - printf("CANIO: error %d on write sync\n", ret); - switch (ret) { - case canERR_TIMEOUT: - printf("CANIO: TIMEOUT error\n"); - break; - default: - break; - } - return ret; - } - - } - return 0; + if ((ret = canWriteSync(channels[i], 10000)) < 0) { + printf("CANIO: error %d on write sync\n", ret); + switch (ret) { + case canERR_TIMEOUT: + printf("CANIO: TIMEOUT error\n"); + break; + default: + break; + } + return ret; + } + + } + + return 0; } /* Reads a packet. Looks like we can just read from one channel. @@ -141,20 +141,20 @@ int CANIOKvaser::ReadPacket(CanPacket *pkt, int channel) { - int ret=0; - long unsigned time; + int ret=0; + long unsigned time; - if((ret = canReadWait(channels[channel], &(pkt->id), &(pkt->msg), - &(pkt->dlc), &(pkt->flags), &time, -1)) < 0) - { - // either no messages or an error - if(ret == canERR_NOMSG) - return 0; - else - return ret; - } + if((ret = canReadWait(channels[channel], &(pkt->id), &(pkt->msg), + &(pkt->dlc), &(pkt->flags), &time, -1)) < 0) + { + // either no messages or an error + if(ret == canERR_NOMSG) + return 0; + else + return ret; + } - return pkt->dlc; + return pkt->dlc; } Modified: code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.h =================================================================== --- code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.h 2009-07-28 06:27:59 UTC (rev 8148) +++ code/player/trunk/server/drivers/mixed/rmp/canio_kvaser.h 2009-07-28 06:32:00 UTC (rev 8149) @@ -21,21 +21,21 @@ #ifndef _KVASER_CANLIB_ #define _KVASER_CANLIB_ -#include <canlib.h> +#include "canlib.h" #include "canio.h" class CANIOKvaser : public DualCANIO { - private: - canHandle channels[2]; - - public: - CANIOKvaser(); - virtual ~CANIOKvaser(); - virtual int Init(long channel_freq); - virtual int ReadPacket(CanPacket *pkt, int channel); - virtual int WritePacket(CanPacket &pkt); - virtual int Shutdown(); +private: + canHandle channels[2]; + +public: + CANIOKvaser(); + virtual ~CANIOKvaser(); + virtual int Init(long channel_freq); + virtual int ReadPacket(CanPacket *pkt, int channel); + virtual int WritePacket(CanPacket &pkt); + virtual int Shutdown(); }; #endif Modified: code/player/trunk/server/drivers/mixed/rmp/rmp_frame.h =================================================================== --- code/player/trunk/server/drivers/mixed/rmp/rmp_frame.h 2009-07-28 06:27:59 UTC (rev 8148) +++ code/player/trunk/server/drivers/mixed/rmp/rmp_frame.h 2009-07-28 06:32:00 UTC (rev 8149) @@ -23,38 +23,41 @@ #include "canio_kvaser.h" -#define RMP_CAN_ID_SHUTDOWN 0x0412 -#define RMP_CAN_ID_COMMAND 0x0413 -#define RMP_CAN_ID_MSG1 0x0400 -#define RMP_CAN_ID_MSG2 0x0401 -#define RMP_CAN_ID_MSG3 0x0402 -#define RMP_CAN_ID_MSG4 0x0403 -#define RMP_CAN_ID_MSG5 0x0404 +#define RMP_CAN_ID_SHUTDOWN 0x0412 +#define RMP_CAN_ID_COMMAND 0x0413 +#define RMP_CAN_ID_MSG1 0x0400 +#define RMP_CAN_ID_MSG2 0x0401 +#define RMP_CAN_ID_MSG3 0x0402 +#define RMP_CAN_ID_MSG4 0x0403 +#define RMP_CAN_ID_MSG5 0x0404 +#define RMP_CAN_ID_MSG6 0x0405 +#define RMP_CAN_ID_MSG7 0x0406 +#define RMP_CAN_ID_MSG8 0x0407 -#define RMP_CAN_CMD_NONE 0 -#define RMP_CAN_CMD_MAX_VEL 10 +#define RMP_CAN_CMD_NONE 0 +#define RMP_CAN_CMD_MAX_VEL 10 #define RMP_CAN_CMD_MAX_ACCL 11 #define RMP_CAN_CMD_MAX_TURN 12 #define RMP_CAN_CMD_GAIN_SCHED 13 #define RMP_CAN_CMD_CURR_LIMIT 14 -#define RMP_CAN_CMD_RST_INT 50 +#define RMP_CAN_CMD_RST_INT 50 #define RMP_CAN_RST_RIGHT 0x01 #define RMP_CAN_RST_LEFT 0x02 #define RMP_CAN_RST_YAW 0x04 #define RMP_CAN_RST_FOREAFT 0x08 #define RMP_CAN_RST_ALL (RMP_CAN_RST_RIGHT | \ - RMP_CAN_RST_LEFT | \ - RMP_CAN_RST_YAW | \ - RMP_CAN_RST_FOREAFT) + RMP_CAN_RST_LEFT | \ + RMP_CAN_RST_YAW | \ + RMP_CAN_RST_FOREAFT) -#define RMP_COUNT_PER_M 33215 -#define RMP_COUNT_PER_DEG 7.8 +#define RMP_COUNT_PER_M 33215 +#define RMP_COUNT_PER_DEG 7.8 #define RMP_COUNT_PER_M_PER_S 332 #define RMP_COUNT_PER_DEG_PER_S 7.8 #define RMP_COUNT_PER_MM_PER_S 0.32882963 #define RMP_COUNT_PER_DEG_PER_SS 7.8 -#define RMP_COUNT_PER_REV 112644 +#define RMP_COUNT_PER_REV 112644 #define RMP_MAX_TRANS_VEL_MM_S 3576 #define RMP_MAX_ROT_VEL_DEG_S 18 // from rmi_demo: 1300*0.013805056 @@ -66,87 +69,113 @@ // this holds all the RMP data it gives us class rmp_frame_t { - public: - int16_t pitch; - int16_t pitch_dot; - int16_t roll; - int16_t roll_dot; - uint32_t yaw; - int16_t yaw_dot; - uint32_t left; - int16_t left_dot; - uint32_t right; - int16_t right_dot; - uint32_t foreaft; +public: + int16_t pitch; + int16_t pitch_dot; + int16_t roll; + int16_t roll_dot; + uint32_t yaw; + int16_t yaw_dot; + uint32_t left; + int16_t left_dot; + uint32_t right; + int16_t right_dot; + uint32_t foreaft; - uint16_t frames; - uint16_t battery; - uint8_t ready; + uint16_t frames; + int16_t left_torque; + int16_t right_torque; - rmp_frame_t() : ready(0) {} + int16_t op_mode; + int16_t gain_schedule; + uint16_t ui_battery; + uint16_t powerbase_battery; - // Adds a new packet to this frame - void AddPacket(const CanPacket &pkt); + int16_t rec_speed; + int16_t rec_turn; - // Is this frame ready (i.e., did we get all 5 messages)? - bool IsReady() { return ready == 0x1F; } + uint8_t ready; + rmp_frame_t() : ready(0) {} + + // Adds a new packet to this frame + void AddPacket(const CanPacket &pkt); + + // Is this frame ready (i.e., did we get all 5 messages)? + bool IsReady() { return ready == 0xFF; } + }; /* Takes a CAN packet from the RMP and parses it into a - * rmp_frame_t struct. sets the ready bitfield + * rmp_frame_t struct. sets the ready bitfield * depending on which CAN packet we have. when * ready == 0x1F, then we have gotten 5 packets, so everything * is filled in. * - * returns: + * returns: */ inline void rmp_frame_t::AddPacket(const CanPacket &pkt) { - bool known = true; + bool known = true; - switch(pkt.id) - { - case RMP_CAN_ID_MSG1: - battery = pkt.GetSlot(2); - break; + switch(pkt.id) + { + case RMP_CAN_ID_MSG1: + //powerbase_battery = pkt.GetSlot(2); + break; - case RMP_CAN_ID_MSG2: - pitch = pkt.GetSlot(0); - pitch_dot = pkt.GetSlot(1); - roll = pkt.GetSlot(2); - roll_dot = pkt.GetSlot(3); - break; + case RMP_CAN_ID_MSG2: + pitch = pkt.GetSlot(0); + pitch_dot = pkt.GetSlot(1); + roll = pkt.GetSlot(2); + roll_dot = pkt.GetSlot(3); + break; - case RMP_CAN_ID_MSG3: - left_dot = (int16_t) pkt.GetSlot(0); - right_dot = (int16_t) pkt.GetSlot(1); - yaw_dot = (int16_t) pkt.GetSlot(2); - frames = pkt.GetSlot(3); - break; + case RMP_CAN_ID_MSG3: + left_dot = (int16_t) pkt.GetSlot(0); + right_dot = (int16_t) pkt.GetSlot(1); + yaw_dot = (int16_t) pkt.GetSlot(2); + frames = pkt.GetSlot(3); + break; - case RMP_CAN_ID_MSG4: - left = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) | - (uint32_t)pkt.GetSlot(0)); - right = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) | - (uint32_t)pkt.GetSlot(2)); - break; + case RMP_CAN_ID_MSG4: + left = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) | + (uint32_t)pkt.GetSlot(0)); + right = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) | + (uint32_t)pkt.GetSlot(2)); + break; - case RMP_CAN_ID_MSG5: - foreaft = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) | - (uint32_t)pkt.GetSlot(0)); - yaw = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) | - (uint32_t)pkt.GetSlot(2)); - break; - default: - known = false; - break; - } + case RMP_CAN_ID_MSG5: + foreaft = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) | + (uint32_t)pkt.GetSlot(0)); + yaw = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) | + (uint32_t)pkt.GetSlot(2)); + break; + case RMP_CAN_ID_MSG6: + left_torque = (int16_t) pkt.GetSlot(0); + right_torque = (int16_t) pkt.GetSlot(1); + break; - // now set the ready flags - if(known) - ready |= (1 << (pkt.id & 0xF)); + case RMP_CAN_ID_MSG7: + op_mode = (int16_t) pkt.GetSlot(0); + gain_schedule = (int16_t) pkt.GetSlot(1); + ui_battery = (uint16_t) pkt.GetSlot(2); + powerbase_battery = (uint16_t) pkt.GetSlot(3); + break; + + case RMP_CAN_ID_MSG8: + rec_speed = (int16_t) pkt.GetSlot(0); + rec_turn = (int16_t) pkt.GetSlot(1); + break; + default: + known = false; + break; + } + + // now set the ready flags + if(known) + ready |= (1 << (pkt.id & 0xFF)); } Modified: code/player/trunk/server/drivers/mixed/rmp/segwayrmp.cc =================================================================== --- code/player/trunk/server/drivers/mixed/rmp/segwayrmp.cc 2009-07-28 06:27:59 UTC (rev 8148) +++ code/player/trunk/server/drivers/mixed/rmp/segwayrmp.cc 2009-07-28 06:32:00 UTC (rev 8149) @@ -23,24 +23,31 @@ Author: John Sweeney and Brian Gerkey Date: CVS: $Id$ -*/ + */ /** @ingroup drivers */ /** @{ */ /** @defgroup driver_segwayrmp segwayrmp * @brief Segway RMP mobile robot -@todo This driver is currently disabled because it needs to be updated to -the Player 2.0 API. +@todo Enable advanced functionality (ramp speeds, etc) -The driver has been updated to the point that it compiles, but a few key requests -have been disabled and it has not been tested in anyway. - The segwayrmp driver provides control of a Segway RMP (Robotic Mobility Platform), which is an experimental robotic version of the Segway HT (Human Transport), a kind of two-wheeled, self-balancing -electric scooter. +electric scooter. Four wheeled versions are also available, and consist +of two independent two-wheeled platforms bolted together. Thus, two instances +of this driver should be able to control a four-wheeled version. +The driver has been updated to the 3.0 API to the point that it compiles +and runs. Position2d commands and odometry have been tested and work, +and both batteries report voltages. Testing has only been done over USB, +with a Segway RMP400 platform. + +This segwayrmp driver supports a usb interface for the newer +RMP UI protocol. It also maintains compatibility for the older CAN interface. + + @par Compile-time dependencies - libcanlib (from Kvaser) @@ -50,7 +57,7 @@ - Because of its power, weight, height, and dynamics, the Segway RMP is a potentially dangerous machine. Be @e very careful with it. -- Although the RMP does not actually support motor power control from +- Although the RMP driver does not actually support motor power control from software, for safety you must explicitly enable the motors using a @p PLAYER_POSITION2D_REQ_MOTOR_POWER or @p PLAYER_POSITION3D_REQ_MOTOR_POWER (depending on which interface you are using). You must @e also enable @@ -69,7 +76,11 @@ I/O subsystem within this driver is modular, so that it should be pretty straightforward to add support for other CAN cards. +- This driver uses version 2.0 of the Segway RMP Interface Guide as a +reference. It may break compatibility with the older CAN only models, +but none were available for testing to verify. + @par Provides - @ref interface_position2d @@ -83,6 +94,9 @@ - @ref interface_power - Returns the current battery voltage (72 V when fully charged). +- @ref interface_power + - Returns the current UI battery voltage (12 V when fully charged). + @par Configuration requests - position interface @@ -97,9 +111,17 @@ @par Configuration file options +- bus (string) + - Default: "canbus" + - Communication Protocol: "usb" or "canbus" + +- usb_device (string) + - Default: "/dev/ttyUSB0" + - Device port, when using bus option "usb" + - canio (string) - Default: "kvaser" - - Type of CANbus driver. + - Type of CANbus driver, when using bus option "canbus" - max_xspeed (length / sec) - Default: 0.5 m/s @@ -115,12 +137,16 @@ driver ( name "segwayrmp" - provides ["position:0" "position3d:0" "power:0"] + provides ["position2d:0" "position3d:0" "power:0" "ui:::power:1"] + bus "usb" + usb_device "/dev/ttyUSB1" + max_xspeed 1.5 + max_yawspeed 80 ) @endverbatim -@author John Sweeney, Brian Gerkey, Andrew Howard -*/ +@author John Sweeney, Brian Gerkey, Andrew Howard, Eric Grele, Goutham Mallapragda, Anthony Cascone, Rich Mattes + */ /** @} */ @@ -151,8 +177,8 @@ // A factory creation function Driver* SegwayRMP_Init(ConfigFile* cf, int section) { - // Create and return a new instance of this driver - return ((Driver*) (new SegwayRMP(cf, section))); + // Create and return a new instance of this driver + return ((Driver*) (new SegwayRMP(cf, section))); } @@ -160,177 +186,283 @@ // A driver registration function. void segwayrmp_Register(DriverTable* table) { - table->AddDriver("segwayrmp", SegwayRMP_Init); + table->AddDriver("segwayrmp", SegwayRMP_Init); } //////////////////////////////////////////////////////////////////////////////// // Constructor SegwayRMP::SegwayRMP(ConfigFile* cf, int section) - : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN) +: ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN) { - memset(&this->position_id, 0, sizeof(player_devaddr_t)); - memset(&this->position3d_id, 0, sizeof(player_devaddr_t)); - memset(&this->power_id, 0, sizeof(player_devaddr_t)); + memset(&this->position_id, 0, sizeof(player_devaddr_t)); + memset(&this->position3d_id, 0, sizeof(player_devaddr_t)); + memset(&this->power_base_id, 0, sizeof(player_devaddr_t)); + memset(&this->power_ui_id, 0, sizeof(player_devaddr_t)); - // Do we create a position interface? - if(cf->ReadDeviceAddr(&(this->position_id), section, "provides", - PLAYER_POSITION2D_CODE, -1, NULL) == 0) - { - if(this->AddInterface(this->position_id) != 0) - { - this->SetError(-1); - return; - } - } + // Do we create a position interface? + if(cf->ReadDeviceAddr(&(this->position_id), section, "provides", + PLAYER_POSITION2D_CODE, -1, NULL) == 0) + { + if(this->AddInterface(this->position_id) != 0) + { + this->SetError(-1); + return; + } + } - // Do we create a position3d interface? - if(cf->ReadDeviceAddr(&(this->position3d_id), section, "provides", - PLAYER_POSITION3D_CODE, -1, NULL) == 0) - { - if(this->AddInterface(this->position3d_id) != 0) - { - this->SetError(-1); - return; - } - } + // Do we create a position3d interface? + if(cf->ReadDeviceAddr(&(this->position3d_id), section, "provides", + PLAYER_POSITION3D_CODE, -1, NULL) == 0) + { + if(this->AddInterface(this->position3d_id) != 0) + { + this->SetError(-1); + return; + } + } - // Do we create a power interface? - if(cf->ReadDeviceAddr(&(this->power_id), section, "provides", - PLAYER_POWER_CODE, -1, NULL) == 0) - { - if(this->AddInterface(this->power_id) != 0) - { - this->SetError(-1); - return; - } - } + // Do we create a power interface for powerbase? + if(cf->ReadDeviceAddr(&(this->power_base_id), section, "provides", + PLAYER_POWER_CODE, -1, NULL) == 0) + { + if(this->AddInterface(this->power_base_id) != 0) + { + this->SetError(-1); + return; + } + } - this->canio = NULL; - this->caniotype = cf->ReadString(section, "canio", "kvaser"); - this->max_xspeed = (int) (1000.0 * cf->ReadLength(section, "max_xspeed", 0.5)); - if(this->max_xspeed < 0) - this->max_xspeed = -this->max_xspeed; - this->max_yawspeed = (int) (RTOD(cf->ReadAngle(section, "max_yawspeed", 40))); - if(this->max_yawspeed < 0) - this->max_yawspeed = -this->max_yawspeed; + // Do we create a power interface for userinterface? + if(cf->ReadDeviceAddr(&(this->power_ui_id), section, "provides", + PLAYER_POWER_CODE, -1, "ui") == 0) + { + if(this->AddInterface(this->power_ui_id) != 0) + { + this->SetError(-1); + return; + } + } - return; + // Determine if we're using the CAN or USB bus + bus_type = UNKNOWN; + const char *bus_setting = cf->ReadString(section,"bus","canbus"); + if( strcmp( bus_setting, "canbus" ) == 0 ) + { + bus_type = CANBUS; + PLAYER_MSG0(2, "Got CAN Bus"); + + } + else if(strcmp( bus_setting, "usb" ) == 0 ) + { + PLAYER_MSG0(2, "Got USB Bus"); + bus_type = USB; + } + else + { + PLAYER_ERROR1("Unknown Bus Type \"%s\"", bus_setting); + this->SetError(-1); + return; + } + + this->canio = NULL; + this->usbio = 0; + + // Read in port settings + this->caniotype = cf->ReadString(section, "canio", "kvaser"); + this->usb_device = cf->ReadString(section, "usb_device", "/dev/ttyUSB0" ); + + // Read in maximum speeds + this->max_xspeed = (int) (1000.0 * cf->ReadLength(section, "max_xspeed", 0.5)); + if(this->max_xspeed < 0) + this->max_xspeed = -this->max_xspeed; + this->max_yawspeed = (int) (RTOD(cf->ReadAngle(section, "max_yawspeed", 40))); + if(this->max_yawspeed < 0) + this->max_yawspeed = -this->max_yawspeed; + + return; } SegwayRMP::~SegwayRMP() { - return; + return; } int SegwayRMP::MainSetup() { - // Clear the command buffers + // Clear the command buffers #if 0 - if (this->position_id.code) - ClearCommand(this->position_id); - if (this->position3d_id.code) - ClearCommand(this->position3d_id); + if (this->position_id.code) + ClearCommand(this->position_id); + if (this->position3d_id.code) + ClearCommand(this->position3d_id); #endif - PLAYER_MSG0(2, "CAN bus initializing"); + // Start the USB or CAN bus with a helper function + if( bus_type == CANBUS ) + { + if( CanBusSetup() != 0 ) + { + return -1; + } + } + else if ( bus_type == USB) + { + if (USBSetup() !=0) + { + return -1; + } + } + else + { + PLAYER_ERROR("Unknown Bus type, please select CAN or USB"); + return -1; + } - if(!strcmp(this->caniotype, "kvaser")) - assert(this->canio = new CANIOKvaser); - else - { - PLAYER_ERROR1("Unknown CAN I/O type: \"%s\"", this->caniotype); - return(-1); - } + // Initialize odometry + this->odom_x = this->odom_y = this->odom_yaw = 0.0; - // start the CAN at 500 kpbs - if(this->canio->Init(BAUD_500K) < 0) - { - PLAYER_ERROR("error on CAN Init"); - return(-1); - } + this->curr_xspeed = this->curr_yawspeed = 0.0; + this->motor_allow_enable = false; + this->motor_enabled = false; + this->firstread = true; + this->timeout_counter = 0; - // Initialize odometry - this->odom_x = this->odom_y = this->odom_yaw = 0.0; + return(0); +} - this->curr_xspeed = this->curr_yawspeed = 0.0; - this->motor_allow_enable = false; - this->motor_enabled = false; - this->firstread = true; - this->timeout_counter = 0; +int SegwayRMP::CanBusSetup() { - return(0); + PLAYER_MSG0(2, "CAN bus initializing"); + + if(!strcmp(this->caniotype, "kvaser")) + assert(this->canio = new CANIOKvaser); + else + { + PLAYER_ERROR1("Unknown CAN I/O type: \"%s\"", this->caniotype); + return(-1); + } + + // start the CAN at 500 kpbs + if(this->canio->Init(BAUD_500K) < 0) + { + PLAYER_ERROR("Error on CAN Init, could not start CAN bus"); + return(-1); + } + return 0; } +int SegwayRMP::USBSetup() +{ + usbio = new USBIO(); + PLAYER_MSG0(2,"Starting USB BUS"); + if( !usbio ) + { + PLAYER_ERROR("Could not create USBIO device"); + return -1; + } + if( usbio->Init(usb_device) != 0 ) + { + PLAYER_ERROR("Error or USB Init, could not start USB bus"); + return -1; + } + + return 0; +} + + void SegwayRMP::MainQuit() { - PLAYER_MSG0(2, "Shutting down CAN bus"); - fflush(stdout); + PLAYER_MSG0(2, "Shutting down CAN bus"); + fflush(stdout); - // send zero velocities, for some semblance of safety - CanPacket pkt; + // send zero velocities, for some semblance of safety + CanPacket pkt; - MakeVelocityCommand(&pkt,0,0); - Write(pkt); + MakeVelocityCommand(&pkt,0,0); + Write(pkt); - // shutdown the CAN - canio->Shutdown(); - delete canio; - canio = NULL; + // shutdown the appropraite device + if( bus_type == CANBUS ) { + // shutdown the CAN + PLAYER_MSG0(2, "Shutting down CAN bus"); + fflush(stdout); + canio->Shutdown(); + delete canio; + canio = NULL; + } + else if (bus_type == USB ) { + // shutdown the USB + PLAYER_MSG0(2, "Shutting down USB bus"); + fflush(stdout); + usbio->Shutdown(); + delete usbio; + usbio = 0; + } + return; } // Main function for device thread. void SegwayRMP::Main() { - //unsigned char buffer[256]; - //size_t buffer_len; - //player_position2d_cmd_t position_cmd; - //player_position3d_cmd_t position3d_cmd; - //void *client; - CanPacket pkt; - //int32_t xspeed,yawspeed; - //bool got_command; - bool first; + //unsigned char buffer[256]; + //size_t buffer_len; + //player_position2d_cmd_t position_cmd; + //player_position3d_cmd_t position3d_cmd; + //void *client; + CanPacket pkt; + //int32_t xspeed,yawspeed; + //bool got_command; + bool first; + struct timeval now; + double time_stamp, sent_command_time; - pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); - pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL); + pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL); + pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL); - first = true; - PLAYER_MSG0(2, "starting main loop"); + first = true; + sent_command_time = 0.0; + PLAYER_MSG0(2, "starting main loop"); - for(;;) - { - pthread_testcancel(); - ProcessMessages(); - // Read from the RMP - if(Read() < 0) - { - PLAYER_ERROR("Read() errored; bailing"); - pthread_exit(NULL); - } + for(;;) + { + pthread_testcancel(); + speed_change = false; - // Note that we got some data - if (first) - { - first = false; - PLAYER_MSG0(2, "got data from rmp"); - } + ProcessMessages(); + // Read from the RMP - // TODO: report better timestamps, possibly using time info from the RMP + if(Read() < 0) + { + PLAYER_ERROR("Read() errored; bailing"); + pthread_exit(NULL); + } - // Send data to clients - Publish(this->position_id, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, - &this->position_data, sizeof(this->position_data), NULL); - Publish(this->position3d_id, PLAYER_MSGTYPE_DATA, PLAYER_POSITION3D_DATA_STATE, - &this->position3d_data, sizeof(this->position3d_data), NULL); - Publish(this->power_id, PLAYER_MSGTYPE_DATA, PLAYER_POWER_DATA_STATE, - &this->power_data, sizeof(this->power_data), NULL); + // Note that we got some data + if (first) + { + first = false; + PLAYER_MSG0(2, "got data from rmp"); + } -/* // check for config requests from the position interface + ///@ TODO + /// report better timestamps, possibly using time info from the RMP + + // Send data to clients + Publish(this->position_id, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, + &this->position_data, sizeof(this->position_data), NULL); + Publish(this->position3d_id, PLAYER_MSGTYPE_DATA, PLAYER_POSITION3D_DATA_STATE, + &this->position3d_data, sizeof(this->position3d_data), NULL); + Publish(this->power_base_id, PLAYER_MSGTYPE_DATA, PLAYER_POWER_DATA_STATE, + &this->power_base_data, sizeof(this->power_base_data), NULL); + Publish(this->power_ui_id, PLAYER_MSGTYPE_DATA, PLAYER_POWER_DATA_STATE, + &this->power_ui_data, sizeof(this->power_ui_data), NULL); + +/* // check for config requests from the position interface if((buffer_len = GetConfig(this->position_id, &client, buffer, sizeof(buffer),NULL)) > 0) { // if we write to the CAN bus as a result of the config, don't write @@ -396,82 +528,100 @@ } // No commands, so we may timeout soon if (!got_command)*/ - timeout_counter++; + // Increment counter, note, counter is reset when a valid command is received + // in ProcessMessages + timeout_counter++; - if(timeout_counter >= RMP_TIMEOUT_CYCLES) - { - if(curr_xspeed || curr_yawspeed) - { - PLAYER_WARN("timeout exceeded without new commands; stopping robot"); - curr_xspeed = 0; - curr_yawspeed = 0; - } - // set it to the limit, to prevent overflow, but keep the robot - // stopped until a new command comes in. - timeout_counter = RMP_TIMEOUT_CYCLES; - } + if(timeout_counter >= RMP_TIMEOUT_CYCLES) + { + if(curr_xspeed || curr_yawspeed) + { + /// @todo + /// We are getting intermittent timeouts. Does not break functionality, but should be checked. - if(!motor_enabled) - { - curr_xspeed = 0; - curr_yawspeed = 0; - } + PLAYER_WARN("timeout exceeded without new commands; stopping robot"); + curr_xspeed = 0; + curr_yawspeed = 0; + speed_change = true; + } + // set it to the limit, to prevent overflow, but keep the robot + // stopped until a new command comes in. + timeout_counter = RMP_TIMEOUT_CYCLES; + } - // make a velocity command... could be zero - MakeVelocityCommand(&pkt,static_cast<int> (curr_xspeed),static_cast<int> (curr_yawspeed)); - if(Write(pkt) < 0) - PLAYER_ERROR("error on write"); - } + if(!motor_enabled) + { + curr_xspeed = 0; + curr_yawspeed = 0; + } + PLAYER_MSG2(2,"setting velocity to curr_xspeed %f curr_yawspeed %f\n",curr_xspeed,curr_yawspeed); + + // make a velocity command... could be zero + // make sure we send commands at a rate of 20Hz, or on speed change + // note this condition does not affect the timeout function above + gettimeofday( &now, 0 ); + time_stamp = (1.0*now.tv_sec) + (0.000001*now.tv_usec); + + if( time_stamp - sent_command_time > 0.05 || speed_change ) + { + MakeVelocityCommand(&pkt,static_cast<int> (curr_xspeed),static_cast<int> (curr_yawspeed)); + + if(Write(pkt) < 0) + PLAYER_ERROR("error on write"); + + sent_command_time = time_stamp; + } + } } int SegwayRMP::ProcessMessage(QueuePointer & resp_queue, - player_msghdr * hdr, - void * data) + player_msghdr * hdr, + void * data) { - /// @todo - /// Handle config requests + /// @todo + /// Handle config requests - // 2-D velocity command - if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, - PLAYER_POSITION2D_CMD_VEL, - this->position_id)) - { - player_position2d_cmd_vel_t* cmd = (player_position2d_cmd_vel_t*)data; - this->curr_xspeed = cmd->vel.px * 1000.0; //Added multiply by 1000.0, BMS. - this->curr_yawspeed = cmd->vel.pa * 180.0/M_PI; //Added multiply by 1000.0, BMS. - this->motor_enabled = cmd->state & this->motor_allow_enable; - this->timeout_counter = 0; - return 0; - } - // 3-D velocity command - if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, - PLAYER_POSITION3D_CMD_SET_VEL, - this->position3d_id)) - { - player_position3d_cmd_vel_t* cmd = (player_position3d_cmd_vel_t*)data; - this->curr_xspeed = cmd->vel.px * 1000.0; //Added multiply by 1000.0, BMS. - this->curr_yawspeed = cmd->vel.pyaw * 180.0/M_PI; ////Added multiply by 1000.0, BMS. - this->motor_enabled = cmd->state & this->motor_allow_enable; - this->timeout_counter = 0; - return 0; - } + // 2-D velocity command + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, + PLAYER_POSITION2D_CMD_VEL, + this->position_id)) + { + player_position2d_cmd_vel_t* cmd = (player_position2d_cmd_vel_t*)data; + this->curr_xspeed = cmd->vel.px * 1000.0; //Added multiply by 1000.0, BMS. + this->curr_yawspeed = cmd->vel.pa * 180.0/M_PI; //Added multiply by 1000.0, BMS. + this->motor_enabled = cmd->state & this->motor_allow_enable; + this->timeout_counter = 0; + return 0; + } + // 3-D velocity command + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, + PLAYER_POSITION3D_CMD_SET_VEL, + this->position3d_id)) + { + player_position3d_cmd_vel_t* cmd = (player_position3d_cmd_vel_t*)data; + this->curr_xspeed = cmd->vel.px * 1000.0; //Added multiply by 1000.0, BMS. + this->curr_yawspeed = cmd->vel.pyaw * 180.0/M_PI; ////Added multiply by 1000.0, BMS. + this->motor_enabled = cmd->state & this->motor_allow_enable; + this->timeout_counter = 0; + return 0; + } - if (hdr->type == PLAYER_MSGTYPE_REQ && hdr->addr.interf == position_id.interf - && hdr->addr.index == position_id.index) - { - return HandlePositionConfig(resp_queue, hdr->subtype, data, hdr->size); - } + if (hdr->type == PLAYER_MSGTYPE_REQ && hdr->addr.interf == position_id.interf + && hdr->addr.index == position_id.index) + { + return HandlePositionConfig(resp_queue, hdr->subtype, data, hdr->size); + } - if (hdr->type == PLAYER_MSGTYPE_REQ && hdr->addr.interf == position3d_id.interf - && hdr->addr.index == position3d_id.index) - { - return HandlePosition3DConfig(resp_queue, hdr->subtype, data, hdr->size); - } + if (hdr->type == PLAYER_MSGTYPE_REQ && hdr->addr.interf == position3d_id.interf + && hdr->addr.index == position3d_id.index) + { + return HandlePosition3DConfig(resp_queue, hdr->subtype, data, hdr->size); + } - return(-1); + return(-1); } // helper to handle config requests @@ -480,62 +630,65 @@ int SegwayRMP::HandlePositionConfig(QueuePointer &client, uint32_t subtype, void* buffer, size_t len) { - uint16_t rmp_cmd,rmp_val; - //player_rmp_config_t *rmp; - CanPacket pkt; + //uint16_t rmp_cmd,rmp_val; + //player_rmp_config_t *rmp; + CanPacket pkt; - switch(subtype) - { - case PLAYER_POSITION2D_REQ_MOTOR_POWER: - // just set a flag telling us whether we should - // act on motor commands - // set the commands to 0... think it will automatically - // do this for us. - if(((char *) buffer)[0]) - this->motor_allow_enable = true; - else - this->motor_allow_enable = false; + switch(subtype) + { + case PLAYER_POSITION2D_REQ_MOTOR_POWER: + // just set a flag telling us whether we should + // act on motor commands + // set the commands to 0... think it will automatically + // do this for us. + if(((char *) buffer)[0]) + this->motor_allow_enable = true; + else + this->motor_allow_enable = false; - printf("SEGWAYRMP: motors state: %d\n", this->motor_allow_enable); + printf("SEGWAYRMP: motors state: %d\n", this->motor_allow_enable); - Publish(position_id, client, PLAYER_MSGTYPE_RESP_ACK,subtype); - return 0; + Publish(position_id, client, PLAYER_MSGTYPE_RESP_ACK,subtype); + return 0; - case PLAYER_POSITION2D_REQ_GET_GEOM: - player_position2d_geom_t geom; - geom.pose.px = 0; - geom.pose.py = 0; - geom.pose.pa = 0; - geom.size.sw = 0.508; - geom.size.sl = 0.610; + case PLAYER_POSITION2D_REQ_GET_GEOM: + player_position2d_geom_t geom; + geom.pose.px = 0; + geom.pose.py = 0; + geom.pose.pz = 0; + geom.pose.proll= 0; + geom.pose.ppitch = 0; + geom.pose.pyaw = 0; + geom.size.sw = 0.508; + geom.size.sl = 0.610; - Publish(position_id, client, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_GET_GEOM, &geom, sizeof(geom),NULL); - return 0; + Publish(position_id, client, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_GET_GEOM, &geom, sizeof(geom),NULL); + return 0; - case PLAYER_POSITION2D_REQ_RESET_ODOM: - // we'll reset all the integrators + case PLAYER_POSITION2D_REQ_RESET_ODOM: + // we'll reset all the integrators - MakeStatusCommand(&pkt, (uint16_t)RMP_CAN_CMD_RST_INT, - (uint16_t)RMP_CAN_RST_ALL); - if(Write(pkt) < 0) - { - Publish(position_id, client, PLAYER_MSGTYPE_RESP_NACK,PLAYER_POSITION2D_REQ_RESET_ODOM); - } - else - { + MakeStatusCommand(&pkt, (uint16_t)RMP_CAN_CMD_RST_INT, + (uint16_t)RMP_CAN_RST_ALL); + if(Write(pkt) < 0) + { + Publish(position_id, client, PLAYER_MSGTYPE_RESP_NACK,PLAYER_POSITION2D_REQ_RESET_ODOM); + } + else + { - if (Write(pkt) < 0) { - Publish(position_id, client, PLAYER_MSGTYPE_RESP_NACK,PLAYER_POSITION2D_REQ_RESET_ODOM); - } else { - Publish(position_id, client, PLAYER_MSGTYPE_RESP_ACK,PLAYER_POSITION2D_REQ_RESET_ODOM); - } - } + if (Write(pkt) < 0) { + Publish(position_id, client, PLAYER_MSGTYPE_RESP_NACK,PLAYER_POSITION2D_REQ_RESET_ODOM); + } else { + Publish(position_id, client, PLAYER_MSGTYPE_RESP_ACK,PLAYER_POSITION2D_REQ_RESET_ODOM); + } + } - odom_x = odom_y = odom_yaw = 0.0; - firstread = true; + odom_x = odom_y = odom_yaw = 0.0; + firstread = true; - // return 1 to indicate that we wrote to the CAN bus this time - return(0); + // return 1 to indicate that we wrote to the CAN bus this time + return(0); /* case PLAYER_POSITION_RMP_VELOCITY_SCALE: rmp_cmd = RMP_CAN_CMD_MAX_VEL; @@ -679,17 +832,14 @@ } // return 1 to indicate that we wrote to the CAN bus this time return(1); -*/ - default: - printf("segwayrmp received unknown config request %d\n", - subtype); -/* if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK,NULL)) - PLAYER_ERROR("Failed to PutReply in segwayrmp\n");*/ - break; - } + */ + default: + printf("segwayrmp received unknown config request %d\n",subtype); + break; + } - // return -1, to indicate that we did NOT handle the message - return(-1); + // return -1, to indicate that we did NOT handle the message + return(-1); } // helper to handle config requests @@ -698,214 +848,270 @@ int SegwayRMP::HandlePosition3DConfig(QueuePointer &client, uint32_t subtype, void* buffer, size_t len) { - switch(subtype) - { - case PLAYER_POSITION3D_REQ_MOTOR_POWER: - // just set a flag telling us whether we should - // act on motor commands - // set the commands to 0... think it will automatically - // do this for us. - if(((char*)buffer)[0]) - this->motor_allow_enable = true; - else - this->motor_allow_enable = false; + switch(subtype) + { + case PLAYER_POSITION3D_REQ_MOTOR_POWER: + // just set a flag telling us whether we should + // act on motor commands + // set the commands to 0... think it will automatically + // do this for us. + if(((char*)buffer)[0]) + this->motor_allow_enable = true; + else + this->motor_allow_enable = false; - printf("SEGWAYRMP: motors state: %d\n", this->motor_allow_enable); + printf("SEGWAYRMP: motors state: %d\n", this->motor_allow_enable); - Publish(this->position3d_id, client, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION3D_REQ_MOTOR_POWER); - return 0; + Publish(this->position3d_id, client, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION3D_REQ_MOTOR_POWER); + return 0; - default: - printf("segwayrmp received unknown config request %d\n", - subtype); - } + default: + printf("segwayrmp received unknown config request %d\n", + subtype); + } - // return -1, to indicate that we did process the message - return(-1); + // return -1, to indicate that we did process the message + return(-1); } int SegwayRMP::Read() { - CanPacket pkt; - int channel; - int ret; - rmp_frame_t data_frame[2]; + if( bus_type == CANBUS ) + return CanBusRead(); + else if( bus_type == USB ) + return USBRead(); + else + return -1; +} - //static struct timeval last; - //struct timeval curr; +int SegwayRMP::CanBusRead() +{ + CanPacket pkt; + int channel; + int ret; + rmp_frame_t data_frame[2]; - data_frame[0].ready = 0; - data_frame[1].ready = 0; + //static struct timeval last; + //struct timeval curr; - // read one cycle of data from each channel - for(channel = 0; channel < DUALCAN_NR_CHANNELS; channel++) - { - ret=0; - // read until we've gotten all 5 packets for this cycle on this channel - while((ret = canio->ReadPacket(&pkt, channel)) >= 0) - { - // then we got a new packet... + data_frame[0].ready = 0; + data_frame[1].ready = 0; - //printf("SEGWAYIO: pkt: %s\n", pkt.toString()); + // read one cycle of data from each channel + for(channel = 0; channel < DUALCAN_NR_CHANNELS; channel++) + { + ret=0; + // read until we've gotten all 5 packets for this cycle on this channel + while((ret = canio->ReadPacket(&pkt, channel)) >= 0) + { + // then we got a new packet... + //printf("SEGWAYIO: pkt: %s\n", pkt.toString()); - data_frame[channel].AddPacket(pkt); + data_frame[channel].AddPacket(pkt); - // if data has been filled in, then let's make it the latest data - // available to player... - if(data_frame[channel].IsReady()) - { - // Only bother doing the data conversions for one channel. - // It appears that the data on channel 0 is garbarge, so we'll read - // from channel 1. - if(channel == 1) - { - UpdateData(&data_frame[channel]); - } + // if data has been filled in, then let's make it the latest data + // available to player... + if(data_frame[channel].IsReady()) + { + // Only bother doing the data conversions for one channel. + // It appears that the data on channel 0 is garbarge, so we'll read + // from channel 1. + if(channel == 1) + { + UpdateData(&data_frame[channel]); + } - data_frame[channel].ready = 0; - break; - } - } + data_frame[channel].ready = 0; + break; + } + } - if (ret < 0) - { - PLAYER_ERROR2("error (%d) reading packet on channel %d", - ret, channel); - } - } + if (ret < 0) + { + PLAYER_ERROR2("error (%d) reading packet on channel %d", ret, channel); + } + } - return(0); + return(0); } +int SegwayRMP::USBRead() { + CanPacket pkt; + int ret; + rmp_frame_t data_frame; + + //static struct timeval last; + //struct timeval curr; + + data_frame.ready = 0; + + ret=0; + // read until we've gotten all 8 packets for this cycle on this channel + while((ret = usbio->ReadPacket(&pkt)) >= 0) + { + // then we got a new packet... + /*printf("SEGWAYIO: READ : pkt: %s\n", pkt.toString()); + fflush(stdout);*/ + + data_frame.AddPacket(pkt); + + // if data has been filled in, then let's make it the latest data + // available to player... + + if(data_frame.IsReady()) { + UpdateData(&data_frame); + data_frame.ready = 0; + break; + } + } + + if (ret < 0) { + PLAYER_ERROR1("error (%d) reading packet on usb", ret); + } + + return(0); +} + + void SegwayRMP::UpdateData(rmp_frame_t * data_frame) { - int delta_lin_raw, delta_ang_raw; - double delta_lin, delta_ang; - double tmp; + int delta_lin_raw, delta_ang_raw; + double delta_lin, delta_ang; + double tmp; - // Get the new linear and angular encoder values and compute - // odometry. Note that we do the same thing here, regardless of - // whether we're presenting 2D or 3D position info. - delta_lin_raw = Diff(this->last_raw_foreaft, - data_frame->foreaft, - this->firstread); - this->last_raw_foreaft = data_frame->foreaft; + // Get the new linear and angular encoder values and compute + // odometry. Note that we do the same thing here, regardless of + // whether we're presenting 2D or 3D position info. + delta_lin_raw = Diff(this->last_raw_foreaft, + data_frame->foreaft, + this->firstread); + this->last_raw_foreaft = data_frame->foreaft; - delta_ang_raw = Diff(this->last_raw_yaw, - data_frame->yaw, - this->firstread); - this->last_raw_yaw = data_frame->yaw; + delta_ang_raw = Diff(this->last_raw_yaw, + data_frame->yaw, + this->firstread); + this->last_raw_yaw = data_frame->yaw; - delta_lin = (double)delta_lin_raw / (double)RMP_COUNT_PER_M; - delta_ang = DTOR((double)delta_ang_raw / - (double)RMP_COUNT_PER_REV * 360.0); + delta_lin = (double)delta_lin_raw / (double)RMP_COUNT_PER_M; + delta_ang = DTOR((double)delta_ang_raw / + (double)RMP_COUNT_PER_REV * 360.0); - // First-order odometry integration - this->odom_x += delta_lin * cos(this->odom_yaw); - this->odom_y += delta_lin * sin(this->odom_yaw); - this->odom_yaw += delta_ang; + // First-order odometry integration + this->odom_x += delta_lin * cos(this->odom_yaw); + this->odom_y += delta_lin * sin(this->odom_yaw); + this->odom_yaw += delta_ang; - // Normalize yaw in [0, 360] - this->odom_yaw = atan2(sin(this->odom_yaw), cos(this->odom_yaw)); - if (this->odom_yaw < 0) - this->odom_yaw += 2 * M_PI; + // Normalize yaw in [0, 360] + this->odom_yaw = atan2(sin(this->odom_yaw), cos(this->odom_yaw)); + if (this->odom_yaw < 0) + this->odom_yaw += 2 * M_PI; - // first, do 2D info. - this->position_data.pos.px = this->odom_x; - this->position_data.pos.py = this->odom_y; - this->position_data.pos.pa = this->odom_yaw; + // first, do 2D info. + this->position_data.pos.px = this->odom_x; + this->position_data.pos.py = this->odom_y; + this->position_data.pos.pa = this->odom_yaw; - // combine left and right wheel velocity to get foreward velocity - // change from counts/s into mm/s - this->position_data.vel.px = ((double)data_frame->left_dot + - (double)data_frame->right_dot) / - (double)RMP_COUNT_PER_M_PER_S - / 2.0; + // combine left and right wheel velocity to get foreward velocity + // change from counts/s into mm/s + this->position_data.vel.px = ((double)data_frame->left_dot + + (double)data_frame->right_dot) / + (double)RMP_COUNT_PER_M_PER_S + / 2.0; - // no side speeds for this bot - this->position_data.vel.py = 0; + // no side speeds for this bot + this->position_data.vel.py = 0; - // from counts/sec into deg/sec. also, take the additive - // inverse, since the RMP reports clockwise angular velocity as - // positive. - this->position_data.vel.pa = - DTOR(-(double)data_frame->yaw_dot / (double)RMP_COUNT_PER_DEG_PER_S); + // from counts/sec into deg/sec. also, take the additive + // inverse, since the RMP reports clockwise angular velocity as + // positive. + this->position_data.vel.pa = + DTOR(-(double)data_frame->yaw_dot / (double)RMP_COUNT_PER_DEG_PER_S); - this->position_data.stall = 0; + this->position_data.stall = 0; - // now, do 3D info. - this->position3d_data.pos.px = this->odom_x; - this->position3d_data.pos.py = this->odom_y; - // this robot doesn't fly - this->position3d_data.pos.pz = 0; + // now, do 3D info. + this->position3d_data.pos.px = this->odom_x; + this->position3d_data.pos.py = this->odom_y; + // this robot doesn't fly + this->position3d_data.pos.pz = 0; - /// TODO left off here + /// TODO left off here - // normalize angles to [0,360] - tmp = NORMALIZE(DTOR((double)data_frame->roll / - (double)RMP_COUNT_PER_DEG)); - if(tmp < 0) - tmp += 2*M_PI; - this->position3d_data.pos.proll = tmp;//htonl((int32_t)rint(tmp * 1000.0)); + // normalize angles to [0,360] + tmp = NORMALIZE(DTOR((double)data_frame->roll / + (double)RMP_COUNT_PER_DEG)); + if(tmp < 0) + tmp += 2*M_PI; + this->position3d_data.pos.proll = tmp;//htonl((int32_t)rint(tmp * 1000.0)); - // normalize angles to [0,360] - tmp = NORMALIZE(DTOR((double)data_frame->pitch / - (double)RMP_COUNT_PER_DEG)); - if(tmp < 0) - tmp += 2*M_PI; - this->position3d_data.pos.ppitch = tmp;//htonl((int32_t)rint(tmp * 1000.0)); + // normalize angles to [0,360] + tmp = NORMALIZE(DTOR((double)data_frame->pitch / + (double)RMP_COUNT_PER_DEG)); + if(tmp < 0) + tmp += 2*M_PI; + this->position3d_data.pos.ppitch = tmp;//htonl((int32_t)rint(tmp * 1000.0)); - this->position3d_data.pos.pyaw = tmp;//htonl(((int32_t)(this->odom_yaw * 1000.0))); + this->position3d_data.pos.pyaw = tmp;//htonl(((int32_t)(this->odom_yaw * 1000.0))); - // combine left and right wheel velocity to get foreward velocity - // change from counts/s into m/s - this->position3d_data.vel.px = - ((double)data_frame->left_dot + - (double)data_frame->right_dot) / - (double)RMP_COUNT_PER_M_PER_S - / 2.0; - // no side or vertical speeds for this bot - this->position3d_data.vel.py = 0; - this->position3d_data.vel.pz = 0; + // combine left and right wheel velocity to get foreward velocity + // change from counts/s into m/s + this->position3d_data.vel.px = + ((double)data_frame->left_dot + + (double)data_frame->right_dot) / + (double)RMP_COUNT_PER_M_PER_S + / 2.0; + // no side or vertical speeds for this bot + this->position3d_data.vel.py = 0; + this->position3d_data.vel.pz = 0; - this->position3d_data.vel.proll = - (double)data_frame->roll_dot / - (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180; - this->position3d_data.vel.ppitch = - (double)data_frame->pitch_dot / - (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180; - // from counts/sec into millirad/sec. also, take the additive - // inverse, since the RMP reports clockwise angular velocity as - // positive. + this->position3d_data.vel.proll = + (double)data_frame->roll_dot / + (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180; + this->position3d_data.vel.ppitch = + (double)data_frame->pitch_dot / + (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180; + // from counts/sec into millirad/sec. also, take the additive + // inverse, since the RMP reports clockwise angular velocity as + // positive. - // This one uses left_dot and right_dot, which comes from odometry - this->position3d_data.vel.pyaw = - (double)(data_frame->right_dot - data_frame->left_dot) / - (RMP_COUNT_PER_M_PER_S * RMP_GEOM_WHEEL_SEP * M_PI); - // This one uses yaw_dot, which comes from the IMU - //data.position3d_data.yawspeed = - // htonl((int32_t)(-rint((double)data_frame->yaw_dot / - // (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180 * 1000))); + // This one uses left_dot and right_dot, which comes from odometry + this->position3d_data.vel.pyaw = + (double)(data_frame->right_dot - data_frame->left_dot) / + (RMP_COUNT_PER_M_PER_S * RMP_GEOM_WHEEL_SEP * M_PI); + // This one uses yaw_dot, which comes from the IMU + //data.position3d_data.yawspeed = + // htonl((int32_t)(-rint((double)data_frame->yaw_dot / + // (double)RMP_COUNT_PER_DEG_PER_S * M_PI / 180 * 1000))); - this->position3d_data.stall = 0; + this->position3d_data.stall = 0; - // fill in power data. the RMP returns a percentage of full, - // and the specs for the HT say that it's a 72 volt system. assuming - // that the RMP is the same, we'll convert to decivolts for Player. - this->power_data.volts = - data_frame->battery * 72; - firstread = false; + // Get Battery voltage from base (72v pack) + // Manual says the segway returns 4 counts per volt + this->power_base_data.volts = + data_frame->powerbase_battery /4; + this->power_base_data.valid = PLAYER_POWER_MASK_VOLTS; + + // Manual says the segway returns (1.4 + counts * .0125) volts + this->power_ui_data.volts = + (data_frame->ui_battery * .0125) + 1.4; + this->power_ui_data.valid = PLAYER_POWER_MASK_VOLTS; + firstread = false; } int SegwayRMP::Write(CanPacket& pkt) { - return(canio->WritePacket(pkt)); + if( bus_type == CANBUS ) + return(canio->WritePacket(pkt)); + else if( bus_type == USB ) + return usbio->WritePacket(pkt); + else + return -1; } /* Creates a status CAN packet from the given arguments @@ -913,41 +1119,41 @@ void SegwayRMP::MakeStatusCommand(CanPacket* pkt, uint16_t cmd, uint16_t val) { - int16_t trans,rot; + int16_t trans,rot; - pkt->id = RMP_CAN_ID_COMMAND; - pkt->PutSlot(2, cmd); + pkt->id = RMP_CAN_ID_COMMAND; + pkt->PutSlot(2, cmd); - // it was noted in the windows demo code that they - // copied the 8-bit value into both bytes like this - pkt->PutByte(6, val); - pkt->PutByte(7, val); + // it was noted in the windows demo code that they + // copied the 8-bit value into both bytes like this + pkt->PutByte(6, val); + pkt->PutByte(7, val); - trans = (int16_t) rint((double)this->curr_xspeed * - (double)RMP_COUNT_PER_MM_PER_S); + trans = (int16_t) rint((double)this->curr_xspeed * + (double)RMP_COUNT_PER_MM_PER_S); - if(trans > RMP_MAX_TRANS_VEL_COUNT) - trans = RMP_MAX_TRANS_VEL_COUNT; - else if(trans < -RMP_MAX_TRANS_VEL_COUNT) - trans = -RMP_MAX_TRANS_VEL_COUNT; + if(trans > RMP_MAX_TRANS_VEL_COUNT) + trans = RMP_MAX_TRANS_VEL_COUNT; + else if(trans < -RMP_MAX_TRANS_VEL_COUNT) + trans = -RMP_MAX_TRANS_VEL_COUNT; - rot = (int16_t) rint((double)this->curr_yawspeed * - (double)RMP_COUNT_PER_DEG_PER_SS); + rot = (int16_t) rint((double)this->curr_yawspeed * + (double)RMP_COUNT_PER_DEG_PER_SS); - if(rot > RMP_MAX_ROT_VEL_COUNT) - rot = RMP_MAX_ROT_VEL_COUNT; - else if(rot < -RMP_MAX_ROT_VEL_COUNT) - rot = -RMP_MAX_ROT_VEL_COUNT; + if(rot > RMP_MAX_ROT_VEL_COUNT) + rot = RMP_MAX_ROT_VEL_COUNT; + else if(rot < -RMP_MAX_ROT_VEL_COUNT) + rot = -RMP_MAX_ROT_VEL_COUNT; - // put in the last speed commands as well - pkt->PutSlot(0,(uint16_t)trans); - pkt->PutSlot(1,(uint16_t)rot); + // put in the last speed commands as well + pkt->PutSlot(0,(uint16_t)trans); + pkt->PutSlot(1,(uint16_t)rot); - if(cmd) - { - printf("SEGWAYIO: STATUS: cmd: %02x val: %02x pkt: %s\n", - cmd, val, pkt->toString()); - } + if(cmd) + { + printf("SEGWAYIO: STATUS: cmd: %02x val: %02x pkt: %s\n", + cmd, val, pkt->toString()); + } } /* takes a player command (in host byte order) and turns it into CAN packets @@ -955,77 +1161,77 @@ */ void SegwayRMP::MakeVelocityCommand(CanPacket* pkt, - int32_t xspeed, - int32_t yawspeed) + int32_t xspeed, + int32_t yawspeed) { - pkt->id = RMP_CAN_ID_COMMAND; - pkt->PutSlot(2, (uint16_t)RMP_CAN_CMD_NONE); + pkt->id = RMP_CAN_ID_COMMAND; + pkt->PutSlot(2, (uint16_t)RMP_CAN_CMD_NONE); - // we only care about cmd.xspeed and cmd.yawspeed - // translational velocity is given to RMP in counts - // [-1176,1176] ([-8mph,8mph]) + // we only care about cmd.xspeed and cmd.yawspeed + // translational velocity is given to RMP in counts + // [-1176,1176] ([-8mph,8mph]) - // player is mm/s - // 8mph is 3576.32 mm/s - // so then mm/s -> counts = (1176/3576.32) = 0.32882963 + // player is mm/s + // 8mph is 3576.32 mm/s + // so then mm/s -> counts = (1176/3576.32) = 0.32882963 - if(xspeed > this->max_xspeed) - { - PLAYER_WARN2("xspeed thresholded! (%d > %d)", xspeed, this->max_xspeed); - xspeed = this->max_xspeed; - } - else if(xspeed < -this->max_xspeed) - { - PLAYER_WARN2("xspeed thresholded! (%d < %d)", xspeed, -this->max_xspeed); - xspeed = -this->max_xspeed; - } + if(xspeed > this->max_xspeed) + { + PLAYER_WARN2("xspeed thresholded! (%d > %d)", xspeed, this->max_xspeed); + xspeed = this->max_xspeed; + } + else if(xspeed < -this->max_xspeed) + { + PLAYER_WARN2("xspeed thresholded! (%d < %d)", xspeed, -this->max_xspeed); + xspeed = -this->max_xspeed; + } - this->curr_xspeed = xspeed; + this->curr_xspeed = xspeed; - int16_t trans = (int16_t) rint((double)xspeed * - (double)RMP_COUNT_PER_MM_PER_S); + int16_t trans = (int16_t) rint((double)xspeed * + (double)RMP_COUNT_PER_MM_PER_S); - if(trans > RMP_MAX_TRANS_VEL_COUNT) - trans = RMP_MAX_TRANS_VEL_COUNT; - else if(trans < -RMP_MAX_TRANS_VEL_COUNT) - trans = -RMP_MAX_TRANS_VEL_COUNT; + if(trans > RMP_MAX_TRANS_VEL_COUNT) + trans = RMP_MAX_TRANS_VEL_COUNT; + else if(trans < -RMP_MAX_TRANS_VEL_COUNT) + trans = -RMP_MAX_TRANS_VEL_COUNT; - if(yawspeed > this->max_yawspeed) - { - PLAYER_WARN2("yawspeed thresholded! (%d > %d)", - yawspeed, this->max_yawspeed); - yawspeed = this->max_yawspeed; - } - else if(yawspeed < -this->max_yawspeed) - { - PLAYER_WARN2("yawspeed thresholded! (%d < %d)", - ... [truncated message content] |
From: <gb...@us...> - 2009-07-28 06:43:14
|
Revision: 8151 http://playerstage.svn.sourceforge.net/playerstage/?rev=8151&view=rev Author: gbiggs Date: 2009-07-28 06:43:06 +0000 (Tue, 28 Jul 2009) Log Message: ----------- Applied patch #2828031 Modified Paths: -------------- code/player/trunk/server/drivers/position/CMakeLists.txt Added Paths: ----------- code/player/trunk/server/drivers/position/deadstop/ code/player/trunk/server/drivers/position/deadstop/CMakeLists.txt code/player/trunk/server/drivers/position/deadstop/deadstop.cc Modified: code/player/trunk/server/drivers/position/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/position/CMakeLists.txt 2009-07-28 06:40:27 UTC (rev 8150) +++ code/player/trunk/server/drivers/position/CMakeLists.txt 2009-07-28 06:43:06 UTC (rev 8151) @@ -1,5 +1,6 @@ ADD_SUBDIRECTORY (ascension) ADD_SUBDIRECTORY (bumpersafe) +ADD_SUBDIRECTORY (deadstop) ADD_SUBDIRECTORY (globalize) ADD_SUBDIRECTORY (goto) ADD_SUBDIRECTORY (isense) Added: code/player/trunk/server/drivers/position/deadstop/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/position/deadstop/CMakeLists.txt (rev 0) +++ code/player/trunk/server/drivers/position/deadstop/CMakeLists.txt 2009-07-28 06:43:06 UTC (rev 8151) @@ -0,0 +1,2 @@ +PLAYERDRIVER_OPTION (deadstop build_deadstop ON) +PLAYERDRIVER_ADD_DRIVER (deadstop build_deadstop SOURCES deadstop.cc) Added: code/player/trunk/server/drivers/position/deadstop/deadstop.cc =================================================================== --- code/player/trunk/server/drivers/position/deadstop/deadstop.cc (rev 0) +++ code/player/trunk/server/drivers/position/deadstop/deadstop.cc 2009-07-28 06:43:06 UTC (rev 8151) @@ -0,0 +1,307 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2000 Brian Gerkey et al. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +///////////////////////////////////////////////////////////////////////////// +// +// Desc: This forwards and tracks commands through position2d interface, +// if no command is sent for too long it repeats on sending stop +// velocity command in order to cause emergency stop. +// Author: Paul Osmialowski +// +///////////////////////////////////////////////////////////////////////////// + +/** @ingroup drivers */ +/** @{ */ +/** @defgroup driver_deadstop deadstop + * @brief stop while dead + +This forwards and tracks commands through position2d interface, +if no command is sent for too long it repeats on sending stop +velocity command in order to cause emergency stop. + +@par Compile-time dependencies + +- none + +@par Provides + +- @ref interface_position2d + +@par Requires + +- @ref interface_position2d + +@par Configuration requests + +- none + +@par Configuration file options + +- timeout (float) + - Default: 0.5 (half a second) + - how long to wait for new command to forward + +- cheat_time (float) + - Default: 0.0 (no effect) + - how long to send stop command before start to cheat + +@par Example + +@verbatim +driver +( + name "deadstop" + requires ["position2d:1"] + provides ["position2d:0"] +) +@endverbatim + +@author Paul Osmialowski + +*/ +/** @} */ + +#include <stddef.h> +#include <math.h> +#include <assert.h> +#include <pthread.h> +#include <libplayercore/playercore.h> + +#define EPS 0.000000001 + +class DeadStop : public ThreadedDriver +{ + public: DeadStop(ConfigFile * cf, int section); + public: virtual ~DeadStop(); + + public: virtual int MainSetup(); + public: virtual void MainQuit(); + + // This method will be invoked on each incoming message + public: virtual int ProcessMessage(QueuePointer & resp_queue, + player_msghdr * hdr, + void * data); + + private: virtual void Main(); + + private: player_devaddr_t position2d_provided_addr, position2d_required_addr; + private: Device * ppos; + + private: double timeout; + private: double cheat_time; + private: double last_time; + private: int stop_command; +}; + +Driver * DeadStop_Init(ConfigFile * cf, int section) +{ + return reinterpret_cast<Driver *>(new DeadStop(cf, section)); +} + +void deadstop_Register(DriverTable * table) +{ + table->AddDriver("deadstop", DeadStop_Init); +} + +DeadStop::DeadStop(ConfigFile * cf, int section) + : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN) +{ + memset(&(this->position2d_provided_addr), 0, sizeof(player_devaddr_t)); + memset(&(this->position2d_required_addr), 0, sizeof(player_devaddr_t)); + this->ppos = NULL; + this->timeout = 0.0; + this->cheat_time = 0.0; + this->last_time = 0.0; + this->stop_command = 0; + if (cf->ReadDeviceAddr(&(this->position2d_provided_addr), section, "provides", PLAYER_POSITION2D_CODE, -1, NULL)) + { + this->SetError(-1); + return; + } + if (this->AddInterface(this->position2d_provided_addr)) + { + this->SetError(-1); + return; + } + if (cf->ReadDeviceAddr(&(this->position2d_required_addr), section, "requires", PLAYER_POSITION2D_CODE, -1, NULL)) + { + this->SetError(-1); + return; + } + this->timeout = cf->ReadFloat(section, "timeout", 0.5); + if ((this->timeout) < 0.0) + { + PLAYER_ERROR("Invalid timeout value"); + this->SetError(-1); + return; + } + this->cheat_time = cf->ReadFloat(section, "cheat_time", 0.0); + if ((this->cheat_time) < 0.0) + { + PLAYER_ERROR("Invalid cheat_time value"); + this->SetError(-1); + return; + } +} + +DeadStop::~DeadStop() { } + +int DeadStop::MainSetup() +{ + if (Device::MatchDeviceAddress(this->position2d_required_addr, this->position2d_provided_addr)) + { + PLAYER_ERROR("attempt to subscribe to self"); + return -1; + } + this->ppos = deviceTable->GetDevice(this->position2d_required_addr); + if (!(this->ppos)) + { + PLAYER_ERROR("unable to locate suitable position2d device"); + return -1; + } + if (this->ppos->Subscribe(this->InQueue) != 0) + { + PLAYER_ERROR("unable to subscribe to position2d device"); + this->ppos = NULL; + return -1; + } + return 0; +} + +void DeadStop::MainQuit() +{ + if (this->ppos) this->ppos->Unsubscribe(this->InQueue); + this->ppos = NULL; +} + +void DeadStop::Main() +{ + player_position2d_cmd_vel_t vel_cmd; + struct timespec tspec; + double d; + int stopping; + double stop_time; + + GlobalTime->GetTimeDouble(&(this->last_time)); + GlobalTime->GetTimeDouble(&(stop_time)); + stopping = 0; + this->stop_command = !0; + for (;;) + { + pthread_testcancel(); + this->ProcessMessages(); + pthread_testcancel(); + GlobalTime->GetTimeDouble(&d); + if ((d - (this->last_time)) >= (this->timeout)) + { + memset(&vel_cmd, 0, sizeof vel_cmd); + vel_cmd.vel.px = 0.0; + vel_cmd.vel.py = 0.0; + vel_cmd.vel.pa = 0.0; + switch (stopping) + { + case 0: + stopping = 1; + stop_time = d; + break; + case 1: + if (((this->cheat_time) > 0.0) && (!(this->stop_command))) + { + if ((d - stop_time) >= (this->cheat_time)) + { + vel_cmd.vel.pa = 0.4; + stopping = 2; + } + } + break; + case 2: + assert((this->cheat_time) > 0.0); + if ((d - stop_time) >= ((this->cheat_time) + (this->timeout))) + { + vel_cmd.vel.pa = 0.0; + stopping = 0; + } else vel_cmd.vel.pa = 0.4; + break; + default: + assert(!"internal error"); + } + vel_cmd.state = !0; + this->ppos->PutMsg(this->InQueue, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, reinterpret_cast<void *>(&vel_cmd), 0, NULL); + } else stopping = 0; + pthread_testcancel(); + tspec.tv_sec = 0; + tspec.tv_nsec = 1000000; + nanosleep(&tspec, NULL); + } +} + +int DeadStop::ProcessMessage(QueuePointer &resp_queue, + player_msghdr * hdr, + void * data) +{ + Message * msg; + player_msghdr_t newhdr; + player_position2d_cmd_vel_t vel_cmd; + + if (!hdr) + { + PLAYER_ERROR("NULL header"); + return -1; + } + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, + -1, // -1 means 'all message subtypes' + this->position2d_required_addr)) + { + if (!data) return -1; + this->Publish(this->position2d_provided_addr, + PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, + data, 0, NULL, true); // copy = true, do not dispose data placed who knows where! + return 0; + } + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, -1, this->position2d_provided_addr)) + { + if (!data) return -1; + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, this->position2d_provided_addr)) + { + vel_cmd = *(reinterpret_cast<player_position2d_cmd_vel_t *>(data)); + if ((fabs(vel_cmd.vel.px) < EPS) && (fabs(vel_cmd.vel.py) < EPS) && (fabs(vel_cmd.vel.pa) < EPS)) this->stop_command = !0; + else this->stop_command = 0; + } + GlobalTime->GetTimeDouble(&(this->last_time)); + this->ppos->PutMsg(this->InQueue, hdr, data); + return 0; + } + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, -1, this->position2d_provided_addr)) + { + if (!data) PLAYER_WARN2("NULL data request %d:%d", hdr->type, hdr->subtype); + hdr->addr = this->position2d_required_addr; + msg = this->ppos->Request(this->InQueue, hdr->type, hdr->subtype, data, 0, NULL, true); // threaded = true + if (!msg) + { + PLAYER_WARN("failed to forward request"); + return -1; + } + newhdr = *(msg->GetHeader()); + newhdr.addr = this->position2d_provided_addr; + this->Publish(resp_queue, &newhdr, msg->GetPayload(), true); // copy = true, do not dispose published data as we're disposing whole source message in the next line + delete msg; + return 0; + } + return -1; +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-07-28 06:40:41
|
Revision: 8150 http://playerstage.svn.sourceforge.net/playerstage/?rev=8150&view=rev Author: gbiggs Date: 2009-07-28 06:40:27 +0000 (Tue, 28 Jul 2009) Log Message: ----------- Applied patch #2825176 Modified Paths: -------------- code/player/trunk/server/drivers/position/CMakeLists.txt Added Paths: ----------- code/player/trunk/server/drivers/position/segwayrmp400/ code/player/trunk/server/drivers/position/segwayrmp400/CMakeLists.txt code/player/trunk/server/drivers/position/segwayrmp400/segwayrmp400.cc code/player/trunk/server/drivers/position/segwayrmp400/segwayrmp400.h Modified: code/player/trunk/server/drivers/position/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/position/CMakeLists.txt 2009-07-28 06:32:00 UTC (rev 8149) +++ code/player/trunk/server/drivers/position/CMakeLists.txt 2009-07-28 06:40:27 UTC (rev 8150) @@ -9,6 +9,7 @@ ADD_SUBDIRECTORY (motionmind) ADD_SUBDIRECTORY (nav200) ADD_SUBDIRECTORY (nd) +ADD_SUBDIRECTORY (segwayrmp400) ADD_SUBDIRECTORY (snd) ADD_SUBDIRECTORY (roboteq) ADD_SUBDIRECTORY (vfh) Added: code/player/trunk/server/drivers/position/segwayrmp400/CMakeLists.txt =================================================================== --- code/player/trunk/server/drivers/position/segwayrmp400/CMakeLists.txt (rev 0) +++ code/player/trunk/server/drivers/position/segwayrmp400/CMakeLists.txt 2009-07-28 06:40:27 UTC (rev 8150) @@ -0,0 +1,2 @@ +PLAYERDRIVER_OPTION (segwayrmp400 build_segwayrmp400 ON) +PLAYERDRIVER_ADD_DRIVER (segwayrmp400 build_segwayrmp400 SOURCES segwayrmp400.cc) Added: code/player/trunk/server/drivers/position/segwayrmp400/segwayrmp400.cc =================================================================== --- code/player/trunk/server/drivers/position/segwayrmp400/segwayrmp400.cc (rev 0) +++ code/player/trunk/server/drivers/position/segwayrmp400/segwayrmp400.cc 2009-07-28 06:40:27 UTC (rev 8150) @@ -0,0 +1,495 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2009 Goutham Mallapragda, Anthony Cascone, Rich Mattes & Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* + Desc: Segway RMP 400 Driver + Authors: Goutham Mallapragada, Anthony Cascone, Rich Mattes + Updated: July 21, 2009 +*/ + + +/** @ingroup drivers */ +/** @{ */ +/** @defgroup driver_segwayrmp400 segwayrmp400 + * @brief SegwayRMP400 Mobile Robot + +%Device driver for the Segway RMP 400, subscribes to two "segwayrmp" drivers + +This driver subscribes to both the front and rear modules of the RMP400 and +provides a common interface to control the unit. It wraps two "segwayrmp" drivers +into a single interface and provides the same output to both units. It also averages the +incoming odometry and returns the combined data. + +This driver was developed by the Networked Robotics and Sensors Laboratory at +The Pennsylvania State University, University Park, PA 16802. +(http://nrsl.mne.psu.edu). + +@par Note Some Dell laptops have trouble talking and staying connected to both rmp modules. +It is necessary to use a USB hub between the rmp units and the laptop. + +@par Compile-time dependencies + +- none + +@par Requires + +- @ref interface_position2d + - This interface is required if you chose to run the RMP400 + from the position2d interface. The RMP400 driver can run in 2d, 3d, or both 2d and 3d modes. +- @ref interface_position3d + - This interface is required if you chose to run the RMP400 + from the position3d interface. The RMP400 driver can run in 2d, 3d, or both 2d and 3d modes. + +@par Provides + +- @ref interface_position2d + - This interface returns odometry data (x,y, and yaw), + and accepts velocity commands (x vel and yaw vel). + +- @ref interface_position3d + - This interface returns odometry data (x, y and yaw) from the wheel + encoders, and attitude data (pitch and roll) from the IMU. The + driver accepts velocity commands (x vel and yaw vel). + +@par Configuration requests + +- none + +@par Configuration file options + +- fullspeed_data + - Default: 1 + - If set to 0, the driver will only publish every tenth data state. This can help + prevent queue overflows in certain situations since the underlying RMP devices + publish their data at around 100Hz. When set to 1, all incoming data is published. + +@par Example + +@verbatim +driver +( + name "segwayrmp400" + provides ["position2d:0" "position3d:0"] + requires ["front:::position3d:1" "back:::position3d:2" "front2d:::position2d:1""back2d:::position2d:2"] + fullspeed_data 1 +) +@endverbatim + +@authors Goutham Mallapragada, Anthony Cascone, Rich Mattes + + */ + +#include "segwayrmp400.h" + +//////////////////////////////////////////////////// +// Init +Driver* SegwayRMP400_Init(ConfigFile* cf, int section) +{ + return ((Driver*)(new SegwayRMP400(cf, section))); +} + +//////////////////////////////////////////////////// +// Register with drivertable +void segwayrmp400_Register(DriverTable *table) +{ + table->AddDriver("segwayrmp400", SegwayRMP400_Init); +} + +//////////////////////////////////////////////////// +// Constructor +SegwayRMP400::SegwayRMP400(ConfigFile* cf, int section): ThreadedDriver(cf,section,true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN) +{ + // Start clean + this->provide_2d = false; + this->provide_3d = false; + + memset(&this->position2d_id,0,sizeof(player_devaddr_t)); + memset(&this->position3d_id,0,sizeof(player_devaddr_t)); + memset(&this->segwayrmp2d_id[0],0,sizeof(player_devaddr_t)); + memset(&this->segwayrmp2d_id[1],0,sizeof(player_devaddr_t)); + memset(&this->segwayrmp3d_id[0],0,sizeof(player_devaddr_t)); + memset(&this->segwayrmp3d_id[1],0,sizeof(player_devaddr_t)); + + newfront2d = false; + newback2d = false; + newfront3d = false; + newback3d = false; + + // Do we create a position3d interface? + if( cf->ReadDeviceAddr(&this->position3d_id,section,"provides",PLAYER_POSITION3D_CODE,-1,NULL) == 0 ) + { + if( cf->ReadDeviceAddr(&this->segwayrmp3d_id[0], section, "requires", PLAYER_POSITION3D_CODE, -1, "front") != 0) + { + this->SetError(-1); + PLAYER_ERROR("Front Segway Position3d not present"); + return; + } + if( cf->ReadDeviceAddr(&this->segwayrmp3d_id[1], section, "requires", PLAYER_POSITION3D_CODE, -1, "back") != 0 ) + { + this->SetError(-1); + PLAYER_ERROR("Back Segway Position2d not present"); + return; + } + + if(this->AddInterface(this->position3d_id)!=0) + { + this->SetError(-1); + PLAYER_ERROR("Unable Add SegwayRMP400 Position3d Device"); + return; + } + + this->provide_3d = true; + PLAYER_MSG0(2, "SegwayRMP400 Providing Position3d Device"); + } + + // Do we create a position2d interface? + if( cf->ReadDeviceAddr(&this->position2d_id,section,"provides",PLAYER_POSITION2D_CODE,-1,NULL) == 0 ) + { + if( cf->ReadDeviceAddr(&this->segwayrmp2d_id[0], section, "requires", PLAYER_POSITION2D_CODE, -1, "front2d") != 0) + { + this->SetError(-1); + PLAYER_ERROR("Front Segway Position2d not present"); + return; + } + if( cf->ReadDeviceAddr(&this->segwayrmp2d_id[1], section, "requires", PLAYER_POSITION2D_CODE, -1, "back2d") != 0 ) + { + this->SetError(-1); + PLAYER_ERROR("Back Segway Position2d not present"); + return; + } + + if(this->AddInterface(this->position2d_id)!=0) + { + this->SetError(-1); + PLAYER_ERROR("Unable Add SegwayRMP400 Position2d Device"); + return; + } + + this->provide_2d = true; + PLAYER_MSG0(2, "SegwayRMP400 Providing Position2d Device"); + } + counter = 0; + + // Check config file for additional options + this->fullspeed = cf->ReadInt(section, "fullspeed_data", 1); + + +} + +//////////////////////////////////////////////////// +// Setup the connections and subscribe to required devices +int SegwayRMP400::MainSetup() +{ + int i; + PLAYER_MSG0(0,"SegwayRMP400 Initializing ..."); + + for( i=0; i<2; i++) + { + if (this->provide_3d) + { + if(!(this->segwayrmp3d[i] = deviceTable->GetDevice(this->segwayrmp3d_id[i]))) + { + PLAYER_ERROR1("Unable to locate segwayrmp position3d device[%d]",i); + return(-1); + } + } + if (this->provide_2d) + { + if(!(this->segwayrmp2d[i] = deviceTable->GetDevice(this->segwayrmp2d_id[i]))) + { + PLAYER_ERROR1("Unable to locate segwayrmp position3d device[%d]",i); + return(-1); + } + } + } + + for(i=0;i<2;i++) + { + if (this->provide_3d) + { + if(this->segwayrmp3d[i]->Subscribe(this->InQueue) != 0 ) + { + PLAYER_ERROR1("Unable to subscribe to host segwayrmp position2d device[%d]",i); + return(-1); + } + } + if (this->provide_2d) + { + if(this->segwayrmp2d[i]->Subscribe(this->InQueue) != 0 ) + { + PLAYER_ERROR1("Unable to subscribe to host segwayrmp position2d device[%d]",i); + return(-1); + } + } + } + PLAYER_MSG0(0, "SegwayRMP Initialized"); + return 0; +} + +//////////////////////////////////////////////////// +// Shutdown connection to subscribed devices +void SegwayRMP400::MainQuit() +{ + PLAYER_MSG0(0,"Shutting SegwayRMP400 down..."); + + if (this->provide_3d) + { + for(int i=0;i<2;i++) + this->segwayrmp3d[i]->Unsubscribe(this->InQueue); + } + if (this->provide_2d) + { + for(int i=0;i<2;i++) + this->segwayrmp2d[i]->Unsubscribe(this->InQueue); + } + + PLAYER_MSG0(0, "SegwayRMP400 has been shutdown"); + return; +} + +//////////////////////////////////////////////////// +// Main, you get the idea +void SegwayRMP400::Main() +{ + // Setup Thread + pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL); + + // Main Loop + for(;;) + { + // Check for time to quit + pthread_testcancel(); + + // Yep, everyone has to do it. + ProcessMessages(); + + // Fill the data structure, and publish position data. + ProcessData(); + + // Sleep for 1 ms, everyone needs some sleep + usleep(1000); + } +} + +//////////////////////////////////////////////////// +// Process Message because we should +int SegwayRMP400::ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data) +{ + + // is it new position3d data? + if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_DATA, PLAYER_POSITION3D_DATA_STATE, this->segwayrmp3d_id[0])) + { + assert(hdr->size == sizeof(player_position3d_data_t)); + rmp3d_data[0] = *(player_position3d_data_t *)data; + newfront3d = true; + } + else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_DATA, PLAYER_POSITION3D_DATA_STATE, this->segwayrmp3d_id[1])) + { + assert(hdr->size == sizeof(player_position3d_data_t)); + rmp3d_data[1] = *(player_position3d_data_t *)data; + newback3d = true; + } + + // is it new position3d cmd? + else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD, PLAYER_POSITION3D_CMD_SET_VEL, this->position3d_id)) + { + return HandlePosition3DCmd((player_position3d_cmd_vel_t*)data); + } + + // is it new position2d data? + else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, this->segwayrmp2d_id[0])) + { + assert(hdr->size == sizeof(player_position2d_data_t)); + rmp2d_data[0] = *(player_position2d_data_t *)data; + newfront2d = true; + } + else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, this->segwayrmp2d_id[1])) + { + assert(hdr->size == sizeof(player_position2d_data_t)); + rmp2d_data[1] = *(player_position2d_data_t *)data; + newback2d = true; + } + + // is it new position2d cmd? + else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_CMD, PLAYER_POSITION2D_CMD_VEL, this->position2d_id)) + { + return HandlePosition2DCmd((player_position2d_cmd_vel_t*)data); + } + + // is it a geometry request? + else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,PLAYER_POSITION2D_REQ_GET_GEOM,this->position2d_id)) + { + player_position2d_geom_t geom; + geom.pose.px = 0; + geom.pose.py = 0; + geom.pose.pyaw = 0; + geom.size.sw = 0.508; + geom.size.sl = 0.610; + + this->Publish(position2d_id, PLAYER_MSGTYPE_RESP_ACK, PLAYER_POSITION2D_REQ_GET_GEOM, &geom, sizeof(geom),NULL); + } + + // pass the request to the underlying segway rmp position devices for 2d + else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,-1,this->position2d_id)) + { + Message *msg; + //@TODO: compare responses of both segways. + // hack: sending both segways the command, forwarding the response of the second segway rmp only; + msg = this->segwayrmp2d[0]->Request(this->InQueue,hdr->type,hdr->subtype,(void*)data,hdr->size,&hdr->timestamp); + msg = this->segwayrmp2d[1]->Request(this->InQueue,hdr->type,hdr->subtype,(void*)data,hdr->size,&hdr->timestamp); + + player_msghdr_t* rephdr = msg->GetHeader(); + void* repdata = msg->GetPayload(); + rephdr->addr = this->position2d_id; + this->Publish(resp_queue,rephdr,repdata); + delete msg; + } + + // pass the request to the underlying segway rmp position devices for 3d + else if(Message::MatchMessage(hdr,PLAYER_MSGTYPE_REQ,-1,this->position3d_id)) + { + Message *msg; + //@TODO: Compare responses of both segways. + // hack: sending both segways the command, forwarding the response of the second segway rmp only; + msg = this->segwayrmp3d[0]->Request(this->InQueue,hdr->type,hdr->subtype,(void*)data,hdr->size,&hdr->timestamp); + msg = this->segwayrmp3d[1]->Request(this->InQueue,hdr->type,hdr->subtype,(void*)data,hdr->size,&hdr->timestamp); + + player_msghdr_t* rephdr = msg->GetHeader(); + void* repdata = msg->GetPayload(); + rephdr->addr = this->position3d_id; + this->Publish(resp_queue,rephdr,repdata); + delete msg; + } + + // is it something we can't handle? + else + return -1; + + // if all went well + return 0; +} + +//////////////////////////////////////////////////// +// Simple function to forward position 3d commands +int SegwayRMP400::HandlePosition3DCmd(player_position3d_cmd_vel_t *cmd) +{ + + for(int i=0;i<2;i++) + { + // Push the message forward to each of the subscribed devices + this->segwayrmp3d[i]->PutMsg(this->InQueue, PLAYER_MSGTYPE_CMD, + PLAYER_POSITION3D_CMD_SET_VEL, + (void*)cmd,sizeof(player_position3d_cmd_vel_t),NULL); + } + return 0; +} + +//////////////////////////////////////////////////// +// Simple function to forward position 2d commands +int SegwayRMP400::HandlePosition2DCmd(player_position2d_cmd_vel_t *cmd) +{ + + for(int i=0;i<2;i++) + { + // Push the message forward to each of the subscribed devices + this->segwayrmp2d[i]->PutMsg(this->InQueue, PLAYER_MSGTYPE_CMD, + PLAYER_POSITION2D_CMD_VEL, + (void*)cmd,sizeof(player_position2d_cmd_vel_t),NULL); + } + return 0; +} + +//////////////////////////////////////////////////// +// Simple function to package position data and publish +// Makes an effort to not use or publish duplicate data. +void SegwayRMP400::ProcessData() +{ + /// @todo + /// Add smarter approach to combine the data from both RMPs. For now, just average. + + bool pos3dready = false; + bool pos2dready = false; + + // Repackage the position3d data if we have new data from both interfaces + if (newfront3d && newback3d) + { + position3d_data.pos.px = (rmp3d_data[0].pos.px + rmp3d_data[1].pos.px)/2.0; + position3d_data.pos.py = (rmp3d_data[0].pos.py + rmp3d_data[1].pos.py)/2.0; + position3d_data.pos.pz = (rmp3d_data[0].pos.pz + rmp3d_data[1].pos.pz)/2.0; + position3d_data.pos.proll = (rmp3d_data[0].pos.proll + rmp3d_data[1].pos.proll)/2.0; + position3d_data.pos.ppitch = (rmp3d_data[0].pos.ppitch + rmp3d_data[1].pos.ppitch)/2.0; + position3d_data.pos.pyaw = (rmp3d_data[0].pos.pyaw + rmp3d_data[1].pos.pyaw)/2.0; + + position3d_data.vel.px = (rmp3d_data[0].vel.px + rmp3d_data[1].vel.px)/2.0; + position3d_data.vel.py = (rmp3d_data[0].vel.py + rmp3d_data[1].vel.py)/2.0; + position3d_data.vel.pz = (rmp3d_data[0].vel.pz + rmp3d_data[1].vel.pz)/2.0; + position3d_data.vel.proll = (rmp3d_data[0].vel.proll + rmp3d_data[1].vel.proll)/2.0; + position3d_data.vel.ppitch = (rmp3d_data[0].vel.ppitch + rmp3d_data[1].vel.ppitch)/2.0; + position3d_data.vel.pyaw = (rmp3d_data[0].vel.pyaw + rmp3d_data[1].vel.pyaw)/2.0; + + newfront3d = false; + newback3d = false; + pos3dready = true; + } + + // Repackage the position 2d data if we have new data from both interfaces + if (newfront2d && newback2d) + { + position2d_data.pos.px = (rmp2d_data[0].pos.px + rmp2d_data[1].pos.px)/2.0; + position2d_data.pos.py = (rmp2d_data[0].pos.py + rmp2d_data[1].pos.py)/2.0; + position2d_data.pos.pa = (rmp2d_data[0].pos.pa + rmp2d_data[1].pos.pa)/2.0; + + position2d_data.vel.px = (rmp2d_data[0].vel.px + rmp2d_data[1].vel.px)/2.0; + position2d_data.vel.py = (rmp2d_data[0].vel.py + rmp2d_data[1].vel.py)/2.0; + position2d_data.vel.pa = (rmp2d_data[0].vel.pa + rmp2d_data[1].vel.pa)/2.0; + + newfront2d = false; + newback2d = false; + pos2dready = true; + } + + // Publish every tenth message from both interfaces, making sure it's new data. + if (!fullspeed && counter >= 10) + { + if (pos3dready) + this->Publish(position3d_id, PLAYER_MSGTYPE_DATA, PLAYER_POSITION3D_DATA_STATE, + (void*)&position3d_data,sizeof(position3d_data),NULL); + if (pos2dready) + this->Publish(position2d_id, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, + (void*)&position2d_data,sizeof(position2d_data),NULL); + counter = 0; + } + + // Publish all valid messages we are able to combine, only uses new data. + else if (fullspeed) + { + if (pos3dready) + this->Publish(position3d_id, PLAYER_MSGTYPE_DATA, PLAYER_POSITION3D_DATA_STATE, + (void*)&position3d_data,sizeof(position3d_data),NULL); + if (pos2dready) + this->Publish(position2d_id, PLAYER_MSGTYPE_DATA, PLAYER_POSITION2D_DATA_STATE, + (void*)&position2d_data,sizeof(position2d_data),NULL); + counter = 0; + } + + counter++; + + + +} Added: code/player/trunk/server/drivers/position/segwayrmp400/segwayrmp400.h =================================================================== --- code/player/trunk/server/drivers/position/segwayrmp400/segwayrmp400.h (rev 0) +++ code/player/trunk/server/drivers/position/segwayrmp400/segwayrmp400.h 2009-07-28 06:40:27 UTC (rev 8150) @@ -0,0 +1,105 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2009 Goutham Mallapragda, Anthony Cascone, Rich Mattes & Brian Gerkey + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ +/* + Desc: Segway RMP 400 Driver + Authors: Goutham Mallapragada, Anthony Cascone, Rich Mattes + Updated: July 21, 2009 +*/ + +#ifndef __SEGWAYRMP400_H_ +#define __SEGWAYRMP400_H_ + +#include <stdio.h> +#include <pthread.h> +#include <unistd.h> +#include <netinet/in.h> +#include <string.h> +#include <stdlib.h> +#include <math.h> + +#include <libplayercore/playercore.h> + + +/// SegwayRMP400 Position Driver +class SegwayRMP400 : public ThreadedDriver +{ + +public: + + /// Standard Constructor + SegwayRMP400(ConfigFile* cf, int section); + + /// Initialize (Player Standard) + virtual int MainSetup(); + + /// Shutdown (Player Standard) + virtual void MainQuit(); + +private: + + // Devices + Device* segwayrmp2d[2]; ///< child segwayrmp200 devices for 2d subsrciption + Device* segwayrmp3d[2]; ///< child segwayrmp200 devices for 3d subsrciption + + // Device Addresses + player_devaddr_t segwayrmp2d_id[2]; ///< 2d Position Interface Address (Output) + player_devaddr_t segwayrmp3d_id[2]; ///< 3d Position Interface Address (Output) + + player_devaddr_t position3d_id; ///< 3d Position Interface Address (Input) + player_devaddr_t position2d_id; ///< 2d Position Interface Address (Input) + + // Device Data Structures + player_position2d_data_t rmp2d_data[2]; ///< Incoming data from child segwayrmp200 devices 2d + player_position3d_data_t rmp3d_data[2]; ///< Incoming data from child segwayrmp200 devices 3d + + player_position2d_data_t position2d_data; ///< Output data for parent segwayrmp400 device 2d + player_position2d_cmd_vel_t position2d_cmd; ///< Output cmd for parent segwayrmp400 device 2d + + player_position3d_data_t position3d_data; ///< Output data for parent segwayrmp400 device 3d + player_position3d_cmd_vel_t position3d_cmd; ///< Output cmd for parent segwayrmp400 device 3d + + // Flags + bool provide_2d; ///< Provide 2d interface Flag + bool provide_3d; ///< Provide 3d interface Flag + + int counter; + /// Main + void Main(); + + /// Process message function (Player Standard) + int ProcessMessage(QueuePointer &resp_queue, player_msghdr_t* hdr, void* data); + + /// Packages position data and publishes + void ProcessData(); + + /// Internal method to handle position 3D commands + int HandlePosition3DCmd(player_position3d_cmd_vel_t* cmd); + + /// Internal method to handle position 2D commands + int HandlePosition2DCmd(player_position2d_cmd_vel_t* cmd); + + /// Flags for new data + bool newfront3d, newback3d, newfront2d, newback2d; + + /// Flag for full speed data reporting. + bool fullspeed; +}; + +#endif This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-07-28 06:28:07
|
Revision: 8148 http://playerstage.svn.sourceforge.net/playerstage/?rev=8148&view=rev Author: thjc Date: 2009-07-28 06:27:59 +0000 (Tue, 28 Jul 2009) Log Message: ----------- add copyright header to Brians author check script Modified Paths: -------------- code/player/trunk/server/codetools/authorcheck.py Modified: code/player/trunk/server/codetools/authorcheck.py =================================================================== --- code/player/trunk/server/codetools/authorcheck.py 2009-07-28 06:24:55 UTC (rev 8147) +++ code/player/trunk/server/codetools/authorcheck.py 2009-07-28 06:27:59 UTC (rev 8148) @@ -1,5 +1,24 @@ #!/usr/bin/env python +# * Player - One Hell of a Robot Server +# * Copyright (C) 2008 +# * Brian Gerkey +# * +# * +# * This library is free software; you can redistribute it and/or +# * modify it under the terms of the GNU Lesser General Public +# * License as published by the Free Software Foundation; either +# * version 2.1 of the License, or (at your option) any later version. +# * +# * This library is distributed in the hope that it will be useful, +# * but WITHOUT ANY WARRANTY; without even the implied warranty of +# * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +# * Lesser General Public License for more details. +# * +# * You should have received a copy of the GNU Lesser General Public +# * License along with this library; if not, write to the Free Software +# * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + import sys import popen2 This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-07-28 03:51:06
|
Revision: 8146 http://playerstage.svn.sourceforge.net/playerstage/?rev=8146&view=rev Author: gbiggs Date: 2009-07-28 03:50:55 +0000 (Tue, 28 Jul 2009) Log Message: ----------- dos2unixified xsens driver Modified Paths: -------------- code/player/trunk/server/drivers/imu/xsens/cmt1.cpp code/player/trunk/server/drivers/imu/xsens/cmt1.h code/player/trunk/server/drivers/imu/xsens/cmt2.cpp code/player/trunk/server/drivers/imu/xsens/cmt2.h code/player/trunk/server/drivers/imu/xsens/cmt3.cpp code/player/trunk/server/drivers/imu/xsens/cmt3.h code/player/trunk/server/drivers/imu/xsens/cmtdef.h code/player/trunk/server/drivers/imu/xsens/cmtf.h code/player/trunk/server/drivers/imu/xsens/cmtmessage.cpp code/player/trunk/server/drivers/imu/xsens/cmtmessage.h code/player/trunk/server/drivers/imu/xsens/cmtpacket.cpp code/player/trunk/server/drivers/imu/xsens/cmtpacket.h code/player/trunk/server/drivers/imu/xsens/cmtscan.cpp code/player/trunk/server/drivers/imu/xsens/cmtscan.h code/player/trunk/server/drivers/imu/xsens/xsens_fifoqueue.h code/player/trunk/server/drivers/imu/xsens/xsens_janitors.h code/player/trunk/server/drivers/imu/xsens/xsens_list.cpp code/player/trunk/server/drivers/imu/xsens/xsens_list.h code/player/trunk/server/drivers/imu/xsens/xsens_list.hpp code/player/trunk/server/drivers/imu/xsens/xsens_std.cpp code/player/trunk/server/drivers/imu/xsens/xsens_std.h code/player/trunk/server/drivers/imu/xsens/xsens_time.cpp code/player/trunk/server/drivers/imu/xsens/xsens_time.h Modified: code/player/trunk/server/drivers/imu/xsens/cmt1.cpp =================================================================== --- code/player/trunk/server/drivers/imu/xsens/cmt1.cpp 2009-07-28 03:46:00 UTC (rev 8145) +++ code/player/trunk/server/drivers/imu/xsens/cmt1.cpp 2009-07-28 03:50:55 UTC (rev 8146) @@ -1,1309 +1,1309 @@ - -/*! \file Cmt1.cpp - - For information about objects in this file, see the appropriate header: - \ref Cmt1.h - - # The originator of this source code is Xsens Technologies B.V.. This source code is intended for use with Xsens Motion Trackers only. - - THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY - KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE - IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A - PARTICULAR PURPOSE. - - \section FileChangelog Changelog - \par 2006-04-12, v0.0.1 - \li Job Mulder: Created - \par 2006-07-21, v0.1.0 - \li Job Mulder: Updated file for release 0.1.0 -*/ - -#include "cmt1.h" -#include <errno.h> -#ifndef _WIN32 -# include <unistd.h> // close -# include <sys/ioctl.h> // ioctl -# include <fcntl.h> // open, O_RDWR -# include <string.h> // strcpy -//# include <malloc.h> // malloc -# include <sys/param.h> -// We have to redefine PATH_MAX from 4096 to CMT_MAX_FILENAME_LENGTH to mainain compatibility -// The PATH_MAX definition is used by realpath() to determine the maximum path length. According -// to the realpath (3) man page, the function is best avoided and it might be necessary to -// write a custom function for it (couldn't find a proper replacement). -# undef PATH_MAX -# define PATH_MAX CMT_MAX_FILENAME_LENGTH -# include <stdlib.h> -#else -# include <io.h> -#endif - -#ifndef _CRT_SECURE_NO_DEPRECATE -# define _CRT_SECURE_NO_DEPRECATE -# ifdef _WIN32 -# pragma warning(disable:4996) -# endif -#endif - -#ifdef _WIN32 -# define FSEEK(x) _fseeki64(m_handle, x, SEEK_SET) -# define FSEEK_R(x) _fseeki64(m_handle, x, SEEK_END) -# define FTELL() _ftelli64(m_handle) -#else -# define FSEEK(x) fseeko(m_handle, x, SEEK_SET) -# define FSEEK_R(x) fseeko(m_handle, x, SEEK_END) -# define FTELL() ftello(m_handle) -#endif - -// The namespace of all Xsens software since 2006. -namespace xsens { - -#ifndef _WIN32 -int _wcsnicmp(const wchar_t* s1, const wchar_t* s2,int count) -{ - for (int i = 0; i < count; ++i, ++s1, ++s2) - if (*s1 == L'\0') - if (*s2 == L'\0') - return 0; - else - return -1; - else - if (*s2 == L'\0') - return 1; - else - if (*s1 < *s2) - return -1; - else if (*s1 > *s2) - return 1; - return 0; -} -#endif - - -#if defined(_DEBUG) || defined(_LOG_ALWAYS) - #if !defined(_LOG_TO_DBVIEW) - #ifdef _LOG_TO_STDOUT - #else // !dbview && !stdout - FILE* debug_log_fp = NULL; - int32_t debug_log_valid = 0; - - FILE* debug_qlog_fp = NULL; - int32_t debug_qlog_valid = 0; - #endif - #endif - -// write to a log file/screen/debug-stream -void CMTLOG(const char *str, ...) -{ - #ifdef _LOG_TO_STDOUT - va_list ptr; - va_start(ptr,str); - vprintf(str,ptr); - #else - #ifdef _LOG_TO_DBVIEW - char buf[2048]; - - va_list ptr; - va_start(ptr,str); - vsprintf(buf,str,ptr); - - OutputDebugString(buf); - #else - if (debug_log_valid == 0) - { - fopen_s(&debug_log_fp,"debug_log_cmt.log","w"); - if (debug_log_fp != NULL) - debug_log_valid = 1; - else - debug_log_valid = -1; - } - if (debug_log_valid == 1) - { - char buf[2048]; - - va_list ptr; - va_start(ptr,str); - int32_t sz = vsprintf_s(buf,str,ptr); - - uint32_t nw = getTimeOfDay(); - fprintf(debug_log_fp,"%5u.%03u %s",nw/1000,nw%1000,buf); - //fwrite(buf,1,sz,debug_log_fp); - fflush(debug_log_fp); - } - #endif - #endif -} -#endif - -// maybe log to nothing at this level -#ifdef _LOG_CMT1 - #define CMT1LOG CMTLOG -#else - #define CMT1LOG(...) -#endif - -////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////// Cmt1s ///////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////////////////// -// Default constructor, initializes all members to their default values. -Cmt1s::Cmt1s() : - m_onBytesReceived(NULL) -{ - m_port = 0; - m_isOpen = false; - m_lastResult = XRV_OK; - m_timeout = CMT1_DEFAULT_TIMEOUT; - m_endTime = 0; - m_baudrate = 0; - - #ifdef _LOG_RX_TX - rx_log = NULL; - tx_log = NULL; - #endif -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Destructor, de-initializes, frees memory allocated for buffers, etc. -Cmt1s::~Cmt1s() -{ - close(); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Close the serial communication port. -XsensResultValue Cmt1s::close (void) -{ - #ifdef _LOG_RX_TX - if (rx_log != NULL) - fclose(rx_log); - if (tx_log != NULL) - fclose(tx_log); - rx_log = NULL; - tx_log = NULL; - #endif - if (!m_isOpen) - return m_lastResult = XRV_NOPORTOPEN; - - #ifdef _WIN32 - ::FlushFileBuffers(m_handle); - // read all data before closing the handle, a Flush is not enough for FTDI devices unfortunately - // we first need to set the COMM timeouts to instantly return when no more data is available - COMMTIMEOUTS cto; - ::GetCommTimeouts(m_handle,&cto); - cto.ReadIntervalTimeout = MAXDWORD; - cto.ReadTotalTimeoutConstant = 0; - cto.ReadTotalTimeoutMultiplier = 0; - ::SetCommTimeouts(m_handle,&cto); - char buffer[1024]; - uint32_t length; - do { - ::ReadFile(m_handle, buffer, 1024, reinterpret_cast<LPDWORD> (&length), NULL); - } while (length > 0); - ::CloseHandle(m_handle); - #else - ::close(m_handle); - #endif - m_isOpen = false; - m_endTime = 0; - - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Manipulate the Serial control lines -XsensResultValue Cmt1s::escape (const CmtControlLine mask, const CmtControlLine state) -{ - if (!m_isOpen) - return (m_lastResult = XRV_NOPORTOPEN); -#ifdef _WIN32 - BOOL rv = 0; - if (mask & CMT_CONTROL_DTR) - { - if (state & CMT_CONTROL_DTR) - rv = EscapeCommFunction(m_handle,SETDTR); - else - rv = EscapeCommFunction(m_handle,CLRDTR); - } - - if (mask & CMT_CONTROL_RTS) - { - if (state & CMT_CONTROL_RTS) - rv = EscapeCommFunction(m_handle,SETRTS); - else - rv = EscapeCommFunction(m_handle,CLRRTS); - } - if (rv) - return m_lastResult = XRV_OK; - else - return m_lastResult = XRV_ERROR; -#else - bool rv = true; - int32_t status; - if (mask & CMT_CONTROL_DTR) - { - if (ioctl(m_handle, TIOCMGET, &status) == -1) - { - if (state & CMT_CONTROL_DTR) status |= TIOCM_DTR; - else status &= ~TIOCM_DTR; - rv = (ioctl(m_handle, TIOCMSET, &status) == -1); - } - else - rv = false; - } - if (rv && (mask & CMT_CONTROL_RTS)) - { - if (ioctl(m_handle, TIOCMGET, &status) == -1) - { - if (state & CMT_CONTROL_RTS) status |= TIOCM_RTS; - else status &= ~TIOCM_RTS; - rv = (ioctl(m_handle, TIOCMSET, &status) == -1); - } - else - rv = false; - } - if (rv) - return m_lastResult = XRV_OK; - else - return m_lastResult = XRV_ERROR; -#endif -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Flush all data to be transmitted / received. -XsensResultValue Cmt1s::flushData (void) -{ - #ifdef _WIN32 - // Remove any 'old' data in buffer - PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR); - #else - tcflush(m_handle, TCIOFLUSH); - #endif - m_endTime = 0; - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Open a communication channel to the given serial port name. -XsensResultValue Cmt1s::open( const char *portName, - const uint32_t baudRate, - uint32_t readBufSize, - uint32_t writeBufSize) -{ - m_endTime = 0; - - CMT1LOG("L1: Open port %s at %d baud\n", portName, baudRate); - - if (m_isOpen) - { - CMT1LOG("L1: Port already open\n"); - return (m_lastResult = XRV_ALREADYOPEN); - } - m_baudrate = baudRate; - -#ifdef _WIN32 - char winPortName[32]; - - // Open port - sprintf(winPortName, "\\\\.\\%s", portName); - m_handle = CreateFile(winPortName, GENERIC_READ | GENERIC_WRITE, 0, NULL, - OPEN_EXISTING, 0, NULL); - if (m_handle == INVALID_HANDLE_VALUE) - { - CMT1LOG("L1: Port cannot be opened\n"); - return (m_lastResult = XRV_INPUTCANNOTBEOPENED); - } - - // Once here, port is open - m_isOpen = true; - - //Get the current state & then change it - GetCommState(m_handle, &m_commState); // Get current state - - m_commState.BaudRate = baudRate; // Setup the baud rate - m_commState.Parity = NOPARITY; // Setup the Parity - m_commState.ByteSize = 8; // Setup the data bits - m_commState.StopBits = TWOSTOPBITS; // Setup the stop bits - m_commState.fDsrSensitivity = FALSE; // Setup the flow control - m_commState.fOutxCtsFlow = FALSE; // NoFlowControl: - m_commState.fOutxDsrFlow = FALSE; - m_commState.fOutX = FALSE; - m_commState.fInX = FALSE; - if (!SetCommState(m_handle, (LPDCB)&m_commState)) {// Set new state - // Bluetooth ports cannot always be opened with 2 stopbits - // Now try to open port with 1 stopbit. - m_commState.StopBits = ONESTOPBIT; - if (!SetCommState(m_handle, (LPDCB)&m_commState)) { - CloseHandle(m_handle); - m_handle = INVALID_HANDLE_VALUE; - m_isOpen = false; - return (m_lastResult = XRV_INPUTCANNOTBEOPENED); - } - } - m_port = atoi(&portName[3]); - sprintf(m_portname, "%s", portName); - - setTimeout(m_timeout); - - // Other initialization functions - EscapeCommFunction(m_handle, SETRTS); // Enable RTS (for Xbus Master use) - // Set DTR (Calibration sensors need DTR to startup, won't hurt otherwise - EscapeCommFunction(m_handle, SETDTR); - SetupComm(m_handle,readBufSize,writeBufSize); // Set queue size - - // Remove any 'old' data in buffer - //PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR); - PurgeComm(m_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR); -#else // !_WIN32 - // Open port -#ifdef __APPLE__ - // Changed to non-block, cannot open otherwise - m_handle = ::open(portName, O_RDWR | O_NOCTTY | O_NONBLOCK); -#else - m_handle = ::open(portName, O_RDWR | O_NOCTTY); -#endif - // O_RDWR: Read+Write - // O_NOCTTY: Raw input, no "controlling terminal" - // O_NDELAY: Don't care about DCD signal - - if (m_handle < 0) { - // Port not open - printf("CMT1: Opening failed\n"); - return m_lastResult = XRV_INPUTCANNOTBEOPENED; - } - - // Once here, port is open - m_isOpen = true; - - /* Start configuring of port for non-canonical transfer mode */ - // Get current options for the port - if (tcgetattr(m_handle, &m_commState) < 0) - printf("Failed to get attr\n"); - - // Set baudrate. - cfsetispeed(&m_commState, baudRate); - cfsetospeed(&m_commState, baudRate); - - // Enable the receiver and set local mode - m_commState.c_cflag |= (CLOCAL | CREAD); - // Set character size to data bits and set no parity Mask the characte size bits - m_commState.c_cflag &= ~(CSIZE|PARENB); - m_commState.c_cflag |= CS8; // Select 8 data bits - m_commState.c_cflag |= CSTOPB; // send 2 stop bits - // Disable hardware flow control - m_commState.c_cflag &= ~CRTSCTS; - m_commState.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN); - // Disable software flow control - m_commState.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON); - // Set Raw output - m_commState.c_oflag &= ~OPOST; - // Timeout 0.001 sec for first byte, read minimum of 0 bytes - m_commState.c_cc[VMIN] = 0; - m_commState.c_cc[VTIME] = (m_timeout+99)/100; // 1 - - // Set the new options for the port - if (tcsetattr(m_handle,TCSANOW, &m_commState) < 0) - printf("Failed to set attr"); - - m_port = 0; - sprintf(m_portname, "%s", portName); - - tcflush(m_handle, TCIOFLUSH); - - // setting RTS and DTR; RTS for Xbus Master, DTR for calibration sensors - int cmbits; - if (ioctl(m_handle, TIOCMGET, &cmbits) < 0) - { - return (m_lastResult = XRV_ERROR); - } - - cmbits |= TIOCM_RTS|TIOCM_DTR; - - if (ioctl(m_handle, TIOCMSET, &cmbits) < 0) - { - return (m_lastResult = XRV_ERROR); - } -#endif // !_WIN32 - - CMT1LOG("L1: Port opened\n"); - return (m_lastResult = XRV_OK); -} - -#ifdef _WIN32 -////////////////////////////////////////////////////////////////////////////////////////// -// Open a communication channel to the given COM port number. -XsensResultValue Cmt1s::open ( const uint32_t portNumber, - const uint32_t baudRate, - uint32_t readBufSize, - uint32_t writeBufSize) -{ - char comFileName[32]; - - // Create file name - sprintf(comFileName, "COM%d", portNumber); - - return Cmt1s::open(comFileName, baudRate, readBufSize, writeBufSize); -} -#endif - -////////////////////////////////////////////////////////////////////////////////////////// -// Read data from the serial port and put it into the data buffer. -XsensResultValue Cmt1s::readData (const uint32_t maxLength, uint8_t* data, - uint32_t* length) -{ - CMT1LOG("L1: readData, maxlength=%u, length=%p\n",maxLength,length); - uint32_t ln; - if (length == NULL) - length = &ln; - - if (!m_isOpen) - return (m_lastResult = XRV_NOPORTOPEN); - -#ifdef _WIN32 - BOOL rres = ::ReadFile(m_handle, data, maxLength, reinterpret_cast<LPDWORD> (length), NULL); - if (m_onBytesReceived != NULL && *length > 0) - { - CmtBinaryData* bytes = (CmtBinaryData*) malloc(sizeof(CmtBinaryData)); - bytes->m_size = *length; - bytes->m_portNr = m_port; - memcpy(bytes->m_data,data,*length); -#ifdef _LOG_CALLBACKS - CMTLOG("C1: onBytesReceived(%d,(%d,%d),%p)\n",(int32_t) m_onBytesReceivedInstance, (int32_t) bytes->m_size, (int32_t) bytes->m_portNr, m_onBytesReceivedParam); -#endif - m_onBytesReceived(m_onBytesReceivedInstance,CMT_CALLBACK_ONBYTESRECEIVED,bytes,m_onBytesReceivedParam); - } - - if (!rres) - { - CMT1LOG("L1: readData, ReadFile returned error %u\n",::GetLastError()); - return (m_lastResult = XRV_ERROR); - } -#else - *length = read(m_handle, data, maxLength); -#endif - -#ifdef _LOG_RX_TX - if (*length > 0) - { - if (rx_log == NULL) - { - char fname[CMT_MAX_FILENAME_LENGTH]; - sprintf(fname,"rx_%03d_%d.log",(int32_t) m_port,m_baudrate); - rx_log = fopen(fname,"wb"); - } - fwrite(data,1,*length,rx_log); - } -#endif - CMT1LOG((length[0]?"L1: readData returned success, read %u of %u bytes, first: %02x\n":"L1: readData returned success, read %u bytes\n"),length[0],maxLength,data[0]); - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Set the callback function for when bytes have been received -XsensResultValue Cmt1s::setCallbackFunction(CmtCallbackSelector tp, int32_t instance, CmtCallbackFunction func, void* param) -{ - if (tp == CMT_CALLBACK_ONBYTESRECEIVED) - { - m_onBytesReceived = func; - m_onBytesReceivedInstance = instance; - m_onBytesReceivedParam = param; - return m_lastResult = XRV_OK; - } - return m_lastResult = XRV_INVALIDPARAM; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Set the default timeout value to use in blocking operations. -XsensResultValue Cmt1s::setTimeout (const uint32_t ms) -{ - CMT1LOG("L1: Setting timeout to %u ms\n",ms); - - m_timeout = ms; -#ifdef _WIN32 - // Set COM timeouts - COMMTIMEOUTS commTimeouts; - - GetCommTimeouts(m_handle,&commTimeouts); // Fill CommTimeouts structure - - // immediate return if data is available, wait 1ms otherwise - if (m_timeout > 0) - { - commTimeouts.ReadIntervalTimeout = 0; - commTimeouts.ReadTotalTimeoutConstant = m_timeout; // ms time - commTimeouts.ReadTotalTimeoutMultiplier = 0; - commTimeouts.WriteTotalTimeoutConstant = m_timeout; - commTimeouts.WriteTotalTimeoutMultiplier = 0; - } - else - { - // immediate return whether data is available or not - commTimeouts.ReadIntervalTimeout = MAXDWORD; - commTimeouts.ReadTotalTimeoutConstant = 0; - commTimeouts.ReadTotalTimeoutMultiplier = 0; - commTimeouts.WriteTotalTimeoutConstant = 0; - commTimeouts.WriteTotalTimeoutMultiplier = 0; - } - - SetCommTimeouts(m_handle, &commTimeouts); // Set CommTimeouts structure -#else - // Timeout 0.1 sec for first byte, read minimum of 0 bytes - m_commState.c_cc[VMIN] = 0; - m_commState.c_cc[VTIME] = (m_timeout+99)/100; // ds time - - // Set the new options for the port if it is open - if (m_isOpen) - tcsetattr(m_handle,TCSANOW, &m_commState); -#endif - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Wait for data to arrive or a timeout to occur. -XsensResultValue Cmt1s::waitForData (const uint32_t maxLength, - uint8_t* data, uint32_t* length) -{ - CMT1LOG("L1: waitForData, mto=%u, length=%p\n",m_timeout,length); - uint32_t timeout = m_timeout; - - uint32_t ln; - if (length == NULL) - length = &ln; - uint32_t eTime = getTimeOfDay(NULL) + timeout; - uint32_t newLength = 0; - - *length = 0; - while ((*length < maxLength) && (getTimeOfDay() <= eTime)) - { - readData(maxLength - *length, data + *length, &newLength); - *length += newLength; - } - CMT1LOG("L1: waitForData result: read %u of %u bytes\n",length[0],maxLength); - - if (length[0] < maxLength) - return (m_lastResult = XRV_TIMEOUT); - else - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Write the data to the serial port. -XsensResultValue Cmt1s::writeData (const uint32_t length, const uint8_t* data, - uint32_t* written) -{ - uint32_t bytes; - if (written == NULL) - written = &bytes; - - if (!m_isOpen) - return (m_lastResult = XRV_NOPORTOPEN); - -#ifdef _WIN32 - if (WriteFile(m_handle, data, length, reinterpret_cast<LPDWORD> (written), NULL)) - { -#ifdef _LOG_RX_TX - if (written[0] > 0) - { - if (tx_log == NULL) - { - char fname[CMT_MAX_FILENAME_LENGTH]; - sprintf(fname,"tx_%03d_%d.log",(int32_t) m_port,m_baudrate); - tx_log = fopen(fname,"wb"); - } - fwrite(data,1,*written,tx_log); - } -#endif - return (m_lastResult = XRV_OK); - } - else - return (m_lastResult = XRV_ERROR); -#else - *written = write(m_handle, data, length); -// if (*written == length) - return (m_lastResult = XRV_OK); -// else -// return (m_lastResult = XRV_ERROR); -#endif -} - -////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////// Cmt1f ///////////////////////////////////////// -////////////////////////////////////////////////////////////////////////////////////////// - -////////////////////////////////////////////////////////////////////////////////////////// -// Default constructor, initializes all members to their default values. -Cmt1f::Cmt1f() -{ - m_readPos = 0; - m_writePos = 0; - m_lastResult = XRV_OK; - m_reading = true; - m_isOpen = false; - m_filename[0] = '\0'; - m_fileSize = 0; - m_readOnly = false; - m_unicode = false; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Destructor. -Cmt1f::~Cmt1f() -{ - close(); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Write data to the end of the file. -XsensResultValue Cmt1f::appendData (const uint32_t length, const void* data) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - if (m_reading || m_writePos != m_fileSize) - { - m_reading = false; - FSEEK_R(0); - } - fwrite(data, 1, length, m_handle); - m_writePos = FTELL(); - m_fileSize = m_writePos; - - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Close the file. -XsensResultValue Cmt1f::close (void) -{ - if (m_isOpen) - { - #ifdef _WIN32 - fflush(m_handle); - fclose(m_handle); - #else - ::fflush(m_handle); - ::fclose(m_handle); - #endif - } - m_isOpen = false; - m_readPos = 0; - m_writePos = 0; - m_reading = true; - m_isOpen = false; - m_fileSize = 0; - m_readOnly = false; - - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Close the file and delete it. -XsensResultValue Cmt1f::closeAndDelete(void) -{ - if (m_isOpen) - { - #ifdef _WIN32 - fflush(m_handle); - fclose(m_handle); - #else - ::fflush(m_handle); - ::fclose(m_handle); - #endif - if (m_readOnly) - m_lastResult = XRV_READONLY; - else - { -#ifdef _WIN32 - if (m_unicode) - { - if (_wremove(m_filename_w) != 0) - m_lastResult = XRV_READONLY; - else - m_lastResult = XRV_OK; - } - else -#endif - { -#ifdef _WIN32 - if (_unlink(m_filename) != 0) -#else - if (unlink(m_filename) != 0) -#endif - m_lastResult = XRV_READONLY; - else - m_lastResult = XRV_OK; - } - } - } - else - m_lastResult = XRV_NOFILEOPEN; - - m_isOpen = false; - m_readPos = 0; - m_writePos = 0; - m_reading = true; - m_isOpen = false; - m_fileSize = 0; - m_readOnly = false; - - return m_lastResult; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Create a new file. -XsensResultValue Cmt1f::create (const char* filename) -{ - if (m_isOpen) - return m_lastResult = XRV_ALREADYOPEN; - - //! \test does this work for non-existing files? Or do we need a check and create? - m_handle = fopen(filename, "w+b"); // open for update (r/w) - if (m_handle == NULL) - return m_lastResult = XRV_OUTPUTCANNOTBEOPENED; - - #ifdef _WIN32 - if (_fullpath(m_filename,filename,CMT_MAX_FILENAME_LENGTH) == NULL) - { - fclose(m_handle); - remove(filename); - return m_lastResult = XRV_INVALIDPARAM; - } - #else - // based on the assumption that this doesn't concern the serial port, handle - // it the same way using realpath(). Apparently realpath() doesn't require a - // maximum length. One would possibly want to write a wrapper for it. - if (realpath(filename, m_filename) == NULL) - { - fclose(m_handle); - remove(filename); - return m_lastResult = XRV_INVALIDPARAM; - } - #endif - mbstowcs(m_filename_w,m_filename,CMT_MAX_FILENAME_LENGTH); - m_unicode = false; - - m_isOpen = true; - m_readPos = 0; - m_writePos = 0; - m_fileSize = 0; - m_reading = true; - m_readOnly = false; - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Create a new file. -XsensResultValue Cmt1f::create (const wchar_t* filename) -{ - if (m_isOpen) - return m_lastResult = XRV_ALREADYOPEN; - -#ifdef _WIN32 - //! \test does this work for non-existing files? Or do we need a check and create? - m_handle = _wfopen(filename, L"w+b"); // open for update (r/w) - if (m_handle == NULL) - return m_lastResult = XRV_OUTPUTCANNOTBEOPENED; - - if (_wfullpath(m_filename_w,filename,CMT_MAX_FILENAME_LENGTH) == NULL) - { - fclose(m_handle); - _wremove(filename); - return m_lastResult = XRV_INVALIDPARAM; - } - wcstombs(m_filename,m_filename_w,CMT_MAX_FILENAME_LENGTH); - - m_isOpen = true; - m_readPos = 0; - m_writePos = 0; - m_fileSize = 0; - m_reading = true; - m_readOnly = false; -#else - char tFilename[CMT_MAX_FILENAME_LENGTH*2]; - wcstombs(tFilename,m_filename_w,CMT_MAX_FILENAME_LENGTH); - XsensResultValue res = create(tFilename); - if (res != XRV_OK) - return res; -#endif - m_unicode = true; - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Delete the given data from the file. -XsensResultValue Cmt1f::deleteData (const CmtFilePos start, const uint32_t length) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - gotoWrite(); - - CmtFilePos wPos = start; - CmtFilePos rPos = wPos + length; - - size_t read1; - CmtFilePos endPos = (start + (CmtFilePos) length); - if (endPos < m_fileSize) - { - CmtFilePos remaining = m_fileSize - endPos; - char buffer[512]; - - // copy data - FSEEK(rPos); - - while (remaining > 0) - { - if (remaining >= 512) - read1 = fread(buffer,1,512,m_handle); - else - read1 = fread(buffer,1,(size_t) remaining,m_handle); - - remaining -= read1; - rPos += read1; - - // write block to the correct position - FSEEK(wPos); - wPos += fwrite(buffer, 1, read1, m_handle); - FSEEK(rPos); - } - m_fileSize -= length; - } - else - { - m_fileSize = start; - } - -#ifdef _WIN32 - int32_t rv = _chsize(_fileno(m_handle),(int32_t) m_fileSize); -#else - int32_t rv = (ftruncate(fileno(m_handle),(int32_t) m_fileSize) == 0); -#endif - int32_t eno = 0; - if (rv != 0) - eno = errno; - m_writePos = start; - FSEEK(wPos); - if (rv != 0) - { - switch(eno) - { - case EACCES: - return m_lastResult = XRV_BUSY; - case EBADF: - return m_lastResult = XRV_INVALIDINSTANCE; - case ENOSPC: - return m_lastResult = XRV_OUTOFMEMORY; - case EINVAL: - return m_lastResult = XRV_INVALIDPARAM; - default: - return m_lastResult = XRV_ERROR; - } - } - - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Find a string of bytes in the file -XsensResultValue Cmt1f::find (const void* needleV, const uint32_t needleLength, CmtFilePos& pos) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - - const char* needle = (const char*) needleV; - - gotoRead(); - - pos = 0; - - char buffer[512]; - uint32_t bufferPos, needlePos = 0; - size_t readBytes; - if (m_readPos & 0x1FF) // read a block of data - readBytes = fread(buffer,1,(512-((size_t) m_readPos & 0x1FF)),m_handle); - else - readBytes = fread(buffer,1,512,m_handle); // read a block of data - - while (readBytes > 0) - { - m_readPos += readBytes; - bufferPos = 0; - - while (bufferPos < readBytes && needlePos < needleLength) - { - if (buffer[bufferPos] == needle[needlePos]) - { - // found a byte - ++needlePos; - } - else - { - if (needlePos > 0) - needlePos = 0; - else - if (buffer[bufferPos] == needle[0]) - { - // found a byte - needlePos = 1; - } - } - ++bufferPos; - } - if (needlePos < needleLength) - readBytes = fread(buffer,1,512,m_handle); // read next block - else - { - m_readPos = m_readPos + bufferPos - readBytes - needleLength; // or without needleLength - pos = m_readPos; // - needleLength; - FSEEK(m_readPos); - return m_lastResult = XRV_OK; - } - } - return m_lastResult = XRV_ENDOFFILE; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Flush all data to be written. -XsensResultValue Cmt1f::flushData (void) -{ - fflush(m_handle); - - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Retrieve the filename that was last successfully opened. -XsensResultValue Cmt1f::getName(char* filename) const -{ - strcpy(filename, m_filename); - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Retrieve the filename that was last successfully opened. -XsensResultValue Cmt1f::getName(wchar_t* filename) const -{ -#ifdef _WIN32 - wcscpy(filename, m_filename_w); -#else - mbstowcs(filename, m_filename, CMT_MAX_FILENAME_LENGTH); -#endif - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Change from writing to reading mode -void Cmt1f::gotoRead(void) -{ - if (m_reading) - return; - - FSEEK(m_readPos); - m_reading = true; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Change from reading to writing mode -void Cmt1f::gotoWrite(void) -{ - if (!m_reading) - return; - - FSEEK(m_writePos); - m_reading = false; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Insert the given data into the file. -XsensResultValue Cmt1f::insertData (const CmtFilePos start, const uint32_t length, const void* data) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - gotoWrite(); - - CmtFilePos rPos = start; - CmtFilePos wPos = rPos + length; - - size_t read1, read2; - CmtFilePos remaining = m_fileSize - start; - size_t bsize = (length > 512)?length:512; - char* buffer1 = (char*) malloc(bsize); - char* buffer2 = (char*) malloc(bsize); - char* btemp; - - // copy data - FSEEK(rPos); - - if (remaining >= (CmtFilePos) bsize) - read1 = fread(buffer1,1,bsize,m_handle); - else - read1 = fread(buffer1,1,(size_t) remaining,m_handle); - - remaining -= read1; - rPos += read1; - - while(remaining > 0) - { - // move data to correct buffer - read2 = read1; - btemp = buffer1; buffer1 = buffer2; buffer2 = btemp; - - // read next block - if (remaining >= (CmtFilePos) bsize) - read1 = fread(buffer1,1,bsize,m_handle); - else - read1 = fread(buffer1,1,(size_t) remaining,m_handle); - - remaining -= read1; - rPos += read1; - - // write block to the correct position - FSEEK(wPos); - wPos += fwrite(buffer2, 1, read2, m_handle); - FSEEK(rPos); - } - - FSEEK(wPos); - wPos += fwrite(buffer1, 1, read1, m_handle); - - FSEEK(start); - m_writePos = start + fwrite(data, 1, length, m_handle); - m_fileSize += length; - - free(buffer1); - free(buffer2); - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Open a file. -XsensResultValue Cmt1f::open(const char* filename, const bool create, const bool readOnly) -{ - if (m_isOpen) - return m_lastResult = XRV_ALREADYOPEN; - - //! \test does this work for non-existing files? Or do we need a check and create? - m_readOnly = readOnly; - if (readOnly) - m_handle = fopen(filename, "rb"); // open for read only (r) - else - m_handle = fopen(filename, "r+b"); // open for update (r/w) - if (m_handle == NULL) - { - if (create) - m_handle = fopen(filename, "w+b"); // create for update (r/w) - else - { - m_handle = fopen(filename, "rb"); // open for read only (r) - m_readOnly = true; - } - } - if (m_handle == NULL) - return m_lastResult = XRV_INPUTCANNOTBEOPENED; - - #ifdef _WIN32 - if (_fullpath(m_filename,filename,CMT_MAX_FILENAME_LENGTH) == NULL) - { - fclose(m_handle); - return m_lastResult = XRV_INVALIDPARAM; - } - #else - // use the same trick again. - if (realpath(filename, m_filename) == NULL) - { - fclose(m_handle); - return m_lastResult = XRV_INVALIDPARAM; - } - #endif - mbstowcs(m_filename_w,m_filename,CMT_MAX_FILENAME_LENGTH); - m_unicode = false; - - m_isOpen = true; - m_readPos = 0; - m_writePos = 0; - m_reading = true; - FSEEK_R(0); - m_fileSize = FTELL(); - FSEEK(0); - return (m_lastResult = XRV_OK); -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Open a file. -XsensResultValue Cmt1f::open(const wchar_t* filename, const bool create, const bool readOnly) -{ - if (m_isOpen) - return m_lastResult = XRV_ALREADYOPEN; - -#ifdef _WIN32 - //! \test does this work for non-existing files? Or do we need a check and create? - m_readOnly = readOnly; - if (readOnly) - m_handle = _wfopen(filename, L"rb"); // open for read only (r) - else - m_handle = _wfopen(filename, L"r+b"); // open for update (r/w) - if (m_handle == NULL) - { - if (create) - m_handle = _wfopen(filename, L"w+b"); // create for update (r/w) - else - { - m_handle = _wfopen(filename, L"rb"); // open for read only (r) - m_readOnly = true; - } - } - if (m_handle == NULL) - return m_lastResult = XRV_INPUTCANNOTBEOPENED; - - if (_wfullpath(m_filename_w,filename,CMT_MAX_FILENAME_LENGTH) == NULL) - { - fclose(m_handle); - return m_lastResult = XRV_INVALIDPARAM; - } - wcstombs(m_filename,m_filename_w,CMT_MAX_FILENAME_LENGTH); - - m_isOpen = true; - m_readPos = 0; - m_writePos = 0; - m_reading = true; - FSEEK_R(0); - m_fileSize = FTELL(); - FSEEK(0); -#else - char tFilename[CMT_MAX_FILENAME_LENGTH*2]; - wcstombs(tFilename,filename,CMT_MAX_FILENAME_LENGTH*2); - XsensResultValue res = open(tFilename,create,readOnly); - if (res != XRV_OK) - return res; -#endif - m_unicode = true; - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Read data from the file and put it into the data buffer. -XsensResultValue Cmt1f::readData(const uint32_t maxLength, void* data, uint32_t* length) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - - if (maxLength == 0) - return m_lastResult = XRV_OK; - - uint32_t len; - if (length == NULL) - length = &len; - - gotoRead(); - - length[0] = (uint32_t) fread(data,1,maxLength,m_handle); - if (length[0] == 0) - return (m_lastResult = XRV_ENDOFFILE); - - m_readPos += length[0]; - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Read data from the file until the terminator and put it into the data buffer. -XsensResultValue Cmt1f::readData (const uint32_t maxLength, const char terminator, void* dataV, uint32_t* length) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - - uint32_t len; - if (length == NULL) - length = &len; - - char* data = (char*) dataV; - int32_t readChar; - - gotoRead(); - - *length = 0; - readChar = (uint32_t) fgetc(m_handle); - - while (!feof(m_handle) && !ferror(m_handle)) - { - data[*length] = (char) readChar; - ++(*length); - ++m_readPos; - - if (((char) readChar == terminator) || ((*length) >= maxLength)) - return m_lastResult = XRV_OK; - } - return m_lastResult = XRV_ENDOFFILE; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Set the new absolute read position -XsensResultValue Cmt1f::setReadPos (const CmtFilePos pos) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - - if (m_readPos != pos) - { - m_readPos = pos; - if (m_reading) - FSEEK(m_readPos); - } - - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Set the new absolute write position -XsensResultValue Cmt1f::setWritePos(const CmtFilePos pos) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - if (pos == -1) - { - if (m_reading) - m_reading = false; - FSEEK_R(0); - m_writePos = FTELL(); - } - else - { - if (m_writePos != pos) - { - m_writePos = pos; - if (!m_reading) - FSEEK(m_writePos); - } - } - - return m_lastResult = XRV_OK; -} - -////////////////////////////////////////////////////////////////////////////////////////// -// Write data to the file. -XsensResultValue Cmt1f::writeData (const uint32_t length, const void* data) -{ - if (!m_isOpen) - return m_lastResult = XRV_NOFILEOPEN; - if (m_readOnly) - return m_lastResult = XRV_READONLY; - - gotoWrite(); - m_writePos += fwrite(data, 1, length, m_handle); - - if (m_writePos > m_fileSize) - m_fileSize = m_writePos; - - return m_lastResult = XRV_OK; -} - -} // end of xsens namespace + +/*! \file Cmt1.cpp + + For information about objects in this file, see the appropriate header: + \ref Cmt1.h + + # The originator of this source code is Xsens Technologies B.V.. This source code is intended for use with Xsens Motion Trackers only. + + THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY + KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE + IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A + PARTICULAR PURPOSE. + + \section FileChangelog Changelog + \par 2006-04-12, v0.0.1 + \li Job Mulder: Created + \par 2006-07-21, v0.1.0 + \li Job Mulder: Updated file for release 0.1.0 +*/ + +#include "cmt1.h" +#include <errno.h> +#ifndef _WIN32 +# include <unistd.h> // close +# include <sys/ioctl.h> // ioctl +# include <fcntl.h> // open, O_RDWR +# include <string.h> // strcpy +//# include <malloc.h> // malloc +# include <sys/param.h> +// We have to redefine PATH_MAX from 4096 to CMT_MAX_FILENAME_LENGTH to mainain compatibility +// The PATH_MAX definition is used by realpath() to determine the maximum path length. According +// to the realpath (3) man page, the function is best avoided and it might be necessary to +// write a custom function for it (couldn't find a proper replacement). +# undef PATH_MAX +# define PATH_MAX CMT_MAX_FILENAME_LENGTH +# include <stdlib.h> +#else +# include <io.h> +#endif + +#ifndef _CRT_SECURE_NO_DEPRECATE +# define _CRT_SECURE_NO_DEPRECATE +# ifdef _WIN32 +# pragma warning(disable:4996) +# endif +#endif + +#ifdef _WIN32 +# define FSEEK(x) _fseeki64(m_handle, x, SEEK_SET) +# define FSEEK_R(x) _fseeki64(m_handle, x, SEEK_END) +# define FTELL() _ftelli64(m_handle) +#else +# define FSEEK(x) fseeko(m_handle, x, SEEK_SET) +# define FSEEK_R(x) fseeko(m_handle, x, SEEK_END) +# define FTELL() ftello(m_handle) +#endif + +// The namespace of all Xsens software since 2006. +namespace xsens { + +#ifndef _WIN32 +int _wcsnicmp(const wchar_t* s1, const wchar_t* s2,int count) +{ + for (int i = 0; i < count; ++i, ++s1, ++s2) + if (*s1 == L'\0') + if (*s2 == L'\0') + return 0; + else + return -1; + else + if (*s2 == L'\0') + return 1; + else + if (*s1 < *s2) + return -1; + else if (*s1 > *s2) + return 1; + return 0; +} +#endif + + +#if defined(_DEBUG) || defined(_LOG_ALWAYS) + #if !defined(_LOG_TO_DBVIEW) + #ifdef _LOG_TO_STDOUT + #else // !dbview && !stdout + FILE* debug_log_fp = NULL; + int32_t debug_log_valid = 0; + + FILE* debug_qlog_fp = NULL; + int32_t debug_qlog_valid = 0; + #endif + #endif + +// write to a log file/screen/debug-stream +void CMTLOG(const char *str, ...) +{ + #ifdef _LOG_TO_STDOUT + va_list ptr; + va_start(ptr,str); + vprintf(str,ptr); + #else + #ifdef _LOG_TO_DBVIEW + char buf[2048]; + + va_list ptr; + va_start(ptr,str); + vsprintf(buf,str,ptr); + + OutputDebugString(buf); + #else + if (debug_log_valid == 0) + { + fopen_s(&debug_log_fp,"debug_log_cmt.log","w"); + if (debug_log_fp != NULL) + debug_log_valid = 1; + else + debug_log_valid = -1; + } + if (debug_log_valid == 1) + { + char buf[2048]; + + va_list ptr; + va_start(ptr,str); + int32_t sz = vsprintf_s(buf,str,ptr); + + uint32_t nw = getTimeOfDay(); + fprintf(debug_log_fp,"%5u.%03u %s",nw/1000,nw%1000,buf); + //fwrite(buf,1,sz,debug_log_fp); + fflush(debug_log_fp); + } + #endif + #endif +} +#endif + +// maybe log to nothing at this level +#ifdef _LOG_CMT1 + #define CMT1LOG CMTLOG +#else + #define CMT1LOG(...) +#endif + +////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////// Cmt1s ///////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////////////////// +// Default constructor, initializes all members to their default values. +Cmt1s::Cmt1s() : + m_onBytesReceived(NULL) +{ + m_port = 0; + m_isOpen = false; + m_lastResult = XRV_OK; + m_timeout = CMT1_DEFAULT_TIMEOUT; + m_endTime = 0; + m_baudrate = 0; + + #ifdef _LOG_RX_TX + rx_log = NULL; + tx_log = NULL; + #endif +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Destructor, de-initializes, frees memory allocated for buffers, etc. +Cmt1s::~Cmt1s() +{ + close(); +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Close the serial communication port. +XsensResultValue Cmt1s::close (void) +{ + #ifdef _LOG_RX_TX + if (rx_log != NULL) + fclose(rx_log); + if (tx_log != NULL) + fclose(tx_log); + rx_log = NULL; + tx_log = NULL; + #endif + if (!m_isOpen) + return m_lastResult = XRV_NOPORTOPEN; + + #ifdef _WIN32 + ::FlushFileBuffers(m_handle); + // read all data before closing the handle, a Flush is not enough for FTDI devices unfortunately + // we first need to set the COMM timeouts to instantly return when no more data is available + COMMTIMEOUTS cto; + ::GetCommTimeouts(m_handle,&cto); + cto.ReadIntervalTimeout = MAXDWORD; + cto.ReadTotalTimeoutConstant = 0; + cto.ReadTotalTimeoutMultiplier = 0; + ::SetCommTimeouts(m_handle,&cto); + char buffer[1024]; + uint32_t length; + do { + ::ReadFile(m_handle, buffer, 1024, reinterpret_cast<LPDWORD> (&length), NULL); + } while (length > 0); + ::CloseHandle(m_handle); + #else + ::close(m_handle); + #endif + m_isOpen = false; + m_endTime = 0; + + return m_lastResult = XRV_OK; +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Manipulate the Serial control lines +XsensResultValue Cmt1s::escape (const CmtControlLine mask, const CmtControlLine state) +{ + if (!m_isOpen) + return (m_lastResult = XRV_NOPORTOPEN); +#ifdef _WIN32 + BOOL rv = 0; + if (mask & CMT_CONTROL_DTR) + { + if (state & CMT_CONTROL_DTR) + rv = EscapeCommFunction(m_handle,SETDTR); + else + rv = EscapeCommFunction(m_handle,CLRDTR); + } + + if (mask & CMT_CONTROL_RTS) + { + if (state & CMT_CONTROL_RTS) + rv = EscapeCommFunction(m_handle,SETRTS); + else + rv = EscapeCommFunction(m_handle,CLRRTS); + } + if (rv) + return m_lastResult = XRV_OK; + else + return m_lastResult = XRV_ERROR; +#else + bool rv = true; + int32_t status; + if (mask & CMT_CONTROL_DTR) + { + if (ioctl(m_handle, TIOCMGET, &status) == -1) + { + if (state & CMT_CONTROL_DTR) status |= TIOCM_DTR; + else status &= ~TIOCM_DTR; + rv = (ioctl(m_handle, TIOCMSET, &status) == -1); + } + else + rv = false; + } + if (rv && (mask & CMT_CONTROL_RTS)) + { + if (ioctl(m_handle, TIOCMGET, &status) == -1) + { + if (state & CMT_CONTROL_RTS) status |= TIOCM_RTS; + else status &= ~TIOCM_RTS; + rv = (ioctl(m_handle, TIOCMSET, &status) == -1); + } + else + rv = false; + } + if (rv) + return m_lastResult = XRV_OK; + else + return m_lastResult = XRV_ERROR; +#endif +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Flush all data to be transmitted / received. +XsensResultValue Cmt1s::flushData (void) +{ + #ifdef _WIN32 + // Remove any 'old' data in buffer + PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR); + #else + tcflush(m_handle, TCIOFLUSH); + #endif + m_endTime = 0; + return (m_lastResult = XRV_OK); +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Open a communication channel to the given serial port name. +XsensResultValue Cmt1s::open( const char *portName, + const uint32_t baudRate, + uint32_t readBufSize, + uint32_t writeBufSize) +{ + m_endTime = 0; + + CMT1LOG("L1: Open port %s at %d baud\n", portName, baudRate); + + if (m_isOpen) + { + CMT1LOG("L1: Port already open\n"); + return (m_lastResult = XRV_ALREADYOPEN); + } + m_baudrate = baudRate; + +#ifdef _WIN32 + char winPortName[32]; + + // Open port + sprintf(winPortName, "\\\\.\\%s", portName); + m_handle = CreateFile(winPortName, GENERIC_READ | GENERIC_WRITE, 0, NULL, + OPEN_EXISTING, 0, NULL); + if (m_handle == INVALID_HANDLE_VALUE) + { + CMT1LOG("L1: Port cannot be opened\n"); + return (m_lastResult = XRV_INPUTCANNOTBEOPENED); + } + + // Once here, port is open + m_isOpen = true; + + //Get the current state & then change it + GetCommState(m_handle, &m_commState); // Get current state + + m_commState.BaudRate = baudRate; // Setup the baud rate + m_commState.Parity = NOPARITY; // Setup the Parity + m_commState.ByteSize = 8; // Setup the data bits + m_commState.StopBits = TWOSTOPBITS; // Setup the stop bits + m_commState.fDsrSensitivity = FALSE; // Setup the flow control + m_commState.fOutxCtsFlow = FALSE; // NoFlowControl: + m_commState.fOutxDsrFlow = FALSE; + m_commState.fOutX = FALSE; + m_commState.fInX = FALSE; + if (!SetCommState(m_handle, (LPDCB)&m_commState)) {// Set new state + // Bluetooth ports cannot always be opened with 2 stopbits + // Now try to open port with 1 stopbit. + m_commState.StopBits = ONESTOPBIT; + if (!SetCommState(m_handle, (LPDCB)&m_commState)) { + CloseHandle(m_handle); + m_handle = INVALID_HANDLE_VALUE; + m_isOpen = false; + return (m_lastResult = XRV_INPUTCANNOTBEOPENED); + } + } + m_port = atoi(&portName[3]); + sprintf(m_portname, "%s", portName); + + setTimeout(m_timeout); + + // Other initialization functions + EscapeCommFunction(m_handle, SETRTS); // Enable RTS (for Xbus Master use) + // Set DTR (Calibration sensors need DTR to startup, won't hurt otherwise + EscapeCommFunction(m_handle, SETDTR); + SetupComm(m_handle,readBufSize,writeBufSize); // Set queue size + + // Remove any 'old' data in buffer + //PurgeComm(m_handle, PURGE_TXCLEAR | PURGE_RXCLEAR); + PurgeComm(m_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR); +#else // !_WIN32 + // Open port +#ifdef __APPLE__ + // Changed to non-block, cannot open otherwise + m_handle = ::open(portName, O_RDWR | O_NOCTTY | O_NONBLOCK); +#else + m_handle = ::open(portName, O_RDWR | O_NOCTTY); +#endif + // O_RDWR: Read+Write + // O_NOCTTY: Raw input, no "controlling terminal" + // O_NDELAY: Don't care about DCD signal + + if (m_handle < 0) { + // Port not open + printf("CMT1: Opening failed\n"); + return m_lastResult = XRV_INPUTCANNOTBEOPENED; + } + + // Once here, port is open + m_isOpen = true; + + /* Start configuring of port for non-canonical transfer mode */ + // Get current options for the port + if (tcgetattr(m_handle, &m_commState) < 0) + printf("Failed to get attr\n"); + + // Set baudrate. + cfsetispeed(&m_commState, baudRate); + cfsetospeed(&m_commState, baudRate); + + // Enable the receiver and set local mode + m_commState.c_cflag |= (CLOCAL | CREAD); + // Set character size to data bits and set no parity Mask the characte size bits + m_commState.c_cflag &= ~(CSIZE|PARENB); + m_commState.c_cflag |= CS8; // Select 8 data bits + m_commState.c_cflag |= CSTOPB; // send 2 stop bits + // Disable hardware flow control + m_commState.c_cflag &= ~CRTSCTS; + m_commState.c_lflag &= ~(ECHO|ECHONL|ICANON|ISIG|IEXTEN); + // Disable software flow control + m_commState.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON); + // Set Raw output + m_commState.c_oflag &= ~OPOST; + // Timeout 0.001 sec for first byte, read minimum of 0 bytes + m_commState.c_cc[VMIN] = 0; + m_commState.c_cc[VTIME] = (m_timeout+99)/100; // 1 + + // Set the new options for the port + if (tcsetattr(m_handle,TCSANOW, &m_commState) < 0) + printf("Failed to set attr"); + + m_port = 0; + sprintf(m_portname, "%s", portName); + + tcflush(m_handle, TCIOFLUSH); + + // setting RTS and DTR; RTS for Xbus Master, DTR for calibration sensors + int cmbits; + if (ioctl(m_handle, TIOCMGET, &cmbits) < 0) + { + return (m_lastResult = XRV_ERROR); + } + + cmbits |= TIOCM_RTS|TIOCM_DTR; + + if (ioctl(m_handle, TIOCMSET, &cmbits) < 0) + { + return (m_lastResult = XRV_ERROR); + } +#endif // !_WIN32 + + CMT1LOG("L1: Port opened\n"); + return (m_lastResult = XRV_OK); +} + +#ifdef _WIN32 +////////////////////////////////////////////////////////////////////////////////////////// +// Open a communication channel to the given COM port number. +XsensResultValue Cmt1s::open ( const uint32_t portNumber, + const uint32_t baudRate, + uint32_t readBufSize, + uint32_t writeBufSize) +{ + char comFileName[32]; + + // Create file name + sprintf(comFileName, "COM%d", portNumber); + + return Cmt1s::open(comFileName, baudRate, readBufSize, writeBufSize); +} +#endif + +////////////////////////////////////////////////////////////////////////////////////////// +// Read data from the serial port and put it into the data buffer. +XsensResultValue Cmt1s::readData (const uint32_t maxLength, uint8_t* data, + uint32_t* length) +{ + CMT1LOG("L1: readData, maxlength=%u, length=%p\n",maxLength,length); + uint32_t ln; + if (length == NULL) + length = &ln; + + if (!m_isOpen) + return (m_lastResult = XRV_NOPORTOPEN); + +#ifdef _WIN32 + BOOL rres = ::ReadFile(m_handle, data, maxLength, reinterpret_cast<LPDWORD> (length), NULL); + if (m_onBytesReceived != NULL && *length > 0) + { + CmtBinaryData* bytes = (CmtBinaryData*) malloc(sizeof(CmtBinaryData)); + bytes->m_size = *length; + bytes->m_portNr = m_port; + memcpy(bytes->m_data,data,*length); +#ifdef _LOG_CALLBACKS + CMTLOG("C1: onBytesReceived(%d,(%d,%d),%p)\n",(int32_t) m_onBytesReceivedInstance, (int32_t) bytes->m_size, (int32_t) bytes->m_portNr, m_onBytesReceivedParam); +#endif + m_onBytesReceived(m_onBytesReceivedInstance,CMT_CALLBACK_ONBYTESRECEIVED,bytes,m_onBytesReceivedParam); + } + + if (!rres) + { + CMT1LOG("L1: readData, ReadFile returned error %u\n",::GetLastError()); + return (m_lastResult = XRV_ERROR); + } +#else + *length = read(m_handle, data, maxLength); +#endif + +#ifdef _LOG_RX_TX + if (*length > 0) + { + if (rx_log == NULL) + { + char fname[CMT_MAX_FILENAME_LENGTH]; + sprintf(fname,"rx_%03d_%d.log",(int32_t) m_port,m_baudrate); + rx_log = fopen(fname,"wb"); + } + fwrite(data,1,*length,rx_log); + } +#endif + CMT1LOG((length[0]?"L1: readData returned success, read %u of %u bytes, first: %02x\n":"L1: readData returned success, read %u bytes\n"),length[0],maxLength,data[0]); + return (m_lastResult = XRV_OK); +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Set the callback function for when bytes have been received +XsensResultValue Cmt1s::setCallbackFunction(CmtCallbackSelector tp, int32_t instance, CmtCallbackFunction func, void* param) +{ + if (tp == CMT_CALLBACK_ONBYTESRECEIVED) + { + m_onBytesReceived = func; + m_onBytesReceivedInstance = instance; + m_onBytesReceivedParam = param; + return m_lastResult = XRV_OK; + } + return m_lastResult = XRV_INVALIDPARAM; +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Set the default timeout value to use in blocking operations. +XsensResultValue Cmt1s::setTimeout (const uint32_t ms) +{ + CMT1LOG("L1: Setting timeout to %u ms\n",ms); + + m_timeout = ms; +#ifdef _WIN32 + // Set COM timeouts + COMMTIMEOUTS commTimeouts; + + GetCommTimeouts(m_handle,&commTimeouts); // Fill CommTimeouts structure + + // immediate return if data is available, wait 1ms otherwise + if (m_timeout > 0) + { + commTimeouts.ReadIntervalTimeout = 0; + commTimeouts.ReadTotalTimeoutConstant = m_timeout; // ms time + commTimeouts.ReadTotalTimeoutMultiplier = 0; + commTimeouts.WriteTotalTimeoutConstant = m_timeout; + commTimeouts.WriteTotalTimeoutMultiplier = 0; + } + else + { + // immediate return whether data is available or not + commTimeouts.ReadIntervalTimeout = MAXDWORD; + commTimeouts.ReadTotalTimeoutConstant = 0; + commTimeouts.ReadTotalTimeoutMultiplier = 0; + commTimeouts.WriteTotalTimeoutConstant = 0; + commTimeouts.WriteTotalTimeoutMultiplier = 0; + } + + SetCommTimeouts(m_handle, &commTimeouts); // Set CommTimeouts structure +#else + // Timeout 0.1 sec for first byte, read minimum of 0 bytes + m_commState.c_cc[VMIN] = 0; + m_commState.c_cc[VTIME] = (m_timeout+99)/100; // ds time + + // Set the new options for the port if it is open + if (m_isOpen) + tcsetattr(m_handle,TCSANOW, &m_commState); +#endif + return (m_lastResult = XRV_OK); +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Wait for data to arrive or a timeout to occur. +XsensResultValue Cmt1s::waitForData (const uint32_t maxLength, + uint8_t* data, uint32_t* length) +{ + CMT1LOG("L1: waitForData, mto=%u, length=%p\n",m_timeout,length); + uint32_t timeout = m_timeout; + + uint32_t ln; + if (length == NULL) + length = &ln; + uint32_t eTime = getTimeOfDay(NULL) + timeout; + uint32_t newLength = 0; + + *length = 0; + while ((*length < maxLength) && (getTimeOfDay() <= eTime)) + { + readData(maxLength - *length, data + *length, &newLength); + *length += newLength; + } + CMT1LOG("L1: waitForData result: read %u of %u bytes\n",length[0],maxLength); + + if (length[0] < maxLength) + return (m_lastResult = XRV_TIMEOUT); + else + return (m_lastResult = XRV_OK); +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Write the data to the serial port. +XsensResultValue Cmt1s::writeData (const uint32_t length, const uint8_t* data, + uint32_t* written) +{ + uint32_t bytes; + if (written == NULL) + written = &bytes; + + if (!m_isOpen) + return (m_lastResult = XRV_NOPORTOPEN); + +#ifdef _WIN32 + if (WriteFile(m_handle, data, length, reinterpret_cast<LPDWORD> (written), NULL)) + { +#ifdef _LOG_RX_TX + if (written[0] > 0) + { + if (tx_log == NULL) + { + char fname[CMT_MAX_FILENAME_LENGTH]; + sprintf(fname,"tx_%03d_%d.log",(int32_t) m_port,m_baudrate); + tx_log = fopen(fname,"wb"); + } + fwrite(data,1,*written,tx_log); + } +#endif + return (m_lastResult = XRV_OK); + } + else + return (m_lastResult = XRV_ERROR); +#else + *written = write(m_handle, data, length); +// if (*written == length) + return (m_lastResult = XRV_OK); +// else +// return (m_lastResult = XRV_ERROR); +#endif +} + +////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////// Cmt1f ///////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////////////////// + +////////////////////////////////////////////////////////////////////////////////////////// +// Default constructor, initializes all members to their default values. +Cmt1f::Cmt1f() +{ + m_readPos = 0; + m_writePos = 0; + m_lastResult = XRV_OK; + m_reading = true; + m_isOpen = false; + m_filename[0] = '\0'; + m_fileSize = 0; + m_readOnly = false; + m_unicode = false; +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Destructor. +Cmt1f::~Cmt1f() +{ + close(); +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Write data to the end of the file. +XsensResultValue Cmt1f::appendData (const uint32_t length, const void* data) +{ + if (!m_isOpen) + return m_lastResult = XRV_NOFILEOPEN; + if (m_readOnly) + return m_lastResult = XRV_READONLY; + + if (m_reading || m_writePos != m_fileSize) + { + m_reading = false; + FSEEK_R(0); + } + fwrite(data, 1, length, m_handle); + m_writePos = FTELL(); + m_fileSize = m_writePos; + + return (m_lastResult = XRV_OK); +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Close the file. +XsensResultValue Cmt1f::close (void) +{ + if (m_isOpen) + { + #ifdef _WIN32 + fflush(m_handle); + fclose(m_handle); + #else + ::fflush(m_handle); + ::fclose(m_handle); + #endif + } + m_isOpen = false; + m_readPos = 0; + m_writePos = 0; + m_reading = true; + m_isOpen = false; + m_fileSize = 0; + m_readOnly = false; + + return m_lastResult = XRV_OK; +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Close the file and delete it. +XsensResultValue Cmt1f::closeAndDelete(void) +{ + if (m_isOpen) + { + #ifdef _WIN32 + fflush(m_handle); + fclose(m_handle); + #else + ::fflush(m_handle); + ::fclose(m_handle); + #endif + if (m_readOnly) + m_lastResult = XRV_READONLY; + else + { +#ifdef _WIN32 + if (m_unicode) + { + if (_wremove(m_filename_w) != 0) + m_lastResult = XRV_READONLY; + else + m_lastResult = XRV_OK; + } + else +#endif + { +#ifdef _WIN32 + if (_unlink(m_filename) != 0) +#else + if (unlink(m_filename) != 0) +#endif + m_lastResult = XRV_READONLY; + else + m_lastResult = XRV_OK; + } + } + } + else + m_lastResult = XRV_NOFILEOPEN; + + m_isOpen = false; + m_readPos = 0; + m_writePos = 0; + m_reading = true; + m_isOpen = false; + m_fileSize = 0; + m_readOnly = false; + + return m_lastResult; +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Create a new file. +XsensResultValue Cmt1f::create (const char* filename) +{ + if (m_isOpen) + return m_lastResult = XRV_ALREADYOPEN; + + //! \test does this work for non-existing files? Or do we need a check and create? + m_handle = fopen(filename, "w+b"); // open for update (r/w) + if (m_handle == NULL) + return m_lastResult = XRV_OUTPUTCANNOTBEOPENED; + + #ifdef _WIN32 + if (_fullpath(m_filename,filename,CMT_MAX_FILENAME_LENGTH) == NULL) + { + fclose(m_handle); + remove(filename); + return m_lastResult = XRV_INVALIDPARAM; + } + #else + // based on the assumption that this doesn't concern the serial port, handle + // it the same way using realpath(). Apparently realpath() doesn't require a + // maximum length. One would possibly want to write a wrapper for it. + if (realpath(filename, m_filename) == NULL) + { + fclose(m_handle); + remove(filename); + return m_lastResult = XRV_INVALIDPARAM; + } + #endif + mbstowcs(m_filename_w,m_filename,CMT_MAX_FILENAME_LENGTH); + m_unicode = false; + + m_isOpen = true; + m_readPos = 0; + m_writePos = 0; + m_fileSize = 0; + m_reading = true; + m_readOnly = false; + return m_lastResult = XRV_OK; +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Create a new file. +XsensResultValue Cmt1f::create (const wchar_t* filename) +{ + if (m_isOpen) + return m_lastResult = XRV_ALREADYOPEN; + +#ifdef _WIN32 + //! \test does this work for non-existing files? Or do we need a check and create? + m_handle = _wfopen(filename, L"w+b"); // open for update (r/w) + if (m_handle == NULL) + return m_lastResult = XRV_OUTPUTCANNOTBEOPENED; + + if (_wfullpath(m_filename_w,filename,CMT_MAX_FILENAME_LENGTH) == NULL) + { + fclose(m_handle); + _wremove(filename); + return m_lastResult = XRV_INVALIDPARAM; + } + wcstombs(m_filename,m_filename_w,CMT_MAX_FILENAME_LENGTH); + + m_isOpen = true; + m_readPos = 0; + m_writePos = 0; + m_fileSize = 0; + m_reading = true; + m_readOnly = false; +#else + char tFilename[CMT_MAX_FILENAME_LENGTH*2]; + wcstombs(tFilename,m_filename_w,CMT_MAX_FILENAME_LENGTH); + XsensResultValue res = create(tFilename); + if (res != XRV_OK) + return res; +#endif + m_unicode = true; + return m_lastResult = XRV_OK; +} + +////////////////////////////////////////////////////////////////////////////////////////// +// Delete the given data from the file. +XsensResultValue Cmt1f::deleteData (const C... [truncated message content] |
From: <gb...@us...> - 2009-07-28 03:46:10
|
Revision: 8145 http://playerstage.svn.sourceforge.net/playerstage/?rev=8145&view=rev Author: gbiggs Date: 2009-07-28 03:46:00 +0000 (Tue, 28 Jul 2009) Log Message: ----------- Applied patch #2826950 Modified Paths: -------------- code/player/trunk/server/drivers/mixed/evolution/er1/er.cc Modified: code/player/trunk/server/drivers/mixed/evolution/er1/er.cc =================================================================== --- code/player/trunk/server/drivers/mixed/evolution/er1/er.cc 2009-07-28 03:41:35 UTC (rev 8144) +++ code/player/trunk/server/drivers/mixed/evolution/er1/er.cc 2009-07-28 03:46:00 UTC (rev 8145) @@ -113,6 +113,9 @@ #define DEG2RAD_CONV(x) ((x)*(M_PI/180)) #include <libplayercore/playercore.h> +#if !defined (HAVE_CFMAKERAW) + #include <replace/replace.h> +#endif static float lastlvel, lastrvel; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <gb...@us...> - 2009-07-28 03:41:42
|
Revision: 8144 http://playerstage.svn.sourceforge.net/playerstage/?rev=8144&view=rev Author: gbiggs Date: 2009-07-28 03:41:35 +0000 (Tue, 28 Jul 2009) Log Message: ----------- Applied patch #2824989 Modified Paths: -------------- code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.c code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.h code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc Modified: code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.c =================================================================== --- code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.c 2009-07-27 14:59:59 UTC (rev 8143) +++ code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.c 2009-07-28 03:41:35 UTC (rev 8144) @@ -60,7 +60,7 @@ } int -roomba_open(roomba_comm_t* r, unsigned char fullcontrol) +roomba_open(roomba_comm_t* r, unsigned char fullcontrol, int roomba500) { struct termios term; int flags; @@ -98,8 +98,17 @@ } cfmakeraw(&term); - cfsetispeed(&term, B57600); - cfsetospeed(&term, B57600); + + if (roomba500) + { + cfsetispeed(&term, B115200); + cfsetospeed(&term, B115200); + } + else + { + cfsetispeed(&term, B57600); + cfsetospeed(&term, B57600); + } if(tcsetattr(r->fd, TCSAFLUSH, &term) < 0 ) { Modified: code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.h =================================================================== --- code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.h 2009-07-27 14:59:59 UTC (rev 8143) +++ code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_comms.h 2009-07-28 03:41:35 UTC (rev 8144) @@ -123,7 +123,7 @@ roomba_comm_t* roomba_create(const char* serial_port); void roomba_destroy(roomba_comm_t* r); -int roomba_open(roomba_comm_t* r, unsigned char fullcontrol); +int roomba_open(roomba_comm_t* r, unsigned char fullcontrol, int roomba500); int roomba_init(roomba_comm_t* r, unsigned char fullcontrol); int roomba_close(roomba_comm_t* r); int roomba_set_speeds(roomba_comm_t* r, double tv, double rv); Modified: code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc =================================================================== --- code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc 2009-07-27 14:59:59 UTC (rev 8143) +++ code/player/trunk/server/drivers/mixed/irobot/roomba/roomba_driver.cc 2009-07-28 03:41:35 UTC (rev 8144) @@ -80,6 +80,10 @@ - bumplock (integer) - Default: 0 - If set to 1, the robot will stop whenever bumpers are closed +- roomba500 (boolean) + - Default: false + - If set to true, the driver will start at the faster default baudrate + of the Roomba 500 series @par Example @@ -90,6 +94,7 @@ provides ["position2d:0" "power:0" "bumper:0" "ir:0" "opaque:0"] port "/dev/ttyS2" safe 1 + roomba500 false ) @endverbatim @@ -144,6 +149,8 @@ bool bumplock; bool bumplocked; + bool roomba500; + player_devaddr_t position_addr; player_devaddr_t power_addr; player_devaddr_t bumper_addr; @@ -253,6 +260,7 @@ this->serial_port = cf->ReadString(section, "port", "/dev/ttyS0"); this->safe = cf->ReadInt(section, "safe", 1); this->bumplock = cf->ReadInt(section, "bumplock", 0); + this->roomba500 = cf->ReadBool(section, "roomba500", 0); this->bumplocked = false; this->roomba_dev = NULL; } @@ -262,7 +270,7 @@ { this->roomba_dev = roomba_create(this->serial_port); - if(roomba_open(this->roomba_dev, !this->safe) < 0) + if(roomba_open(this->roomba_dev, !this->safe, this->roomba500) < 0) { roomba_destroy(this->roomba_dev); this->roomba_dev = NULL; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-07-27 13:50:22
|
Revision: 8142 http://playerstage.svn.sourceforge.net/playerstage/?rev=8142&view=rev Author: thjc Date: 2009-07-27 13:50:10 +0000 (Mon, 27 Jul 2009) Log Message: ----------- version bump post release Modified Paths: -------------- code/player/branches/release-2-1-patches/configure.ac Modified: code/player/branches/release-2-1-patches/configure.ac =================================================================== --- code/player/branches/release-2-1-patches/configure.ac 2009-07-27 13:49:11 UTC (rev 8141) +++ code/player/branches/release-2-1-patches/configure.ac 2009-07-27 13:50:10 UTC (rev 8142) @@ -2,7 +2,7 @@ dnl Initialize autoconf with name and version of package to be built. dnl PACKAGE and VERSION will be so defined in <config.h>. -AC_INIT(player,2.1.3) +AC_INIT(player,2.1.4~svn) dnl Player version information. In an ideal world, we would just declare dnl the following 3 values, then reuse them throughout. Unfortunately, @@ -10,13 +10,13 @@ dnl PLAYER_VERSION_INFO. So, just be damned sure that they all agree! PLAYER_MAJOR_VERSION="2" PLAYER_MINOR_VERSION="1" -PLAYER_MICRO_VERSION="3" +PLAYER_MICRO_VERSION="4" AC_SUBST(PLAYER_MAJOR_VERSION) AC_SUBST(PLAYER_MINOR_VERSION) AC_SUBST(PLAYER_MICRO_VERSION) dnl this is used by libtool to assign version numbers -PLAYER_VERSION_INFO="-version-number 2:1:3" +PLAYER_VERSION_INFO="-version-number 2:1:4" AC_SUBST(PLAYER_VERSION_INFO) AC_CONFIG_SRCDIR(server/server.cc) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-07-27 13:49:22
|
Revision: 8141 http://playerstage.svn.sourceforge.net/playerstage/?rev=8141&view=rev Author: thjc Date: 2009-07-27 13:49:11 +0000 (Mon, 27 Jul 2009) Log Message: ----------- really release player 2.1.3 Added Paths: ----------- code/player/tags/release-2-1-3/release-2-1-patches/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |