Skip to content

Commit b670ff4

Browse files
technolojinHansRobo
technolojin
authored and
HansRobo
committed
feat(autoware_auto_perception_rviz_plugin): indicate object orientation, parameter adjustments, bugfix (#6469)
* fix(autoware_auto_perception_rviz_plugin): align marker lifetime Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * feat(autoware_auto_perception_rviz_plugin): bounding box orientation indication Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix(autoware_auto_perception_rviz_plugin): adjust twist covariance color Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix(autoware_auto_perception_rviz_plugin): adjust bbox orientation indicator Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix(autoware_auto_perception_rviz_plugin): yaw rate marker in predicted object Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
1 parent 04e8c5f commit b670ff4

File tree

3 files changed

+110
-48
lines changed

3 files changed

+110
-48
lines changed

‎common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp

+8
Original file line numberDiff line numberDiff line change
@@ -186,6 +186,10 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_lin
186186
const autoware_auto_perception_msgs::msg::Shape & shape,
187187
std::vector<geometry_msgs::msg::Point> & points);
188188

189+
AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_orientation_line_list(
190+
const autoware_auto_perception_msgs::msg::Shape & shape,
191+
std::vector<geometry_msgs::msg::Point> & points);
192+
189193
AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list(
190194
const autoware_auto_perception_msgs::msg::Shape & shape,
191195
std::vector<geometry_msgs::msg::Point> & points);
@@ -194,6 +198,10 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_dir
194198
const autoware_auto_perception_msgs::msg::Shape & shape,
195199
std::vector<geometry_msgs::msg::Point> & points);
196200

201+
AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_orientation_line_list(
202+
const autoware_auto_perception_msgs::msg::Shape & shape,
203+
std::vector<geometry_msgs::msg::Point> & points);
204+
197205
AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list(
198206
const autoware_auto_perception_msgs::msg::Shape & shape,
199207
std::vector<geometry_msgs::msg::Point> & points);

‎common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp

