static void *callback(enum mg_event event,
struct mg_connection *conn,
const struct mg_request_info *request_info)
{
const int buf_max = 1000;
char result[buf_max] = "none";
if (event == MG_NEW_REQUEST) {
//const char* uri = request_info->uri;
// get query args
char cmd[32];
char timeoutStr[5];
int timeout;
char str[256];
get_qsvar(request_info, "cmd", cmd, sizeof(cmd));
get_qsvar(request_info, "str", str, sizeof(str));
get_qsvar(request_info, "timeout", timeoutStr, sizeof(timeoutStr));
timeout = timeoutStr[0] == '\0' ? 5000 : atoi(timeoutStr);
if( ! cmd[0] ) { // cmd is empty
/// do something useful here
}
else {
if( strcasecmp(cmd, "read")==0 ) {
serialport_flush(fd);
serialport_read_until(fd, str, '\n', buf_max, timeout);
sprintf(result, "read:\n%s\n",str); // debug
}
else if( strcasecmp(cmd, "send")==0 ) {
serialport_write(fd, str);
sprintf(result, "send:\n%s\n",str);
}
else if( strcasecmp(cmd, "sendline")==0 ) {
sprintf(str, "%s\r\n ", str);
serialport_write(fd, str);
sprintf(result, "sendline:\n%s",str);
}
else {
sprintf(result, "uknown cmd '%s'",cmd);
}
}
printf("result:%s\n",result);
// Echo requested URI back to the client
mg_printf(conn, "HTTP/1.1 200 OK\r\n"
"Content-Type: text/plain\r\n\r\n"
"%s"
"\n",
result
);
return ""; // Mark as processed
} else {
return NULL;
}
}
void message_callback(struct mosquitto *mosq, void *userdata, const struct mosquitto_message *message) {
DBG("%s -> %s\n", message->topic, (char *) message->payload);
global_data_t *g = userdata;
if(message->payload != NULL) {
serialport_write(g->arduino_fd, message->payload);
serialport_write(g->arduino_fd, "\n");
}
}
int serialport_send_slip_static(unsigned char *data, unsigned int size)
{
unsigned char buf[STATIC_SLIP_BUF_SIZE];
unsigned out_pos = 0;
for (int i = 0; i < size; ++i)
{
unsigned char cur = data[i];
if (cur == 0xC0)
{
buf[out_pos++] = subst_C0[0];
buf[out_pos++] = subst_C0[1];
}
else if (cur == 0xDB)
{
buf[out_pos++] = subst_DB[0];
buf[out_pos++] = subst_DB[1];
}
else
buf[out_pos++] = cur;
}
if(serialport_write(buf, out_pos) != out_pos)
{
LOGERR("failed sending %d bytes", out_pos);
return 0;
}
serialport_drain();
return size;
}
int main(int argc, char* argv[]) {
if (argc == 1) {
printf("You have to pass the name of a serial port.\n");
return -1;
}
int baudrate = B9600;
int arduino = serialport_init(argv[1], baudrate);
if (arduino == -1) {
printf("Could not open serial port %s.\n", argv[1]);
return -1;
}
sleep(2);
char line[MAX_LINE];
while (1) {
int rc = serialport_write(arduino, "a0\n");
if (rc == -1) {
printf("Could not write to serial port.\n");
} else {
serialport_read_until(arduino, line, '\n');
printf("%s", line);
}
}
return 0;
}
vector<uint8_t> ModCom::comunicate(vector<uint8_t> buffer, bool readdata, int sizeread) {
uint8_t *buffer_out = &(buffer[0]);
uint8_t buffer_in[255];
serialport_flush(fd);
serialport_write(fd, (char*)buffer_out);
if (!readdata) {
vector<uint8_t> toreturn;
return toreturn;
} // if only send
//serial_read(fd, buffer_in, 1, DEF_TIMEOUT);
serialport_read(fd, buffer_in, sizeread, DEF_TIMEOUT);
if (buffer_in[0] != 0xFF) {
vector<uint8_t> toreturn;
return toreturn;
}
vector<uint8_t> vr(buffer_in, buffer_in + sizeread);
return vr;
}
int main(int argc, char** argv)
{
/********************
* Set up ROS node
*/
ros::init(argc, argv, "talker");
ros::NodeHandle n;
ros::Publisher tableDir_pub = n.advertise<alfred_msg::FSRDirection>("table_direction", 100);
ros::Publisher tableAngle_pub = n.advertise<alfred_msg::TableAngleStatus>("TableAngleStatus", 100);
// Set up serial connection
char buf[20];
char* modemDevice = "/dev/ttyACM0";
if( argc > 1 )
modemDevice = argv[1];
int fd = serialport_init(modemDevice, BAUDRATE);
if(fd < 0) return -1;
usleep(3000 * 1000 );
printf("Connected to Arduino, starting loop\n");
ros::Rate rate(50);
while(ros::ok())
{
// Get the X coordinate
serialport_write(fd, "x\0");
serialport_read_until(fd, buf, ':');
directionMessage.x = atof(buf);
//printf("Received X:%f\n", directionMessage.x);
// Get the Y coordinate
serialport_write(fd, "y");
serialport_read_until(fd, buf, ':');
directionMessage.y = atof(buf);
//printf("Received Y:%f\n", directionMessage.y);
// Get the follow indicator
serialport_write(fd, "f");
serialport_read_until(fd, buf, ':');
directionMessage.followPressed = atof(buf) == 1;
//printf("Received F:%s\n", directionMessage.followPressed ? "true":"false" );
tableDir_pub.publish(directionMessage);
rate.sleep();
}
}
int serialport_send_slip(unsigned char *data, unsigned int size)
{
unsigned int sent;
unsigned char cur_byte;
if (size < STATIC_SLIP_BUF_SIZE / 2)
return serialport_send_slip_static(data, size);
sent = 0;
while(sent != size)
{
cur_byte = *data++;
if(cur_byte == 0xC0)
{
if(serialport_write(subst_C0, 2) != 2)
{
LOGERR("failed substituting 0xC0");
return 0;
}
}
else if(cur_byte == 0xDB)
{
if(serialport_write(subst_DB, 2) != 2)
{
LOGERR("failed substituting 0xDB");
return 0;
}
}
else
{
if(serialport_write(&cur_byte, 1) != 1)
{
LOGERR("failed sending byte %i", sent);
return 0;
}
}
sent++;
}
serialport_drain();
return sent;
}
//render a frame to real device or stdout
void render_frame(const char *dev, const char *frame) {
if (dev==NULL) { //dummy
dummy_output(frame);
return;
}
//open port if no open port
if (s_port == -1) {
s_port = serialport_init(dev, 115200);
if (s_port == -1) {
fprintf(stderr, "Could not open serial device file! Aborting.\n");
exit(EXIT_FAILURE);
}
}
//write frame
serialport_write(s_port, frame);
serialport_write(s_port, "\n");
}
int WriteTX(ServoStates* st){
short* a = st->Angle;
char buf[128];
sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d\r\n",
a[0], a[1], a[2], a[3],
a[4], a[5], a[6], a[7]
);
return serialport_write(USB, buf);
}
IplImage *process(IplImage **_img)
{
fprintf(stderr, "Processing image:\n");
IplImage *img = *_img;
/* Convert to HSV */
print_time("Converting to HSV");
CvSize size = cvGetSize(img);
IplImage *hsv = cvCreateImage(size, IPL_DEPTH_8U, 3);
cvCvtColor(img, hsv, CV_BGR2HSV);
/* Generate mask */
CvMat *mask = cvCreateMat(size.height, size.width, CV_8UC1);
//cvInRangeS(hsv, cvScalar(0.11*256, 0.60*256, 0.20*256, 0),cvScalar(0.14*256, 1.00*256, 1.00*256, 0), mask);
cvInRangeS(hsv, cvScalar(0,0.6*256,0.6*256,0),cvScalar(0.21*256,256,256,0), mask);
cvReleaseImage(&hsv);
/* Perform morphological ops */
print_time("Performing morphologies");
IplConvKernel *se21 = cvCreateStructuringElementEx(21, 21, 10, 10, CV_SHAPE_RECT, NULL);
IplConvKernel *se11 = cvCreateStructuringElementEx(11, 11, 5, 5, CV_SHAPE_RECT, NULL);
cvClose(mask, mask, se21);
cvOpen(mask, mask, se11);
cvReleaseStructuringElement(&se21);
cvReleaseStructuringElement(&se11);
/* Hough transform */
IplImage *hough_in = cvCreateImage(size, 8, 1);
cvCopy(mask, hough_in, NULL);
int rows=size.height;
int cols=size.width;
int j,k;
int breakflag=0;
for(j=0;j<rows;j++) {
for(k=0;k<cols;k++)
{
CvScalar val=cvGet2D(hough_in,j,k);
if(val.val[0]==255)
{
sprintf(dat,"%d-%d:",k,j);
int rc = serialport_write(fd, dat);
if(rc==-1) return 0;
fprintf(fp,"%d %d\n",k,j);
breakflag=1;
break;
}
}
if(breakflag)
break;
}
return hough_in;
}
int serialport_send_C0(void)
{
unsigned char b;
b = 0xC0;
if(serialport_write(&b, 1) != 1)
{
LOGERR("failed sending 0xC0");
return 0;
}
serialport_drain();
return 1;
}
int main(int argc, char *argv[])
{
int baudrate = B9600; // default
int fd;
char cmd = 0;
char cmdv[3] = "a";
fd = serialport_init(ARD_LOC, baudrate);
if((argv[1][0]) == 'p'){
while(1){
printf("Water Turret$: ");
scanf("%c",&cmd);
// printf("1\n");
cmdv[0] = (char)cmd;
// printf("2\n");
if(cmd == 'x'){
// printf("3\n");
return 0;
}
// printf("4\n");
serialport_write(fd, cmdv);
}
}
serialport_write(fd, "a");
serialport_write(fd, "w");
serialport_write(fd, "a");
serialport_write(fd, "w");
serialport_write(fd, "d");
serialport_write(fd, "d");
serialport_write(fd, "s");
serialport_write(fd, "s");
}
int send_string(char* in_string){
//printf("in send_string\n");
int rc = -1;
if( fd == -1 ){
printf("serial port not opened");
return -1;
}
//sprintf(in_string, "%s", optarg);
printf("send string:%s\n", in_string);
rc = serialport_write(fd, in_string);
if(rc==-1){
printf("error writing");
return -2;
}
return 0;
}
//发送串口通讯数据包
static inline void CommSend(BYTE size)
{
WORD sum;
BYTE i;
UartSend(0x46);
UartSend(0xb9);
UartSend(0x6a);
UartSend(0x00);
sum = size + 6 + 0x6a;
UartSend(size + 6);
serialport_write(handle_download_port,TxBuffer,size);
for (i=0; i<size; i++)
{
sum += TxBuffer[i];
}
UartSend(HIBYTE(sum));
UartSend(LOBYTE(sum));
UartSend(0x16);
CommInit();
}
void web_child(int connfd, int fd){
int n;
char buffer[256];
bzero(buffer,256);
n = read(connfd,buffer,255);
if (n < 0){
perror("ERROR reading from socket");
exit(1);
}
//printf("Here is the message: %s\n",buffer);
//strncat(buffer,"\n", sizeof buffer);
//n = write(connfd,"I got your message",18);
serialport_write(fd, buffer); //I have to error check this
FILE *fp = fopen("out1.txt","a"); /* Open file in append monde */
fprintf(fp, "Read string: %s", buffer); /* Write output in file */
fclose(fp);/* Close file */
if (n < 0){
perror("ERROR writing to socket");
exit(1);
}
return;
}
int send_string(char* in_string){
//printf("in send_string\n");
int rc = -1;
if( fd == -1 ){
printf("serial port not opened\n");
return -1;
}
//print strings that aren't spacers
if (in_string[0] != ' ')
printf("string:%s\n", in_string);
rc = serialport_write(fd, in_string);
if(rc==-1){
printf("error writing\n");
return -2;
}
return 0;
}
int main(int argc, char *argv[])
{
int fd = 0;
char serialport[256];
int baudrate = B9600; // default
char buf[256];
int rc,n;
if (argc==1) {
usage();
exit(EXIT_SUCCESS);
}
/* parse options */
int option_index = 0, opt;
static struct option loptions[] = {
{"help", no_argument, 0, 'h'},
{"port", required_argument, 0, 'p'},
{"baud", required_argument, 0, 'b'},
{"send", required_argument, 0, 's'},
{"receive", no_argument, 0, 'r'},
{"num", required_argument, 0, 'n'},
{"delay", required_argument, 0, 'd'}
};
while(1) {
opt = getopt_long (argc, argv, "hp:b:s:rn:d:",
loptions, &option_index);
if (opt==-1) break;
switch (opt) {
case '0': break;
case 'd':
n = strtol(optarg,NULL,10);
usleep(n * 1000 ); // sleep milliseconds
break;
case 'h':
usage();
break;
case 'b':
baudrate = strtol(optarg,NULL,10);
break;
case 'p':
strcpy(serialport,optarg);
fd = serialport_init(optarg, baudrate);
if(fd==-1) return -1;
break;
case 'n':
n = strtol(optarg, NULL, 10); // convert string to number
rc = serialport_writebyte(fd, (uint8_t)n);
if(rc==-1) return -1;
break;
case 's':
strcpy(buf,optarg);
rc = serialport_write(fd, buf);
if(rc==-1) return -1;
break;
case 'r':
serialport_read_until(fd, buf, '\n');
printf("read: %s\n",buf);
break;
}
}
exit(EXIT_SUCCESS);
} // end main
int main(int argc, char *argv[])
{
/*accelerometer related declarations*/
const int buf_max = 2048;
int fd = -1;
char serialport[buf_max];
int baudrate = 9600; // default
char quiet=0;
char eolchar = ' ';
int timeout = 5000;
char buf[buf_max];
int rc,n,p;
float xDat[103],yDat[103],zDat[103];
float sumX=0,sumY=0,sumZ=0;
float sum1=0,sum2=0,sum3=0;
float stdX=0,stdY=0,stdZ=0,avgX=0,avgY=0,avgZ=0,varX=0,varY=0,varZ=0,recal=0;
int total=1;
int calibrate=0;
/*---------------------------------*/
// char stdbuffer[10];
/*socket related declarations*/
int sockfd, portno, n2;
struct sockaddr_in serv_addr;
struct hostent *server;
char buffer[256];
int sendMessage=1;
/*----------------------------*/
if (argc==1) {
usage();
}
/* parse options */
int option_index = 0, opt;
static struct option loptions[] = {
{"help", no_argument, 0, 'h'},
{"port", required_argument, 0, 'p'},
{"baud", required_argument, 0, 'b'},
{"send", required_argument, 0, 's'},
{"setup", no_argument, 0, 'S'},
{"receive", no_argument, 0, 'r'},
{"flush", no_argument, 0, 'F'},
{"num", required_argument, 0, 'n'},
{"delay", required_argument, 0, 'd'},
{"eolchar", required_argument, 0, 'e'},
{"timeout", required_argument, 0, 't'},
{"quiet", no_argument, 0, 'q'},
{NULL, 0, 0, 0}
};
while(1) {
opt = getopt_long (argc, argv, "hp:b:s:rSFn:d:qe:t:",
loptions, &option_index);
if (opt==-1) break;
switch (opt) {
case '0': break;
case 'q':
quiet = 1;
break;
case 'e':
eolchar = optarg[0];
if(!quiet) printf("eolchar set to '%c'\n",eolchar);
break;
case 't':
timeout = strtol(optarg,NULL,10);
if( !quiet ) printf("timeout set to %d millisecs\n",timeout);
break;
case 'd':
n = strtol(optarg,NULL,10);
if( !quiet ) printf("sleep %d millisecs\n",n);
usleep(n * 1000 ); // sleep milliseconds
break;
case 'h':
usage();
break;
case 'b':
baudrate = strtol(optarg,NULL,10);
break;
case 'p':
if( fd!=-1 ) {
serialport_close(fd);
if(!quiet) printf("closed port %s\n",serialport);
}
strcpy(serialport,optarg);
fd = serialport_init(optarg, baudrate);
if( fd==-1 ) error("couldn't open port");
if(!quiet) printf("opened port %s\n",serialport);
serialport_flush(fd);
break;
case 'n':
if( fd == -1 ) error("serial port not opened");
n = strtol(optarg, NULL, 10); // convert string to number
rc = serialport_writebyte(fd, (uint8_t)n);
if(rc==-1) error("error writing");
break;
case 'S':
portno = 52002;//atoi(argv[2]);
/* Create a socket point*/
sockfd = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd < 0)
{
perror("ERROR opening socket");
exit(1);
}
server = gethostbyname("192.168.2.103");//gethostbyname(argv[1]);
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
bzero((char *) &serv_addr, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
bcopy((char *)server->h_addr,
(char *)&serv_addr.sin_addr.s_addr,
server->h_length);
serv_addr.sin_port = htons(portno);
/* Now connect to the server */
if (connect(sockfd,&serv_addr,sizeof(serv_addr)) < 0)
{
perror("ERROR connecting");
exit(1);
}
while(1){
int r;
int a1, a2, a3;
char a4[sizeof(float)*3+3];
memset (buf, 0, 256);
r = serialport_read_until(fd, buf);
//fprintf (stderr, "VAL: %s -->", buf);
if (r >= 0)
{
sscanf (buf, "A1:%d\tA2:%d\tA3:%d", &a1, &a2, &a3);
// fprintf (stderr, "(%d)(%d)(%d)\n", a1, a2, a3);
if(calibrate==0){
sumX= sumX + a1; sumY= sumY + a2; sumZ= sumZ + a3;
xDat[total]=a1;yDat[total]=a2;zDat[total]=a3;
total++;
if (total==100){
calibrate=1;
avgX = sumX/(float)total; avgY = sumY/(float)total; avgZ = sumZ/(float)total;
for(p = 1; p<total;p++){
sum1 = sum1 + pow((xDat[p]-avgX),2); sum2 = sum2 + pow((yDat[p]-avgY),2); sum3 = sum3 + pow((zDat[p]-avgZ),2);
}
varX= sum1/(float)total; varY=sum2/(float)total; varZ=sum3/(float)total;
stdX=sqrt(varX); stdY=sqrt(varY); stdZ=sqrt(varZ);
}
}
if(calibrate==1){snprintf(a4, sizeof a4*7, "%f %f %f %f %f %f %f %f %f", (float)a1, (float)a2,(float)a3,avgX,avgY,avgZ,stdX,stdY,stdZ);}
while(sendMessage==1&&calibrate==1){
/* Send message to the server*/
//printf("here");
n2 = write(sockfd,a4,strlen(a4));
if (n2 < 0)
{
perror("ERROR writing to socket");
exit(1);
}
/* Now read server response */
bzero(buffer,256);
n2 = read(sockfd,buffer,255);
if (n2 < 0)
{
perror("ERROR reading from socket");
exit(1);
}
// printf("Received message: %s\n",buffer);
sendMessage=0;
}
sendMessage=1;
}
sendMessage=1;
fflush (0);
//sleep (5);
}
break;
case 's':
if( fd == -1 ) error("serial port not opened");
sprintf(buf, (opt=='S' ? "%s\n" : "%s"), optarg);
if( !quiet ) printf("send string:%s\n", buf);
rc = serialport_write(fd, buf);
if(rc==-1) error("error writing");
break;
case 'r':
while (1)
{
int r;
int a1, a2, a3;
memset (buf, 0, 256);
r = serialport_read_until(fd, buf);
fprintf (stderr, "VAL: %s -->", buf);
// printf("%d",r);
if (r >= 0)
{
sscanf (buf, "A1:%d\tA2:%d\tA3:%d", &a1, &a2, &a3);
fprintf (stderr, "(%d)(%d)(%d)\n", a1, a2, a3);
//fgets(stdbuffer,10,stdin);
//if(strcmp(stdbuffer,"getData")==0){
// printf("%d",a1);
// printf("%d",a2);
// printf("%d",a3);}
//}
/* while(sendMessage==1){
/* Send message to the server
n2 = write(sockfd,"measure",strlen("measure"));
if (n2 < 0)
{
perror("ERROR writing to socket");
exit(1);
}
/* Now read server response
bzero(buffer,256);
n2 = read(sockfd,buffer,255);
if (n2 < 0)
{
perror("ERROR reading from socket");
exit(1);
}
printf("Received message: %s\n",buffer);
sendMessage=0;
}*/
}
fflush (0);
usleep (10000);
}
break;
case 'F':
if( fd == -1 ) error("serial port not opened");
if( !quiet ) printf("flushing receive buffer\n");
serialport_flush(fd);
break;
}
}
exit(EXIT_SUCCESS);
} // end main
//串口数据发送程序
static inline BYTE UartSend(BYTE dat)
{
serialport_write(handle_download_port,&dat,1);
return dat;
}
int main(int argc, char** argv){
/*******************
* Base Drive Variables
*
* LOOP_RATE defines how fast we send orders to the arduino
* ORDER_TIMEOUT is the maximum amount of time between recieving orders
* before we shut the motors off
*
* BAUDRATE baudrate to arduino
* SERIAL_DEV the defauly dev file pointing to the arduino
* READ_CHAR_TIMEOUT the timeout to wait for the next character in a
* message, in us
* SERIAL_RESPONSE_TIMEOUT The timeout to wait for a response to our
* query, in us
*/
double LOOP_RATE = 10.0;
double ORDER_TIMEOUT = 0.25;
int BAUDRATE = B115200;
char* SERIAL_DEV = "/dev/ttyUSB0";
int READ_CHAR_TIMEOUT = 30;
int SERIAL_RESPONSE_TIMEOUT = 100;
char START_CHAR = '`';
/********************
* Set up ROS node
*/
ros::init(argc, argv, "TableMotorToSerial");
ros::NodeHandle n;
ROS_INFO( "Starting Subscriber" );
ros::Subscriber sub = n.subscribe("DriveTable", 2, callbackTableMotorToSerial );
ros::Publisher pub = n.advertise<alfred_msg::TableEncoders>("TableEncoders", 2);
/********************
* Setup loop variables and start serial
*/
char* serialDevice = SERIAL_DEV;
if( argc > 1 )
serialDevice = argv[1];
int fd = serialport_init(serialDevice, BAUDRATE);
if(fd < 0){
ROS_ERROR( "Could not open serial device \'%s\'\n", serialDevice);
return -1;
}
usleep( 3000 * 1000 );
ros::Rate rate(LOOP_RATE);
int maxNumLoopsWithoutOrder = (int)( ORDER_TIMEOUT * LOOP_RATE );
int numLoopsWithoutOrder(-1);
while (n.ok()) {
// If we haven't yet started
if( numLoopsWithoutOrder == -1){
tableOrder_serialArduino1 = 0.0;
tableOrder_serialArduino2 = 0.0;
tableOrder_serialArduino3 = 0.0;
tableOrder_serialArduino4 = 0.0;
}
// Safety Check 1 : time without orders
else if( numLoopsWithoutOrder >= maxNumLoopsWithoutOrder ){
tableOrder_serialArduino1 = 0.0;
tableOrder_serialArduino2 = 0.0;
tableOrder_serialArduino3 = 0.0;
tableOrder_serialArduino4 = 0.0;
ROS_ERROR( "Base Motor to Serial hasn't received a new motor order in %f mS!", 1000 * ORDER_TIMEOUT );
}
// Safety check passed, send
if( numLoopsWithoutOrder >= 0 )
numLoopsWithoutOrder++;
if( receivedOrder_serialArduino == true ){
numLoopsWithoutOrder = 0;
receivedOrder_serialArduino = false;
}
ROS_INFO( "Sending output to table: [%f, %f, %f, %f]",
tableOrder_serialArduino1,
tableOrder_serialArduino2,
tableOrder_serialArduino3,
tableOrder_serialArduino4);
//write
std::stringstream ss;
ss << "`"
<< tableOrder_serialArduino1 << ";"
<< tableOrder_serialArduino2 << ";"
<< tableOrder_serialArduino3 << ";"
<< tableOrder_serialArduino4 << ";\0";
std::string msg = ss.str();
int isok = serialport_write(fd, msg.c_str() );
// now read, and wait for the arduino to reply with the encoder values
int encoders[4];
char buf[10];
// First, wait for a response
if( serialport_read_until(fd, buf, START_CHAR, SERIAL_RESPONSE_TIMEOUT) <= 0)
{
ROS_ERROR("ERROR: Timeout on serial response message start!");
for( int i(0); i < 4; i++ )
encoders[i]=-1;
}
else{
for( int i(0); i < 4; i++ ){
isok = serialport_read_until(fd, buf, ';', READ_CHAR_TIMEOUT );
if( isok <= 0 ){
ROS_ERROR("ERROR: Timeout reading message");
break;
}
encoders[i] = std::atoi(buf);
}
if( isok <= 0 )
for( int i(0); i < 4; i++ )
encoders[i]=-1;
}
ROS_INFO( "Publishing Encoder Vals: [%d, %d, %d, %d]",
encoders[0],
encoders[1],
encoders[2],
encoders[3]);
alfred_msg::TableEncoders encTopic;
encTopic.enc1 = encoders[0];
encTopic.enc2 = encoders[1];
encTopic.enc3 = encoders[2];
encTopic.enc4 = encoders[3];
pub.publish(encTopic);
ROS_INFO("\n");
rate.sleep();
ros::spinOnce();
}
serialport_close(fd);
return 0;
}