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