+101-47
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr(
5757
marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
5858
marker_ptr->ns = std::string("path confidence");
5959
marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
60-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
60+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
6161
marker_ptr->scale.x = 0.5;
6262
marker_ptr->scale.y = 0.5;
6363
marker_ptr->scale.z = 0.5;
@@ -78,14 +78,14 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr(
7878
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
7979
marker_ptr->ns = std::string("path");
8080
marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
81-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
81+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
8282
marker_ptr->pose = initPose();
8383
marker_ptr->color = predicted_path_color;
8484
marker_ptr->color.a = 0.6;
8585
marker_ptr->scale.x = 0.015;
8686
calc_path_line_list(predicted_path, marker_ptr->points, is_simple);
8787
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;
8989
}
9090
return marker_ptr;
9191
}
@@ -112,7 +112,7 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr(
112112
point.z = twist_with_covariance.twist.linear.z;
113113
marker_ptr->points.push_back(point);
114114

115-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
115+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
116116
marker_ptr->color.a = 0.999;
117117
marker_ptr->color.r = 1.0;
118118
marker_ptr->color.g = 0.0;
@@ -160,19 +160,19 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr(
160160
// ellipse orientation
161161
marker_ptr->pose.orientation.x = 0.0;
162162
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);
165165

166166
// ellipse size
167167
marker_ptr->scale.x = sigma1 * confidence_interval_coefficient;
168168
marker_ptr->scale.y = sigma2 * confidence_interval_coefficient;
169169
marker_ptr->scale.z = 0.05;
170170

171-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
171+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
172172
marker_ptr->color.a = alpha;
173-
marker_ptr->color.r = 1.0;
174-
marker_ptr->color.g = 0.2;
175-
marker_ptr->color.b = 0.4;
173+
marker_ptr->color.r = 0.2;
174+
marker_ptr->color.g = 0.4;
175+
marker_ptr->color.b = 0.9;
176176

177177
return marker_ptr;
178178
}
@@ -213,7 +213,7 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr(
213213
point.z = 0;
214214
marker_ptr->points.push_back(point);
215215

216-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
216+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
217217
marker_ptr->color.a = 0.9;
218218
marker_ptr->color.r = 1.0;
219219
marker_ptr->color.g = 0.0;
@@ -274,7 +274,7 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr(
274274
};
275275
calc_line_list_from_points(point_list, point_pairs, 4, marker_ptr->points);
276276

277-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
277+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
278278
marker_ptr->color.a = 0.9;
279279
marker_ptr->color.r = 1.0;
280280
marker_ptr->color.g = 0.2;
@@ -299,7 +299,7 @@ visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr(
299299
marker_ptr->text = std::to_string(static_cast<int>(vel * 3.6)) + std::string("[km/h]");
300300
marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
301301
marker_ptr->pose.position = vis_pos;
302-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
302+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
303303
marker_ptr->color = color_rgba;
304304
return marker_ptr;
305305
}
@@ -320,7 +320,7 @@ visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr(
320320
marker_ptr->text = getRoundedDoubleString(acc) + std::string("[m/s^2]");
321321
marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
322322
marker_ptr->pose.position = vis_pos;
323-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
323+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
324324
marker_ptr->color = color_rgba;
325325
return marker_ptr;
326326
}
@@ -378,8 +378,8 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr(
378378
// ellipse orientation
379379
marker_ptr->pose.orientation.x = 0.0;
380380
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);
383383

384384
// ellipse size
385385
marker_ptr->scale.x = sigma1 * confidence_interval_coefficient;
@@ -392,11 +392,11 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr(
392392
alpha = std::max(0.1, alpha);
393393

394394
// marker configuration
395-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
395+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
396396
marker_ptr->color.a = alpha;
397-
marker_ptr->color.r = 1.0;
398-
marker_ptr->color.g = 1.0;
399-
marker_ptr->color.b = 1.0;
397+
marker_ptr->color.r = 0.8;
398+
marker_ptr->color.g = 0.8;
399+
marker_ptr->color.b = 0.8;
400400
return marker_ptr;
401401
}
402402

@@ -434,7 +434,7 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr(
434434
marker_ptr->points.push_back(point);
435435

436436
// marker configuration
437-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
437+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
438438
marker_ptr->color.a = 0.9;
439439
marker_ptr->color.r = 1.0;
440440
marker_ptr->color.g = 1.0;
@@ -469,7 +469,7 @@ visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr(
469469
marker_ptr->text = label;
470470
marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
471471
marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation);
472-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
472+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
473473
marker_ptr->color = color_rgba;
474474
return marker_ptr;
475475
}
@@ -486,7 +486,7 @@ visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr(
486486
marker_ptr->text = std::to_string(existence_probability);
487487
marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
488488
marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation);
489-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
489+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
490490
marker_ptr->color = color_rgba;
491491
return marker_ptr;
492492
}
@@ -506,6 +506,8 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
506506
calc_bounding_box_line_list(shape_msg, marker_ptr->points);
507507
if (is_orientation_available) {
508508
calc_bounding_box_direction_line_list(shape_msg, marker_ptr->points);
509+
} else {
510+
calc_bounding_box_orientation_line_list(shape_msg, marker_ptr->points);
509511
}
510512
} else if (shape_msg.type == Shape::CYLINDER) {
511513
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
@@ -520,7 +522,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
520522

521523
marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
522524
marker_ptr->pose = to_pose(centroid, orientation);
523-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
525+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
524526
marker_ptr->scale.x = line_width;
525527
marker_ptr->color = color_rgba;
526528

@@ -542,6 +544,8 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr(
542544
calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points);
543545
if (is_orientation_available) {
544546
calc_2d_bounding_box_bottom_direction_line_list(shape_msg, marker_ptr->points);
547+
} else {
548+
calc_2d_bounding_box_bottom_orientation_line_list(shape_msg, marker_ptr->points);
545549
}
546550
} else if (shape_msg.type == Shape::CYLINDER) {
547551
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
@@ -556,7 +560,7 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr(
556560

557561
marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
558562
marker_ptr->pose = to_pose(centroid, orientation);
559-
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5);
563+
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15);
560564
marker_ptr->scale.x = line_width;
561565
marker_ptr->color = color_rgba;
562566

