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