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 int getSensorValue(int sock, ros::Publisher pub[TOPIC_NR])
{
int info[2];
if(recv(sock, info, sizeof(info), 0) == -1) {
perror("recv");
return -1;
}
fprintf(stderr, "info=%d value=%d\n", info[0], info[1]);
switch(info[0]) {
case 1: { // GEAR
tablet_socket::gear_cmd msg;
msg.gear = info[1];
pub[0].publish(msg);
break;
}
case 2: { // MODE
tablet_socket::mode_cmd msg;
msg.mode = info[1];
pub[1].publish(msg);
break;
}
case 3: { // ROUTE
tablet_socket::route_cmd msg;
tablet_socket::Waypoint point;
size_t size = info[1];
double *points;
int points_nr;
ssize_t nbytes;
if (!size)
break;
points = (double *)malloc(size);
if (points == NULL) {
perror("malloc");
return -1;
}
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;
}
}
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;
}
default: // TERMINATOR
fprintf(stderr, "receive %d, terminated.\n", info[0]);
sendSignal(sock);
return -1;
}
return 0;
}
