int make_file(int fd) { int ret, count, i, fd1, offset; unsigned char b[256*4]; count = 0; offset = part_offset("FILE"); if (offset <= 0) return -1; printf("making FILE...\n"); printf("offset %o\n", offset); while (1) { memset(b, 0, sizeof(b)); write_block(fd, offset+count, b); count++; if (ret < 256*4) break; } printf("%d blocks\n", count); return 0; }
int make_lod2(int fd) { int ret, count, i, fd1, offset; unsigned char b[256*4]; printf("making LOD2...\n"); fd1 = open(lod2_filename, O_RDONLY); if (fd1 < 0) { perror(lod2_filename); return -1; } count = 0; offset = part_offset("LOD2"); printf("offset %o\n", offset); while (1) { ret = read(fd1, b, 256*4); if (ret <= 0) break; /* LOD2 start XXX */ write_block(fd, offset/*041674*/+count, b); count++; if (ret < 256*4) break; } printf("%d blocks\n", count); return 0; }
tf::Transform process_measured_points(tf::Vector3 point1, tf::Vector3 point2, tf::Vector3 point3) { tf::Transform pose; //0.0546165, 0.030832, 0.0565557 double part_x_offset, part_y_offset, part_z_offset; part_x_offset = 0.0546165; part_y_offset = 0.030832; part_z_offset = 0.0565557; tf::Vector3 part_offset(part_x_offset, part_y_offset, part_z_offset); /*part_x_offset = point1.x() - part_center.x(); part_y_offset = point1.y() - part_center.y(); part_z_offset = point1.z() - part_center.z();*/ ROS_INFO_STREAM("Part offsets x, y, z: "<<part_x_offset<<", "<<part_y_offset<<", "<<part_z_offset); tf::Vector3 x_axis, y_axis, z_axis; x_axis = point2 - point3; //x_axis = point3-point2; ROS_INFO_STREAM("X vector for part: "<< x_axis.x() <<", "<<x_axis.y()<<", "<< x_axis.z()); y_axis = point1 - point2; //y_axis=point1-point2; z_axis = tf::tfCross(x_axis, y_axis); x_axis.normalize(); ROS_INFO_STREAM("Normalized X vector for part: "<< x_axis.x() <<", "<<x_axis.y()<<", "<< x_axis.z()); y_axis.normalize(); z_axis.normalize(); tf::Matrix3x3 part_o; part_o.setValue(x_axis.getX(), y_axis.getX(), z_axis.getX(), x_axis.getY(), y_axis.getY(), z_axis.getY(), x_axis.getZ(), y_axis.getZ(), z_axis.getZ()); double yaw, pitch, roll; part_o.getEulerYPR(yaw, pitch, roll); tf::Quaternion part_q; part_q.setRPY(roll, pitch, yaw); //tf::Transform part_pose; pose.setRotation(part_q); pose.setOrigin(tf::Vector3(0, 0, 0)); tf::Vector3 offset_inpart = pose * part_offset; //double part_o_x, part_o_y, part_o_z; tf::Vector3 part_or; part_or = point1 - offset_inpart; //part_o_x = point1.x() + part_x_offset; //part_o_y = point1.y() + part_y_offset; //part_o_z = point1.z() + part_z_offset; tf::Vector3 origin(part_or.x(), part_or.y(), part_or.z()); ROS_INFO_STREAM("Part origin " << origin.x()<<", "<< origin.y()<<", "<< origin.z()); pose.setOrigin(origin); return pose; }
int make_mcr1(int fd) { int ret, count, i, fd1, offset; unsigned char b[256*4]; printf("making MCR1...\n"); fd1 = open(mcr_filename, O_RDONLY); if (fd1 < 0) { perror(mcr_filename); return -1; } count = 0; offset = part_offset("MCR1"); printf("offset %o\n", offset); while (1) { ret = read(fd1, b, 256*4); if (ret <= 0) break; #ifndef NEED_SWAP swapbytes((unsigned int *)b); #endif /* MCR1 start XXX */ write_block(fd, offset/*021*/+count, b); count++; if (ret < 256*4) break; } printf("%d blocks\n", count); return 0; }
int zero_part(int fd, char *name) { int ret, count, i, fd1, offset, size; unsigned char b[256*4]; offset = part_offset(name); size = part_size(name); if (offset <= 0) return -1; printf("making %s...\n", name); printf("offset %o\n", offset); for (count = 0; count < size; count++) { memset(b, 0, sizeof(b)); write_block(fd, offset+count, b); } printf("%d blocks\n", count); return 0; }