diff --git a/common/autoware_perception_rviz_plugin/CMakeLists.txt b/common/autoware_perception_rviz_plugin/CMakeLists.txt index 74671d74f7397..2233b65a1f46f 100644 --- a/common/autoware_perception_rviz_plugin/CMakeLists.txt +++ b/common/autoware_perception_rviz_plugin/CMakeLists.txt @@ -6,6 +6,8 @@ autoware_package() find_package(Qt5 REQUIRED COMPONENTS Widgets) +find_package(PCL REQUIRED) + set(OD_PLUGIN_LIB_SRC src/object_detection/detected_objects_display.cpp src/object_detection/tracked_objects_display.cpp @@ -46,9 +48,14 @@ ament_auto_add_library(${PROJECT_NAME} SHARED target_link_libraries(${PROJECT_NAME} rviz_common::rviz_common Qt5::Widgets + ${PCL_LIBRARIES} ) target_include_directories(${PROJECT_NAME} PRIVATE include) +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC +"${PCL_INCLUDE_DIRS}" +) + # Settings to improve the build as suggested on https://github.com/ros2/rviz/blob/ros2/docs/plugin_development.md target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") target_compile_definitions(${PROJECT_NAME} PRIVATE "OBJECT_DETECTION_PLUGINS_BUILDING_LIBRARY") diff --git a/common/autoware_perception_rviz_plugin/README.md b/common/autoware_perception_rviz_plugin/README.md index ed6f3e1675ace..74308847d3ee5 100644 --- a/common/autoware_perception_rviz_plugin/README.md +++ b/common/autoware_perception_rviz_plugin/README.md @@ -19,9 +19,10 @@ Example: #### Input Types -| Name | Type | Description | -| ---- | ------------------------------------------------ | ---------------------- | -| | `autoware_perception_msgs::msg::DetectedObjects` | detection result array | +| Name | Type | Description | +| ---- | ------------------------------------------------ | ------------------------- | +| | `autoware_perception_msgs::msg::DetectedObjects` | detection result array | +| | `sensor_msgs::msg::PointCloud2` | point cloud for filtering | #### Visualization Result @@ -31,9 +32,10 @@ Example: #### Input Types -| Name | Type | Description | -| ---- | ----------------------------------------------- | --------------------- | -| | `autoware_perception_msgs::msg::TrackedObjects` | tracking result array | +| Name | Type | Description | +| ---- | ----------------------------------------------- | ------------------------- | +| | `autoware_perception_msgs::msg::TrackedObjects` | tracking result array | +| | `sensor_msgs::msg::PointCloud2` | point cloud for filtering | #### Visualization Result @@ -45,9 +47,11 @@ Overwrite tracking results with detection results. #### Input Types -| Name | Type | Description | -| ---- | ------------------------------------------------- | ----------------------- | -| | `autoware_perception_msgs::msg::PredictedObjects` | prediction result array | +<<<<<<< HEAD:common/autoware_perception_rviz_plugin/README.md +| Name | Type | Description | +| ---- | ------------------------------------------------------ | ------------------------ | +| | `autoware_perception_msgs::msg::PredictedObjects` | prediction result array | +| | `sensor_msgs::msg::PointCloud2` | pointcloud for filtering | #### Visualization Result @@ -55,6 +59,12 @@ Overwrite prediction results with tracking results. ![predicted-object-visualization-description](./images/predicted-object-visualization-description.jpg) +### Visualization with active point cloud publishing + +Publishing colored point clouds. With colors according to different classes of detected objects, same as polygons. + +![visualization-with-pointcloud](./images/3d_pointcloud.png) + ## References/External links [1] diff --git a/common/autoware_perception_rviz_plugin/images/3d_pointcloud.png b/common/autoware_perception_rviz_plugin/images/3d_pointcloud.png new file mode 100644 index 0000000000000..1887bcec33994 Binary files /dev/null and b/common/autoware_perception_rviz_plugin/images/3d_pointcloud.png differ diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp index 4dcc4e9ea1545..508eebf229ad3 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp @@ -16,7 +16,11 @@ #include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" +#include #include +#include + +#include namespace autoware { @@ -37,6 +41,9 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay private: void processMessage(DetectedObjects::ConstSharedPtr msg) override; + void processPointCloud( + const DetectedObjects::ConstSharedPtr & input_objs_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); }; } // namespace object_detection diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp index 8d29900e1da26..a02f1e0ad38a4 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp @@ -32,6 +32,13 @@ #include #include +#include +#include +#include +#include +#include +#include + #include #include #include @@ -196,6 +203,10 @@ AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_lis const autoware_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_triangle_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( const autoware_perception_msgs::msg::Shape & shape, std::vector & points); diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index b05ba5f27f551..fa13ffe21f247 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -17,18 +17,57 @@ #include "autoware_perception_rviz_plugin/common/color_alpha_property.hpp" #include "autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp" #include "autoware_perception_rviz_plugin/visibility_control.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rviz_common/properties/ros_topic_property.hpp" +#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" #include +#include #include #include #include +#include +#include #include #include +// #include "autoware/universe_utils/autoware_universe_utils.hpp" + +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" + #include +#include +#include +#include #include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include #include +#include #include #include #include @@ -41,6 +80,39 @@ namespace rviz_plugins { namespace object_detection { + +using Polygon2d = autoware::universe_utils::Polygon2d; +using Shape = autoware_perception_msgs::msg::Shape; + +// struct for creating objects buffer +struct object_info +{ + Shape shape; + geometry_msgs::msg::Pose position; + std::vector classification; +}; + +inline pcl::PointXYZRGB toPCL(const double x, const double y, const double z) +{ + pcl::PointXYZRGB pcl_point; + pcl_point.x = x; + pcl_point.y = y; + pcl_point.z = z; + return pcl_point; +} + +inline pcl::PointXYZRGB toPCL(const geometry_msgs::msg::Point & point) +{ + return toPCL(point.x, point.y, point.z); +} + +// Define a custom comparator function to compare pointclouds by timestamp +inline bool comparePointCloudsByTimestamp( + const sensor_msgs::msg::PointCloud2 & pc1, const sensor_msgs::msg::PointCloud2 & pc2) +{ + return (static_cast(pc1.header.stamp.nanosec - pc2.header.stamp.nanosec)) < 0; +} + /// \brief Base rviz plugin class for all object msg types. The class defines common properties /// for the plugin and also defines common helper functions that can be used by its derived /// classes. @@ -59,9 +131,10 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase using PolygonPropertyMap = std::unordered_map; - explicit ObjectPolygonDisplayBase(const std::string & default_topic) - : m_marker_common(this), - // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this}, + explicit ObjectPolygonDisplayBase( + const std::string & default_topic, const std::string & default_pointcloud_topic) + : point_cloud_common(new rviz_default_plugins::PointCloudCommon(this)), + m_marker_common(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_velocity_text_property{ @@ -91,7 +164,8 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase this}, m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this}, - m_default_topic{default_topic} + m_default_topic{default_topic}, + qos_profile_points(5) { m_display_type_property = new rviz_common::properties::EnumProperty( "Polygon Type", "3d", "Type of the polygon to display object.", this); @@ -112,6 +186,14 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_confidence_interval_property->addOption("95%", 2); m_confidence_interval_property->addOption("99%", 3); + m_publish_objs_pointcloud = new rviz_common::properties::BoolProperty( + "Publish Objects Pointcloud", true, "Enable/disable objects pointcloud publishing"); + m_default_pointcloud_topic = new rviz_common::properties::RosTopicProperty( + "Input pointcloud topic", QString::fromStdString(default_pointcloud_topic), "", + "Input for pointcloud visualization of objects detection pipeline.", this, + SLOT(updateTopic())); + qos_profile_points_property = new rviz_common::properties::QosProfileProperty( + m_default_pointcloud_topic, qos_profile_points); m_object_fill_type_property = new rviz_common::properties::EnumProperty( "Object Fill Type", "skeleton", "Change object fill type in visualization", this); m_object_fill_type_property->addOption( @@ -139,12 +221,39 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase void onInitialize() override { + static int init_count = 0; + RCLCPP_INFO( + rclcpp::get_logger("autoware_perception_plugin"), "onInitialize called %d times", + init_count++); + RosTopicDisplay::RTDClass::onInitialize(); m_marker_common.initialize(this->context_, this->scene_node_); QString message_type = QString::fromStdString(rosidl_generator_traits::name()); this->topic_property_->setMessageType(message_type); this->topic_property_->setValue(m_default_topic.c_str()); this->topic_property_->setDescription("Topic to subscribe to."); + + qos_profile_points_property->initialize([this](rclcpp::QoS profile) { + this->qos_profile_points = profile; + RosTopicDisplay::updateTopic(); + }); + + // get weak_ptr to properly init RosTopicProperty + rviz_ros_node = this->context_->getRosNodeAbstraction(); + + m_default_pointcloud_topic->initialize(rviz_ros_node); + QString points_message_type = + QString::fromStdString(rosidl_generator_traits::name()); + m_default_pointcloud_topic->setMessageType(points_message_type); + + // get access to rviz node to sub and to pub to topics + rclcpp::Node::SharedPtr raw_node = + this->context_->getRosNodeAbstraction().lock()->get_raw_node(); + + tf_buffer = std::make_unique(raw_node->get_clock()); + tf_listener = std::make_shared(*tf_buffer); + + point_cloud_common->initialize(this->context_, this->scene_node_); } void load(const rviz_common::Config & config) override @@ -153,12 +262,90 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_marker_common.load(config); } - void update(float wall_dt, float ros_dt) override { m_marker_common.update(wall_dt, ros_dt); } + void update(float wall_dt, float ros_dt) override + { + m_marker_common.update(wall_dt, ros_dt); + point_cloud_common->update(wall_dt, ros_dt); + } void reset() override { + static int reset_count = 0; + RCLCPP_INFO( + rclcpp::get_logger("autoware_perception_plugin"), "reset called %d times", reset_count++); + RosTopicDisplay::reset(); m_marker_common.clearMarkers(); + point_cloud_common->reset(); + } + + void subscribe() + { + if (!RosTopicDisplay::isEnabled()) { + return; + } + + RosTopicDisplay::subscribe(); + points_subscribe(); + } + + void updateTopic() override + { + static int update_count = 0; + RCLCPP_INFO( + rclcpp::get_logger("autoware_perception_plugin"), "updateTopic called %d times", + update_count++); + unsubscribe(); + reset(); + subscribe(); + this->context_->queueRender(); + } + + void points_subscribe() + { + if (!m_publish_objs_pointcloud->getBool()) { + return; + } + + if (m_default_pointcloud_topic->isEmpty()) { + RosTopicDisplay::setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + QString("Error subscribing: Empty topic name")); + return; + } + + try { + rclcpp::SubscriptionOptions sub_opts; + sub_opts.event_callbacks.message_lost_callback = [&](rclcpp::QOSMessageLostInfo & info) { + std::ostringstream sstream; + sstream << "Some messages were lost:\n>\tNumber of new lost messages: " + << info.total_count_change + << " \n>\tTotal number of messages lost: " << info.total_count; + RosTopicDisplay::setStatus( + rviz_common::properties::StatusProperty::Warn, "Topic", QString(sstream.str().c_str())); + }; + + rclcpp::Node::SharedPtr raw_node = + this->context_->getRosNodeAbstraction().lock()->get_raw_node(); + pointcloud_subscription = raw_node->create_subscription( + m_default_pointcloud_topic->getTopicStd(), rclcpp::SensorDataQoS(), + std::bind(&ObjectPolygonDisplayBase::pointCloudCallback, this, std::placeholders::_1)); + RosTopicDisplay::setStatus(rviz_common::properties::StatusProperty::Ok, "Topic", "OK"); + } catch (rclcpp::exceptions::InvalidTopicNameError & e) { + RosTopicDisplay::setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + QString("Error subscribing: ") + e.what()); + } + } + + void unsubscribe() + { + RosTopicDisplay::unsubscribe(); + pointcloud_subscription.reset(); + static int unsubscribe_count = 0; + RCLCPP_INFO( + rclcpp::get_logger("autoware_perception_plugin"), "Unsubscribe called %d times", + unsubscribe_count++); } void clear_markers() { m_marker_common.clearMarkers(); } @@ -178,6 +365,61 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_marker_common.deleteMarker(marker_id); } + void add_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud) + { + point_cloud_common->addMessage(cloud); + } + + // transform detected object pose to target frame and return bool result + bool transformObjects( + const MsgT & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + MsgT & output_msg) + { + output_msg = input_msg; + + // transform to world coordinate + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + { + const auto ros_target2objects_world = getTransform( + tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (auto & object : output_msg.objects) { + tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose); + } + } + return true; + } + + // get transformation from tf2 + boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) + { + try { + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("autoware_perception_plugin"), ex.what()); + return boost::none; + } + } + + // variables for transfer detected objects information between callbacks + std::shared_ptr tf_listener{nullptr}; + std::unique_ptr tf_buffer; + protected: /// \brief Convert given shape msg into a Marker /// \tparam ClassificationContainerT List type with ObjectClassificationMsg @@ -523,6 +765,114 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } + // helper function to get radius for kd-search + std::optional getMaxRadius(object_info & object) + { + if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) { + return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); + } + + if (object.shape.type == Shape::POLYGON) { + float max_dist = 0.0; + for (const auto & point : object.shape.footprint.points) { + const float dist = std::hypot(point.x, point.y); + max_dist = std::max(max_dist, dist); + } + return max_dist; + } + + return std::nullopt; + } + + void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) + { + if (!m_publish_objs_pointcloud->getBool()) { + return; + } + + pointCloudBuffer_mutex.lock(); + pointCloudBuffer.push_front(input_pointcloud_msg); + if (pointCloudBuffer.size() > 5) { + pointCloudBuffer.pop_back(); + } + pointCloudBuffer_mutex.unlock(); + } + + void filterPolygon( + const pcl::PointCloud::ConstPtr & in_cloud, + pcl::PointCloud::Ptr out_cloud, const object_info & object) + { + pcl::PointCloud filtered_cloud; + std::vector vertices_array; + pcl::Vertices vertices; + + Polygon2d poly2d = autoware::universe_utils::toPolygon2d(object.position, object.shape); + if (boost::geometry::is_empty(poly2d)) { + return; + } + + pcl::PointCloud::Ptr hull_cloud(new pcl::PointCloud); + for (size_t i = 0; i < poly2d.outer().size(); ++i) { + vertices.vertices.emplace_back(i); + vertices_array.emplace_back(vertices); + hull_cloud->emplace_back( + static_cast(poly2d.outer().at(i).x()), static_cast(poly2d.outer().at(i).y()), + static_cast(0.0)); + } + + pcl::CropHull crop_hull_filter; + crop_hull_filter.setInputCloud(in_cloud); + crop_hull_filter.setDim(2); + crop_hull_filter.setHullIndices(vertices_array); + crop_hull_filter.setHullCloud(hull_cloud); + crop_hull_filter.setCropOutside(true); + + crop_hull_filter.filter(filtered_cloud); + + const std_msgs::msg::ColorRGBA color_rgba = + get_color_rgba(object.classification); // need to be converted to int + + for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) { + cloud_it->r = std::max(0, std::min(255, static_cast(floor(color_rgba.r * 256.0)))); + cloud_it->g = std::max(0, std::min(255, static_cast(floor(color_rgba.g * 256.0)))); + cloud_it->b = std::max(0, std::min(255, static_cast(floor(color_rgba.b * 256.0)))); + } + + *out_cloud += filtered_cloud; + } + + // Function that returns the pointcloud with the nearest timestamp from a buffer of pointclouds + sensor_msgs::msg::PointCloud2 getNearestPointCloud( + std::deque & buffer, const rclcpp::Time & timestamp) + { + if (buffer.empty()) { + // Handle the case where the buffer is empty + throw std::runtime_error("Buffer is empty"); + } + sensor_msgs::msg::PointCloud2 result = buffer.front(); + rclcpp::Duration diff = timestamp - rclcpp::Time(result.header.stamp); + pointCloudBuffer_mutex.lock(); + for (size_t i = 0; i != buffer.size(); i++) { + if (diff.nanoseconds() > (timestamp - rclcpp::Time(buffer[i].header.stamp)).nanoseconds()) { + diff = timestamp - rclcpp::Time(buffer[i].header.stamp); + result = buffer[i]; + } + } + pointCloudBuffer_mutex.unlock(); + + return result; + } + + rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic; + // Property to enable/disable objects pointcloud publishing + rviz_common::properties::BoolProperty * m_publish_objs_pointcloud; + rclcpp::Subscription::SharedPtr pointcloud_subscription; + // plugin to visualize pointclouds + std::unique_ptr point_cloud_common; + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node; + std::deque pointCloudBuffer; + std::mutex pointCloudBuffer_mutex; + private: // All rviz plugins should have this. Should be initialized with pointer to this class MarkerCommon m_marker_common; @@ -571,6 +921,8 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::string m_default_topic; std::vector predicted_path_colors; + rclcpp::QoS qos_profile_points; + rviz_common::properties::QosProfileProperty * qos_profile_points_property; }; } // namespace object_detection } // namespace rviz_plugins diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp index 1445f02e34290..2947b2a36fa40 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp @@ -127,6 +127,15 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay void update(float wall_dt, float ros_dt) override; + bool transformObjects( + const autoware_perception_msgs::msg::PredictedObjects & input_msg, + const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + autoware_perception_msgs::msg::PredictedObjects & output_msg); + + void processPointCloud( + const PredictedObjects::ConstSharedPtr & input_objs_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); + std::unordered_map> id_map; // std::unordered_map id_map; std::list unused_marker_ids; diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp index 9ccaa5cd150f6..379b89576b8d9 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp @@ -16,6 +16,7 @@ #include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" +#include #include #include @@ -23,6 +24,7 @@ #include #include +#include #include #include @@ -105,6 +107,10 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay return id_map.at(uuid); } + void processPointCloud( + const TrackedObjects::ConstSharedPtr & input_objs_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); + std::map id_map; std::list unused_marker_ids; int32_t marker_id = 0; diff --git a/common/autoware_perception_rviz_plugin/package.xml b/common/autoware_perception_rviz_plugin/package.xml index 460186c33b7d8..92bf2a2431bf7 100644 --- a/common/autoware_perception_rviz_plugin/package.xml +++ b/common/autoware_perception_rviz_plugin/package.xml @@ -19,8 +19,18 @@ qtbase5-dev autoware_perception_msgs + autoware_universe_utils + pcl_conversions + perception_utils rviz_common rviz_default_plugins + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + libqt5-widgets rviz2 diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 9dd0b2923f09f..71e31132eedcd 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -24,7 +24,11 @@ namespace rviz_plugins { namespace object_detection { -DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("detected_objects") + +DetectedObjectsDisplay::DetectedObjectsDisplay() +: ObjectPolygonDisplayBase( + "/perception/object_recognition/detection/objects", + "/perception/obstacle_segmentation/pointcloud") { } @@ -153,6 +157,70 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(marker_ptr); } } + + if (pointCloudBuffer.empty()) { + return; + } + // pointcloud pub + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = + std::make_shared( + getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + processPointCloud(msg, closest_pointcloud); +} + +void DetectedObjectsDisplay::processPointCloud( + const DetectedObjects::ConstSharedPtr & input_objs_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) +{ + if (!m_publish_objs_pointcloud->getBool()) { + return; + } + // Transform to pointcloud frame + autoware_perception_msgs::msg::DetectedObjects transformed_objects; + if (!transformObjects( + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { + return; + } + // convert to pcl pointcloud + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud); + // Create a new point cloud with RGB color information and copy data from input cloud + pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); + pcl::copyPointCloud(*temp_cloud, *colored_cloud); + // Create Kd-tree to search neighbor pointcloud to reduce cost. + pcl::search::Search::Ptr kdtree = + pcl::make_shared>(false); + kdtree->setInputCloud(colored_cloud); + + pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); + + for (const auto & object : transformed_objects.objects) { + std::vector labels = object.classification; + object_info unified_object = { + object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + + const auto search_radius = getMaxRadius(unified_object); + // Search neighbor pointcloud to reduce cost. + pcl::PointCloud::Ptr neighbor_pointcloud( + new pcl::PointCloud); + std::vector indices; + std::vector distances; + kdtree->radiusSearch( + toPCL(unified_object.position.position), search_radius.value(), indices, distances); + for (const auto & index : indices) { + neighbor_pointcloud->push_back(colored_cloud->at(index)); + } + + filterPolygon(neighbor_pointcloud, out_cloud, unified_object); + } + + sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( + new sensor_msgs::msg::PointCloud2); + pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); + + output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; + + add_pointcloud(output_pointcloud_msg_ptr); } } // namespace object_detection diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 1d06454582a2f..4f3565bddf007 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -549,11 +549,15 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( { auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); + marker_ptr->scale.x = line_width; using autoware_perception_msgs::msg::Shape; if (shape_msg.type == Shape::BOUNDING_BOX) { - marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; - calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); + marker_ptr->type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + calc_2d_bounding_box_bottom_triangle_list(shape_msg, marker_ptr->points); + marker_ptr->scale.x = 1.0; // Set scale to 1 for TRIANGLE_LIST + marker_ptr->scale.y = 1.0; // Set scale to 1 for TRIANGLE_LIST + marker_ptr->scale.z = 1.0; // Set scale to 1 for TRIANGLE_LIST if (is_orientation_available) { calc_2d_bounding_box_bottom_direction_line_list(shape_msg, marker_ptr->points); } else { @@ -573,12 +577,82 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; marker_ptr->pose = to_pose(centroid, orientation); marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.15); - marker_ptr->scale.x = line_width; + + marker_ptr->mesh_use_embedded_materials = false; // Ensure the material is not embedded + marker_ptr->color = color_rgba; + marker_ptr->color.a = 0.75f; return marker_ptr; } +void calc_2d_bounding_box_bottom_triangle_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double length_half = shape.dimensions.x * 0.5; + const double width_half = shape.dimensions.y * 0.5; + const double height_half = shape.dimensions.z * 0.5; + + // Define Boost.Geometry types + using BoostPoint = boost::geometry::model::d2::point_xy; + using BoostPolygon = boost::geometry::model::polygon; + + // Create a rectangle polygon + BoostPolygon boost_polygon; + boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, width_half)); + boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, -width_half)); + boost::geometry::append(boost_polygon.outer(), BoostPoint(-length_half, -width_half)); + boost::geometry::append(boost_polygon.outer(), BoostPoint(-length_half, width_half)); + boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, width_half)); + boost::geometry::correct(boost_polygon); + + // Define buffer strategy with larger rounded corners and smoother approximation + boost::geometry::strategy::buffer::distance_symmetric distance_strategy( + 0.4); // Adjust distance as needed + boost::geometry::strategy::buffer::join_round join_strategy( + 16); // Increase points per circle for smoother roundness + boost::geometry::strategy::buffer::end_flat end_strategy; + boost::geometry::strategy::buffer::point_circle point_strategy(16); // Points per circle + boost::geometry::strategy::buffer::side_straight side_strategy; + + // Create buffered polygon with rounded corners + std::vector buffered_polygons; + boost::geometry::buffer( + boost_polygon, buffered_polygons, distance_strategy, side_strategy, join_strategy, end_strategy, + point_strategy); + + if (buffered_polygons.empty()) { + RCLCPP_WARN(rclcpp::get_logger("ObjectPolygonDisplayBase"), "Buffering produced no output."); + return; + } + + // Convert buffered polygon to RViz marker points + const auto & outer_ring = buffered_polygons.front().outer(); + std::vector polygon_points; + + for (const auto & point : outer_ring) { + geometry_msgs::msg::Point p; + p.x = point.x(); + p.y = point.y(); + p.z = -height_half; // Use the bottom part of the shape + polygon_points.push_back(p); + } + + // Create triangles from polygon points + for (size_t i = 1; i < polygon_points.size() - 1; ++i) { + // Add front face + points.push_back(polygon_points[0]); + points.push_back(polygon_points[i]); + points.push_back(polygon_points[i + 1]); + + // Add back face + points.push_back(polygon_points[0]); + points.push_back(polygon_points[i + 1]); + points.push_back(polygon_points[i]); + } +} + void calc_line_list_from_points( const double point_list[][3], const int point_pairs[][2], const int & num_pairs, std::vector & points) @@ -677,23 +751,71 @@ void calc_2d_bounding_box_bottom_line_list( const double length_half = shape.dimensions.x * 0.5; const double width_half = shape.dimensions.y * 0.5; const double height_half = shape.dimensions.z * 0.5; - geometry_msgs::msg::Point point; - // bounding box corner points - // top surface, clockwise - const double point_list[4][3] = { - {length_half, width_half, -height_half}, - {length_half, -width_half, -height_half}, - {-length_half, -width_half, -height_half}, - {-length_half, width_half, -height_half}, - }; - const int point_pairs[4][2] = { - {0, 1}, - {1, 2}, - {2, 3}, - {3, 0}, - }; - calc_line_list_from_points(point_list, point_pairs, 4, points); + // Define Boost.Geometry types + using BoostPoint = boost::geometry::model::d2::point_xy; + using BoostPolygon = boost::geometry::model::polygon; + + // Create a rectangle polygon + BoostPolygon boost_polygon; + boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, width_half)); + boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, -width_half)); + boost::geometry::append(boost_polygon.outer(), BoostPoint(-length_half, -width_half)); + boost::geometry::append(boost_polygon.outer(), BoostPoint(-length_half, width_half)); + boost::geometry::append(boost_polygon.outer(), BoostPoint(length_half, width_half)); + boost::geometry::correct(boost_polygon); + + // Define buffer strategy with rounded corners + boost::geometry::strategy::buffer::distance_symmetric distance_strategy( + 0.4); // Adjust distance as needed + boost::geometry::strategy::buffer::join_round join_strategy( + 16); // Adjust points per circle for roundness + boost::geometry::strategy::buffer::end_flat end_strategy; + boost::geometry::strategy::buffer::point_circle point_strategy(16); // Points per circle + boost::geometry::strategy::buffer::side_straight side_strategy; + + // Create buffered polygon with rounded corners + std::vector buffered_polygons; + boost::geometry::buffer( + boost_polygon, buffered_polygons, distance_strategy, side_strategy, join_strategy, end_strategy, + point_strategy); + + if (buffered_polygons.empty()) { + RCLCPP_WARN(rclcpp::get_logger("ObjectPolygonDisplayBase"), "Buffering produced no output."); + return; + } + + // Convert buffered polygon to RViz marker points + const auto & outer_ring = buffered_polygons.front().outer(); + for (size_t i = 0; i < outer_ring.size() - 1; ++i) { // -1 to avoid duplicating the closing point + geometry_msgs::msg::Point point; + point.x = outer_ring[i].x(); + point.y = outer_ring[i].y(); + point.z = -height_half; // Use the bottom part of the shape + points.push_back(point); + + point.x = outer_ring[i + 1].x(); + point.y = outer_ring[i + 1].y(); + point.z = -height_half; // Use the bottom part of the shape + points.push_back(point); + } + + // Closing the polygon + if (!outer_ring.empty()) { + geometry_msgs::msg::Point point; + point.x = outer_ring.back().x(); + point.y = outer_ring.back().y(); + point.z = -height_half; + points.push_back(point); + + point.x = outer_ring.front().x(); + point.y = outer_ring.front().y(); + point.z = -height_half; + points.push_back(point); + } + + RCLCPP_INFO( + rclcpp::get_logger("ObjectPolygonDisplayBase"), "Marker points size: %zu", points.size()); } void calc_2d_bounding_box_bottom_direction_line_list( diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index d11aba912854d..668d9a30f4414 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -23,7 +23,8 @@ namespace rviz_plugins { namespace object_detection { -PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") +PredictedObjectsDisplay::PredictedObjectsDisplay() +: ObjectPolygonDisplayBase("predictions", "/perception/obstacle_segmentation/pointcloud") { max_num_threads = 1; // hard code the number of threads to be created @@ -260,6 +261,15 @@ std::vector PredictedObjectsDisplay: } } + if (pointCloudBuffer.empty()) { + return markers; + } + // pointcloud pub + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = + std::make_shared( + getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + processPointCloud(msg, closest_pointcloud); + return markers; } @@ -299,6 +309,97 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) wall_dt, ros_dt); } +bool PredictedObjectsDisplay::transformObjects( + const autoware_perception_msgs::msg::PredictedObjects & input_msg, + const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + autoware_perception_msgs::msg::PredictedObjects & output_msg) +{ + output_msg = input_msg; + + // transform to world coordinate + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + { + const auto ros_target2objects_world = + getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (auto & object : output_msg.objects) { + tf2::fromMsg(object.kinematics.initial_pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, object.kinematics.initial_pose_with_covariance.pose); + } + } + return true; +} + +void PredictedObjectsDisplay::processPointCloud( + const PredictedObjects::ConstSharedPtr & input_objs_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) +{ + if (!m_publish_objs_pointcloud->getBool()) { + return; + } + // Transform to pointcloud frame + autoware_perception_msgs::msg::PredictedObjects transformed_objects; + if (!transformObjects( + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { + return; + } + // convert to pcl pointcloud + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud); + // Create a new point cloud with RGB color information and copy data from input cloud + pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); + pcl::copyPointCloud(*temp_cloud, *colored_cloud); + // Create Kd-tree to search neighbor pointcloud to reduce cost. + pcl::search::Search::Ptr kdtree = + pcl::make_shared>(false); + + // check if the pointcloud is empty to not call setInputCloud with an empty pointcloud + if (colored_cloud->empty()) { + return; + } + + kdtree->setInputCloud(colored_cloud); + + pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); + + for (const auto & object : transformed_objects.objects) { + std::vector labels = object.classification; + object_info unified_object = { + object.shape, object.kinematics.initial_pose_with_covariance.pose, object.classification}; + + const auto search_radius = getMaxRadius(unified_object); + // Search neighbor pointcloud to reduce cost. + pcl::PointCloud::Ptr neighbor_pointcloud( + new pcl::PointCloud); + std::vector indices; + std::vector distances; + kdtree->radiusSearch( + toPCL(unified_object.position.position), search_radius.value(), indices, distances); + for (const auto & index : indices) { + neighbor_pointcloud->push_back(colored_cloud->at(index)); + } + + filterPolygon(neighbor_pointcloud, out_cloud, unified_object); + } + + sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( + new sensor_msgs::msg::PointCloud2); + pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); + + output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; + + add_pointcloud(output_pointcloud_msg_ptr); +} + } // namespace object_detection } // namespace rviz_plugins } // namespace autoware diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index 214ed9ce70e63..d85637a23b4a1 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -24,7 +24,11 @@ namespace rviz_plugins { namespace object_detection { -TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") + +TrackedObjectsDisplay::TrackedObjectsDisplay() +: ObjectPolygonDisplayBase( + "/perception/object_recognition/tracking/objects", + "/perception/obstacle_segmentation/pointcloud") { // Option for selective visualization by object dynamics m_select_object_dynamics_property = new rviz_common::properties::EnumProperty( @@ -203,6 +207,70 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(marker_ptr); } } + + if (pointCloudBuffer.empty()) { + return; + } + // pointcloud pub + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = + std::make_shared( + getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + processPointCloud(msg, closest_pointcloud); +} + +void TrackedObjectsDisplay::processPointCloud( + const TrackedObjects::ConstSharedPtr & input_objs_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) +{ + if (!m_publish_objs_pointcloud->getBool()) { + return; + } + // Transform to pointcloud frame + autoware_perception_msgs::msg::TrackedObjects transformed_objects; + if (!transformObjects( + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { + return; + } + // convert to pcl pointcloud + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud); + // Create a new point cloud with RGB color information and copy data from input cloud + pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); + pcl::copyPointCloud(*temp_cloud, *colored_cloud); + // Create Kd-tree to search neighbor pointcloud to reduce cost. + pcl::search::Search::Ptr kdtree = + pcl::make_shared>(false); + kdtree->setInputCloud(colored_cloud); + + pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); + + for (const auto & object : transformed_objects.objects) { + std::vector labels = object.classification; + object_info unified_object = { + object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + + const auto search_radius = getMaxRadius(unified_object); + // Search neighbor pointcloud to reduce cost. + pcl::PointCloud::Ptr neighbor_pointcloud( + new pcl::PointCloud); + std::vector indices; + std::vector distances; + kdtree->radiusSearch( + toPCL(unified_object.position.position), search_radius.value(), indices, distances); + for (const auto & index : indices) { + neighbor_pointcloud->push_back(colored_cloud->at(index)); + } + + filterPolygon(neighbor_pointcloud, out_cloud, unified_object); + } + + sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( + new sensor_msgs::msg::PointCloud2); + pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); + + output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; + + add_pointcloud(output_pointcloud_msg_ptr); } } // namespace object_detection