@@ -85,7 +85,7 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr(
85
85
marker_ptr->scale .x = 0.015 ;
86
86
calc_path_line_list (predicted_path, marker_ptr->points , is_simple);
87
87
for (size_t k = 0 ; k < marker_ptr->points .size (); ++k) {
88
- marker_ptr->points .at (k).z -= shape.dimensions .z / 2.0 ;
88
+ marker_ptr->points .at (k).z -= shape.dimensions .z * 0.5 ;
89
89
}
90
90
return marker_ptr;
91
91
}
@@ -160,8 +160,8 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr(
160
160
// ellipse orientation
161
161
marker_ptr->pose .orientation .x = 0.0 ;
162
162
marker_ptr->pose .orientation .y = 0.0 ;
163
- marker_ptr->pose .orientation .z = std::sin (phi / 2.0 );
164
- marker_ptr->pose .orientation .w = std::cos (phi / 2.0 );
163
+ marker_ptr->pose .orientation .z = std::sin (phi * 0.5 );
164
+ marker_ptr->pose .orientation .w = std::cos (phi * 0.5 );
165
165
166
166
// ellipse size
167
167
marker_ptr->scale .x = sigma1 * confidence_interval_coefficient;
@@ -378,8 +378,8 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr(
378
378
// ellipse orientation
379
379
marker_ptr->pose .orientation .x = 0.0 ;
380
380
marker_ptr->pose .orientation .y = 0.0 ;
381
- marker_ptr->pose .orientation .z = std::sin (yaw / 2.0 );
382
- marker_ptr->pose .orientation .w = std::cos (yaw / 2.0 );
381
+ marker_ptr->pose .orientation .z = std::sin (yaw * 0.5 );
382
+ marker_ptr->pose .orientation .w = std::cos (yaw * 0.5 );
383
383
384
384
// ellipse size
385
385
marker_ptr->scale .x = sigma1 * confidence_interval_coefficient;
@@ -588,9 +588,9 @@ void calc_bounding_box_line_list(
588
588
const autoware_auto_perception_msgs::msg::Shape & shape,
589
589
std::vector<geometry_msgs::msg::Point > & points)
590
590
{
591
- const double length_half = shape.dimensions .x / 2.0 ;
592
- const double width_half = shape.dimensions .y / 2.0 ;
593
- const double height_half = shape.dimensions .z / 2.0 ;
591
+ const double length_half = shape.dimensions .x * 0.5 ;
592
+ const double width_half = shape.dimensions .y * 0.5 ;
593
+ const double height_half = shape.dimensions .z * 0.5 ;
594
594
geometry_msgs::msg::Point point;
595
595
596
596
// bounding box corner points
@@ -612,10 +612,10 @@ void calc_bounding_box_direction_line_list(
612
612
std::vector<geometry_msgs::msg::Point > & points)
613
613
{
614
614
// direction triangle
615
- const double length_half = shape.dimensions .x / 2.0 ;
616
- const double width_half = shape.dimensions .y / 2.0 ;
617
- const double height_half = shape.dimensions .z / 2.0 ;
618
- const double triangle_size_half = shape. dimensions . y / 1.4 ;
615
+ const double length_half = shape.dimensions .x * 0.5 ;
616
+ const double width_half = shape.dimensions .y * 0.5 ;
617
+ const double height_half = shape.dimensions .z * 0.5 ;
618
+ const double triangle_size_half = std::min (width_half * 1.4 , shape. dimensions . x ) ;
619
619
geometry_msgs::msg::Point point;
620
620
621
621
// triangle-shaped direction indicator
@@ -637,22 +637,23 @@ void calc_bounding_box_orientation_line_list(
637
637
const autoware_auto_perception_msgs::msg::Shape & shape,
638
638
std::vector<geometry_msgs::msg::Point > & points)
639
639
{
640
- const double length_half = shape.dimensions .x / 2.0 ;
641
- const double width_half = shape.dimensions .y / 2.0 ;
642
- const double height_half = shape.dimensions .z / 2.0 ;
643
- const double tick_width = width_half * 0.4 ;
644
- const double tick_length = std::min (tick_width * 1.4 , length_half);
640
+ const double length_half = shape.dimensions .x * 0.5 ;
641
+ const double width_half = shape.dimensions .y * 0.5 ;
642
+ const double height_half = shape.dimensions .z * 0.5 ;
643
+ const double tick_width = width_half * 0.5 ;
644
+ const double tick_length = std::min (tick_width, length_half);
645
645
geometry_msgs::msg::Point point;
646
646
647
- // triangle-shaped direction indicator
648
- const double point_list[3 ][3 ] = {
649
- {length_half, 0 , height_half},
650
- {length_half - tick_length, tick_width, height_half},
651
- {length_half - tick_length, -tick_width, height_half},
647
+ // front corner cuts for orientation
648
+ const double point_list[4 ][3 ] = {
649
+ {length_half, width_half - tick_width, height_half},
650
+ {length_half - tick_length, width_half, height_half},
651
+ {length_half, -width_half + tick_width, height_half},
652
+ {length_half - tick_length, -width_half, height_half},
652
653
};
653
654
const int point_pairs[2 ][2 ] = {
654
655
{0 , 1 },
655
- {0 , 2 },
656
+ {2 , 3 },
656
657
};
657
658
calc_line_list_from_points (point_list, point_pairs, 2 , points);
658
659
}
@@ -661,9 +662,9 @@ void calc_2d_bounding_box_bottom_line_list(
661
662
const autoware_auto_perception_msgs::msg::Shape & shape,
662
663
std::vector<geometry_msgs::msg::Point > & points)
663
664
{
664
- const double length_half = shape.dimensions .x / 2.0 ;
665
- const double width_half = shape.dimensions .y / 2.0 ;
666
- const double height_half = shape.dimensions .z / 2.0 ;
665
+ const double length_half = shape.dimensions .x * 0.5 ;
666
+ const double width_half = shape.dimensions .y * 0.5 ;
667
+ const double height_half = shape.dimensions .z * 0.5 ;
667
668
geometry_msgs::msg::Point point;
668
669
669
670
// bounding box corner points
@@ -687,9 +688,9 @@ void calc_2d_bounding_box_bottom_direction_line_list(
687
688
const autoware_auto_perception_msgs::msg::Shape & shape,
688
689
std::vector<geometry_msgs::msg::Point > & points)
689
690
{
690
- const double length_half = shape.dimensions .x / 2.0 ;
691
- const double width_half = shape.dimensions .y / 2.0 ;
692
- const double height_half = shape.dimensions .z / 2.0 ;
691
+ const double length_half = shape.dimensions .x * 0.5 ;
692
+ const double width_half = shape.dimensions .y * 0.5 ;
693
+ const double height_half = shape.dimensions .z * 0.5 ;
693
694
const double triangle_size_half = std::min (width_half * 1.4 , shape.dimensions .x );
694
695
geometry_msgs::msg::Point point;
695
696
@@ -711,22 +712,23 @@ void calc_2d_bounding_box_bottom_orientation_line_list(
711
712
const autoware_auto_perception_msgs::msg::Shape & shape,
712
713
std::vector<geometry_msgs::msg::Point > & points)
713
714
{
714
- const double length_half = shape.dimensions .x / 2.0 ;
715
- const double width_half = shape.dimensions .y / 2.0 ;
716
- const double height_half = shape.dimensions .z / 2.0 ;
717
- const double tick_width = width_half * 0.4 ;
718
- const double tick_length = std::min (tick_width * 1.4 , length_half);
715
+ const double length_half = shape.dimensions .x * 0.5 ;
716
+ const double width_half = shape.dimensions .y * 0.5 ;
717
+ const double height_half = shape.dimensions .z * 0.5 ;
718
+ const double tick_width = width_half * 0.5 ;
719
+ const double tick_length = std::min (tick_width, length_half);
719
720
geometry_msgs::msg::Point point;
720
721
721
- // triangle-shaped direction indicator
722
- const double point_list[3 ][3 ] = {
723
- {length_half, 0 , -height_half},
724
- {length_half - tick_length, tick_width, -height_half},
725
- {length_half - tick_length, -tick_width, -height_half},
722
+ // front corner cuts for orientation
723
+ const double point_list[4 ][3 ] = {
724
+ {length_half, width_half - tick_width, height_half},
725
+ {length_half - tick_length, width_half, height_half},
726
+ {length_half, -width_half + tick_width, height_half},
727
+ {length_half - tick_length, -width_half, height_half},
726
728
};
727
729
const int point_pairs[2 ][2 ] = {
728
730
{0 , 1 },
729
- {0 , 2 },
731
+ {2 , 3 },
730
732
};
731
733
calc_line_list_from_points (point_list, point_pairs, 2 , points);
732
734
}
@@ -836,41 +838,41 @@ void calc_polygon_line_list(
836
838
geometry_msgs::msg::Point point;
837
839
point.x = shape.footprint .points .at (i).x ;
838
840
point.y = shape.footprint .points .at (i).y ;
839
- point.z = shape.dimensions .z / 2.0 ;
841
+ point.z = shape.dimensions .z * 0.5 ;
840
842
points.push_back (point);
841
843
point.x = shape.footprint .points
842
844
.at (static_cast <int >(i + 1 ) % static_cast <int >(shape.footprint .points .size ()))
843
845
.x ;
844
846
point.y = shape.footprint .points
845
847
.at (static_cast <int >(i + 1 ) % static_cast <int >(shape.footprint .points .size ()))
846
848
.y ;
847
- point.z = shape.dimensions .z / 2.0 ;
849
+ point.z = shape.dimensions .z * 0.5 ;
848
850
points.push_back (point);
849
851
}
850
852
for (size_t i = 0 ; i < shape.footprint .points .size (); ++i) {
851
853
geometry_msgs::msg::Point point;
852
854
point.x = shape.footprint .points .at (i).x ;
853
855
point.y = shape.footprint .points .at (i).y ;
854
- point.z = -shape.dimensions .z / 2.0 ;
856
+ point.z = -shape.dimensions .z * 0.5 ;
855
857
points.push_back (point);
856
858
point.x = shape.footprint .points
857
859
.at (static_cast <int >(i + 1 ) % static_cast <int >(shape.footprint .points .size ()))
858
860
.x ;
859
861
point.y = shape.footprint .points
860
862
.at (static_cast <int >(i + 1 ) % static_cast <int >(shape.footprint .points .size ()))
861
863
.y ;
862
- point.z = -shape.dimensions .z / 2.0 ;
864
+ point.z = -shape.dimensions .z * 0.5 ;
863
865
points.push_back (point);
864
866
}
865
867
for (size_t i = 0 ; i < shape.footprint .points .size (); ++i) {
866
868
geometry_msgs::msg::Point point;
867
869
point.x = shape.footprint .points .at (i).x ;
868
870
point.y = shape.footprint .points .at (i).y ;
869
- point.z = shape.dimensions .z / 2.0 ;
871
+ point.z = shape.dimensions .z * 0.5 ;
870
872
points.push_back (point);
871
873
point.x = shape.footprint .points .at (i).x ;
872
874
point.y = shape.footprint .points .at (i).y ;
873
- point.z = -shape.dimensions .z / 2.0 ;
875
+ point.z = -shape.dimensions .z * 0.5 ;
874
876
points.push_back (point);
875
877
}
876
878
}
@@ -889,15 +891,15 @@ void calc_2d_polygon_bottom_line_list(
889
891
geometry_msgs::msg::Point point;
890
892
point.x = shape.footprint .points .at (i).x ;
891
893
point.y = shape.footprint .points .at (i).y ;
892
- point.z = -shape.dimensions .z / 2.0 ;
894
+ point.z = -shape.dimensions .z * 0.5 ;
893
895
points.push_back (point);
894
896
point.x = shape.footprint .points
895
897
.at (static_cast <int >(i + 1 ) % static_cast <int >(shape.footprint .points .size ()))
896
898
.x ;
897
899
point.y = shape.footprint .points
898
900
.at (static_cast <int >(i + 1 ) % static_cast <int >(shape.footprint .points .size ()))
899
901
.y ;
900
- point.z = -shape.dimensions .z / 2.0 ;
902
+ point.z = -shape.dimensions .z * 0.5 ;
901
903
points.push_back (point);
902
904
}
903
905
}
0 commit comments