int main(void) { struct udp_struct udp; int msglen; char buf[512]; if(udpServer_Init(&udp,9930,1)) diep("udpServer_Init"); for (int i=0; i<10; i++) { int cnt=0; do { msglen = udpServer_Receive(&udp, buf, 512); cnt++; } while(msglen<=0); printf("Received at cnt=%d -> '%s'\n\n", cnt, buf); } udpClient_Close(&udp); return 0; }
int main(int argc, char **argv) { udp_struct udp; ardrone_islab::Navdataislab navdata; int msglen; char buf[512]; char * pch; ardrone_islab::Navdataislab navdataformat; ros::init(argc, argv, "ardrone_ground_node_server"); ros::NodeHandle n; ros::Rate loop_rate(300); ros::Publisher navdata_pub = n.advertise<ardrone_islab::Navdataislab>("ardrone/navdata",1000); printf("check1"); // Broadcast a simple log message ROS_INFO_STREAM("Start ROS node!"); if(udpServer_Init(&udp,9930,0)) diep("udpServer_Init"); printf("check2"); while (ros::ok()) { int cnt=0; do { msglen = udpServer_Receive(&udp, buf, 512); cnt++; } while(msglen<=0); pch = strtok (buf," ,"); int countpart = 0; //printf("%d",len(buf)); while (countpart<=42) { if (countpart == 0) navdata.tm = atof(pch); if (countpart == 1) navdata.tm_pre = atof(pch); if (countpart == 2) navdata.frame = atof(pch); if (countpart == 3) navdata.pitch_w = atof(pch); if (countpart == 4) navdata.pitch_a = atof(pch); if (countpart == 5) navdata.pitch = atof(pch); if (countpart == 6) navdata.roll_w = atof(pch); if (countpart == 7) navdata.roll_a = atof(pch); if (countpart == 8) navdata.roll = atof(pch); if (countpart == 9) navdata.yaw_w = atof(pch); if (countpart == 10) navdata.yaw_m = atof(pch); if (countpart == 11) navdata.yaw = atof(pch); if (countpart == 12) navdata.dt = atof(pch); if (countpart == 13) navdata.dt2 = atof(pch); if (countpart == 14) navdata.q_est[0] = atof(pch); if (countpart == 15) navdata.q_est[1] = atof(pch); if (countpart == 16) navdata.q_est[2] = atof(pch); if (countpart == 17) navdata.q_est[3] = atof(pch); if (countpart == 18) navdata.b_est[0] = atof(pch); if (countpart == 19) navdata.b_est[1] = atof(pch); if (countpart == 20) navdata.b_est[2] = atof(pch); if (countpart == 21) navdata.ts = atof(pch); if (countpart == 22) navdata.hraw = atof(pch); if (countpart == 23) navdata.h_meas = atof(pch); if (countpart == 24) navdata.ax = atof(pch); if (countpart == 25) navdata.ay = atof(pch); if (countpart == 26) navdata.az = atof(pch); if (countpart == 27) navdata.wx = atof(pch); if (countpart == 28) navdata.wy = atof(pch); if (countpart == 29) navdata.wz = atof(pch); if (countpart == 30) navdata.magX = atof(pch); if (countpart == 31) navdata.magY = atof(pch); if (countpart == 32) navdata.magZ = atof(pch); if (countpart == 33) navdata.pressure = atof(pch); if (countpart == 34) navdata.rotX = atof(pch); if (countpart == 35) navdata.rotY = atof(pch); if (countpart == 36) navdata.rotZ = atof(pch); if (countpart == 37) navdata.altd = atof(pch); if (countpart == 38) navdata.motor1 = atof(pch); if (countpart == 39) navdata.motor2 = atof(pch); if (countpart == 40) navdata.motor3 = atof(pch); if (countpart == 41) navdata.motor4 = atof(pch); countpart ++; pch = strtok (NULL, " ,"); } //printf(" %.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%f,%f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f ", navdata.tm,navdata.pitch_w,navdata.pitch_a,navdata.pitch,navdata.roll_w,navdata.roll_a,navdata.roll,navdata.yaw_w,navdata.yaw_m,navdata.yaw, navdata.dt,navdata.ts,navdata.hraw,navdata.h_meas,navdata.ax,navdata.ay,navdata.az,navdata.wx,navdata.wy,navdata.wz,navdata.magX,navdata.magY,navdata.magZ,navdata.pressure,navdata.rotX,navdata.rotY,navdata.rotZ,navdata.altd,navdata.motor1,navdata.motor2,navdata.motor3,navdata.motor4); navdata.header.stamp = ros::Time::now(); navdata_pub.publish(navdata); //printf(" %.3f", navdata.tm); ros::spinOnce(); loop_rate.sleep(); } udpClient_Close(&udp); return 0; }