| 625 | |
| 626 | template<typename PointT> |
| 627 | inline |
| 628 | geometry_msgs::PolygonStamped PatchWork<PointT>::set_polygons(int r_idx, int theta_idx, int num_split) { |
| 629 | geometry_msgs::PolygonStamped polygons; |
| 630 | // Set point of polygon. Start from RL and ccw |
| 631 | geometry_msgs::Point32 point; |
| 632 | |
| 633 | // RL |
| 634 | double r_len = r_idx * ring_size + min_range_; |
| 635 | double angle = theta_idx * sector_size; |
| 636 | |
| 637 | point.x = r_len * cos(angle); |
| 638 | point.y = r_len * sin(angle); |
| 639 | point.z = MARKER_Z_VALUE; |
| 640 | polygons.polygon.points.push_back(point); |
| 641 | // RU |
| 642 | r_len = r_len + ring_size; |
| 643 | point.x = r_len * cos(angle); |
| 644 | point.y = r_len * sin(angle); |
| 645 | point.z = MARKER_Z_VALUE; |
| 646 | polygons.polygon.points.push_back(point); |
| 647 | |
| 648 | // RU -> LU |
| 649 | for (int idx = 1; idx <= num_split; ++idx) { |
| 650 | angle = angle + sector_size / num_split; |
| 651 | point.x = r_len * cos(angle); |
| 652 | point.y = r_len * sin(angle); |
| 653 | point.z = MARKER_Z_VALUE; |
| 654 | polygons.polygon.points.push_back(point); |
| 655 | } |
| 656 | |
| 657 | r_len = r_len - ring_size; |
| 658 | point.x = r_len * cos(angle); |
| 659 | point.y = r_len * sin(angle); |
| 660 | point.z = MARKER_Z_VALUE; |
| 661 | polygons.polygon.points.push_back(point); |
| 662 | |
| 663 | for (int idx = 1; idx < num_split; ++idx) { |
| 664 | angle = angle - sector_size / num_split; |
| 665 | point.x = r_len * cos(angle); |
| 666 | point.y = r_len * sin(angle); |
| 667 | point.z = MARKER_Z_VALUE; |
| 668 | polygons.polygon.points.push_back(point); |
| 669 | } |
| 670 | |
| 671 | return polygons; |
| 672 | } |
| 673 | |
| 674 | template<typename PointT> |
| 675 | inline |