Skip to content

Commit

Permalink
feat(autoware_auto_perception_rviz_plugin): improve rviz performance (a…
Browse files Browse the repository at this point in the history 
…utowarefoundation#2780)

* add choose box element to control type of visualization polygon for detected objects

Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>

* WIP: add switching logic and defininition for 2d poligons functions, working

Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>

* add function for making 2d polygon from cylinder shape

Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>

* add code to make 2d polygones from unknown objects

Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>

* wip, add creation of milty dummy objects by one click

Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>

* modificate empty_objects_publisher for testing purpose

Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>

* feat/2d_obj_rviz: cleaning for pull request. Remove dummy_empty_obj.launch and revert testing modification.

Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>

* feat/2d-obj-rviz refactor, update names of functions to more meaningfull, remove unnecessary code which was drawing additional lines on circle, remove TODO

Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>

* style(pre-commit): autofix

* feat/2d-obj-rviz correct spelling mistakes

Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>

---------

Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@lexavtanke @pre-commit-ci @asana17
2 people authored and asana17 committed Feb 8, 2023
1 parent 87ce43c commit 8f070e3
Show file tree
Hide file tree
Showing 3 changed files with 157 additions and 6 deletions.
18 changes: 18 additions & 0 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 @@ -84,6 +84,12 @@ get_shape_marker_ptr(
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width);

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);

/// \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
/// \return Marker ptr. Id and header will have to be set by the caller
Expand Down Expand Up @@ -131,10 +137,18 @@ 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_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_cylinder_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_cylinder_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_circle_line_list(
const geometry_msgs::msg::Point center, const double radius,
std::vector<geometry_msgs::msg::Point> & points, const int n);
Expand All @@ -143,6 +157,10 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_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_polygon_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_path_line_list(
const autoware_auto_perception_msgs::msg::PredictedPath & paths,
std::vector<geometry_msgs::msg::Point> & points);
Expand Down
28 changes: 22 additions & 6 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 @@ -14,6 +14,8 @@
#ifndef OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_
#define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_

#include "rviz_common/properties/enum_property.hpp"

#include <common/color_alpha_property.hpp>
#include <object_detection/object_polygon_detail.hpp>
#include <rviz_common/display.hpp>
Expand Down Expand Up @@ -59,8 +61,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase

explicit ObjectPolygonDisplayBase(const std::string & default_topic)
: m_marker_common(this),
m_display_3d_property{
"Display 3d polygon", true, "Enable/disable height visualization of the polygon", this},
// m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this},
m_display_label_property{"Display Label", true, "Enable/disable label visualization", this},
m_display_uuid_property{"Display UUID", true, "Enable/disable uuid visualization", this},
m_display_pose_with_covariance_property{
Expand All @@ -79,6 +80,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this},
m_default_topic{default_topic}
{
m_display_type_property = new rviz_common::properties::EnumProperty(
"Polygon Type", "3d", "Type of the polygon to display object.", this, SLOT(updatePalette()));
// Option values here must correspond to indices in palette_textures_ array in onInitialize()
// below.
m_display_type_property->addOption("3d", 0);
m_display_type_property->addOption("2d", 1);
m_display_type_property->addOption("Disable", 2);
// iterate over default values to create and initialize the properties.
for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) {
const auto & class_property_values = map_property_it.second;
Expand Down Expand Up @@ -150,14 +158,22 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
const ClassificationContainerT & labels, const double & line_width) const
{
const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels);

if (m_display_3d_property.getBool()) {
if (m_display_type_property->getOptionInt() == 0) {
return detail::get_shape_marker_ptr(shape_msg, centroid, orientation, color_rgba, line_width);
} else if (m_display_type_property->getOptionInt() == 1) {
return detail::get_2d_shape_marker_ptr(
shape_msg, centroid, orientation, color_rgba, line_width);
} else {
return std::nullopt;
}
}

template <typename ClassificationContainerT>
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);

/// \brief Convert given shape msg into a Marker to visualize label name
/// \tparam ClassificationContainerT List type with ObjectClassificationMsg
/// \param centroid Centroid position of the shape in Object.header.frame_id frame
Expand Down Expand Up @@ -366,8 +382,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
std::list<rviz_common::properties::Property> m_class_group_properties;
// Map to store class labels and its corresponding properties
PolygonPropertyMap m_polygon_properties;
// Property to enable/disable height visualization of the polygon
rviz_common::properties::BoolProperty m_display_3d_property;
// Property to choose type of visualization polygon
rviz_common::properties::EnumProperty * m_display_type_property;
// Property to enable/disable label visualization
rviz_common::properties::BoolProperty m_display_label_property;
// Property to enable/disable uuid visualization
Expand Down
117 changes: 117 additions & 0 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 @@ -276,6 +276,38 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
return 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)
{
auto marker_ptr = std::make_shared<Marker>();
marker_ptr->ns = std::string("shape");

using autoware_auto_perception_msgs::msg::Shape;
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);
} 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);
} else if (shape_msg.type == Shape::POLYGON) {
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
calc_2d_polygon_bottom_line_list(shape_msg, marker_ptr->points);
} else {
marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST;
calc_2d_polygon_bottom_line_list(shape_msg, marker_ptr->points);
}

marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
marker_ptr->pose = to_pose(centroid, orientation);
marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
marker_ptr->scale.x = line_width;
marker_ptr->color = color_rgba;

return marker_ptr;
}

void calc_bounding_box_line_list(
const autoware_auto_perception_msgs::msg::Shape & shape,
std::vector<geometry_msgs::msg::Point> & points)
Expand Down Expand Up @@ -392,6 +424,49 @@ void calc_bounding_box_line_list(
points.push_back(point);
}

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

void calc_cylinder_line_list(
const autoware_auto_perception_msgs::msg::Shape & shape,
std::vector<geometry_msgs::msg::Point> & points)
Expand Down Expand Up @@ -435,6 +510,21 @@ void calc_cylinder_line_list(
}
}

void calc_2d_cylinder_bottom_line_list(
const autoware_auto_perception_msgs::msg::Shape & shape,
std::vector<geometry_msgs::msg::Point> & points)
{
const double radius = shape.dimensions.x * 0.5;
{
constexpr int n = 20;
geometry_msgs::msg::Point center;
center.x = 0.0;
center.y = 0.0;
center.z = -shape.dimensions.z * 0.5;
calc_circle_line_list(center, radius, points, n);
}
}

void calc_circle_line_list(
const geometry_msgs::msg::Point center, const double radius,
std::vector<geometry_msgs::msg::Point> & points, const int n)
Expand Down Expand Up @@ -521,6 +611,33 @@ void calc_polygon_line_list(
}
}

void calc_2d_polygon_bottom_line_list(
const autoware_auto_perception_msgs::msg::Shape & shape,
std::vector<geometry_msgs::msg::Point> & points)
{
if (shape.footprint.points.size() < 2) {
RCLCPP_WARN(
rclcpp::get_logger("ObjectPolygonDisplayBase"),
"there are no enough footprint to visualize polygon");
return;
}
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;
points.push_back(point);
point.x = shape.footprint.points
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
.x;
point.y = shape.footprint.points
.at(static_cast<int>(i + 1) % static_cast<int>(shape.footprint.points.size()))
.y;
point.z = -shape.dimensions.z / 2.0;
points.push_back(point);
}
}

void calc_path_line_list(
const autoware_auto_perception_msgs::msg::PredictedPath & paths,
std::vector<geometry_msgs::msg::Point> & points)
Expand Down

0 comments on commit 8f070e3

Please sign in to comment.