int RMessagePassing::begin(RMP_Node &n, RMP_Node &nm)
{
int ret;
// set node to normal node
m_isMaster = 0;
// insert to node map
RMP_Node *pn = new RMP_Node,
*pnm = new RMP_Node;
*pn = n;
*pnm = nm;
pn->setNodeInfo();
pn->mpMe = this;
pn->mpObj = this;
pnm->mpMe = this;
pnm->isMaster = 1;
m_nodeMe = pn;
m_nodeMaster = pnm;
m_nodeMe->print();
m_nodeMaster->print();
m_nodeMap.setMP(this);
m_nodeMap.insert(pn, 0);
m_nodeMap.insert(pnm, 0);
// start server
ret = m_socket.startServer(pn->inet4_port);
if( ret != 0 ) {
dbg_pe("Can not start socket_server: %s (%s)\n",
n.nodeName.c_str(), n.hostName.c_str());
return -1;
}
// start receiving thread
start();
// send my node information to master node
RMessage msg;
msg.msgType = 0xFFFFFFF0;
pn->toStream(msg.data);
if( pnm->sendMsg(&msg) < 0 ) {
dbg_pe("Please start master node first!\n");
return -2;
}
return 0;
}
int UART::write(void *d, int len, int master)
{
UART_inner_data *pd;
BOOL bRes;
unsigned long iByteWritten;
// handle structure
pd = (UART_inner_data *) data;
if( pd->m_hCom != NULL ) {
// wirte data
bRes = WriteFile( pd->m_hCom, /* Handle to file */
d, /* Data buffer */
len, /* Number of bytes to write */
&iByteWritten, /* Number of bytes written */
NULL /* overlapped buffer */
);
if( !bRes ) {
dbg_pe("UART::write Send data error, errorcode: %d", GetLastError());
return 2;
}
if( iByteWritten < len ) {
dbg_pw("UART::write Send data byte leng is not correct!");
return 3;
}
} else {
return 1;
}
return 0;
}
int UART::read(void *d, int len, int master)
{
UART_inner_data *pd;
int r;
pd = (UART_inner_data *) data;
if( pd->fd < 0 ) {
dbg_pe("UART port not opened yet!\n");
return -1;
}
if( m_options & UART_BUFFER ) {
r = pd->getReadBuff((uint8_t*)d, len);
if( r < 1 ) {
int l;
uint8_t buf[m_bufSize];
l = _read(buf, m_bufSize);
pd->putReadBuf(buf, l);
}
} else {
r = _read(d, len);
}
return r;
}
/******************************************************************************\
* 1-D Spline interpolation
*
* Prameters:
* xa [in] x coordinate
* ya [in] y values
* y2a [in] second derivative
* n [in] data lenght
* x [in] sampling point x coordinate
* y [out] output value at x
\******************************************************************************/
void splint_s(double *_xa, double *_ya, double *_y2a, int n, double x, double *y)
{
double *xa, *ya, *y2a;
int klo, khi, k;
double h, b, a;
xa = _xa - 1;
ya = _ya - 1;
y2a = _y2a - 1;
klo=1;
khi=n;
while (khi-klo > 1) {
k=(khi+klo) >> 1;
if (xa[k] > x) khi=k;
else klo=k;
}
h=xa[khi]-xa[klo];
if (h == 0.0) {
dbg_pe("Bad xa input to routine splint");
exit(1);
}
a=(xa[khi]-x)/h;
b=(x-xa[klo])/h;
*y= a*ya[klo] + b*ya[khi] +
((a*a*a-a)*y2a[klo] + (b*b*b-b)*y2a[khi])*(h*h)/6.0;
}
int RDateTime::fromStream(RDataStream &s, int isSingleDS)
{
ri32 dt;
ru32 d_magic, d_ver;
if( isSingleDS ) {
s.seek(0);
// get magic number & version
s.getHeader(d_magic, d_ver);
if( d_magic != 0xEABF ) {
dbg_pe("Magic number error! %x\n", d_magic);
return -1;
}
}
// read date/time
if( 0 != s.read(year) ) return -2;
if( 0 != s.read(month) ) return -2;
if( 0 != s.read(day) ) return -2;
if( 0 != s.read(hour) ) return -2;
if( 0 != s.read(min) ) return -2;
if( 0 != s.read(sec) ) return -2;
if( 0 != s.read(nano_sec) ) return -2;
if( 0 != s.read(timeZone) ) return -2;
// read date/time type
if( 0 != s.read(dt) ) return -2;
dt_type = (DateTime_Type) dt;
return 0;
}
int RGBDVideoData::fromStream(pi::RDataStream &ds)
{
ru32 d_magic, d_ver;
int w, h, c, n;
ru8 *pix = NULL;
// rewind to begining
ds.rewind();
// get magic & version number
ds.getHeader(d_magic, d_ver);
if( d_magic != 0x83F8 ) {
dbg_pe("Input data magic number error! %x\n", d_magic);
return -1;
}
// read data
if( 0 != ds.read(w) ) return -2;
if( 0 != ds.read(h) ) return -2;
if( 0 != ds.read(c) ) return -2;
if( 0 != ds.read(timestamp) ) return -2;
if( 0 != ds.read((ru8*)&asso,sizeof(AssociateData))) return -2;
n = w*h*c;
if ( n == 0 ) return -3;
if( c == 1 ) {
img.create(h, w, CV_8UC1);
} else if( c == 3 ) {
img.create(h, w, CV_8UC3);
} else {
dbg_pe("image channel not support! c:%d\n", c);
return -4;
}
pix = img.data;
if( 0 != ds.read(pix, n) ) return -2;
return 0;
}
int UART::_write(void *d, int len)
{
UART_inner_data *pd;
int r;
pd = (UART_inner_data *) data;
if( pd->fd >= 0 ) {
r = ::write(pd->fd, d, len);
return r;
} else {
dbg_pe("UART port not opened yet!\n");
return -1;
}
}
UART::~UART()
{
UART_inner_data *pd;
int r;
pd = (UART_inner_data *) data;
if( pd->m_hCom != NULL ) {
r = CloseHandle(pd->m_hCom);
if( !r ) dbg_pe("UART::~UART Close port error!");
pd->m_hCom = NULL;
}
delete pd;
data = NULL;
}
UART::~UART()
{
UART_inner_data *pd;
int r;
pd = (UART_inner_data *) data;
if( pd->fd >= 0 ) {
r = ::close(pd->fd);
if( r == -1 ) {
dbg_pe("close port error!\n");
}
}
delete pd;
data = NULL;
}
int UART::close(void)
{
UART_inner_data *pd;
int r;
pd = (UART_inner_data *) data;
if( pd->fd >= 0 ) {
r = ::close(pd->fd);
if( r == -1 ) {
dbg_pe("close port error!\n");
}
pd->fd = -1;
}
return r;
}
int RMessagePassing::sendString(std::string nn, std::string msg, bool isCommand)
{
RMP_Node *n;
n = m_nodeMap.get(nn);
if( NULL == n ) {
dbg_pe("Can not find node: %s\n", nn.c_str());
return -1;
}
RMessage rmsg;
if( isCommand ) rmsg.msgType = 0xFFFFFFF2;
else rmsg.msgType = 1;
rmsg.msgID = 1;
rmsg.data.write(msg);
return n->sendMsg(&rmsg);
}
int UART::close(void)
{
UART_inner_data *pd;
int iRes;
// handle structure
pd = (UART_inner_data *) data;
if( pd->m_hCom != NULL ) {
iRes = CloseHandle(pd->m_hCom);
if( !iRes ) {
dbg_pe("UART::close Close COM port error!");
return 2;
}
pd->m_hCom = NULL;
} else {
return 1;
}
return 0;
}
int RMessagePassing::begin(RMP_Node &n)
{
int ret;
// set self node to master node
m_isMaster = 1;
RMP_Node *pn = new RMP_Node;
*pn = n;
pn->isMaster = 1;
pn->setNodeInfo();
pn->mpObj = this;
pn->mpMe = this;
m_nodeMe = pn;
m_nodeMaster = pn;
pn->print();
// insert to node map
m_nodeMap.setMP(this);
m_nodeMap.insert(pn, 0);
// start server
ret = m_socket.startServer(pn->inet4_port);
if( ret != 0 ) {
dbg_pe("Can not start socket_server: %s (%s)\n",
n.nodeName.c_str(), n.hostName.c_str());
return -1;
}
// start receiving thread
start();
return 0;
}
int UART::read(void *d, int len, int master)
{
UART_inner_data *pd;
BOOL bRes;
int iByteRead = len;
unsigned long byte_read_act;
// handle structure
pd = (UART_inner_data *) data;
// read data
if( pd->m_hCom != NULL ) {
bRes = ReadFile( pd->m_hCom, /* Handle to file */
d, /* Data buffer */
iByteRead, /* Number of byte to read */
&byte_read_act, /* Number of bytes readed */
NULL
);
if( !bRes ) {
dbg_pe("UART::read Read data error, errorcode: %d", GetLastError());
return 2;
}
len = byte_read_act;
if( byte_read_act < iByteRead ) {
dbg_pw("UART::read Read data byte leng is not correct!");
return 3;
}
} else {
return 1;
}
return 0;
}
int RMessagePassing::thread_func(void *arg)
{
// accept connection forever
while( getAlive() ) {
RSocket *new_socket = new RSocket;
// accept new connection
if( 0 != m_socket.accept(*new_socket) ) {
dbg_pe("Accept new connection failed!");
delete new_socket;
break;
}
// create new receiving thread
RMP_SocketThread *new_thread;
new_thread = new RMP_SocketThread(new_socket, this);
new_thread->setNodeMe(m_nodeMe);
// start receiving thread
new_thread->start();
}
return 0;
}
int RMessagePassing::begin(string name)
{
string node_ip = "127.0.0.1";
int node_port;
string nodeMaster_ip = "127.0.0.1";
int nodeMaster_port = 30000;
int ret;
if( "Master" == name ) {
m_isMaster = 1;
node_port = 30000;
} else {
m_isMaster = 0;
node_port = 30001;
}
// load self information (IP address, port)
node_ip = svar.GetString(name+".ip", node_ip);
node_port = svar.GetInt(name+".port", node_port);
// start message passing system
if( m_isMaster ) {
RMP_Node *pn = new RMP_Node;
pn->set("Master", node_ip, node_port);
pn->isMaster = 1;
pn->setNodeInfo();
pn->mpObj = this;
pn->mpMe = this;
m_nodeMe = pn;
m_nodeMaster = pn;
pn->print();
// insert to node map
m_nodeMap.setMP(this);
m_nodeMap.insert(pn, 0);
// start server
ret = m_socket.startServer(pn->inet4_port);
if( ret != 0 ) {
dbg_pe("Can not start socket_server: %s (%s)\n",
pn->nodeName.c_str(), pn->hostName.c_str());
return -1;
}
// start receiving thread
start();
} else {
// insert to node map
RMP_Node *pn = new RMP_Node,
*pnm = new RMP_Node;
nodeMaster_ip = svar.GetString("Master.ip", nodeMaster_ip);
nodeMaster_port = svar.GetInt("Master.port", 30000);
// set self/master node information
pn->set(name, node_ip, node_port);
pnm->set("Master", nodeMaster_ip, nodeMaster_port);
pn->setNodeInfo();
pn->mpMe = this;
pn->mpObj = this;
pnm->mpMe = this;
pnm->isMaster = 1;
m_nodeMe = pn;
m_nodeMaster = pnm;
m_nodeMe->print();
m_nodeMaster->print();
m_nodeMap.setMP(this);
m_nodeMap.insert(pn, 0);
m_nodeMap.insert(pnm, 0);
// start server
ret = m_socket.startServer(pn->inet4_port);
if( ret != 0 ) {
dbg_pe("Can not start socket_server: %s (%s)\n",
pn->nodeName.c_str(), pn->hostName.c_str());
return -1;
}
// start receiving thread
start();
// send my node information to master node
RMessage msg;
msg.msgType = 0xFFFFFFF0;
pn->toStream(msg.data);
if( pnm->sendMsg(&msg) < 0 ) {
dbg_pe("Please start master node first!\n");
return -2;
}
}
return 0;
}
// Reference:
// http://stackoverflow.com/questions/6947413/how-to-open-read-and-write-from-serial-port-in-c
// http://stackoverflow.com/questions/18108932/linux-c-serial-port-reading-writing
int UART::open(const std::string &portName, int baudRate)
{
UART_inner_data *pd;
int r = 0;
pd = (UART_inner_data *) data;
if( portName.size() ) port_name = portName;
if( baudRate ) baud_rate = baudRate;
//pd->fd = ::open(port_name, O_RDWR|O_NOCTTY|O_SYNC);
//pd->fd = ::open(port_name, O_RDWR| O_NONBLOCK | O_NDELAY);
if( m_options & UART_NONBLOCK )
pd->fd = open_c(port_name.c_str(), O_RDWR|O_NOCTTY|O_SYNC|O_NONBLOCK);
else
pd->fd = open_c(port_name.c_str(), O_RDWR|O_NOCTTY|O_SYNC);
if( pd->fd < 0 ) {
dbg_pe("open port error! (%s)\n", port_name.c_str());
return -1;
}
// set port baud rate
speed_t b_speed;
switch(baud_rate) {
case 110:
b_speed = B110;
break;
case 300:
b_speed = B300;
break;
case 600:
b_speed = B600;
break;
case 1200:
b_speed = B1200;
break;
case 2400:
b_speed = B2400;
break;
case 4800:
b_speed = B4800;
break;
case 9600:
b_speed = B9600;
break;
case 19200:
b_speed = B19200;
break;
case 38400:
b_speed = B38400;
break;
case 57600:
b_speed = B57600;
break;
case 115200:
b_speed = B115200;
break;
case 230400:
b_speed = B230400;
break;
case 460800:
b_speed = B460800;
break;
case 500000:
b_speed = B500000;
break;
case 576000:
b_speed = B576000;
break;
case 921600:
b_speed = B921600;
break;
case 1000000:
b_speed = B1000000;
break;
default:
dbg_pe("unsupported baud rate: %d\n", baud_rate);
r = -2;
goto UART_OPEN_ERR;
}
struct termios tty;
memset(&tty, 0, sizeof(tty));
if( tcgetattr(pd->fd, &tty) != 0 ) {
dbg_pe("error %d from tcgetattr\n", errno);
r = -3;
goto UART_OPEN_ERR;
}
cfsetispeed(&tty, b_speed);
cfsetospeed(&tty, b_speed);
/* Setting other Port Stuff */
tty.c_cflag &= ~PARENB; // Make 8n1
tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~CSIZE;
tty.c_cflag |= CS8;
tty.c_cflag &= ~CRTSCTS; // no flow control
tty.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines
if( m_options & UART_NONBLOCK ) {
tty.c_cc[VMIN] = 0; // read doesn't block
tty.c_cc[VTIME] = 0; // 0.5 seconds read timeout
} else {
tty.c_cc[VMIN] = 1; // read doesn't block
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
}
/* Make raw */
cfmakeraw(&tty);
tcflush( pd->fd, TCIFLUSH );
if (tcsetattr (pd->fd, TCSANOW, &tty) != 0) {
dbg_pe("error %d from tcsetattr\n", errno);
r = -4;
goto UART_OPEN_ERR;
}
goto UART_OPEN_RET;
UART_OPEN_ERR:
::close(pd->fd);
pd->fd = -1;
UART_OPEN_RET:
return r;
}
/******************************************************************************\
* 1-D Spline interpolation
*
* Prameters:
* xa [in] x coordinate
* ya [in] y values
* xia [in] interpolated x array
* yia [out] interpolated y array
* n [in] data length
* ni [in] interpolated length
\******************************************************************************/
void splint(double *_xa, double *_ya,
double *_xia, double *_yia,
int n, int ni,
double yp1, double ypn)
{
double *xa, *ya;
double *xia, *yia;
double *y2a, *_y2a;
int klo, khi, k, j;
double h, b, a;
double x;
// calculate second derivtive
_y2a = (double*) malloc(sizeof(double)*n);
y2a = _y2a - 1;
spline(_xa, _ya, n, yp1, ypn, _y2a);
// interpolate each point
xa = _xa - 1;
ya = _ya - 1;
xia = _xia - 1;
yia = _yia - 1;
klo = 1;
khi = klo + 1;
for(j=1; j<=ni; j++) {
x = xia[j];
#if 1
// update klo
while( klo+1<=n && x>xa[klo+1]) {
klo ++;
}
// update khi
while( x>xa[khi] ) {
khi ++;
}
#else
klo=1;
khi=n;
while (khi-klo > 1) {
k=(khi+klo) >> 1;
if (xa[k] > x) khi=k;
else klo=k;
}
#endif
h=xa[khi]-xa[klo];
if (h == 0.0) {
dbg_pe("Bad xa input to routine splint");
exit(1);
}
a = (xa[khi]-x)/h;
b = (x-xa[klo])/h;
yia[j] = a*ya[klo] + b*ya[khi] +
((a*a*a-a)*y2a[klo] + (b*b*b-b)*y2a[khi])*(h*h)/6.0;
}
free(_y2a);
}
int UART::open(const std::string &portName, int baudRate)
{
UART_inner_data *pd;
COMMTIMEOUTS oComTimeout;
BOOL fSuccess;
int ret_code = 0;
// handle structure
pd = (UART_inner_data *) data;
if( portName.size() ) port_name = portName;
if( baudRate ) baud_rate = baudRate;
// open port
pd->m_hCom = CreateFile(
port_name.c_str(), // COM port Name
GENERIC_READ | GENERIC_WRITE, // read/write flags
0, // comm devices must be opened w/exclusive-access
NULL, // no security attributes
OPEN_EXISTING, // COM devices must use OPEN_EXISTING
0, // no overlapped I/O
NULL // hTemplate must be NULL for COM devices
);
if (pd->m_hCom == INVALID_HANDLE_VALUE) {
// Handle the error.
dbg_pe("UART::open CreateFile failed with wrror %d.", GetLastError());
return 1;
}
// We will build on the current configuration, and skip setting the size
// of the input and output buffers with SetupComm.
fSuccess = GetCommState(pd->m_hCom, &(pd->m_oDCB));
if ( !fSuccess ) {
dbg_pe("UART::open GetComState failed with error %d.", GetLastError());
ret_code = 2;
goto UART_OPEN_ERR;
}
// Fill in the DCB
switch(baud_rate) {
case 110:
pd->m_oDCB.BaudRate = CBR_110;
break;
case 300:
pd->m_oDCB.BaudRate = CBR_300;
break;
case 600:
pd->m_oDCB.BaudRate = CBR_600;
break;
case 1200:
pd->m_oDCB.BaudRate = CBR_1200;
break;
case 2400:
pd->m_oDCB.BaudRate = CBR_2400;
break;
case 4800:
pd->m_oDCB.BaudRate = CBR_4800;
break;
case 9600:
pd->m_oDCB.BaudRate = CBR_9600;
break;
case 14400:
pd->m_oDCB.BaudRate = CBR_14400;
break;
case 19200:
pd->m_oDCB.BaudRate = CBR_19200;
break;
case 38400:
pd->m_oDCB.BaudRate = CBR_38400;
break;
case 56000:
pd->m_oDCB.BaudRate = CBR_56000;
break;
case 57600:
pd->m_oDCB.BaudRate = CBR_57600;
break;
case 115200:
pd->m_oDCB.BaudRate = CBR_115200;
break;
case 128000:
pd->m_oDCB.BaudRate = CBR_128000;
break;
case 2560000:
pd->m_oDCB.BaudRate = CBR_256000;
break;
default:
dbg_pe("UART::open Unsupport baud rate %d.", baud_rate);;
ret_code = 3;
goto UART_OPEN_ERR;
}
if( DTR_sw )
pd->m_oDCB.fDtrControl = DTR_CONTROL_DISABLE;
if( RTS_sw )
pd->m_oDCB.fRtsControl = RTS_CONTROL_DISABLE;
pd->m_oDCB.ByteSize = byte_size;
pd->m_oDCB.Parity = parity_sw;
switch( stop_bits ) {
case 1:
pd->m_oDCB.StopBits = ONESTOPBIT;
break;
case 15:
pd->m_oDCB.StopBits = ONE5STOPBITS;
break;
case 2:
pd->m_oDCB.StopBits = TWOSTOPBITS;
break;
}
fSuccess = SetCommState(pd->m_hCom, &(pd->m_oDCB));
if (!fSuccess) {
// Handle the error.
dbg_pe("UART::open SetComState failed with error %d.", GetLastError());
ret_code = 4;
goto UART_OPEN_ERR;
}
// timout options
if( timeout_sw ) {
fSuccess = GetCommTimeouts( pd->m_hCom, /* Handle to comm device */
&oComTimeout /* time-out values */
);
if( !fSuccess ) {
dbg_pe("UART::open GetCommTimeouts failed with error %d.", GetLastError());
ret_code = 5;
goto UART_OPEN_ERR;
}
oComTimeout.ReadIntervalTimeout = DEFAULT_READINTERVALTIMEOUT;
oComTimeout.ReadTotalTimeoutMultiplier = DEFAULT_READMULTIPLIER;
oComTimeout.ReadTotalTimeoutConstant = DEFAULT_READTOTALTIMEOUT;
oComTimeout.WriteTotalTimeoutMultiplier = DEFAULT_WRITEMULTIPLIER;
oComTimeout.WriteTotalTimeoutConstant = DEFAULT_WRITETOTALTIMEOUT;
fSuccess = SetCommTimeouts( pd->m_hCom,
&oComTimeout
);
if( !fSuccess ) {
dbg_pe("UART::open SetCommTimeouts failed with error %d.", GetLastError());
ret_code = 6;
goto UART_OPEN_ERR;
}
}
// port state
goto UART_OPEN_RET;
UART_OPEN_ERR:
CloseHandle(pd->m_hCom);
pd->m_hCom = NULL;
UART_OPEN_RET:
return ret_code;
}
int RMP_Node::sendMsg(RMessage *m)
{
int ret;
ru8 *buf = NULL;
// same process
if( isSameProcess && mpObj != NULL ) {
mpObj->recvMessageSlot(m);
return 0;
}
// connect to node
if( socketSend == NULL ) {
socketSend = new RSocket;
// create connections
ret = socketSend->startClient(inet4_addr, inet4_port);
if( ret != 0 ) {
dbg_pe("Can not connect to node: %s (%s), addr = %d, port = %d\n",
nodeName.c_str(), hostName.c_str(),
inet4_addr, inet4_port);
ret = -1;
goto SEND_MSG_ERR;
}
// send my node name to pairwise node
RDataStream ds;
ds.write(mpMe->getNodeMe()->nodeName);
ret = socketSend->send(ds);
//dbg_pt("send my node to other: %s\n", nodeName.c_str());
if( ret < ds.size() ) {
dbg_pe("Can not connect to node: %s (%s), addr = %d, port =%d\n",
nodeName.c_str(), hostName.c_str(),
inet4_addr, inet4_port);
ret = -1;
goto SEND_MSG_ERR;
}
}
// send message to node
ru32 header_len;
ru32 msg_magic, msg_ver, msg_header;
ru32 data_len;
header_len = 4*sizeof(ru32);
buf = new ru8[header_len+4];
msg_magic = RTK_MAGIC_MESSAGE;
msg_ver = 1;
msg_header = (msg_magic << 16) | (msg_ver & 0xFFFF);
data_len = m->data.size();
memcpy(buf + 0*sizeof(ru32), &msg_header, sizeof(ru32));
memcpy(buf + 1*sizeof(ru32), &(m->msgType), sizeof(ru32));
memcpy(buf + 2*sizeof(ru32), &(m->msgID), sizeof(ru32));
memcpy(buf + 3*sizeof(ru32), &data_len, sizeof(ru32));
// send header
ret = socketSend->send(buf, header_len);
if( ret < 0 ) goto SEND_MSG_ERR;
// send data
ret = socketSend->send(m->data.data(), m->data.size());
if( ret < 0 ) goto SEND_MSG_ERR;
goto SEND_MSG_RETURN;
SEND_MSG_ERR:
ret = -2;
dbg_pw("Send message error to node: %s (%s), errcode = %d\n",
nodeName.c_str(), hostName.c_str(), ret);
socketSend->close();
socketSend = NULL;
SEND_MSG_RETURN:
if( buf != NULL ) delete [] buf;
return ret;
}
