Skip to content

Commit

Permalink
chore(autoware_auto_perception_rviz_plugin): add existence probabilit…
Browse files Browse the repository at this point in the history 
…y for rviz (autowarefoundation#5986)

* chore: add existence prob for rviz objects

Signed-off-by: badai-nguyen <[email protected]>

* chore: typo

Signed-off-by: badai-nguyen <[email protected]>

* chore: typo

Signed-off-by: badai-nguyen <[email protected]>

* chore: adjust text position

Signed-off-by: badai-nguyen <[email protected]>

* fix: add existence prob for tracked and predicted objects

Signed-off-by: badai-nguyen <[email protected]>

---------

Signed-off-by: badai-nguyen <[email protected]>
Co-authored-by: Shunsuke Miura <[email protected]>
  • Loading branch information
@badai-nguyen @miursh @karishma1911
2 people authored and karishma1911 committed Jun 3, 2024
1 parent f1dc9c7 commit 2182cc2
Show file tree
Hide file tree
Showing 6 changed files with 94 additions and 2 deletions.
6 changes: 6 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 @@ -19,6 +19,7 @@
#include <rclcpp/rclcpp.hpp>
#include <visibility_control.hpp>

#include <autoware_auto_perception_msgs/msg/detected_object.hpp>
#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
#include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
#include <autoware_auto_perception_msgs/msg/shape.hpp>
Expand Down Expand Up @@ -98,6 +99,11 @@ get_label_marker_ptr(
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
const std::string label, const std_msgs::msg::ColorRGBA & color_rgba);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
get_existence_probability_marker_ptr(
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba);

AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
get_uuid_marker_ptr(
const std::string & uuid, const geometry_msgs::msg::Point & centroid,
Expand Down
22 changes: 21 additions & 1 deletion ...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 @@ -77,6 +77,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
m_display_path_confidence_property{
"Display Predicted Path Confidence", true, "Enable/disable predicted paths visualization",
this},

m_display_existence_probability_property{
"Display Existence Probability", false, "Enable/disable existence probability visualization",
this},

m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this},
m_default_topic{default_topic}
{
Expand Down Expand Up @@ -202,6 +207,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
return std::nullopt;
}
}
template <typename ClassificationContainerT>
std::optional<Marker::SharedPtr> get_existence_probability_marker_ptr(
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
const float existence_probability, const ClassificationContainerT & labels) const
{
if (m_display_existence_probability_property.getBool()) {
const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels);
return detail::get_existence_probability_marker_ptr(
centroid, orientation, existence_probability, color_rgba);
} else {
return std::nullopt;
}
}

template <typename ClassificationContainerT>
std::optional<Marker::SharedPtr> get_uuid_marker_ptr(
Expand Down Expand Up @@ -326,7 +344,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
}
return (it->second).label;
}

std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const
{
std::stringstream ss;
Expand Down Expand Up @@ -415,6 +432,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
rviz_common::properties::BoolProperty m_display_predicted_paths_property;
// Property to enable/disable predicted path confidence visualization
rviz_common::properties::BoolProperty m_display_path_confidence_property;

rviz_common::properties::BoolProperty m_display_existence_probability_property;

// Property to decide line width of object shape
rviz_common::properties::FloatProperty m_line_width_property;
// Default topic name to be visualized
Expand Down
16 changes: 16 additions & 0 deletions ...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 @@ -57,6 +57,22 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg)
add_marker(label_marker_ptr);
}

// Get marker for existence probability
geometry_msgs::msg::Point existence_probability_position;
existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5;
existence_probability_position.y = object.kinematics.pose_with_covariance.pose.position.y;
existence_probability_position.z = object.kinematics.pose_with_covariance.pose.position.z + 0.5;
const float existence_probability = object.existence_probability;
auto existence_prob_marker = get_existence_probability_marker_ptr(
existence_probability_position, object.kinematics.pose_with_covariance.pose.orientation,
existence_probability, object.classification);
if (existence_prob_marker) {
auto existence_prob_marker_ptr = existence_prob_marker.value();
existence_prob_marker_ptr->header = msg->header;
existence_prob_marker_ptr->id = id++;
add_marker(existence_prob_marker_ptr);
}

// Get marker for velocity text
geometry_msgs::msg::Point vel_vis_position;
vel_vis_position.x = object.kinematics.pose_with_covariance.pose.position.x - 0.5;
Expand Down
17 changes: 17 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 @@ -242,6 +242,23 @@ visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr(
return marker_ptr;
}

visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr(
const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba)
{
auto marker_ptr = std::make_shared<Marker>();
marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
marker_ptr->ns = std::string("existence probability");
marker_ptr->scale.x = 0.5;
marker_ptr->scale.z = 0.5;
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->color = color_rgba;
return 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,
Expand Down
19 changes: 19 additions & 0 deletions ...n/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,25 @@ std::vector<visualization_msgs::msg::Marker::SharedPtr> PredictedObjectsDisplay:
markers.push_back(pose_with_covariance_marker_ptr);
}

// Get marker for existence probability
geometry_msgs::msg::Point existence_probability_position;
existence_probability_position.x =
object.kinematics.initial_pose_with_covariance.pose.position.x + 0.5;
existence_probability_position.y =
object.kinematics.initial_pose_with_covariance.pose.position.y;
existence_probability_position.z =
object.kinematics.initial_pose_with_covariance.pose.position.z + 0.5;
const float existence_probability = object.existence_probability;
auto existence_prob_marker = get_existence_probability_marker_ptr(
existence_probability_position,
object.kinematics.initial_pose_with_covariance.pose.orientation, existence_probability,
object.classification);
if (existence_prob_marker) {
auto existence_prob_marker_ptr = existence_prob_marker.value();
existence_prob_marker_ptr->header = msg->header;
existence_prob_marker_ptr->id = uuid_to_marker_id(object.object_id);
markers.push_back(existence_prob_marker_ptr);
}
// Get marker for velocity text
geometry_msgs::msg::Point vel_vis_position;
vel_vis_position.x = uuid_vis_position.x - 0.5;
Expand Down
16 changes: 15 additions & 1 deletion common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,21 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg)
pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id);
add_marker(pose_with_covariance_marker_ptr);
}

// Get marker for existence probability
geometry_msgs::msg::Point existence_probability_position;
existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5;
existence_probability_position.y = object.kinematics.pose_with_covariance.pose.position.y;
existence_probability_position.z = object.kinematics.pose_with_covariance.pose.position.z + 0.5;
const float existence_probability = object.existence_probability;
auto existence_prob_marker = get_existence_probability_marker_ptr(
existence_probability_position, object.kinematics.pose_with_covariance.pose.orientation,
existence_probability, object.classification);
if (existence_prob_marker) {
auto existence_prob_marker_ptr = existence_prob_marker.value();
existence_prob_marker_ptr->header = msg->header;
existence_prob_marker_ptr->id = uuid_to_marker_id(object.object_id);
add_marker(existence_prob_marker_ptr);
}
// Get marker for velocity text
geometry_msgs::msg::Point vel_vis_position;
vel_vis_position.x = uuid_vis_position.x - 0.5;
Expand Down

0 comments on commit 2182cc2

Please sign in to comment.