| 849 | |
| 850 | template <typename PointT> |
| 851 | inline geometry_msgs::PolygonStamped PatchWork<PointT>::set_polygons(int ring_idx, |
| 852 | int sector_idx, |
| 853 | int num_split) { |
| 854 | static const auto &boundary_ranges = zone_model_.boundary_ranges_; |
| 855 | int num_sectors = zone_model_.num_sectors_per_ring_[ring_idx]; |
| 856 | geometry_msgs::PolygonStamped polygons; |
| 857 | polygons.header.frame_id = frame_patchwork; |
| 858 | // Set point of polygon. Start from RL and ccw |
| 859 | geometry_msgs::Point32 point; |
| 860 | double sector_size = 2.0 * M_PI / static_cast<double>(num_sectors); |
| 861 | double angle_incremental = sector_size / static_cast<double>(num_split); |
| 862 | // RL |
| 863 | double r_len = boundary_ranges[ring_idx]; |
| 864 | double angle = sector_idx * sector_size; |
| 865 | |
| 866 | point.x = r_len * cos(angle); |
| 867 | point.y = r_len * sin(angle); |
| 868 | point.z = MARKER_Z_VALUE; |
| 869 | polygons.polygon.points.push_back(point); |
| 870 | // RU |
| 871 | r_len = boundary_ranges[ring_idx + 1]; |
| 872 | point.x = r_len * cos(angle); |
| 873 | point.y = r_len * sin(angle); |
| 874 | point.z = MARKER_Z_VALUE; |
| 875 | polygons.polygon.points.push_back(point); |
| 876 | |
| 877 | // RU -> LU |
| 878 | for (int idx = 1; idx <= num_split; ++idx) { |
| 879 | angle = angle + angle_incremental; |
| 880 | point.x = r_len * cos(angle); |
| 881 | point.y = r_len * sin(angle); |
| 882 | point.z = MARKER_Z_VALUE; |
| 883 | polygons.polygon.points.push_back(point); |
| 884 | } |
| 885 | |
| 886 | r_len = boundary_ranges[ring_idx]; |
| 887 | point.x = r_len * cos(angle); |
| 888 | point.y = r_len * sin(angle); |
| 889 | point.z = MARKER_Z_VALUE; |
| 890 | polygons.polygon.points.push_back(point); |
| 891 | |
| 892 | for (int idx = 1; idx < num_split; ++idx) { |
| 893 | angle = angle - angle_incremental; |
| 894 | point.x = r_len * cos(angle); |
| 895 | point.y = r_len * sin(angle); |
| 896 | point.z = MARKER_Z_VALUE; |
| 897 | polygons.polygon.points.push_back(point); |
| 898 | } |
| 899 | return polygons; |
| 900 | } |
| 901 | |
| 902 | template <typename PointT> |
| 903 | inline int PatchWork<PointT>::determine_ground_likelihood_estimation_status( |
nothing calls this directly
no outgoing calls
no test coverage detected