@@ -584,9 +588,9 @@ void calc_bounding_box_line_list(
584588
const autoware_auto_perception_msgs::msg::Shape & shape,
585589
std::vector<geometry_msgs::msg::Point> & points)
586590
{
587-
const double length_half = shape.dimensions.x / 2.0;
588-
const double width_half = shape.dimensions.y / 2.0;
589-
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;
590594
geometry_msgs::msg::Point point;
591595

592596
// bounding box corner points
@@ -608,10 +612,10 @@ void calc_bounding_box_direction_line_list(
608612
std::vector<geometry_msgs::msg::Point> & points)
609613
{
610614
// direction triangle
611-
const double length_half = shape.dimensions.x / 2.0;
612-
const double width_half = shape.dimensions.y / 2.0;
613-
const double height_half = shape.dimensions.z / 2.0;
614-
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);
615619
geometry_msgs::msg::Point point;
616620

617621
// triangle-shaped direction indicator
@@ -629,13 +633,38 @@ void calc_bounding_box_direction_line_list(
629633
calc_line_list_from_points(point_list, point_pairs, 5, points);
630634
}
631635

636+
void calc_bounding_box_orientation_line_list(
637+
const autoware_auto_perception_msgs::msg::Shape & shape,
638+
std::vector<geometry_msgs::msg::Point> & points)
639+
{
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+
geometry_msgs::msg::Point point;
646+
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},
653+
};
654+
const int point_pairs[2][2] = {
655+
{0, 1},
656+
{2, 3},
657+
};
658+
calc_line_list_from_points(point_list, point_pairs, 2, points);
659+
}
660+
632661
void calc_2d_bounding_box_bottom_line_list(
633662
const autoware_auto_perception_msgs::msg::Shape & shape,
634663
std::vector<geometry_msgs::msg::Point> & points)
635664
{
636-
const double length_half = shape.dimensions.x / 2.0;
637-
const double width_half = shape.dimensions.y / 2.0;
638-
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;
639668
geometry_msgs::msg::Point point;
640669

641670
// bounding box corner points
@@ -659,10 +688,10 @@ void calc_2d_bounding_box_bottom_direction_line_list(
659688
const autoware_auto_perception_msgs::msg::Shape & shape,
660689
std::vector<geometry_msgs::msg::Point> & points)
661690
{
662-
const double length_half = shape.dimensions.x / 2.0;
663-
const double width_half = shape.dimensions.y / 2.0;
664-
const double height_half = shape.dimensions.z / 2.0;
665-
const double triangle_size_half = shape.dimensions.y / 1.4;
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;
694+
const double triangle_size_half = std::min(width_half * 1.4, shape.dimensions.x);
666695
geometry_msgs::msg::Point point;
667696

668697
// triangle-shaped direction indicator
@@ -679,6 +708,31 @@ void calc_2d_bounding_box_bottom_direction_line_list(
679708
calc_line_list_from_points(point_list, point_pairs, 3, points);
680709
}
681710

711+
void calc_2d_bounding_box_bottom_orientation_line_list(
712+
const autoware_auto_perception_msgs::msg::Shape & shape,
713+
std::vector<geometry_msgs::msg::Point> & points)
714+
{
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);
720+
geometry_msgs::msg::Point point;
721+
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},
728+
};
729+
const int point_pairs[2][2] = {
730+
{0, 1},
731+
{2, 3},
732+
};
733+
calc_line_list_from_points(point_list, point_pairs, 2, points);
734+
}
735+
682736
void calc_cylinder_line_list(
683737
const autoware_auto_perception_msgs::msg::Shape & shape,
684738
std::vector<geometry_msgs::msg::Point> & points)
@@ -784,41 +838,41 @@ void calc_polygon_line_list(
784838
geometry_msgs::msg::Point point;
785839
point.x = shape.footprint.points.at(i).x;
786840
point.y = shape.footprint.points.at(i).y;
787-
point.z = shape.dimensions.z / 2.0;
841+
point.z = shape.dimensions.z * 0.5;
788842
points.push_back(point);
789843
point.x = shape.footprint.points
790844
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
791845
.x;
792846
point.y = shape.footprint.points
793847
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
794848
.y;
795-
point.z = shape.dimensions.z / 2.0;
849+
point.z = shape.dimensions.z * 0.5;
796850
points.push_back(point);
797851
}
798852
for (size_t i = 0; i < shape.footprint.points.size(); ++i) {
799853
geometry_msgs::msg::Point point;
800854
point.x = shape.footprint.points.at(i).x;
801855
point.y = shape.footprint.points.at(i).y;
802-
point.z = -shape.dimensions.z / 2.0;
856+
point.z = -shape.dimensions.z * 0.5;
803857
points.push_back(point);
804858
point.x = shape.footprint.points
805859
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
806860
.x;
807861
point.y = shape.footprint.points
808862
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
809863
.y;
810-
point.z = -shape.dimensions.z / 2.0;
864+
point.z = -shape.dimensions.z * 0.5;
811865
points.push_back(point);
812866
}
813867
for (size_t i = 0; i < shape.footprint.points.size(); ++i) {
814868
geometry_msgs::msg::Point point;
815869
point.x = shape.footprint.points.at(i).x;
816870
point.y = shape.footprint.points.at(i).y;
817-
point.z = shape.dimensions.z / 2.0;
871+
point.z = shape.dimensions.z * 0.5;
818872
points.push_back(point);
819873
point.x = shape.footprint.points.at(i).x;
820874
point.y = shape.footprint.points.at(i).y;
821-
point.z = -shape.dimensions.z / 2.0;
875+
point.z = -shape.dimensions.z * 0.5;
822876
points.push_back(point);
823877
}
824878
}
@@ -837,15 +891,15 @@ void calc_2d_polygon_bottom_line_list(
837891
geometry_msgs::msg::Point point;
838892
point.x = shape.footprint.points.at(i).x;
839893
point.y = shape.footprint.points.at(i).y;
840-
point.z = -shape.dimensions.z / 2.0;
894+
point.z = -shape.dimensions.z * 0.5;
841895
points.push_back(point);
842896
point.x = shape.footprint.points
843897
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
844898
.x;
845899
point.y = shape.footprint.points
846900
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
847901
.y;
848-
point.z = -shape.dimensions.z / 2.0;
902+
point.z = -shape.dimensions.z * 0.5;
849903
points.push_back(point);
850904
}
851905
}

0 commit comments

Comments
 (0)