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;
}
static int getSensorValue(int sock, ros::Publisher pub[TOPIC_NR])
{
int info[2];
size_t size = sizeof(info);
ssize_t nbytes;
for (char *p = (char *)info; size; size -= nbytes, p += nbytes) {
nbytes = recv(sock, info, size, 0);
if (nbytes == -1) {
perror("recv");
return -1;
}
if (nbytes == 0) {
fprintf(stderr, "peer is shutdown\n");
return -1;
}
}
fprintf(stderr, "info=%d value=%d\n", info[0], info[1]);
switch(info[0]) {
case 1: { // GEAR
tablet_socket_msgs::gear_cmd msg;
msg.gear = info[1];
pub[0].publish(msg);
break;
}
case 2: { // MODE
tablet_socket_msgs::mode_cmd msg;
msg.mode = info[1];
pub[1].publish(msg);
break;
}
case 3: { // ROUTE
size = info[1];
if (!size)
break;
double *points = (double *)malloc(size);
if (points == NULL) {
perror("malloc");
return -1;
}
int points_nr = size / sizeof(double);
for (char *p = (char *)points; size;
size -= nbytes, p += nbytes) {
nbytes = recv(sock, p, size, 0);
if (nbytes == -1) {
perror("recv");
free(points);
return -1;
}
if (nbytes == 0) {
fprintf(stderr, "peer is shutdown\n");
free(points);
return -1;
}
}
tablet_socket_msgs::route_cmd msg;
tablet_socket_msgs::Waypoint point;
for (int i = 0; i < points_nr; i++) {
if (i % 2) {
point.lon = points[i];
msg.point.push_back(point);
} else
point.lat = points[i];
}
free(points);
pub[2].publish(msg);
break;
}
case 4: { // S1
if (info[1] >= 0)
s1.start();
else
s1.stop();
break;
}
case 5: { // S2
if (info[1] >= 0)
s2.start();
else
s2.stop();
break;
}
case 6: { // POSE
size = info[1];
if (!size)
break;
double *buf = (double *)malloc(size);
if (buf == NULL) {
perror("malloc");
return -1;
}
for (char *p = (char *)buf; size;
size -= nbytes, p += nbytes) {
nbytes = recv(sock, p, size, 0);
if (nbytes == -1) {
perror("recv");
free(buf);
return -1;
}
if (nbytes == 0) {
fprintf(stderr, "peer is shutdown\n");
free(buf);
return -1;
}
}
geo.llh_to_xyz(buf[0], buf[1], buf[2]);
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(geo.y(), geo.x(), geo.z()));
q.setRPY(buf[4], buf[5], buf[3]);
transform.setRotation(q);
free(buf);
ros::Time now = ros::Time::now();
tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(transform, now, "map",
"gps"));
geometry_msgs::PoseStamped pose;
pose.header.stamp = now;
pose.header.frame_id = "map";
pose.pose.position.x = geo.y();
pose.pose.position.y = geo.x();
pose.pose.position.z = geo.z();
pose.pose.orientation.x = q.x();
pose.pose.orientation.y = q.y();
pose.pose.orientation.z = q.z();
pose.pose.orientation.w = q.w();
std_msgs::Bool stat;
if (pose.pose.position.x == 0 || pose.pose.position.y == 0 ||
pose.pose.position.z == 0)
stat.data = false;
else
stat.data = true;
pub[3].publish(pose);
pub[4].publish(stat);
break;
}
default: // TERMINATOR
fprintf(stderr, "receive %d, terminated.\n", info[0]);
sendSignal(sock);
return -1;
}
return 0;
}
static void NmeaCallback(const nmea_msgs::Sentence::ConstPtr& msg)
{
static double qq_time, roll, pitch, yaw;
//static double gga_time, x, y, z;
static double gga_time;
static tf::TransformBroadcaster br;
static ros::Time pc_time;
std::vector<std::string> nmea;
csv_div(msg->sentence, &nmea);
//static bool calibration_flag = true;
// printf("%s\n",msg->sentence.c_str());
if (nmea[0].compare(0, 2, "QQ") == 0) {
pc_time = msg->header.stamp;
/*
qq_time = str2double(nmea[3]);
roll = str2double(nmea[4]) * M_PI / 180.;
pitch = -1 * str2double(nmea[5]) * M_PI / 180.;
yaw = -1 * str2double(nmea[6]) * M_PI / 180. + M_PI / 2;
*/
qq_time = stod(nmea[3]);
roll = stod(nmea[4]) * M_PI / 180.;
pitch = -1 * stod(nmea[5]) * M_PI / 180.;
yaw = -1 * stod(nmea[6]) * M_PI / 180. + M_PI / 2;
// new QQ message
/* if (nmea.size() == NEW_QQ_SIZE) {
if (stod(nmea[7]) == 1 && stod(nmea[8]) == 1 && stod(nmea[9]) == 1) {
calibration_flag = true;
} else {
calibration_flag = false;
}
}*/
//printf("angle %f %f %f %f\n",qq_time,roll,pitch,yaw);
}
// if (calibration_flag == true) {
if (nmea[0] == "$GPGGA") {
pc_time = msg->header.stamp;
/*
gga_time = str2double(nmea[1]);
double lat = str2double(nmea[2]);
double lon = str2double(nmea[4]);
double h = str2double(nmea[9]); //+str2double(nmea[11]);
*/
gga_time = stod(nmea[1]);
double lat = stod(nmea[2]);
double lon = stod(nmea[4]);
double h = stod(nmea[9]);
geo.set_llh_nmea_degrees(lat, lon, h);
// printf("pos %f %f %f %f\n",gga_time,geo.x,geo.y,geo.z);
}
// if (qq_time == gga_time) {
if (fabs(qq_time - gga_time) <= __FLT_EPSILON__) {
//printf("%f %f %f %f %f %f %f %f\n", pc_time.toSec(), gga_time, geo.x(), geo.y(), geo.z(), roll, pitch, yaw);
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(geo.y(), geo.x(), geo.z()));
q.setRPY(roll, pitch, yaw);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, pc_time, "map", "gps"));
geometry_msgs::PoseStamped pose;
pose.header = msg->header;
pose.header.frame_id = "map";
pose.pose.position.x = geo.y();
pose.pose.position.y = geo.x();
pose.pose.position.z = geo.z();
pose.pose.orientation.x = q.x();
pose.pose.orientation.y = q.y();
pose.pose.orientation.z = q.z();
pose.pose.orientation.w = q.w();
// set gnss_stat
if (pose.pose.position.x == 0.0 || pose.pose.position.y == 0.0 || pose.pose.position.z == 0.0) {
gnss_stat_msg.data = false;
} else {
gnss_stat_msg.data = true;
}
pose_publisher.publish(pose);
stat_publisher.publish(gnss_stat_msg);
}
/* } else {
std::cout << "not calibrated!!" << std::endl;
}*/
}