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;
}
