Skip to content

Commit

Permalink
feat(perception_rviz_plugin): visualize object heading direction on 3…
Browse files Browse the repository at this point in the history 
…d bounding box (#6035)

* feat(autoware_auto_perception_rviz_plugin): visualize object heading

Signed-off-by: Taekjin LEE <[email protected]>

* feat(autoware_auto_perception_rviz_plugin): visualize heading only it is available

Signed-off-by: Taekjin LEE <[email protected]>

* feat(autoware_auto_perception_rviz_plugin): 2d visualization implemented

Signed-off-by: Taekjin LEE <[email protected]>

* feat(autoware_auto_perception_rviz_plugin): integrating repeating codes

Signed-off-by: Taekjin LEE <[email protected]>

---------

Signed-off-by: Taekjin LEE <[email protected]>
  • Loading branch information
@technolojin
technolojin authored Jan 12, 2024
1 parent d1a86e6 commit 39642d2
Show file tree
Hide file tree
Showing 6 changed files with 142 additions and 156 deletions.
18 changes: 16 additions & 2 deletions ...n/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,13 +83,15 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::Sha
get_shape_marker_ptr(
const autoware_auto_perception_msgs::msg::Shape & shape_msg,
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width);
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
const bool & is_orientation_available = true);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
get_2d_shape_marker_ptr(
const autoware_auto_perception_msgs::msg::Shape & shape_msg,
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width);
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
const bool & is_orientation_available = true);

/// \brief Convert the given polygon into a marker representing the shape in 3d
/// \param centroid Centroid position of the shape in Object.header.frame_id frame
Expand Down Expand Up @@ -139,14 +141,26 @@ get_path_confidence_marker_ptr(
const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
const std_msgs::msg::ColorRGBA & path_confidence_color);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_line_list_from_points(
const double point_list[][3], const int point_pairs[][2], const int & num_pairs,
std::vector<geometry_msgs::msg::Point> & points);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list(
const autoware_auto_perception_msgs::msg::Shape & shape,
std::vector<geometry_msgs::msg::Point> & points);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list(
const autoware_auto_perception_msgs::msg::Shape & shape,
std::vector<geometry_msgs::msg::Point> & 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<geometry_msgs::msg::Point> & points);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list(
const autoware_auto_perception_msgs::msg::Shape & shape,
std::vector<geometry_msgs::msg::Point> & points);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list(
const autoware_auto_perception_msgs::msg::Shape & shape,
std::vector<geometry_msgs::msg::Point> & points);
Expand Down
11 changes: 7 additions & 4 deletions ...ware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,14 +170,16 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
std::optional<Marker::SharedPtr> get_shape_marker_ptr(
const autoware_auto_perception_msgs::msg::Shape & shape_msg,
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
const ClassificationContainerT & labels, const double & line_width) const
const ClassificationContainerT & labels, const double & line_width,
const bool & is_orientation_available) const
{
const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels);
if (m_display_type_property->getOptionInt() == 0) {
return detail::get_shape_marker_ptr(shape_msg, centroid, orientation, color_rgba, line_width);
return detail::get_shape_marker_ptr(
shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available);
} else if (m_display_type_property->getOptionInt() == 1) {
return detail::get_2d_shape_marker_ptr(
shape_msg, centroid, orientation, color_rgba, line_width);
shape_msg, centroid, orientation, color_rgba, line_width, is_orientation_available);
} else {
return std::nullopt;
}
Expand All @@ -187,7 +189,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr(
const autoware_auto_perception_msgs::msg::Shape & shape_msg,
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width);
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
const bool & is_orientation_available);

