int main(int argc, char *argv[])
{
ros::Publisher pub[TOPIC_NR];
int port, plane;
int sock, asock;
struct sigaction act;
memset(&act, 0, sizeof(act));
act.sa_handler = stopChildProcess;
if (sigaction(SIGTERM, &act, NULL) == -1) {
perror("sigaction");
return -1;
}
ros::init(argc, argv, NODE_NAME);
ros::NodeHandle node;
pub[0] = node.advertise<tablet_socket_msgs::gear_cmd>("gear_cmd", 1);
pub[1] = node.advertise<tablet_socket_msgs::mode_cmd>("mode_cmd", 1);
pub[2] = node.advertise<tablet_socket_msgs::route_cmd>("route_cmd", 1);
pub[3] = node.advertise<geometry_msgs::PoseStamped>("gnss_pose", 1);
pub[4] = node.advertise<std_msgs::Bool>("gnss_stat", 1);
node.param<int>("tablet_receiver/port", port, DEFAULT_PORT);
node.param<int>("tablet_receiver/plane", plane, DEFAULT_PLANE);
fprintf(stderr, "listen port=%d\n", port);
geo.set_plane(plane);
//get connect to android
sock = -1;
sigaction(SIGINT, NULL, &act);
act.sa_flags &= ~SA_RESTART;
sigaction(SIGINT, &act, NULL);
while (getConnect(port, &sock, &asock) != -1) {
struct timeval tv[2];
double sec;
int count;
fprintf(stderr, "get connect.\n");
gettimeofday(tv, NULL);
for (count = 0; ; count++) {
if(getSensorValue(asock, pub) == -1)
break;
if(sendSignal(asock) == -1)
break;
}
close(asock);
gettimeofday(tv+1, NULL);
sec = (tv[1].tv_sec - tv[0].tv_sec) +
(tv[1].tv_usec - tv[0].tv_usec) / 1000000.0;
fprintf(stderr, "done, %f sec\n",sec);
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "nmea2tfpose");
ros::NodeHandle n;
ros::NodeHandle private_nh("~");
private_nh.getParam("plane", _plane);
std::cout << "plane number : " << _plane << std::endl;
geo.set_plane(_plane);
ros::Subscriber sub = n.subscribe("nmea_sentence", 1000, NmeaCallback);
pose_publisher = n.advertise<geometry_msgs::PoseStamped>("gnss_pose", 1000);
stat_publisher = n.advertise<std_msgs::Bool>("/gnss_stat", 100);
ros::spin();
return 0;
}
