From f426dbbd4b6a314929c980ad0851eaef7be29d10 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 10 Jan 2024 18:38:04 +0900 Subject: [PATCH 01/14] feat: visualize yaw rate, yaw std Signed-off-by: Taekjin LEE --- .../object_polygon_detail.hpp | 4 + .../object_polygon_detail.cpp | 89 ++++++++++++++++--- 2 files changed, 82 insertions(+), 11 deletions(-) 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 b3ee4a33b28c9..4150cfd0696e1 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 @@ -142,6 +142,10 @@ 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_arc_line_list( + const double start_angle, const double end_angle, const double radius, + std::vector & points); + 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 & 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 97d7212d4f0ce..1666e4253e9df 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 @@ -101,18 +101,34 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; - geometry_msgs::msg::Point pt_s; - pt_s.x = 0.0; - pt_s.y = 0.0; - pt_s.z = 0.0; - marker_ptr->points.push_back(pt_s); - - geometry_msgs::msg::Point pt_e; - pt_e.x = twist_with_covariance.twist.linear.x; - pt_e.y = twist_with_covariance.twist.linear.y; - pt_e.z = twist_with_covariance.twist.linear.z; - marker_ptr->points.push_back(pt_e); + // velocity + geometry_msgs::msg::Point point; + point.x = 0.0; + point.y = 0.0; + point.z = 0.0; + marker_ptr->points.push_back(point); + point.x = twist_with_covariance.twist.linear.x; + point.y = twist_with_covariance.twist.linear.y; + point.z = twist_with_covariance.twist.linear.z; + marker_ptr->points.push_back(point); + // yaw rate + const double yaw_rate = twist_with_covariance.twist.angular.z; + const double velocity = std::sqrt( + twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y + + twist_with_covariance.twist.linear.z * twist_with_covariance.twist.linear.z); + const double velocity_angle = std::atan2( + twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + point.x = twist_with_covariance.twist.linear.x; + point.y = twist_with_covariance.twist.linear.y; + point.z = twist_with_covariance.twist.linear.z; + marker_ptr->points.push_back(point); + point.x = velocity * std::cos(velocity_angle + yaw_rate); + point.y = velocity * std::sin(velocity_angle + yaw_rate); + point.z = 0; + marker_ptr->points.push_back(point); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color.a = 0.999; marker_ptr->color.r = 1.0; @@ -164,6 +180,37 @@ visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr( return marker_ptr; } +void calc_arc_line_list( + const double start_angle, const double end_angle, const double radius, + std::vector & points) +{ + geometry_msgs::msg::Point point; + // fisrt point + point.x = 0; + point.y = 0; + point.z = 0; + // arc points + const double maximum_delta_angle = 10.0 * M_PI / 180.0; + const int num_points = std::max( + 3, static_cast(std::abs(end_angle - start_angle) / maximum_delta_angle)); + for (int i = 0; i < num_points; ++i) { + const double angle = start_angle + (end_angle - start_angle) * static_cast(i) / + static_cast(num_points - 1); + // arc lines + points.push_back(point); + point.x = radius * std::cos(angle); + point.y = radius * std::sin(angle); + point.z = 0; + points.push_back(point); + } + // last line + points.push_back(point); + point.x = 0; + point.y = 0; + point.z = 0; + points.push_back(point); +} + visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) { @@ -178,6 +225,9 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( marker_ptr->pose.orientation.z = 0.0; marker_ptr->pose.orientation.w = 1.0; geometry_msgs::msg::Point point; + + // position covariance + // extract eigen values and eigen vectors Eigen::Matrix2d eigen_pose_with_covariance; eigen_pose_with_covariance << pose_with_covariance.covariance[0], pose_with_covariance.covariance[1], pose_with_covariance.covariance[6], @@ -203,6 +253,23 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( point.y = e2.y() * sigma2; point.z = 0; marker_ptr->points.push_back(point); + + // orientation covariance + double yaw_vector_length = 3.0; + double yaw_sigma = std::sqrt(pose_with_covariance.covariance[35]); // 2.448 sigma is 95% + // get yaw angle from quaternion + double yaw = std::atan2( + 2.0 * (pose_with_covariance.pose.orientation.w * pose_with_covariance.pose.orientation.z + + pose_with_covariance.pose.orientation.x * pose_with_covariance.pose.orientation.y), + 1.0 - 2.0 * (pose_with_covariance.pose.orientation.y * pose_with_covariance.pose.orientation.y + + pose_with_covariance.pose.orientation.z * pose_with_covariance.pose.orientation.z)); + // get arc points + if (yaw_sigma > M_PI) { + yaw_vector_length = 1.0; + } + calc_arc_line_list(yaw - yaw_sigma, yaw + yaw_sigma, yaw_vector_length, marker_ptr->points); + + // marker configuration marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color.a = 0.999; marker_ptr->color.r = 1.0; From f45c6a35dcf40e52c3d759c53d699f542d0e44e9 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 11 Jan 2024 09:45:06 +0900 Subject: [PATCH 02/14] feat: discrete yaw covariance marker Signed-off-by: Taekjin LEE --- .../object_polygon_detail.hpp | 6 ++- .../object_polygon_display_base.hpp | 17 ++++++++- .../object_polygon_detail.cpp | 38 ++++++++++++------- .../predicted_objects_display.cpp | 37 +++++++++++------- .../tracked_objects_display.cpp | 19 ++++++++-- 5 files changed, 85 insertions(+), 32 deletions(-) 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 4150cfd0696e1..d0a6b864a23f9 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 @@ -116,6 +116,10 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::Sha get_pose_with_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & line_width); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, @@ -142,7 +146,7 @@ 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_arc_line_list( +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip( const double start_angle, const double end_angle, const double radius, std::vector & points); diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index a0a21406ba416..01eb876727d22 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -65,7 +65,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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{ - "Display PoseWithCovariance", true, "Enable/disable pose with covariance visualization", + "Display Pose Covariance", true, "Enable/disable pose covariance visualization", + this}, + m_display_yaw_covariance_property{ + "Display Yaw Covariance", true, "Enable/disable yaw covariance visualization", this}, m_display_velocity_text_property{ "Display Velocity", true, "Enable/disable velocity text visualization", this}, @@ -248,6 +251,16 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } + std::optional get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & line_width) const + { + if (m_display_yaw_covariance_property.getBool()) { + return detail::get_yaw_covariance_marker_ptr(pose_with_covariance, line_width); + } else { + return std::nullopt; + } + } + template std::optional get_velocity_text_marker_ptr( const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, @@ -425,6 +438,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::BoolProperty m_display_uuid_property; // Property to enable/disable pose with covariance visualization rviz_common::properties::BoolProperty m_display_pose_with_covariance_property; + // Property to enable/disable yaw covariance visualization + rviz_common::properties::BoolProperty m_display_yaw_covariance_property; // Property to enable/disable velocity text visualization rviz_common::properties::BoolProperty m_display_velocity_text_property; // Property to enable/disable acceleration text visualization 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 1666e4253e9df..50e3b635729a1 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 @@ -180,7 +180,7 @@ visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr( return marker_ptr; } -void calc_arc_line_list( +void calc_arc_line_strip( const double start_angle, const double end_angle, const double radius, std::vector & points) { @@ -189,6 +189,7 @@ void calc_arc_line_list( point.x = 0; point.y = 0; point.z = 0; + points.push_back(point); // arc points const double maximum_delta_angle = 10.0 * M_PI / 180.0; const int num_points = std::max( @@ -196,15 +197,12 @@ void calc_arc_line_list( for (int i = 0; i < num_points; ++i) { const double angle = start_angle + (end_angle - start_angle) * static_cast(i) / static_cast(num_points - 1); - // arc lines - points.push_back(point); point.x = radius * std::cos(angle); point.y = radius * std::sin(angle); point.z = 0; points.push_back(point); } - // last line - points.push_back(point); + // last point point.x = 0; point.y = 0; point.z = 0; @@ -254,24 +252,38 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( point.z = 0; marker_ptr->points.push_back(point); + // marker configuration + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color.a = 0.999; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 1.0; + marker_ptr->color.b = 1.0; + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_STRIP; + marker_ptr->ns = std::string("yaw covariance"); + marker_ptr->scale.x = line_width; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + geometry_msgs::msg::Point point; + // orientation covariance double yaw_vector_length = 3.0; double yaw_sigma = std::sqrt(pose_with_covariance.covariance[35]); // 2.448 sigma is 95% - // get yaw angle from quaternion - double yaw = std::atan2( - 2.0 * (pose_with_covariance.pose.orientation.w * pose_with_covariance.pose.orientation.z + - pose_with_covariance.pose.orientation.x * pose_with_covariance.pose.orientation.y), - 1.0 - 2.0 * (pose_with_covariance.pose.orientation.y * pose_with_covariance.pose.orientation.y + - pose_with_covariance.pose.orientation.z * pose_with_covariance.pose.orientation.z)); // get arc points if (yaw_sigma > M_PI) { yaw_vector_length = 1.0; } - calc_arc_line_list(yaw - yaw_sigma, yaw + yaw_sigma, yaw_vector_length, marker_ptr->points); + calc_arc_line_strip(-yaw_sigma, yaw_sigma, yaw_vector_length, marker_ptr->points); // marker configuration marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); - marker_ptr->color.a = 0.999; + marker_ptr->color.a = 0.9; marker_ptr->color.r = 1.0; marker_ptr->color.g = 1.0; marker_ptr->color.b = 1.0; 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 a9074f9e1bc1a..98ab5b3102a00 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 @@ -73,16 +73,17 @@ std::vector PredictedObjectsDisplay: std::vector markers; for (const auto & object : msg->objects) { + const auto line_width = get_line_width(); // Get marker for shape auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.initial_pose_with_covariance.pose.position, object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification, - get_line_width(), true); + line_width, true); if (shape_marker) { - auto shape_marker_ptr = shape_marker.value(); - shape_marker_ptr->header = msg->header; - shape_marker_ptr->id = uuid_to_marker_id(object.object_id); - markers.push_back(shape_marker_ptr); + auto marker_ptr = shape_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); } // Get marker for label @@ -90,10 +91,10 @@ std::vector PredictedObjectsDisplay: object.kinematics.initial_pose_with_covariance.pose.position, object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification); if (label_marker) { - auto label_marker_ptr = label_marker.value(); - label_marker_ptr->header = msg->header; - label_marker_ptr->id = uuid_to_marker_id(object.object_id); - markers.push_back(label_marker_ptr); + auto marker_ptr = label_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); } // Get marker for id @@ -115,10 +116,20 @@ std::vector PredictedObjectsDisplay: auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr(object.kinematics.initial_pose_with_covariance); if (pose_with_covariance_marker) { - auto pose_with_covariance_marker_ptr = pose_with_covariance_marker.value(); - pose_with_covariance_marker_ptr->header = msg->header; - pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id); - markers.push_back(pose_with_covariance_marker_ptr); + auto marker_ptr = pose_with_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + + // Get marker for yaw covariance + auto yaw_covariance_marker = + get_yaw_covariance_marker_ptr(object.kinematics.initial_pose_with_covariance, line_width/2); + if (yaw_covariance_marker) { + auto marker_ptr = yaw_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); } // Get marker for existence probability diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 54dfb9edf889a..772725a1cbfaf 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -99,11 +99,22 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr(object.kinematics.pose_with_covariance); if (pose_with_covariance_marker) { - auto pose_with_covariance_marker_ptr = pose_with_covariance_marker.value(); - pose_with_covariance_marker_ptr->header = msg->header; - pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id); - add_marker(pose_with_covariance_marker_ptr); + auto marker_ptr = pose_with_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); } + + // Get marker for yaw covariance + auto yaw_covariance_marker = + get_yaw_covariance_marker_ptr(object.kinematics.pose_with_covariance, line_width/2); + if (yaw_covariance_marker) { + auto marker_ptr = yaw_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(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; From e6d3f7ac77cd4493da6fbae2427028fab3831106 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 11 Jan 2024 10:57:06 +0900 Subject: [PATCH 03/14] feat: add yaw covariance to detected object Signed-off-by: Taekjin LEE --- .../object_polygon_detail.hpp | 4 ++-- .../object_polygon_display_base.hpp | 17 ++++++++-------- .../detected_objects_display.cpp | 20 +++++++++++++++++++ .../object_polygon_detail.cpp | 20 +++++++++---------- .../predicted_objects_display.cpp | 11 +++++----- .../tracked_objects_display.cpp | 13 ++++++------ 6 files changed, 53 insertions(+), 32 deletions(-) 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 d0a6b864a23f9..1531e101a5af9 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 @@ -114,11 +114,11 @@ get_uuid_marker_ptr( AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & line_width); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & line_width); + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, const double & line_width); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 01eb876727d22..9231060b5c970 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -65,11 +65,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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{ - "Display Pose Covariance", true, "Enable/disable pose covariance visualization", - this}, + "Display Pose Covariance", true, "Enable/disable pose covariance visualization", this}, m_display_yaw_covariance_property{ - "Display Yaw Covariance", true, "Enable/disable yaw covariance visualization", - this}, + "Display Yaw Covariance", false, "Enable/disable yaw covariance visualization", this}, m_display_velocity_text_property{ "Display Velocity", true, "Enable/disable velocity text visualization", this}, m_display_acceleration_text_property{ @@ -242,20 +240,23 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } std::optional get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) const + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const double & line_width) const { if (m_display_pose_with_covariance_property.getBool()) { - return detail::get_pose_with_covariance_marker_ptr(pose_with_covariance); + return detail::get_pose_with_covariance_marker_ptr(pose_with_covariance, line_width); } else { return std::nullopt; } } std::optional get_yaw_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & line_width) const + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const double & length, + const double & line_width) const { if (m_display_yaw_covariance_property.getBool()) { - return detail::get_yaw_covariance_marker_ptr(pose_with_covariance, line_width); + return detail::get_yaw_covariance_marker_ptr(pose_with_covariance, length, line_width); } else { return std::nullopt; } diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 932ea87ccf5ad..0fa952c130df9 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -59,6 +59,26 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(label_marker_ptr); } + // Get marker for pose with covariance + auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr( + object.kinematics.pose_with_covariance, get_line_width() / 2); + if (pose_with_covariance_marker) { + auto marker_ptr = pose_with_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + + // Get marker for yaw covariance + auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.shape.dimensions.x / 1.6, get_line_width() / 2); + if (yaw_covariance_marker) { + auto marker_ptr = yaw_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(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; 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 50e3b635729a1..fe057268db918 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 @@ -118,8 +118,8 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y + twist_with_covariance.twist.linear.z * twist_with_covariance.twist.linear.z); - const double velocity_angle = std::atan2( - twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + const double velocity_angle = + std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); point.x = twist_with_covariance.twist.linear.x; point.y = twist_with_covariance.twist.linear.y; point.z = twist_with_covariance.twist.linear.z; @@ -128,7 +128,7 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( point.y = velocity * std::sin(velocity_angle + yaw_rate); point.z = 0; marker_ptr->points.push_back(point); - + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color.a = 0.999; marker_ptr->color.r = 1.0; @@ -192,11 +192,11 @@ void calc_arc_line_strip( points.push_back(point); // arc points const double maximum_delta_angle = 10.0 * M_PI / 180.0; - const int num_points = std::max( - 3, static_cast(std::abs(end_angle - start_angle) / maximum_delta_angle)); + const int num_points = + std::max(3, static_cast(std::abs(end_angle - start_angle) / maximum_delta_angle)); for (int i = 0; i < num_points; ++i) { const double angle = start_angle + (end_angle - start_angle) * static_cast(i) / - static_cast(num_points - 1); + static_cast(num_points - 1); point.x = radius * std::cos(angle); point.y = radius * std::sin(angle); point.z = 0; @@ -210,12 +210,12 @@ void calc_arc_line_strip( } visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & line_width) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; marker_ptr->ns = std::string("position covariance"); - marker_ptr->scale.x = 0.03; + marker_ptr->scale.x = line_width; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; marker_ptr->pose.orientation.x = 0.0; @@ -262,7 +262,7 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & line_width) + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, const double & line_width) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::LINE_STRIP; @@ -273,7 +273,7 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( geometry_msgs::msg::Point point; // orientation covariance - double yaw_vector_length = 3.0; + double yaw_vector_length = std::max(length, 1.0); double yaw_sigma = std::sqrt(pose_with_covariance.covariance[35]); // 2.448 sigma is 95% // get arc points if (yaw_sigma > M_PI) { 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 98ab5b3102a00..41973819bf8a6 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 @@ -73,12 +73,11 @@ std::vector PredictedObjectsDisplay: std::vector markers; for (const auto & object : msg->objects) { - const auto line_width = get_line_width(); // Get marker for shape auto shape_marker = get_shape_marker_ptr( object.shape, object.kinematics.initial_pose_with_covariance.pose.position, object.kinematics.initial_pose_with_covariance.pose.orientation, object.classification, - line_width, true); + get_line_width(), true); if (shape_marker) { auto marker_ptr = shape_marker.value(); marker_ptr->header = msg->header; @@ -113,8 +112,8 @@ std::vector PredictedObjectsDisplay: } // Get marker for pose with covariance - auto pose_with_covariance_marker = - get_pose_with_covariance_marker_ptr(object.kinematics.initial_pose_with_covariance); + auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, get_line_width() / 2); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); marker_ptr->header = msg->header; @@ -123,8 +122,8 @@ std::vector PredictedObjectsDisplay: } // Get marker for yaw covariance - auto yaw_covariance_marker = - get_yaw_covariance_marker_ptr(object.kinematics.initial_pose_with_covariance, line_width/2); + auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, object.shape.dimensions.x / 1.6, get_line_width() / 2); if (yaw_covariance_marker) { auto marker_ptr = yaw_covariance_marker.value(); marker_ptr->header = msg->header; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 772725a1cbfaf..390f3db4a3f40 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -55,11 +55,11 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) for (const auto & object : msg->objects) { // Filter by object dynamic status if (!is_object_to_show(showing_dynamic_status, object)) continue; - const auto line_width = get_line_width(); // Get marker for shape 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, line_width, + object.kinematics.pose_with_covariance.pose.orientation, object.classification, + get_line_width(), object.kinematics.orientation_availability == autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { @@ -96,8 +96,8 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) } // Get marker for pose with covariance - auto pose_with_covariance_marker = - get_pose_with_covariance_marker_ptr(object.kinematics.pose_with_covariance); + auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr( + object.kinematics.pose_with_covariance, get_line_width() / 2); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); marker_ptr->header = msg->header; @@ -107,7 +107,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) // Get marker for yaw covariance auto yaw_covariance_marker = - get_yaw_covariance_marker_ptr(object.kinematics.pose_with_covariance, line_width/2); + get_yaw_covariance_marker_ptr(object.kinematics.pose_with_covariance, object.shape.dimensions.x / 1.6, get_line_width() / 2); if (yaw_covariance_marker) { auto marker_ptr = yaw_covariance_marker.value(); marker_ptr->header = msg->header; @@ -161,7 +161,8 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) // Get marker for twist auto twist_marker = get_twist_marker_ptr( - object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, line_width); + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width()); if (twist_marker) { auto twist_marker_ptr = twist_marker.value(); twist_marker_ptr->header = msg->header; From 48ff2a940e5278b9db9c38f6457cea6308c11143 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 11 Jan 2024 11:29:33 +0900 Subject: [PATCH 04/14] feat: visualize twist covariance, yaw only Signed-off-by: Taekjin LEE --- .../object_polygon_detail.hpp | 5 ++ .../object_polygon_display_base.hpp | 15 ++++++ .../detected_objects_display.cpp | 16 +++++- .../object_polygon_detail.cpp | 53 +++++++++++++++++++ .../predicted_objects_display.cpp | 15 +++++- .../tracked_objects_display.cpp | 16 +++++- 6 files changed, 114 insertions(+), 6 deletions(-) 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 1531e101a5af9..ae0e033a62f3c 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 @@ -135,6 +135,11 @@ get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape, diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 9231060b5c970..2185dcf4c6b16 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -73,6 +73,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_display_acceleration_text_property{ "Display Acceleration", true, "Enable/disable acceleration text visualization", this}, m_display_twist_property{"Display Twist", true, "Enable/disable twist visualization", this}, + m_display_twist_covariance_property{"Display Twist Covariance", false, "Enable/disable twist covariance visualization", this}, m_display_predicted_paths_property{ "Display Predicted Paths", true, "Enable/disable predicted paths visualization", this}, m_display_path_confidence_property{ @@ -300,6 +301,18 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } + std::optional get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & line_width) const + { + if (m_display_twist_covariance_property.getBool()) { + return detail::get_twist_covariance_marker_ptr(pose_with_covariance, twist_with_covariance, line_width); + } else { + return std::nullopt; + } + } + std::optional get_predicted_path_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, const autoware_auto_perception_msgs::msg::Shape & shape, @@ -447,6 +460,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::BoolProperty m_display_acceleration_text_property; // Property to enable/disable twist visualization rviz_common::properties::BoolProperty m_display_twist_property; + // Property to enable/disable twist visualization + rviz_common::properties::BoolProperty m_display_twist_covariance_property; // Property to enable/disable predicted paths visualization rviz_common::properties::BoolProperty m_display_predicted_paths_property; // Property to enable/disable predicted path confidence visualization diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 0fa952c130df9..899a790d6fc61 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -61,7 +61,7 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) // Get marker for pose with covariance auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr( - object.kinematics.pose_with_covariance, get_line_width() / 2); + object.kinematics.pose_with_covariance, get_line_width() * 0.5); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); marker_ptr->header = msg->header; @@ -71,7 +71,7 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) // Get marker for yaw covariance auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( - object.kinematics.pose_with_covariance, object.shape.dimensions.x / 1.6, get_line_width() / 2); + object.kinematics.pose_with_covariance, object.shape.dimensions.x * 0.65, get_line_width() * 0.5); if (yaw_covariance_marker) { auto marker_ptr = yaw_covariance_marker.value(); marker_ptr->header = msg->header; @@ -119,6 +119,18 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) twist_marker_ptr->id = id++; add_marker(twist_marker_ptr); } + + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.5); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + } } 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 fe057268db918..7c9d7a3047de2 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 @@ -138,6 +138,59 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + marker_ptr->ns = std::string("twist covariance"); + marker_ptr->scale.x = line_width; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + geometry_msgs::msg::Point point; + + // velocity covariance + // extract eigen values and eigen vectors + + // yaw rate covariance + const double yaw_rate_covariance = twist_with_covariance.covariance[35]; + const double yaw_rate_sigma = std::sqrt(yaw_rate_covariance); // 2.448 sigma is 95% + const double yaw_rate = twist_with_covariance.twist.angular.z; + const double velocity = std::sqrt( + twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y + + twist_with_covariance.twist.linear.z * twist_with_covariance.twist.linear.z); + const double velocity_angle = + std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + + point.x = velocity * std::cos(velocity_angle + yaw_rate); + point.y = velocity * std::sin(velocity_angle + yaw_rate); + point.z = 0; + marker_ptr->points.push_back(point); + point.x = velocity * std::cos(velocity_angle + yaw_rate + yaw_rate_sigma); + point.y = velocity * std::sin(velocity_angle + yaw_rate + yaw_rate_sigma); + point.z = 0; + marker_ptr->points.push_back(point); + + point.x = velocity * std::cos(velocity_angle + yaw_rate); + point.y = velocity * std::sin(velocity_angle + yaw_rate); + point.z = 0; + marker_ptr->points.push_back(point); + point.x = velocity * std::cos(velocity_angle + yaw_rate - yaw_rate_sigma); + point.y = velocity * std::sin(velocity_angle + yaw_rate - yaw_rate_sigma); + point.z = 0; + marker_ptr->points.push_back(point); + + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color.a = 0.9; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 0.0; + marker_ptr->color.b = 0.2; + + return marker_ptr; +} + visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, const std_msgs::msg::ColorRGBA & color_rgba) 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 41973819bf8a6..468139bebd09f 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 @@ -113,7 +113,7 @@ std::vector PredictedObjectsDisplay: // Get marker for pose with covariance auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr( - object.kinematics.initial_pose_with_covariance, get_line_width() / 2); + object.kinematics.initial_pose_with_covariance, get_line_width() * 0.5); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); marker_ptr->header = msg->header; @@ -123,7 +123,7 @@ std::vector PredictedObjectsDisplay: // Get marker for yaw covariance auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( - object.kinematics.initial_pose_with_covariance, object.shape.dimensions.x / 1.6, get_line_width() / 2); + object.kinematics.initial_pose_with_covariance, object.shape.dimensions.x * 0.65, get_line_width() * 0.5); if (yaw_covariance_marker) { auto marker_ptr = yaw_covariance_marker.value(); marker_ptr->header = msg->header; @@ -191,6 +191,17 @@ std::vector PredictedObjectsDisplay: markers.push_back(twist_marker_ptr); } + // Get marker for twist + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance, get_line_width() * 0.5); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + // Add marker for each candidate path int32_t path_count = 0; for (const auto & predicted_path : object.kinematics.predicted_paths) { diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 390f3db4a3f40..2f1c2c72f3330 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -97,7 +97,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) // Get marker for pose with covariance auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr( - object.kinematics.pose_with_covariance, get_line_width() / 2); + object.kinematics.pose_with_covariance, get_line_width() * 0.5); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); marker_ptr->header = msg->header; @@ -107,7 +107,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) // Get marker for yaw covariance auto yaw_covariance_marker = - get_yaw_covariance_marker_ptr(object.kinematics.pose_with_covariance, object.shape.dimensions.x / 1.6, get_line_width() / 2); + get_yaw_covariance_marker_ptr(object.kinematics.pose_with_covariance, object.shape.dimensions.x * 0.65, get_line_width() * 0.5); if (yaw_covariance_marker) { auto marker_ptr = yaw_covariance_marker.value(); marker_ptr->header = msg->header; @@ -169,6 +169,18 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) twist_marker_ptr->id = uuid_to_marker_id(object.object_id); add_marker(twist_marker_ptr); } + + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.5); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + } } From cd34c13fa91311697ac55bceac42c40abeb597f6 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 11 Jan 2024 13:25:22 +0900 Subject: [PATCH 05/14] feat: discrete yaw rate marker Signed-off-by: Taekjin LEE --- .../object_polygon_detail.hpp | 8 +- .../object_polygon_display_base.hpp | 28 ++++- .../detected_objects_display.cpp | 17 ++- .../object_polygon_detail.cpp | 114 ++++++++++++------ .../predicted_objects_display.cpp | 18 ++- .../tracked_objects_display.cpp | 17 ++- 6 files changed, 148 insertions(+), 54 deletions(-) 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 ae0e033a62f3c..3ed4b13918b23 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 @@ -118,7 +118,8 @@ get_pose_with_covariance_marker_ptr( AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, const double & line_width); + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, + const double & line_width); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( @@ -135,6 +136,11 @@ get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 2185dcf4c6b16..0f913f74d1c77 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -73,7 +73,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_display_acceleration_text_property{ "Display Acceleration", true, "Enable/disable acceleration text visualization", this}, m_display_twist_property{"Display Twist", true, "Enable/disable twist visualization", this}, - m_display_twist_covariance_property{"Display Twist Covariance", false, "Enable/disable twist covariance visualization", this}, + m_display_twist_covariance_property{ + "Display Twist Covariance", false, "Enable/disable twist covariance visualization", this}, + m_display_yaw_rate_property{ + "Display Yaw Rate", false, "Enable/disable yaw rate visualization", this}, m_display_predicted_paths_property{ "Display Predicted Paths", true, "Enable/disable predicted paths visualization", this}, m_display_path_confidence_property{ @@ -252,8 +255,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } std::optional get_yaw_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const double & length, + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, const double & line_width) const { if (m_display_yaw_covariance_property.getBool()) { @@ -301,13 +303,27 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } + std::optional get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & line_width) const + { + if (m_display_yaw_rate_property.getBool()) { + return detail::get_yaw_rate_marker_ptr( + pose_with_covariance, twist_with_covariance, line_width); + } else { + return std::nullopt; + } + } + std::optional get_twist_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width) const { if (m_display_twist_covariance_property.getBool()) { - return detail::get_twist_covariance_marker_ptr(pose_with_covariance, twist_with_covariance, line_width); + return detail::get_twist_covariance_marker_ptr( + pose_with_covariance, twist_with_covariance, line_width); } else { return std::nullopt; } @@ -460,7 +476,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::BoolProperty m_display_acceleration_text_property; // Property to enable/disable twist visualization rviz_common::properties::BoolProperty m_display_twist_property; - // Property to enable/disable twist visualization + // Property to enable/disable yaw rate visualization + rviz_common::properties::BoolProperty m_display_yaw_rate_property; + // Property to enable/disable twist covariance visualization rviz_common::properties::BoolProperty m_display_twist_covariance_property; // Property to enable/disable predicted paths visualization rviz_common::properties::BoolProperty m_display_predicted_paths_property; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 899a790d6fc61..ec9ad8dcbdc3f 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -71,7 +71,8 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) // Get marker for yaw covariance auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( - object.kinematics.pose_with_covariance, object.shape.dimensions.x * 0.65, get_line_width() * 0.5); + object.kinematics.pose_with_covariance, object.shape.dimensions.x * 0.65, + get_line_width() * 0.5); if (yaw_covariance_marker) { auto marker_ptr = yaw_covariance_marker.value(); marker_ptr->header = msg->header; @@ -120,17 +121,27 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(twist_marker_ptr); } + // Get marker for twist + auto yaw_rate_marker = get_yaw_rate_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.4); + if (yaw_rate_marker) { + auto marker_ptr = yaw_rate_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + // Get marker for twist covariance auto twist_covariance_marker = get_twist_covariance_marker_ptr( object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, - get_line_width() * 0.5); + get_line_width() * 0.3); if (twist_covariance_marker) { auto marker_ptr = twist_covariance_marker.value(); marker_ptr->header = msg->header; marker_ptr->id = id++; add_marker(marker_ptr); } - } } 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 7c9d7a3047de2..9c66ad2232c04 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 @@ -101,7 +101,7 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; - // velocity + // velocity line geometry_msgs::msg::Point point; point.x = 0.0; point.y = 0.0; @@ -112,6 +112,26 @@ 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->color.a = 0.999; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 0.0; + marker_ptr->color.b = 0.0; + + return marker_ptr; +} + +visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_STRIP; + marker_ptr->ns = std::string("yaw rate"); + marker_ptr->scale.x = line_width; + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + // yaw rate const double yaw_rate = twist_with_covariance.twist.angular.z; const double velocity = std::sqrt( @@ -120,17 +140,20 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( twist_with_covariance.twist.linear.z * twist_with_covariance.twist.linear.z); const double velocity_angle = std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); - point.x = twist_with_covariance.twist.linear.x; - point.y = twist_with_covariance.twist.linear.y; - point.z = twist_with_covariance.twist.linear.z; - marker_ptr->points.push_back(point); - point.x = velocity * std::cos(velocity_angle + yaw_rate); - point.y = velocity * std::sin(velocity_angle + yaw_rate); + const double yaw_mark_length = velocity * 0.8; + + // yaw rate arc + calc_arc_line_strip( + velocity_angle, velocity_angle + yaw_rate, yaw_mark_length, marker_ptr->points); + // last point + geometry_msgs::msg::Point point; + point.x = 0; + point.y = 0; point.z = 0; marker_ptr->points.push_back(point); marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); - marker_ptr->color.a = 0.999; + marker_ptr->color.a = 0.9; marker_ptr->color.r = 1.0; marker_ptr->color.g = 0.0; marker_ptr->color.b = 0.0; @@ -163,30 +186,41 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( twist_with_covariance.twist.linear.z * twist_with_covariance.twist.linear.z); const double velocity_angle = std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); - - point.x = velocity * std::cos(velocity_angle + yaw_rate); - point.y = velocity * std::sin(velocity_angle + yaw_rate); - point.z = 0; - marker_ptr->points.push_back(point); - point.x = velocity * std::cos(velocity_angle + yaw_rate + yaw_rate_sigma); - point.y = velocity * std::sin(velocity_angle + yaw_rate + yaw_rate_sigma); - point.z = 0; - marker_ptr->points.push_back(point); - - point.x = velocity * std::cos(velocity_angle + yaw_rate); - point.y = velocity * std::sin(velocity_angle + yaw_rate); - point.z = 0; - marker_ptr->points.push_back(point); - point.x = velocity * std::cos(velocity_angle + yaw_rate - yaw_rate_sigma); - point.y = velocity * std::sin(velocity_angle + yaw_rate - yaw_rate_sigma); - point.z = 0; - marker_ptr->points.push_back(point); + const double yaw_mark_length = velocity * 0.8; + const double bar_width = std::max(velocity * 0.05, 0.1); + const double velocity_yaw_angle = velocity_angle + yaw_rate; + const double velocity_yaw_p_sigma_angle = velocity_yaw_angle + yaw_rate_sigma; + const double velocity_yaw_n_sigma_angle = velocity_yaw_angle - yaw_rate_sigma; + + const double point_list[7][3] = { + {yaw_mark_length * std::cos(velocity_yaw_angle), yaw_mark_length * std::sin(velocity_yaw_angle), + 0}, + {yaw_mark_length * std::cos(velocity_yaw_p_sigma_angle), + yaw_mark_length * std::sin(velocity_yaw_p_sigma_angle), 0}, + {yaw_mark_length * std::cos(velocity_yaw_n_sigma_angle), + yaw_mark_length * std::sin(velocity_yaw_n_sigma_angle), 0}, + {(yaw_mark_length + bar_width) * std::cos(velocity_yaw_p_sigma_angle), + (yaw_mark_length + bar_width) * std::sin(velocity_yaw_p_sigma_angle), 0}, + {(yaw_mark_length - bar_width) * std::cos(velocity_yaw_p_sigma_angle), + (yaw_mark_length - bar_width) * std::sin(velocity_yaw_p_sigma_angle), 0}, + {(yaw_mark_length + bar_width) * std::cos(velocity_yaw_n_sigma_angle), + (yaw_mark_length + bar_width) * std::sin(velocity_yaw_n_sigma_angle), 0}, + {(yaw_mark_length - bar_width) * std::cos(velocity_yaw_n_sigma_angle), + (yaw_mark_length - bar_width) * std::sin(velocity_yaw_n_sigma_angle), 0}, + }; + const int point_pairs[4][2] = { + {0, 1}, + {0, 2}, + {3, 4}, + {5, 6}, + }; + calc_line_list_from_points(point_list, point_pairs, 4, marker_ptr->points); marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color.a = 0.9; marker_ptr->color.r = 1.0; - marker_ptr->color.g = 0.0; - marker_ptr->color.b = 0.2; + marker_ptr->color.g = 0.5; + marker_ptr->color.b = 0.7; return marker_ptr; } @@ -238,11 +272,6 @@ void calc_arc_line_strip( std::vector & points) { geometry_msgs::msg::Point point; - // fisrt point - point.x = 0; - point.y = 0; - point.z = 0; - points.push_back(point); // arc points const double maximum_delta_angle = 10.0 * M_PI / 180.0; const int num_points = @@ -255,11 +284,6 @@ void calc_arc_line_strip( point.z = 0; points.push_back(point); } - // last point - point.x = 0; - point.y = 0; - point.z = 0; - points.push_back(point); } visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( @@ -315,7 +339,8 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, const double & line_width) + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, + const double & line_width) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::LINE_STRIP; @@ -332,7 +357,18 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( if (yaw_sigma > M_PI) { yaw_vector_length = 1.0; } + // first point + point.x = 0; + point.y = 0; + point.z = 0; + marker_ptr->points.push_back(point); + // arc points calc_arc_line_strip(-yaw_sigma, yaw_sigma, yaw_vector_length, marker_ptr->points); + // last point + point.x = 0; + point.y = 0; + point.z = 0; + marker_ptr->points.push_back(point); // marker configuration marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); 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 468139bebd09f..2bd1d604bd72c 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 @@ -123,7 +123,8 @@ std::vector PredictedObjectsDisplay: // Get marker for yaw covariance auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( - object.kinematics.initial_pose_with_covariance, object.shape.dimensions.x * 0.65, get_line_width() * 0.5); + object.kinematics.initial_pose_with_covariance, object.shape.dimensions.x * 0.65, + get_line_width() * 0.5); if (yaw_covariance_marker) { auto marker_ptr = yaw_covariance_marker.value(); marker_ptr->header = msg->header; @@ -191,10 +192,21 @@ std::vector PredictedObjectsDisplay: markers.push_back(twist_marker_ptr); } - // Get marker for twist + // Get marker for yaw rate + auto yaw_rate_marker = get_yaw_rate_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance, get_line_width() * 0.4); + if (yaw_rate_marker) { + 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); + } + + // Get marker for twist covariance auto twist_covariance_marker = get_twist_covariance_marker_ptr( object.kinematics.initial_pose_with_covariance, - object.kinematics.initial_twist_with_covariance, get_line_width() * 0.5); + object.kinematics.initial_twist_with_covariance, get_line_width() * 0.3); if (twist_covariance_marker) { auto marker_ptr = twist_covariance_marker.value(); marker_ptr->header = msg->header; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 2f1c2c72f3330..2cf469cde51b4 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -106,8 +106,9 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) } // Get marker for yaw covariance - auto yaw_covariance_marker = - get_yaw_covariance_marker_ptr(object.kinematics.pose_with_covariance, object.shape.dimensions.x * 0.65, get_line_width() * 0.5); + auto yaw_covariance_marker = get_yaw_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.shape.dimensions.x * 0.65, + get_line_width() * 0.5); if (yaw_covariance_marker) { auto marker_ptr = yaw_covariance_marker.value(); marker_ptr->header = msg->header; @@ -170,6 +171,17 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(twist_marker_ptr); } + // Get marker for yaw rate + auto yaw_rate_marker = get_yaw_rate_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, + get_line_width() * 0.4); + if (yaw_rate_marker) { + 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); + } + // Get marker for twist covariance auto twist_covariance_marker = get_twist_covariance_marker_ptr( object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, @@ -180,7 +192,6 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) marker_ptr->id = uuid_to_marker_id(object.object_id); add_marker(marker_ptr); } - } } From 2e6c56c76eaa6b0b70aa2b2cb94222b8f91d2abf Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 11 Jan 2024 13:30:37 +0900 Subject: [PATCH 06/14] fix: property order Signed-off-by: Taekjin LEE --- .../object_detection/object_polygon_display_base.hpp | 4 ++-- .../src/object_detection/object_polygon_detail.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 0f913f74d1c77..577e49221a050 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -476,10 +476,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::BoolProperty m_display_acceleration_text_property; // Property to enable/disable twist visualization rviz_common::properties::BoolProperty m_display_twist_property; - // Property to enable/disable yaw rate visualization - rviz_common::properties::BoolProperty m_display_yaw_rate_property; // Property to enable/disable twist covariance visualization rviz_common::properties::BoolProperty m_display_twist_covariance_property; + // Property to enable/disable yaw rate visualization + rviz_common::properties::BoolProperty m_display_yaw_rate_property; // Property to enable/disable predicted paths visualization rviz_common::properties::BoolProperty m_display_predicted_paths_property; // Property to enable/disable predicted path confidence visualization 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 9c66ad2232c04..e4d00af9fb71f 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 @@ -219,8 +219,8 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color.a = 0.9; marker_ptr->color.r = 1.0; - marker_ptr->color.g = 0.5; - marker_ptr->color.b = 0.7; + marker_ptr->color.g = 0.2; + marker_ptr->color.b = 0.4; return marker_ptr; } From 876609fab5ca742851fa825dad18ae5d1e9c7f31 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 11 Jan 2024 16:18:37 +0900 Subject: [PATCH 07/14] chore: parameter order change Signed-off-by: Taekjin LEE --- .../object_polygon_display_base.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 577e49221a050..818cebfb63e8c 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -64,14 +64,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // 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{ - "Display Pose Covariance", true, "Enable/disable pose covariance visualization", this}, - m_display_yaw_covariance_property{ - "Display Yaw Covariance", false, "Enable/disable yaw covariance visualization", this}, m_display_velocity_text_property{ "Display Velocity", true, "Enable/disable velocity text visualization", this}, m_display_acceleration_text_property{ "Display Acceleration", true, "Enable/disable acceleration text visualization", this}, + m_display_pose_with_covariance_property{ + "Display Pose Covariance", true, "Enable/disable pose covariance visualization", this}, + m_display_yaw_covariance_property{ + "Display Yaw Covariance", false, "Enable/disable yaw covariance visualization", this}, m_display_twist_property{"Display Twist", true, "Enable/disable twist visualization", this}, m_display_twist_covariance_property{ "Display Twist Covariance", false, "Enable/disable twist covariance visualization", this}, @@ -466,14 +466,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::BoolProperty m_display_label_property; // Property to enable/disable uuid visualization rviz_common::properties::BoolProperty m_display_uuid_property; - // Property to enable/disable pose with covariance visualization - rviz_common::properties::BoolProperty m_display_pose_with_covariance_property; - // Property to enable/disable yaw covariance visualization - rviz_common::properties::BoolProperty m_display_yaw_covariance_property; // Property to enable/disable velocity text visualization rviz_common::properties::BoolProperty m_display_velocity_text_property; // Property to enable/disable acceleration text visualization rviz_common::properties::BoolProperty m_display_acceleration_text_property; + // Property to enable/disable pose with covariance visualization + rviz_common::properties::BoolProperty m_display_pose_with_covariance_property; + // Property to enable/disable yaw covariance visualization + rviz_common::properties::BoolProperty m_display_yaw_covariance_property; // Property to enable/disable twist visualization rviz_common::properties::BoolProperty m_display_twist_property; // Property to enable/disable twist covariance visualization From a73debddc783bc657efa4abe5d190a6f57a40f4a Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 12 Jan 2024 16:44:16 +0900 Subject: [PATCH 08/14] feat: position covariance as ellipse Signed-off-by: Taekjin LEE --- .../object_polygon_detail.hpp | 6 ++ .../object_polygon_detail.cpp | 61 ++++++++++--------- 2 files changed, 39 insertions(+), 28 deletions(-) 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 3ed4b13918b23..12e4c84eeb2b0 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 @@ -20,6 +20,9 @@ #include #include +#include +#include + #include #include #include @@ -165,6 +168,9 @@ 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 & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors( + const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_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 e4d00af9fb71f..e329d9e60012d 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 @@ -286,20 +286,28 @@ void calc_arc_line_strip( } } +void calc_covariance_eigen_vectors( + const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw) +{ + Eigen::SelfAdjointEigenSolver solver(matrix); + Eigen::Vector2d eigen_values = solver.eigenvalues(); + // eigen values + sigma1 = std::sqrt(eigen_values.x()); + sigma2 = std::sqrt(eigen_values.y()); + // orientation of covariance ellipse + Eigen::Vector2d e1 = solver.eigenvectors().col(0); + yaw = std::atan2(e1.y(), e1.x()); +} + visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & line_width) { auto marker_ptr = std::make_shared(); - marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER; marker_ptr->ns = std::string("position covariance"); marker_ptr->scale.x = line_width; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; - marker_ptr->pose.orientation.x = 0.0; - marker_ptr->pose.orientation.y = 0.0; - marker_ptr->pose.orientation.z = 0.0; - marker_ptr->pose.orientation.w = 1.0; - geometry_msgs::msg::Point point; // position covariance // extract eigen values and eigen vectors @@ -307,31 +315,28 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( eigen_pose_with_covariance << pose_with_covariance.covariance[0], pose_with_covariance.covariance[1], pose_with_covariance.covariance[6], pose_with_covariance.covariance[7]; - Eigen::SelfAdjointEigenSolver solver(eigen_pose_with_covariance); - double sigma1 = 2.448 * std::sqrt(solver.eigenvalues().x()); // 2.448 sigma is 95% - double sigma2 = 2.448 * std::sqrt(solver.eigenvalues().y()); // 2.448 sigma is 95% - Eigen::Vector2d e1 = solver.eigenvectors().col(0); - Eigen::Vector2d e2 = solver.eigenvectors().col(1); - point.x = -e1.x() * sigma1; - point.y = -e1.y() * sigma1; - point.z = 0; - marker_ptr->points.push_back(point); - point.x = e1.x() * sigma1; - point.y = e1.y() * sigma1; - point.z = 0; - marker_ptr->points.push_back(point); - point.x = -e2.x() * sigma2; - point.y = -e2.y() * sigma2; - point.z = 0; - marker_ptr->points.push_back(point); - point.x = e2.x() * sigma2; - point.y = e2.y() * sigma2; - point.z = 0; - marker_ptr->points.push_back(point); + double yaw, sigma1, sigma2; + calc_covariance_eigen_vectors(eigen_pose_with_covariance, sigma1, sigma2, yaw); + + // 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); + + // ellipse size + marker_ptr->scale.x = sigma1 * 2.448; // 2.448 sigma is 95% + marker_ptr->scale.y = sigma2 * 2.448; // 2.448 sigma is 95% + marker_ptr->scale.z = 0.05; + + // ellipse color density + double area = sigma1 * sigma2; + double alpha = std::min(0.5, 3.0 / area); + alpha = std::max(0.1, alpha); // marker configuration marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); - marker_ptr->color.a = 0.999; + marker_ptr->color.a = alpha; marker_ptr->color.r = 1.0; marker_ptr->color.g = 1.0; marker_ptr->color.b = 1.0; From bde133d11ecda1f091dd9740b8d3522e5b999dc2 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 12 Jan 2024 17:56:39 +0900 Subject: [PATCH 09/14] feat: twist covariance Signed-off-by: Taekjin LEE --- .../object_polygon_detail.hpp | 9 ++- .../object_polygon_display_base.hpp | 23 +++++- .../detected_objects_display.cpp | 22 ++++-- .../object_polygon_detail.cpp | 78 +++++++++++++++---- .../predicted_objects_display.cpp | 19 ++++- .../tracked_objects_display.cpp | 18 ++++- 6 files changed, 135 insertions(+), 34 deletions(-) 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 12e4c84eeb2b0..04ec9aa8221e0 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 @@ -117,7 +117,7 @@ get_uuid_marker_ptr( AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & line_width); + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( @@ -139,13 +139,18 @@ get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_twist_covariance_marker_ptr( +get_yaw_rate_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 818cebfb63e8c..4dc308a9cc35e 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -77,6 +77,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase "Display Twist Covariance", false, "Enable/disable twist covariance visualization", this}, m_display_yaw_rate_property{ "Display Yaw Rate", false, "Enable/disable yaw rate visualization", this}, + m_display_yaw_rate_covariance_property{ + "Display Yaw Rate Covariance", false, "Enable/disable yaw rate covariance visualization", this}, m_display_predicted_paths_property{ "Display Predicted Paths", true, "Enable/disable predicted paths visualization", this}, m_display_path_confidence_property{ @@ -303,6 +305,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } + std::optional get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & line_width) const + { + if (m_display_twist_covariance_property.getBool()) { + return detail::get_twist_covariance_marker_ptr( + pose_with_covariance, twist_with_covariance, line_width); + } else { + return std::nullopt; + } + } + std::optional get_yaw_rate_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, @@ -316,13 +331,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - std::optional get_twist_covariance_marker_ptr( + std::optional get_yaw_rate_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width) const { - if (m_display_twist_covariance_property.getBool()) { - return detail::get_twist_covariance_marker_ptr( + if (m_display_yaw_rate_covariance_property.getBool()) { + return detail::get_yaw_rate_covariance_marker_ptr( pose_with_covariance, twist_with_covariance, line_width); } else { return std::nullopt; @@ -480,6 +495,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::BoolProperty m_display_twist_covariance_property; // Property to enable/disable yaw rate visualization rviz_common::properties::BoolProperty m_display_yaw_rate_property; + // Property to enable/disable yaw rate covariance visualization + rviz_common::properties::BoolProperty m_display_yaw_rate_covariance_property; // Property to enable/disable predicted paths visualization rviz_common::properties::BoolProperty m_display_predicted_paths_property; // Property to enable/disable predicted path confidence visualization diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index ec9ad8dcbdc3f..463340d621740 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -61,7 +61,7 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) // Get marker for pose with covariance auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr( - object.kinematics.pose_with_covariance, get_line_width() * 0.5); + object.kinematics.pose_with_covariance); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); marker_ptr->header = msg->header; @@ -121,7 +121,17 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(twist_marker_ptr); } - // Get marker for twist + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = id++; + add_marker(marker_ptr); + } + + // Get marker for yaw rate auto yaw_rate_marker = get_yaw_rate_marker_ptr( object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, get_line_width() * 0.4); @@ -132,12 +142,12 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(marker_ptr); } - // Get marker for twist covariance - auto twist_covariance_marker = get_twist_covariance_marker_ptr( + // Get marker for yaw rate covariance + auto yaw_rate_covariance_marker = get_yaw_rate_covariance_marker_ptr( object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, get_line_width() * 0.3); - if (twist_covariance_marker) { - auto marker_ptr = twist_covariance_marker.value(); + if (yaw_rate_covariance_marker) { + auto marker_ptr = yaw_rate_covariance_marker.value(); marker_ptr->header = msg->header; marker_ptr->id = id++; add_marker(marker_ptr); 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 e329d9e60012d..4c7379b086f4a 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 @@ -121,6 +121,59 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER; + marker_ptr->ns = std::string("twist covariance"); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = pose_with_covariance.pose; + + // position is the tip of the velocity vector + const double velocity = std::sqrt( + twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + + twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y); + const double velocity_angle = + std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); + // pose yaw angle from quaternion (x, y, z, w) + const double pos_yaw_angle = 2.0 * std::atan2( + pose_with_covariance.pose.orientation.z, + pose_with_covariance.pose.orientation.w); // [rad] + marker_ptr->pose.position.x += velocity * std::cos(pos_yaw_angle + velocity_angle); + marker_ptr->pose.position.y += velocity * std::sin(pos_yaw_angle + velocity_angle); + + // velocity covariance + // extract eigen values and eigen vectors + Eigen::Matrix2d eigen_twist_covariance; + eigen_twist_covariance << twist_with_covariance.covariance[0], + twist_with_covariance.covariance[1], twist_with_covariance.covariance[6], + twist_with_covariance.covariance[7]; + double phi, sigma1, sigma2; + calc_covariance_eigen_vectors(eigen_twist_covariance, sigma1, sigma2, phi); + phi = pos_yaw_angle + phi; + + // 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); + + // ellipse size + marker_ptr->scale.x = sigma1 * 2.448; // 2.448 sigma is 95% + marker_ptr->scale.y = sigma2 * 2.448; // 2.448 sigma is 95% + marker_ptr->scale.z = 0.05; + + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); + marker_ptr->color.a = 0.5; + marker_ptr->color.r = 1.0; + marker_ptr->color.g = 0.2; + marker_ptr->color.b = 0.4; + + return marker_ptr; +} + visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width) @@ -161,20 +214,16 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( return marker_ptr; } -visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( +visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; - marker_ptr->ns = std::string("twist covariance"); + marker_ptr->ns = std::string("yaw rate covariance"); marker_ptr->scale.x = line_width; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; - geometry_msgs::msg::Point point; - - // velocity covariance - // extract eigen values and eigen vectors // yaw rate covariance const double yaw_rate_covariance = twist_with_covariance.covariance[35]; @@ -300,29 +349,28 @@ void calc_covariance_eigen_vectors( } visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & line_width) + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER; marker_ptr->ns = std::string("position covariance"); - marker_ptr->scale.x = line_width; marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = pose_with_covariance.pose; // position covariance // extract eigen values and eigen vectors - Eigen::Matrix2d eigen_pose_with_covariance; - eigen_pose_with_covariance << pose_with_covariance.covariance[0], + Eigen::Matrix2d eigen_pose_covariance; + eigen_pose_covariance << pose_with_covariance.covariance[0], pose_with_covariance.covariance[1], pose_with_covariance.covariance[6], pose_with_covariance.covariance[7]; double yaw, sigma1, sigma2; - calc_covariance_eigen_vectors(eigen_pose_with_covariance, sigma1, sigma2, yaw); + calc_covariance_eigen_vectors(eigen_pose_covariance, sigma1, sigma2, yaw); // 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.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); // ellipse size marker_ptr->scale.x = sigma1 * 2.448; // 2.448 sigma is 95% 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 2bd1d604bd72c..850896565e332 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 @@ -113,7 +113,7 @@ std::vector PredictedObjectsDisplay: // Get marker for pose with covariance auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr( - object.kinematics.initial_pose_with_covariance, get_line_width() * 0.5); + object.kinematics.initial_pose_with_covariance); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); marker_ptr->header = msg->header; @@ -192,6 +192,17 @@ std::vector PredictedObjectsDisplay: markers.push_back(twist_marker_ptr); } + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.initial_pose_with_covariance, + object.kinematics.initial_twist_with_covariance); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + markers.push_back(marker_ptr); + } + // Get marker for yaw rate auto yaw_rate_marker = get_yaw_rate_marker_ptr( object.kinematics.initial_pose_with_covariance, @@ -204,11 +215,11 @@ std::vector PredictedObjectsDisplay: } // Get marker for twist covariance - auto twist_covariance_marker = get_twist_covariance_marker_ptr( + auto yaw_rate_covariance_marker = get_yaw_rate_covariance_marker_ptr( object.kinematics.initial_pose_with_covariance, object.kinematics.initial_twist_with_covariance, get_line_width() * 0.3); - if (twist_covariance_marker) { - auto marker_ptr = twist_covariance_marker.value(); + if (yaw_rate_covariance_marker) { + auto marker_ptr = yaw_rate_covariance_marker.value(); marker_ptr->header = msg->header; marker_ptr->id = uuid_to_marker_id(object.object_id); markers.push_back(marker_ptr); diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 2cf469cde51b4..d4acf7167502e 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -97,7 +97,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) // Get marker for pose with covariance auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr( - object.kinematics.pose_with_covariance, get_line_width() * 0.5); + object.kinematics.pose_with_covariance); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); marker_ptr->header = msg->header; @@ -171,6 +171,16 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(twist_marker_ptr); } + // Get marker for twist covariance + auto twist_covariance_marker = get_twist_covariance_marker_ptr( + object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance); + if (twist_covariance_marker) { + auto marker_ptr = twist_covariance_marker.value(); + marker_ptr->header = msg->header; + marker_ptr->id = uuid_to_marker_id(object.object_id); + add_marker(marker_ptr); + } + // Get marker for yaw rate auto yaw_rate_marker = get_yaw_rate_marker_ptr( object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, @@ -183,11 +193,11 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) } // Get marker for twist covariance - auto twist_covariance_marker = get_twist_covariance_marker_ptr( + auto yaw_rate_covariance_marker = get_yaw_rate_covariance_marker_ptr( object.kinematics.pose_with_covariance, object.kinematics.twist_with_covariance, get_line_width() * 0.5); - if (twist_covariance_marker) { - auto marker_ptr = twist_covariance_marker.value(); + if (yaw_rate_covariance_marker) { + auto marker_ptr = yaw_rate_covariance_marker.value(); marker_ptr->header = msg->header; marker_ptr->id = uuid_to_marker_id(object.object_id); add_marker(marker_ptr); From d38b582f46685bc7a5ef5b7367740285113fecea Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 12 Jan 2024 18:10:32 +0900 Subject: [PATCH 10/14] chore: pose covariance marker renamed Signed-off-by: Taekjin LEE --- .../object_detection/object_polygon_detail.hpp | 2 +- .../object_polygon_display_base.hpp | 18 ++++++++---------- .../detected_objects_display.cpp | 4 ++-- .../object_detection/object_polygon_detail.cpp | 15 +++++++++++---- .../predicted_objects_display.cpp | 4 ++-- .../tracked_objects_display.cpp | 4 ++-- 6 files changed, 26 insertions(+), 21 deletions(-) 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 04ec9aa8221e0..1c889490cfae5 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 @@ -116,7 +116,7 @@ get_uuid_marker_ptr( const std_msgs::msg::ColorRGBA & color_rgba); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_pose_with_covariance_marker_ptr( +get_pose_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 4dc308a9cc35e..429a8fdbf0a09 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -68,7 +68,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase "Display Velocity", true, "Enable/disable velocity text visualization", this}, m_display_acceleration_text_property{ "Display Acceleration", true, "Enable/disable acceleration text visualization", this}, - m_display_pose_with_covariance_property{ + m_display_pose_covariance_property{ "Display Pose Covariance", true, "Enable/disable pose covariance visualization", this}, m_display_yaw_covariance_property{ "Display Yaw Covariance", false, "Enable/disable yaw covariance visualization", this}, @@ -245,12 +245,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - std::optional get_pose_with_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const double & line_width) const + std::optional get_pose_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) const { - if (m_display_pose_with_covariance_property.getBool()) { - return detail::get_pose_with_covariance_marker_ptr(pose_with_covariance, line_width); + if (m_display_pose_covariance_property.getBool()) { + return detail::get_pose_covariance_marker_ptr(pose_with_covariance); } else { return std::nullopt; } @@ -307,12 +306,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_twist_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, - const double & line_width) const + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const { if (m_display_twist_covariance_property.getBool()) { return detail::get_twist_covariance_marker_ptr( - pose_with_covariance, twist_with_covariance, line_width); + pose_with_covariance, twist_with_covariance); } else { return std::nullopt; } @@ -486,7 +484,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // Property to enable/disable acceleration text visualization rviz_common::properties::BoolProperty m_display_acceleration_text_property; // Property to enable/disable pose with covariance visualization - rviz_common::properties::BoolProperty m_display_pose_with_covariance_property; + rviz_common::properties::BoolProperty m_display_pose_covariance_property; // Property to enable/disable yaw covariance visualization rviz_common::properties::BoolProperty m_display_yaw_covariance_property; // Property to enable/disable twist visualization diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 463340d621740..8b2a207606cc3 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -59,8 +59,8 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(label_marker_ptr); } - // Get marker for pose with covariance - auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr( + // Get marker for pose covariance + auto pose_with_covariance_marker = get_pose_covariance_marker_ptr( object.kinematics.pose_with_covariance); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); 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 4c7379b086f4a..9dfe17ad3806c 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 @@ -137,7 +137,6 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( twist_with_covariance.twist.linear.y * twist_with_covariance.twist.linear.y); const double velocity_angle = std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); - // pose yaw angle from quaternion (x, y, z, w) const double pos_yaw_angle = 2.0 * std::atan2( pose_with_covariance.pose.orientation.z, pose_with_covariance.pose.orientation.w); // [rad] @@ -153,6 +152,9 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( double phi, sigma1, sigma2; calc_covariance_eigen_vectors(eigen_twist_covariance, sigma1, sigma2, phi); phi = pos_yaw_angle + phi; + double area = sigma1 * sigma2; + double alpha = std::min(0.5, 1.0 / area); + alpha = std::max(0.05, alpha); // ellipse orientation marker_ptr->pose.orientation.x = 0.0; @@ -166,7 +168,7 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( marker_ptr->scale.z = 0.05; marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); - marker_ptr->color.a = 0.5; + marker_ptr->color.a = alpha; marker_ptr->color.r = 1.0; marker_ptr->color.g = 0.2; marker_ptr->color.b = 0.4; @@ -195,11 +197,16 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); const double yaw_mark_length = velocity * 0.8; + geometry_msgs::msg::Point point; + // first point + point.x = 0; + point.y = 0; + point.z = 0; + marker_ptr->points.push_back(point); // yaw rate arc calc_arc_line_strip( velocity_angle, velocity_angle + yaw_rate, yaw_mark_length, marker_ptr->points); // last point - geometry_msgs::msg::Point point; point.x = 0; point.y = 0; point.z = 0; @@ -348,7 +355,7 @@ void calc_covariance_eigen_vectors( yaw = std::atan2(e1.y(), e1.x()); } -visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr( +visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) { auto marker_ptr = std::make_shared(); 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 850896565e332..0ac6a6e60309a 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 @@ -111,8 +111,8 @@ std::vector PredictedObjectsDisplay: markers.push_back(id_marker_ptr); } - // Get marker for pose with covariance - auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr( + // Get marker for pose covariance + auto pose_with_covariance_marker = get_pose_covariance_marker_ptr( object.kinematics.initial_pose_with_covariance); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index d4acf7167502e..5b2bf53b372e5 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -95,8 +95,8 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(id_marker_ptr); } - // Get marker for pose with covariance - auto pose_with_covariance_marker = get_pose_with_covariance_marker_ptr( + // Get marker for pose covariance + auto pose_with_covariance_marker = get_pose_covariance_marker_ptr( object.kinematics.pose_with_covariance); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); From c844d1a7d3700bc92f4d4c6076236011b28b3f55 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Fri, 12 Jan 2024 18:50:25 +0900 Subject: [PATCH 11/14] fix: velocity covariance minimum alpha Signed-off-by: Taekjin LEE --- .../src/object_detection/object_polygon_detail.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 9dfe17ad3806c..61a28d9870b48 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 @@ -154,7 +154,7 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( phi = pos_yaw_angle + phi; double area = sigma1 * sigma2; double alpha = std::min(0.5, 1.0 / area); - alpha = std::max(0.05, alpha); + alpha = std::max(0.1, alpha); // ellipse orientation marker_ptr->pose.orientation.x = 0.0; From 14702fd8bee76eaa4ee2a56bce9cf556cb08312c Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 15 Jan 2024 10:48:59 +0900 Subject: [PATCH 12/14] chore: refactoring Signed-off-by: Taekjin LEE --- .../object_detection/object_polygon_detail.hpp | 8 +++----- .../object_polygon_display_base.hpp | 6 +++--- .../object_detection/detected_objects_display.cpp | 4 ++-- .../object_detection/object_polygon_detail.cpp | 15 +++++++-------- .../predicted_objects_display.cpp | 4 ++-- .../object_detection/tracked_objects_display.cpp | 4 ++-- 6 files changed, 19 insertions(+), 22 deletions(-) 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 1c889490cfae5..6d3a5ce99684d 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 @@ -17,11 +17,10 @@ #include "autoware_auto_perception_rviz_plugin/visibility_control.hpp" -#include -#include - #include #include +#include +#include #include #include @@ -116,8 +115,7 @@ get_uuid_marker_ptr( const std_msgs::msg::ColorRGBA & color_rgba); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_pose_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); +get_pose_covariance_marker_ptr(const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 429a8fdbf0a09..068f11dd9dbae 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -78,7 +78,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_display_yaw_rate_property{ "Display Yaw Rate", false, "Enable/disable yaw rate visualization", this}, m_display_yaw_rate_covariance_property{ - "Display Yaw Rate Covariance", false, "Enable/disable yaw rate covariance visualization", this}, + "Display Yaw Rate Covariance", false, "Enable/disable yaw rate covariance visualization", + this}, m_display_predicted_paths_property{ "Display Predicted Paths", true, "Enable/disable predicted paths visualization", this}, m_display_path_confidence_property{ @@ -309,8 +310,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const { if (m_display_twist_covariance_property.getBool()) { - return detail::get_twist_covariance_marker_ptr( - pose_with_covariance, twist_with_covariance); + return detail::get_twist_covariance_marker_ptr(pose_with_covariance, twist_with_covariance); } else { return std::nullopt; } diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 8b2a207606cc3..53e935fa1850a 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -60,8 +60,8 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) } // Get marker for pose covariance - auto pose_with_covariance_marker = get_pose_covariance_marker_ptr( - object.kinematics.pose_with_covariance); + auto pose_with_covariance_marker = + get_pose_covariance_marker_ptr(object.kinematics.pose_with_covariance); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); marker_ptr->header = msg->header; 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 61a28d9870b48..fe3ee1901d623 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 @@ -138,8 +138,8 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( const double velocity_angle = std::atan2(twist_with_covariance.twist.linear.y, twist_with_covariance.twist.linear.x); const double pos_yaw_angle = 2.0 * std::atan2( - pose_with_covariance.pose.orientation.z, - pose_with_covariance.pose.orientation.w); // [rad] + pose_with_covariance.pose.orientation.z, + pose_with_covariance.pose.orientation.w); // [rad] marker_ptr->pose.position.x += velocity * std::cos(pos_yaw_angle + velocity_angle); marker_ptr->pose.position.y += velocity * std::sin(pos_yaw_angle + velocity_angle); @@ -166,7 +166,7 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( marker_ptr->scale.x = sigma1 * 2.448; // 2.448 sigma is 95% marker_ptr->scale.y = sigma2 * 2.448; // 2.448 sigma is 95% marker_ptr->scale.z = 0.05; - + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); marker_ptr->color.a = alpha; marker_ptr->color.r = 1.0; @@ -234,7 +234,7 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr( // yaw rate covariance const double yaw_rate_covariance = twist_with_covariance.covariance[35]; - const double yaw_rate_sigma = std::sqrt(yaw_rate_covariance); // 2.448 sigma is 95% + const double yaw_rate_sigma = std::sqrt(yaw_rate_covariance) * 2.448; // 2.448 sigma is 95% const double yaw_rate = twist_with_covariance.twist.angular.z; const double velocity = std::sqrt( twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + @@ -367,9 +367,8 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( // position covariance // extract eigen values and eigen vectors Eigen::Matrix2d eigen_pose_covariance; - eigen_pose_covariance << pose_with_covariance.covariance[0], - pose_with_covariance.covariance[1], pose_with_covariance.covariance[6], - pose_with_covariance.covariance[7]; + eigen_pose_covariance << pose_with_covariance.covariance[0], pose_with_covariance.covariance[1], + pose_with_covariance.covariance[6], pose_with_covariance.covariance[7]; double yaw, sigma1, sigma2; calc_covariance_eigen_vectors(eigen_pose_covariance, sigma1, sigma2, yaw); @@ -412,7 +411,7 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( // orientation covariance double yaw_vector_length = std::max(length, 1.0); - double yaw_sigma = std::sqrt(pose_with_covariance.covariance[35]); // 2.448 sigma is 95% + double yaw_sigma = std::sqrt(pose_with_covariance.covariance[35]) * 2.448; // 2.448 sigma is 95% // get arc points if (yaw_sigma > M_PI) { yaw_vector_length = 1.0; 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 0ac6a6e60309a..24b21a15732c3 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 @@ -112,8 +112,8 @@ std::vector PredictedObjectsDisplay: } // Get marker for pose covariance - auto pose_with_covariance_marker = get_pose_covariance_marker_ptr( - object.kinematics.initial_pose_with_covariance); + auto pose_with_covariance_marker = + get_pose_covariance_marker_ptr(object.kinematics.initial_pose_with_covariance); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); marker_ptr->header = msg->header; diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 5b2bf53b372e5..504b51f850444 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -96,8 +96,8 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) } // Get marker for pose covariance - auto pose_with_covariance_marker = get_pose_covariance_marker_ptr( - object.kinematics.pose_with_covariance); + auto pose_with_covariance_marker = + get_pose_covariance_marker_ptr(object.kinematics.pose_with_covariance); if (pose_with_covariance_marker) { auto marker_ptr = pose_with_covariance_marker.value(); marker_ptr->header = msg->header; From b7660dbec00f306a42894e0890e515ad35b144df Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 15 Jan 2024 11:56:29 +0900 Subject: [PATCH 13/14] feat: add option for interval coefficients Signed-off-by: Taekjin LEE --- .../object_polygon_detail.hpp | 12 ++++-- .../object_polygon_display_base.hpp | 40 +++++++++++++++++-- .../object_polygon_detail.cpp | 24 ++++++----- 3 files changed, 58 insertions(+), 18 deletions(-) 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 6d3a5ce99684d..5f9da8531d2ab 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 @@ -115,12 +115,14 @@ get_uuid_marker_ptr( const std_msgs::msg::ColorRGBA & color_rgba); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr -get_pose_covariance_marker_ptr(const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); +get_pose_covariance_marker_ptr( + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const double & confidence_interval_coefficient); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, - const double & line_width); + const double & confidence_interval_coefficient, const double & line_width); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr( @@ -140,7 +142,8 @@ get_twist_marker_ptr( AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance); + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( @@ -150,7 +153,8 @@ get_yaw_rate_marker_ptr( AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width); + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient, const double & line_width); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 068f11dd9dbae..0a1d535896647 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -104,6 +104,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this); m_simple_visualize_mode_property->addOption("Normal", 0); m_simple_visualize_mode_property->addOption("Simple", 1); + // Confidence interval property + m_confidence_interval_property = new rviz_common::properties::EnumProperty( + "Confidence Interval", "95.45%, 2 sigma", "Confidence interval of state estimations.", this); + m_confidence_interval_property->addOption("68.27%, 1 sigma", 0); + m_confidence_interval_property->addOption("86.64%, 1.5 sigma", 1); + m_confidence_interval_property->addOption("95.45%, 2 sigma", 2); + m_confidence_interval_property->addOption("99.73%, 3 sigma", 3); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { @@ -250,7 +257,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) const { if (m_display_pose_covariance_property.getBool()) { - return detail::get_pose_covariance_marker_ptr(pose_with_covariance); + return detail::get_pose_covariance_marker_ptr( + pose_with_covariance, get_confidence_interval()); } else { return std::nullopt; } @@ -261,7 +269,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase const double & line_width) const { if (m_display_yaw_covariance_property.getBool()) { - return detail::get_yaw_covariance_marker_ptr(pose_with_covariance, length, line_width); + return detail::get_yaw_covariance_marker_ptr( + pose_with_covariance, length, get_confidence_interval(), line_width); } else { return std::nullopt; } @@ -310,7 +319,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const { if (m_display_twist_covariance_property.getBool()) { - return detail::get_twist_covariance_marker_ptr(pose_with_covariance, twist_with_covariance); + return detail::get_twist_covariance_marker_ptr( + pose_with_covariance, twist_with_covariance, get_confidence_interval()); } else { return std::nullopt; } @@ -336,7 +346,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase { if (m_display_yaw_rate_covariance_property.getBool()) { return detail::get_yaw_rate_covariance_marker_ptr( - pose_with_covariance, twist_with_covariance, line_width); + pose_with_covariance, twist_with_covariance, get_confidence_interval(), line_width); } else { return std::nullopt; } @@ -464,6 +474,26 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase double get_line_width() { return m_line_width_property.getFloat(); } + double get_confidence_interval() const + { + switch (m_confidence_interval_property->getOptionInt()) { + case 0: + // 68.27%, 1 sigma + return 1.0; + case 1: + // 86.64%, 1.5 sigma + return 1.5; + case 2: + // 95.45%, 2 sigma + return 2.0; + case 3: + // 99.73%, 3 sigma + return 3.0; + default: + return 2.0; + } + } + private: // All rviz plugins should have this. Should be initialized with pointer to this class MarkerCommon m_marker_common; @@ -475,6 +505,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::EnumProperty * m_display_type_property; // Property to choose simplicity of visualization polygon rviz_common::properties::EnumProperty * m_simple_visualize_mode_property; + // Property to set confidence interval of state estimations + rviz_common::properties::EnumProperty * m_confidence_interval_property; // Property to enable/disable label visualization rviz_common::properties::BoolProperty m_display_label_property; // Property to enable/disable uuid visualization 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 fe3ee1901d623..11a0bbbe57d53 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 @@ -123,7 +123,8 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER; @@ -163,8 +164,8 @@ visualization_msgs::msg::Marker::SharedPtr get_twist_covariance_marker_ptr( marker_ptr->pose.orientation.w = std::cos(phi / 2.0); // ellipse size - marker_ptr->scale.x = sigma1 * 2.448; // 2.448 sigma is 95% - marker_ptr->scale.y = sigma2 * 2.448; // 2.448 sigma is 95% + marker_ptr->scale.x = sigma1 * confidence_interval_coefficient; + marker_ptr->scale.y = sigma2 * confidence_interval_coefficient; marker_ptr->scale.z = 0.05; marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.5); @@ -223,7 +224,8 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, const double & line_width) + const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance, + const double & confidence_interval_coefficient, const double & line_width) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; @@ -234,7 +236,7 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_rate_covariance_marker_ptr( // yaw rate covariance const double yaw_rate_covariance = twist_with_covariance.covariance[35]; - const double yaw_rate_sigma = std::sqrt(yaw_rate_covariance) * 2.448; // 2.448 sigma is 95% + const double yaw_rate_sigma = std::sqrt(yaw_rate_covariance) * confidence_interval_coefficient; const double yaw_rate = twist_with_covariance.twist.angular.z; const double velocity = std::sqrt( twist_with_covariance.twist.linear.x * twist_with_covariance.twist.linear.x + @@ -356,7 +358,8 @@ void calc_covariance_eigen_vectors( } visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) + const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, + const double & confidence_interval_coefficient) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::CYLINDER; @@ -379,8 +382,8 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( marker_ptr->pose.orientation.w = std::cos(yaw / 2.0); // ellipse size - marker_ptr->scale.x = sigma1 * 2.448; // 2.448 sigma is 95% - marker_ptr->scale.y = sigma2 * 2.448; // 2.448 sigma is 95% + marker_ptr->scale.x = sigma1 * confidence_interval_coefficient; + marker_ptr->scale.y = sigma2 * confidence_interval_coefficient; marker_ptr->scale.z = 0.05; // ellipse color density @@ -399,7 +402,7 @@ visualization_msgs::msg::Marker::SharedPtr get_pose_covariance_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double & length, - const double & line_width) + const double & confidence_interval_coefficient, const double & line_width) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::LINE_STRIP; @@ -411,7 +414,8 @@ visualization_msgs::msg::Marker::SharedPtr get_yaw_covariance_marker_ptr( // orientation covariance double yaw_vector_length = std::max(length, 1.0); - double yaw_sigma = std::sqrt(pose_with_covariance.covariance[35]) * 2.448; // 2.448 sigma is 95% + double yaw_sigma = + std::sqrt(pose_with_covariance.covariance[35]) * confidence_interval_coefficient; // get arc points if (yaw_sigma > M_PI) { yaw_vector_length = 1.0; From fd81b7f13dda39cc1998ccf0cc52a9d7506b1032 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 15 Jan 2024 17:50:56 +0900 Subject: [PATCH 14/14] fix: confidence interval (1D) and confidence region (2D) Signed-off-by: Taekjin LEE --- .../object_polygon_display_base.hpp | 53 +++++++++++++------ 1 file changed, 36 insertions(+), 17 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 0a1d535896647..4290fdff49bb3 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/autoware_auto_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -106,11 +106,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_simple_visualize_mode_property->addOption("Simple", 1); // Confidence interval property m_confidence_interval_property = new rviz_common::properties::EnumProperty( - "Confidence Interval", "95.45%, 2 sigma", "Confidence interval of state estimations.", this); - m_confidence_interval_property->addOption("68.27%, 1 sigma", 0); - m_confidence_interval_property->addOption("86.64%, 1.5 sigma", 1); - m_confidence_interval_property->addOption("95.45%, 2 sigma", 2); - m_confidence_interval_property->addOption("99.73%, 3 sigma", 3); + "Confidence Interval", "95%", "Confidence interval of state estimations.", this); + m_confidence_interval_property->addOption("70%", 0); + m_confidence_interval_property->addOption("85%", 1); + m_confidence_interval_property->addOption("95%", 2); + m_confidence_interval_property->addOption("99%", 3); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { @@ -257,8 +257,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) const { if (m_display_pose_covariance_property.getBool()) { - return detail::get_pose_covariance_marker_ptr( - pose_with_covariance, get_confidence_interval()); + return detail::get_pose_covariance_marker_ptr(pose_with_covariance, get_confidence_region()); } else { return std::nullopt; } @@ -320,7 +319,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase { if (m_display_twist_covariance_property.getBool()) { return detail::get_twist_covariance_marker_ptr( - pose_with_covariance, twist_with_covariance, get_confidence_interval()); + pose_with_covariance, twist_with_covariance, get_confidence_region()); } else { return std::nullopt; } @@ -478,19 +477,39 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase { switch (m_confidence_interval_property->getOptionInt()) { case 0: - // 68.27%, 1 sigma - return 1.0; + // 70% + return 1.036; case 1: - // 86.64%, 1.5 sigma - return 1.5; + // 85% + return 1.440; case 2: - // 95.45%, 2 sigma - return 2.0; + // 95% + return 1.960; case 3: - // 99.73%, 3 sigma - return 3.0; + // 99% + return 2.576; default: - return 2.0; + return 1.960; + } + } + + double get_confidence_region() const + { + switch (m_confidence_interval_property->getOptionInt()) { + case 0: + // 70% + return 1.552; + case 1: + // 85% + return 1.802; + case 2: + // 95% + return 2.448; + case 3: + // 99% + return 3.035; + default: + return 2.448; } }