| 1015 | } |
| 1016 | |
| 1017 | bool RvizVisualTools::publishABCDPlane(const double A, const double B, const double C, |
| 1018 | const double D, Colors color, double x_width, double y_width) |
| 1019 | { |
| 1020 | // The coefficients A,B,C give the normal to the plane. |
| 1021 | Eigen::Vector3d n(A, B, C); |
| 1022 | |
| 1023 | // Graphic is centered at this point |
| 1024 | double distance = D / n.norm(); |
| 1025 | Eigen::Vector3d center = -distance * n.normalized(); |
| 1026 | |
| 1027 | Eigen::Isometry3d pose; |
| 1028 | pose.translation() = center; |
| 1029 | |
| 1030 | // Calculate the rotation matrix from the original normal z_0 = (0,0,1) to new normal n = (A,B,C) |
| 1031 | Eigen::Vector3d z_0 = Eigen::Vector3d::UnitZ(); |
| 1032 | Eigen::Quaterniond q = Eigen::Quaterniond::FromTwoVectors(z_0, n); |
| 1033 | pose.linear() = q.toRotationMatrix(); |
| 1034 | |
| 1035 | double height = 0.001; // very thin |
| 1036 | publishCuboid(pose, x_width, y_width, height, color); |
| 1037 | |
| 1038 | return true; |
| 1039 | } |
| 1040 | |
| 1041 | bool RvizVisualTools::publishNormalAndDistancePlane(const Eigen::Vector3d& normal, const double d, |
| 1042 | const Colors color, const double x_width, |