From 5338c423bc19ed9caf812d5124b5ff201dad4b7c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 20 Feb 2024 19:13:40 +0900 Subject: [PATCH 1/5] fix(autoware_auto_perception_rviz_plugin): align marker lifetime Signed-off-by: Taekjin LEE --- .../object_polygon_detail.cpp | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 11a0bbbe57d53..d7d707e06ebe0 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -57,7 +57,7 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker_ptr->ns = std::string("path confidence"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->scale.x = 0.5; marker_ptr->scale.y = 0.5; marker_ptr->scale.z = 0.5; @@ -78,7 +78,7 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; marker_ptr->ns = std::string("path"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->pose = initPose(); marker_ptr->color = predicted_path_color; marker_ptr->color.a = 0.6; @@ -112,7 +112,7 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( point.z = twist_with_covariance.twist.linear.z; marker_ptr->points.push_back(point); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color.a = 0.999; marker_ptr->color.r = 1.0; marker_ptr->color.g = 0.0; @@ -168,7 +168,7 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( marker_ptr->scale.y = sigma2 * confidence_interval_coefficient; marker_ptr->scale.z = 0.05; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color.a = alpha; marker_ptr->color.r = 1.0; marker_ptr->color.g = 0.2; @@ -213,7 +213,7 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( point.z = 0; marker_ptr->points.push_back(point); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color.a = 0.9; marker_ptr->color.r = 1.0; marker_ptr->color.g = 0.0; @@ -274,7 +274,7 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr( }; calc_line_list_from_points(point_list, point_pairs, 4, marker_ptr->points); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color.a = 0.9; marker_ptr->color.r = 1.0; marker_ptr->color.g = 0.2; @@ -299,7 +299,7 @@ visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( marker_ptr->text = std::to_string(static_cast(vel * 3.6)) + std::string("[km/h]"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose.position = vis_pos; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color = color_rgba; return marker_ptr; } @@ -320,7 +320,7 @@ visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr( marker_ptr->text = getRoundedDoubleString(acc) + std::string("[m/s^2]"); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose.position = vis_pos; - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color = color_rgba; return marker_ptr; } @@ -392,7 +392,7 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( alpha = std::max(0.1, alpha); // marker configuration - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color.a = alpha; marker_ptr->color.r = 1.0; marker_ptr->color.g = 1.0; @@ -434,7 +434,7 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( marker_ptr->points.push_back(point); // marker configuration - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color.a = 0.9; marker_ptr->color.r = 1.0; marker_ptr->color.g = 1.0; @@ -469,7 +469,7 @@ visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr( marker_ptr->text = label; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color = color_rgba; return marker_ptr; } @@ -486,7 +486,7 @@ visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( marker_ptr->text = std::to_string(existence_probability); marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color = color_rgba; return marker_ptr; } @@ -520,7 +520,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = to_pose(centroid, orientation); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->scale.x = line_width; marker_ptr->color = color_rgba; @@ -556,7 +556,7 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = to_pose(centroid, orientation); - marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->scale.x = line_width; marker_ptr->color = color_rgba; From da5646bc4682e514e26298d97a55909d8de7e0aa Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 20 Feb 2024 19:16:33 +0900 Subject: [PATCH 2/5] feat(autoware_auto_perception_rviz_plugin): bounding box orientation indication Signed-off-by: Taekjin LEE --- .../object_polygon_detail.hpp | 8 +++ .../object_polygon_detail.cpp | 54 ++++++++++++++++++- 2 files changed, 61 insertions(+), 1 deletion(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp index 5f9da8531d2ab..8c6ade475217a 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_detail.hpp @@ -186,6 +186,10 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_lin const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_orientation_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); @@ -194,6 +198,10 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_dir const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_orientation_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index d7d707e06ebe0..6949fc66d4a50 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -506,6 +506,8 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( calc_bounding_box_line_list(shape_msg, marker_ptr->points); if (is_orientation_available) { calc_bounding_box_direction_line_list(shape_msg, marker_ptr->points); + } else { + calc_bounding_box_orientation_line_list(shape_msg, marker_ptr->points); } } else if (shape_msg.type == Shape::CYLINDER) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; @@ -542,6 +544,8 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); if (is_orientation_available) { calc_2d_bounding_box_bottom_direction_line_list(shape_msg, marker_ptr->points); + } else { + calc_2d_bounding_box_bottom_orientation_line_list(shape_msg, marker_ptr->points); } } else if (shape_msg.type == Shape::CYLINDER) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; @@ -629,6 +633,30 @@ void calc_bounding_box_direction_line_list( calc_line_list_from_points(point_list, point_pairs, 5, points); } +void calc_bounding_box_orientation_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; + const double tick_width = width_half * 0.4; + const double tick_length = std::min(tick_width * 1.4, length_half); + geometry_msgs::msg::Point point; + + // triangle-shaped direction indicator + const double point_list[3][3] = { + {length_half, 0, height_half}, + {length_half - tick_length, tick_width, height_half}, + {length_half - tick_length, -tick_width, height_half}, + }; + const int point_pairs[2][2] = { + {0, 1}, + {0, 2}, + }; + calc_line_list_from_points(point_list, point_pairs, 2, points); +} + void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) @@ -662,7 +690,7 @@ void calc_2d_bounding_box_bottom_direction_line_list( const double length_half = shape.dimensions.x / 2.0; const double width_half = shape.dimensions.y / 2.0; const double height_half = shape.dimensions.z / 2.0; - const double triangle_size_half = shape.dimensions.y / 1.4; + const double triangle_size_half = std::min(width_half * 1.4, shape.dimensions.x); geometry_msgs::msg::Point point; // triangle-shaped direction indicator @@ -679,6 +707,30 @@ void calc_2d_bounding_box_bottom_direction_line_list( calc_line_list_from_points(point_list, point_pairs, 3, points); } +void calc_2d_bounding_box_bottom_orientation_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double length_half = shape.dimensions.x / 2.0; + const double width_half = shape.dimensions.y / 2.0; + const double height_half = shape.dimensions.z / 2.0; + const double tick_width = width_half * 0.4; + const double tick_length = std::min(tick_width * 1.4, length_half); + geometry_msgs::msg::Point point; + + // triangle-shaped direction indicator + const double point_list[3][3] = { + {length_half, 0, -height_half}, + {length_half - tick_length, tick_width, -height_half}, + {length_half - tick_length, -tick_width, -height_half}, + }; + const int point_pairs[2][2] = { + {0, 1}, + {0, 2}, + }; + calc_line_list_from_points(point_list, point_pairs, 2, points); +} + void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) From 9801aa06e60a87f30dc6395abca56409e2c5a6b0 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 20 Feb 2024 19:16:54 +0900 Subject: [PATCH 3/5] fix(autoware_auto_perception_rviz_plugin): adjust twist covariance color Signed-off-by: Taekjin LEE --- .../src/object_detection/object_polygon_detail.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 6949fc66d4a50..88227a8912c0a 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -170,9 +170,9 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color.a = alpha; - marker_ptr->color.r = 1.0; - marker_ptr->color.g = 0.2; - marker_ptr->color.b = 0.4; + marker_ptr->color.r = 0.2; + marker_ptr->color.g = 0.4; + marker_ptr->color.b = 0.9; return marker_ptr; } @@ -394,9 +394,9 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( // marker configuration marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); marker_ptr->color.a = alpha; - marker_ptr->color.r = 1.0; - marker_ptr->color.g = 1.0; - marker_ptr->color.b = 1.0; + marker_ptr->color.r = 0.8; + marker_ptr->color.g = 0.8; + marker_ptr->color.b = 0.8; return marker_ptr; } From a3f10c69ae8557c1a1de1d3d312d10cc787eea7b Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 21 Feb 2024 15:31:14 +0900 Subject: [PATCH 4/5] fix(autoware_auto_perception_rviz_plugin): adjust bbox orientation indicator Signed-off-by: Taekjin LEE --- .../object_polygon_detail.cpp | 98 ++++++++++--------- 1 file changed, 50 insertions(+), 48 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 88227a8912c0a..8be9d1c01332e 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -85,7 +85,7 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( marker_ptr->scale.x = 0.015; calc_path_line_list(predicted_path, marker_ptr->points, is_simple); for (size_t k = 0; k < marker_ptr->points.size(); ++k) { - marker_ptr->points.at(k).z -= shape.dimensions.z / 2.0; + marker_ptr->points.at(k).z -= shape.dimensions.z * 0.5; } return marker_ptr; } @@ -160,8 +160,8 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( // ellipse orientation marker_ptr->pose.orientation.x = 0.0; marker_ptr->pose.orientation.y = 0.0; - marker_ptr->pose.orientation.z = std::sin(phi / 2.0); - marker_ptr->pose.orientation.w = std::cos(phi / 2.0); + marker_ptr->pose.orientation.z = std::sin(phi * 0.5); + marker_ptr->pose.orientation.w = std::cos(phi * 0.5); // ellipse size marker_ptr->scale.x = sigma1 * confidence_interval_coefficient; @@ -378,8 +378,8 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( // ellipse orientation marker_ptr->pose.orientation.x = 0.0; marker_ptr->pose.orientation.y = 0.0; - marker_ptr->pose.orientation.z = std::sin(yaw / 2.0); - marker_ptr->pose.orientation.w = std::cos(yaw / 2.0); + marker_ptr->pose.orientation.z = std::sin(yaw * 0.5); + marker_ptr->pose.orientation.w = std::cos(yaw * 0.5); // ellipse size marker_ptr->scale.x = sigma1 * confidence_interval_coefficient; @@ -588,9 +588,9 @@ void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { - const double length_half = shape.dimensions.x / 2.0; - const double width_half = shape.dimensions.y / 2.0; - const double height_half = shape.dimensions.z / 2.0; + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; geometry_msgs::msg::Point point; // bounding box corner points @@ -612,10 +612,10 @@ void calc_bounding_box_direction_line_list( std::vector & points) { // direction triangle - const double length_half = shape.dimensions.x / 2.0; - const double width_half = shape.dimensions.y / 2.0; - const double height_half = shape.dimensions.z / 2.0; - const double triangle_size_half = shape.dimensions.y / 1.4; + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; + const double triangle_size_half = std::min(width_half * 1.4, shape.dimensions.x); geometry_msgs::msg::Point point; // triangle-shaped direction indicator @@ -637,22 +637,23 @@ void calc_bounding_box_orientation_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { - const double length_half = shape.dimensions.x / 2.0; - const double width_half = shape.dimensions.y / 2.0; - const double height_half = shape.dimensions.z / 2.0; - const double tick_width = width_half * 0.4; - const double tick_length = std::min(tick_width * 1.4, length_half); + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; + const double tick_width = width_half * 0.5; + const double tick_length = std::min(tick_width, length_half); geometry_msgs::msg::Point point; - // triangle-shaped direction indicator - const double point_list[3][3] = { - {length_half, 0, height_half}, - {length_half - tick_length, tick_width, height_half}, - {length_half - tick_length, -tick_width, height_half}, + // front corner cuts for orientation + const double point_list[4][3] = { + {length_half, width_half - tick_width, height_half}, + {length_half - tick_length, width_half, height_half}, + {length_half, -width_half + tick_width, height_half}, + {length_half - tick_length, -width_half, height_half}, }; const int point_pairs[2][2] = { {0, 1}, - {0, 2}, + {2, 3}, }; calc_line_list_from_points(point_list, point_pairs, 2, points); } @@ -661,9 +662,9 @@ void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { - const double length_half = shape.dimensions.x / 2.0; - const double width_half = shape.dimensions.y / 2.0; - const double height_half = shape.dimensions.z / 2.0; + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; geometry_msgs::msg::Point point; // bounding box corner points @@ -687,9 +688,9 @@ void calc_2d_bounding_box_bottom_direction_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { - const double length_half = shape.dimensions.x / 2.0; - const double width_half = shape.dimensions.y / 2.0; - const double height_half = shape.dimensions.z / 2.0; + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; const double triangle_size_half = std::min(width_half * 1.4, shape.dimensions.x); geometry_msgs::msg::Point point; @@ -711,22 +712,23 @@ void calc_2d_bounding_box_bottom_orientation_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) { - const double length_half = shape.dimensions.x / 2.0; - const double width_half = shape.dimensions.y / 2.0; - const double height_half = shape.dimensions.z / 2.0; - const double tick_width = width_half * 0.4; - const double tick_length = std::min(tick_width * 1.4, length_half); + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; + const double tick_width = width_half * 0.5; + const double tick_length = std::min(tick_width, length_half); geometry_msgs::msg::Point point; - // triangle-shaped direction indicator - const double point_list[3][3] = { - {length_half, 0, -height_half}, - {length_half - tick_length, tick_width, -height_half}, - {length_half - tick_length, -tick_width, -height_half}, + // front corner cuts for orientation + const double point_list[4][3] = { + {length_half, width_half - tick_width, height_half}, + {length_half - tick_length, width_half, height_half}, + {length_half, -width_half + tick_width, height_half}, + {length_half - tick_length, -width_half, height_half}, }; const int point_pairs[2][2] = { {0, 1}, - {0, 2}, + {2, 3}, }; calc_line_list_from_points(point_list, point_pairs, 2, points); } @@ -836,7 +838,7 @@ void calc_polygon_line_list( geometry_msgs::msg::Point point; point.x = shape.footprint.points.at(i).x; point.y = shape.footprint.points.at(i).y; - point.z = shape.dimensions.z / 2.0; + point.z = shape.dimensions.z * 0.5; points.push_back(point); point.x = shape.footprint.points .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) @@ -844,14 +846,14 @@ void calc_polygon_line_list( point.y = shape.footprint.points .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) .y; - point.z = shape.dimensions.z / 2.0; + point.z = shape.dimensions.z * 0.5; points.push_back(point); } for (size_t i = 0; i < shape.footprint.points.size(); ++i) { geometry_msgs::msg::Point point; point.x = shape.footprint.points.at(i).x; point.y = shape.footprint.points.at(i).y; - point.z = -shape.dimensions.z / 2.0; + point.z = -shape.dimensions.z * 0.5; points.push_back(point); point.x = shape.footprint.points .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) @@ -859,18 +861,18 @@ void calc_polygon_line_list( point.y = shape.footprint.points .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) .y; - point.z = -shape.dimensions.z / 2.0; + point.z = -shape.dimensions.z * 0.5; points.push_back(point); } for (size_t i = 0; i < shape.footprint.points.size(); ++i) { geometry_msgs::msg::Point point; point.x = shape.footprint.points.at(i).x; point.y = shape.footprint.points.at(i).y; - point.z = shape.dimensions.z / 2.0; + point.z = shape.dimensions.z * 0.5; points.push_back(point); point.x = shape.footprint.points.at(i).x; point.y = shape.footprint.points.at(i).y; - point.z = -shape.dimensions.z / 2.0; + point.z = -shape.dimensions.z * 0.5; points.push_back(point); } } @@ -889,7 +891,7 @@ void calc_2d_polygon_bottom_line_list( geometry_msgs::msg::Point point; point.x = shape.footprint.points.at(i).x; point.y = shape.footprint.points.at(i).y; - point.z = -shape.dimensions.z / 2.0; + point.z = -shape.dimensions.z * 0.5; points.push_back(point); point.x = shape.footprint.points .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) @@ -897,7 +899,7 @@ void calc_2d_polygon_bottom_line_list( point.y = shape.footprint.points .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) .y; - point.z = -shape.dimensions.z / 2.0; + point.z = -shape.dimensions.z * 0.5; points.push_back(point); } } From 3fcf897b2cbd0d94cdb9c3b8ecf22f83512ddbb7 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 21 Feb 2024 16:12:26 +0900 Subject: [PATCH 5/5] fix(autoware_auto_perception_rviz_plugin): yaw rate marker in predicted object Signed-off-by: Taekjin LEE --- .../src/object_detection/predicted_objects_display.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 24b21a15732c3..b42ffe1804246 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -211,7 +211,7 @@ std::vector PredictedObjectsDisplay: auto marker_ptr = yaw_rate_marker.value(); marker_ptr->header = msg->header; marker_ptr->id = uuid_to_marker_id(object.object_id); - add_marker(marker_ptr); + markers.push_back(marker_ptr); } // Get marker for twist covariance