/// \brief Convert given shape msg into a Marker to visualize label name
/// \tparam ClassificationContainerT List type with ObjectClassificationMsg
Expand Down
4 changes: 3 additions & 1 deletion ...on/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,9 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg)
auto shape_marker = get_shape_marker_ptr(
object.shape, object.kinematics.pose_with_covariance.pose.position,
object.kinematics.pose_with_covariance.pose.orientation, object.classification,
get_line_width());
get_line_width(),
object.kinematics.orientation_availability ==
autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE);
if (shape_marker) {
auto shape_marker_ptr = shape_marker.value();
shape_marker_ptr->header = msg->header;
Expand Down
259 changes: 112 additions & 147 deletions common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -262,7 +262,8 @@ visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr(
visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
const autoware_auto_perception_msgs::msg::Shape & shape_msg,
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width)
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
const bool & is_orientation_available)
{
auto marker_ptr = std::make_shared<Marker>();
marker_ptr->ns = std::string("shape");
Expand All @@ -271,6 +272,9 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
if (shape_msg.type == Shape::BOUNDING_BOX) {
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
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 if (shape_msg.type == Shape::CYLINDER) {
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
calc_cylinder_line_list(shape_msg, marker_ptr->points);
Expand All @@ -294,7 +298,8 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr(
const autoware_auto_perception_msgs::msg::Shape & shape_msg,
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width)
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width,
const bool & is_orientation_available)
{
auto marker_ptr = std::make_shared<Marker>();
marker_ptr->ns = std::string("shape");
Expand All @@ -303,6 +308,9 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr(
if (shape_msg.type == Shape::BOUNDING_BOX) {
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
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 if (shape_msg.type == Shape::CYLINDER) {
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
calc_2d_cylinder_bottom_line_list(shape_msg, marker_ptr->points);
Expand All @@ -323,163 +331,120 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr(
return marker_ptr;
}

void calc_line_list_from_points(
const double point_list[][3], const int point_pairs[][2], const int & num_pairs,
std::vector<geometry_msgs::msg::Point> & points)
{
geometry_msgs::msg::Point point;
for (int i = 0; i < num_pairs; ++i) {
point.x = point_list[point_pairs[i][0]][0];
point.y = point_list[point_pairs[i][0]][1];
point.z = point_list[point_pairs[i][0]][2];
points.push_back(point);
point.x = point_list[point_pairs[i][1]][0];
point.y = point_list[point_pairs[i][1]][1];
point.z = point_list[point_pairs[i][1]][2];
points.push_back(point);
}
}

void calc_bounding_box_line_list(
const autoware_auto_perception_msgs::msg::Shape & shape,
std::vector<geometry_msgs::msg::Point> & 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;
geometry_msgs::msg::Point point;

// bounding box corner points
// top and bottom surface, clockwise
const double point_list[8][3] = {
{length_half, width_half, height_half}, {length_half, -width_half, height_half},
{-length_half, -width_half, height_half}, {-length_half, width_half, height_half},
{length_half, width_half, -height_half}, {length_half, -width_half, -height_half},
{-length_half, -width_half, -height_half}, {-length_half, width_half, -height_half},
};
const int point_pairs[12][2] = {
{0, 1}, {1, 2}, {2, 3}, {3, 0}, {4, 5}, {5, 6}, {6, 7}, {7, 4}, {0, 4}, {1, 5}, {2, 6}, {3, 7},
};
calc_line_list_from_points(point_list, point_pairs, 12, points);
}

void calc_bounding_box_direction_line_list(
const autoware_auto_perception_msgs::msg::Shape & shape,
std::vector<geometry_msgs::msg::Point> & 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;
geometry_msgs::msg::Point point;
point.x = shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);
point.x = shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);

point.x = shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);
point.x = shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);

point.x = -shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);
point.x = -shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);

point.x = -shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);
point.x = -shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);

// up surface
point.x = shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);
point.x = -shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);

point.x = shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);
point.x = shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);

point.x = -shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);
point.x = -shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);

point.x = shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);
point.x = -shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = shape.dimensions.z / 2.0;
points.push_back(point);

// down surface
point.x = shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);
point.x = -shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);

point.x = shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);
point.x = shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);

point.x = -shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);
point.x = -shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);

point.x = shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);
point.x = -shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);

// triangle-shaped direction indicator
const double point_list[6][3] = {
{length_half, 0, height_half},
{length_half - triangle_size_half, width_half, height_half},
{length_half - triangle_size_half, -width_half, height_half},
{length_half, 0, -height_half},
{length_half, width_half, height_half},
{length_half, -width_half, height_half},
};
const int point_pairs[5][2] = {
{0, 1}, {1, 2}, {0, 2}, {3, 4}, {3, 5},
};
calc_line_list_from_points(point_list, point_pairs, 5, points);
}

void calc_2d_bounding_box_bottom_line_list(
const autoware_auto_perception_msgs::msg::Shape & shape,
std::vector<geometry_msgs::msg::Point> & 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;
geometry_msgs::msg::Point point;
// down surface
point.x = shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);
point.x = -shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);

point.x = shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);
point.x = shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);

point.x = -shape.dimensions.x / 2.0;
point.y = shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);
point.x = -shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);

point.x = shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);
point.x = -shape.dimensions.x / 2.0;
point.y = -shape.dimensions.y / 2.0;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);

// bounding box corner points
// top surface, clockwise
const double point_list[4][3] = {
{length_half, width_half, -height_half},
{length_half, -width_half, -height_half},
{-length_half, -width_half, -height_half},
{-length_half, width_half, -height_half},
};
const int point_pairs[4][2] = {
{0, 1},
{1, 2},
{2, 3},
{3, 0},
};
calc_line_list_from_points(point_list, point_pairs, 4, points);
}

void calc_2d_bounding_box_bottom_direction_line_list(
const autoware_auto_perception_msgs::msg::Shape & shape,
std::vector<geometry_msgs::msg::Point> & 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 triangle_size_half = shape.dimensions.y / 1.4;
geometry_msgs::msg::Point point;

// triangle-shaped direction indicator
const double point_list[6][3] = {
{length_half, 0, -height_half},
{length_half - triangle_size_half, width_half, -height_half},
{length_half - triangle_size_half, -width_half, -height_half},
};
const int point_pairs[3][2] = {
{0, 1},
{1, 2},
{0, 2},
};
calc_line_list_from_points(point_list, point_pairs, 3, points);
}

void calc_cylinder_line_list(
Expand Down
Loading

0 comments on commit 39642d2

Please sign in to comment.