From 955a898790c88f324bdd51282bcb86f2e575ab48 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Wed, 22 Feb 2023 18:13:59 +0800 Subject: [PATCH 01/58] feat(add_perception_objects_pointcloud_publisher) add source files and scripts Signed-off-by: Alexey Panferov --- perception/detected_objects_pointcloud_publisher | 1 + 1 file changed, 1 insertion(+) create mode 160000 perception/detected_objects_pointcloud_publisher diff --git a/perception/detected_objects_pointcloud_publisher b/perception/detected_objects_pointcloud_publisher new file mode 160000 index 000000000000..97dff7b66bda --- /dev/null +++ b/perception/detected_objects_pointcloud_publisher @@ -0,0 +1 @@ +Subproject commit 97dff7b66bdadf4c16697045474ff3bebd3b1444 From 8ed1500b72941f1a82a95f1344bb9d85225ea34b Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Mon, 27 Feb 2023 15:29:06 +0800 Subject: [PATCH 02/58] feat(add_perception_objects_pointcloud_publisher) add to autoware_auto_perception_plugin subscription to pointclouds and publishing pointclouds Signed-off-by: Alexey Panferov --- .../src/object_detection/detected_objects_display.cpp | 2 +- .../src/object_detection/predicted_objects_display.cpp | 2 +- .../src/object_detection/tracked_objects_display.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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 26ba30840b17..cd46ff5f9636 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 @@ -24,7 +24,7 @@ namespace rviz_plugins { namespace object_detection { -DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("detected_objects") {} +DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("detected_objects", "/perception/obstacle_segmentation/pointcloud") {} void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) { 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 870b8f2c7754..edf3353db9c0 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 @@ -22,7 +22,7 @@ namespace rviz_plugins { namespace object_detection { -PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") +PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("tracks", "/perception/obstacle_segmentation/pointcloud") { std::thread worker(&PredictedObjectsDisplay::workerThread, this); worker.detach(); 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 57e3ce905d85..1e739ea48977 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 @@ -24,7 +24,7 @@ namespace rviz_plugins { namespace object_detection { -TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("tracks") {} +TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("tracks", "/perception/obstacle_segmentation/pointcloud") {} void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) { From 532d81d49bb25cb9553d9733340426ba163f6cb1 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 28 Feb 2023 18:21:52 +0800 Subject: [PATCH 03/58] feat(add_perception_objects_pointcloud_publisher) wip add tf, modify message templated plugins, builds, but not woking correctly . Add rviz lauch file for debugging. modify package.xml Signed-off-by: Alexey Panferov --- .../detected_objects_display.hpp | 8 + .../object_polygon_display_base.hpp | 66 +- .../predicted_objects_display.hpp | 6 + .../tracked_objects_display.hpp | 6 + .../package.xml | 8 + .../rviz/debug_config.rivz.rviz | 2167 +++++++++++++++++ .../detected_objects_display.cpp | 101 + .../predicted_objects_display.cpp | 102 + .../tracked_objects_display.cpp | 102 + 9 files changed, 2565 insertions(+), 1 deletion(-) create mode 100644 common/autoware_auto_perception_rviz_plugin/rviz/debug_config.rivz.rviz diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index 67dac25c796b..c1904a8a1f75 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include namespace autoware { @@ -37,6 +39,12 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay private: void processMessage(DetectedObjects::ConstSharedPtr msg) override; + void onInitialize() override; + void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; + + + std::string objects_frame_id_; + std::vector objs_buffer; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index acf4a389ec6e..2228e8ec964d 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -25,6 +25,28 @@ #include #include +// #include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/clock.hpp" +#include +#include +#include "rviz_common/properties/ros_topic_property.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + #include #include @@ -59,7 +81,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase using PolygonPropertyMap = std::unordered_map; - explicit ObjectPolygonDisplayBase(const std::string & default_topic) + explicit ObjectPolygonDisplayBase(const std::string & default_topic, const std::string & default_pointcloud_topic) : m_marker_common(this), // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this}, m_display_label_property{"Display Label", true, "Enable/disable label visualization", this}, @@ -90,6 +112,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_simple_visualize_mode_property = new rviz_common::properties::EnumProperty( "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this, SLOT(updatePalette())); + m_default_pointcloud_topic = new rviz_common::properties::RosTopicProperty( + "Input pointcloud topic", + QString::fromStdString(default_pointcloud_topic), + "", + "Input for pointcloud visualization of Objectcs detection pipeline", + this, + SLOT(updatePalette())); m_simple_visualize_mode_property->addOption("Normal", 0); m_simple_visualize_mode_property->addOption("Simple", 1); // iterate over default values to create and initialize the properties. @@ -119,6 +148,21 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase this->topic_property_->setMessageType(message_type); this->topic_property_->setValue(m_default_topic.c_str()); this->topic_property_->setDescription("Topic to subscribe to."); + + // get access to rivz node to sub and to pub to topics + rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); + publisher_ = raw_node->create_publisher("output/pointcloud", 10); + pointcloud_subscription_ = raw_node->create_subscription( + m_default_pointcloud_topic->getTopicStd(), + rclcpp::SensorDataQoS(), + std::bind(&ObjectPolygonDisplayBase::pointCloudCallback, + this, + std::placeholders::_1)); + + tf_buffer_ = + std::make_unique(raw_node->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); } void load(const rviz_common::Config & config) override @@ -146,6 +190,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase { m_marker_common.addMessage(markers_ptr); } + + std::string objects_frame_id_; protected: /// \brief Convert given shape msg into a Marker @@ -382,6 +428,20 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase double get_line_width() { return m_line_width_property.getFloat(); } + // std::vector objs_buffer; // object buffer to filtrate input pointcloud + // std::string objects_frame_id_; + + // void transformPointCloud() + + std::shared_ptr tf_listener_{nullptr}; + std::unique_ptr tf_buffer_; + // add pointcloud subscription + rclcpp::Subscription::SharedPtr pointcloud_subscription_; + rclcpp::Publisher::SharedPtr publisher_; + + // Default pointcloud topic; + rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic; + private: // All rviz plugins should have this. Should be initialized with pointer to this class MarkerCommon m_marker_common; @@ -415,6 +475,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::string m_default_topic; std::vector predicted_path_colors; + + + virtual void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) = 0; + }; } // namespace object_detection } // namespace rviz_plugins diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index ee2c7e0d5c3b..8e2fc180fc38 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -47,6 +48,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay private: void processMessage(PredictedObjects::ConstSharedPtr msg) override; + void onInitialize() override; + void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) { @@ -112,6 +115,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay std::mutex mutex; std::condition_variable condition; std::vector markers; + + std::string objects_frame_id_; + std::vector objs_buffer; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 9f00a2cb1cde..0099c1f732f7 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -45,6 +46,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay private: void processMessage(TrackedObjects::ConstSharedPtr msg) override; + void onInitialize() override; + void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) { @@ -96,6 +99,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay std::map id_map; std::list unused_marker_ids; int32_t marker_id = 0; + + std::string objects_frame_id_; + std::vector objs_buffer; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index 4771589dc903..81d4425df9b0 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -20,6 +20,14 @@ autoware_auto_perception_msgs rviz_common rviz_default_plugins + sensor_msgs + PCL + pcl_conversions + tf2 + tf2_ros + tf2_sensor_msgs + tf2_geometry_msgs + tf2_eigen libqt5-widgets rviz2 diff --git a/common/autoware_auto_perception_rviz_plugin/rviz/debug_config.rivz.rviz b/common/autoware_auto_perception_rviz_plugin/rviz/debug_config.rivz.rviz new file mode 100644 index 000000000000..635c4c38a4cb --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/rviz/debug_config.rivz.rviz @@ -0,0 +1,2167 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 + - /Perception1 + - /Perception1/ObjectRecognition1 + - /Perception1/ObjectRecognition1/Prediction1/PredictedObjects1 + Splitter Ratio: 0.557669460773468 + Tree Height: 288 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: ~ + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel + Name: InitialPoseButtonPanel + - Class: AutowareDateTimePanel + Name: AutowareDateTimePanel + - Class: rviz_plugins::AutowareStatePanel + Name: AutowareStatePanel +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/SteeringAngle + Enabled: true + Left: 95 + Length: 190 + Name: SteeringAngle + Scale: 17 + Text Color: 25; 255; 240 + Top: 95 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/steering_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Class: rviz_plugins/ConsoleMeter + Enabled: true + Left: 379 + Length: 190 + Name: ConsoleMeter + Text Color: 25; 255; 240 + Top: 95 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Alpha: 0.9990000128746033 + Class: rviz_plugins/VelocityHistory + Color Border Vel Max: 3 + Constant Color: + Color: 255; 255; 255 + Value: true + Enabled: true + Name: VelocityHistory + Scale: 0.30000001192092896 + Timeout: 10 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera0/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera0/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera1/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera1/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera2/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera2/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera3/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera3/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera4/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera4/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera5/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera5/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + gnss_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_kit_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + tamagawa/imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + traffic_light_left_camera/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + traffic_light_left_camera/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + traffic_light_right_camera/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + traffic_light_right_camera/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + velodyne_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_left_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_rear: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_rear_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_right_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_top_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: VehicleModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_plugins/PolarGridDisplay + Color: 255; 255; 255 + Delta Range: 10 + Enabled: true + Max Alpha: 0.5 + Max Range: 100 + Max Wave Alpha: 0.5 + Min Alpha: 0.009999999776482582 + Min Wave Alpha: 0.009999999776482582 + Name: PolarGridDisplay + Reference Frame: base_link + Value: true + Wave Color: 255; 255; 255 + Wave Velocity: 40 + - Class: rviz_plugins/MaxVelocity + Enabled: true + Left: 441 + Length: 71 + Name: MaxVelocity + Text Color: 255; 255; 255 + Top: 207 + Topic: /planning/scenario_planning/current_max_velocity + Value: true + Value Scale: 0.25 + - Class: rviz_plugins/TurnSignal + Enabled: true + Height: 190 + Left: 145 + Name: TurnSignal + Top: 259 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/turn_indicators_status + Value: true + Width: 379 + Enabled: true + Name: Vehicle + Enabled: true + Name: System + - Class: rviz_common/Group + Displays: + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 28.71826171875 + Min Value: -7.4224700927734375 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 237 + Min Color: 211; 215; 207 + Min Intensity: 0 + Name: PointCloudMap + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/pointcloud_map + Use Fixed Frame: true + Use rainbow: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Lanelet2VectorMap + Namespaces: + center_lane_line: false + center_line_arrows: false + crosswalk_lanelets: true + lane_start_bound: false + lanelet_id: false + left_lane_bound: true + right_lane_bound: true + road_lanelets: false + stop_lines: true + traffic_light: true + traffic_light_id: false + traffic_light_triangle: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/vector_map_marker + Value: true + Enabled: true + Name: Map + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.4000000059604645 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5 + Min Value: -1 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ConcatenatePointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/lidar/concatenated/pointcloud + Use Fixed Frame: false + Use rainbow: true + Value: true + - Alpha: 0.9990000128746033 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: MeasurementRange + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/lidar/crop_box_filter/crop_box_polygon + Value: false + Enabled: true + Name: LiDAR + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 233; 185; 110 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.20000000298023224 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.699999988079071 + Head Radius: 1.2000000476837158 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.5 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/gnss/pose_with_covariance + Value: true + Enabled: false + Name: GNSS + Enabled: true + Name: Sensing + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 170; 255 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovInitial + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/initial_pose_with_covariance + Value: false + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 255; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovAligned + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose_with_covariance + Value: false + - Buffer Size: 200 + Class: rviz_plugins::PoseHistory + Enabled: false + Line: + Alpha: 0.9990000128746033 + Color: 170; 255; 127 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Initial + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.5 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /localization/util/downsample/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 85; 255; 127 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Aligned + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.5 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/points_aligned + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MonteCarloInitialPose + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/monte_carlo_initial_pose_marker + Value: true + Enabled: true + Name: NDT + - Class: rviz_common/Group + Displays: + - Buffer Size: 1000 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 0; 255; 255 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_twist_fusion_filter/pose + Value: true + Enabled: true + Name: EKF + Enabled: true + Name: Localization + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 15 + Min Value: -2 + Value: false + Axis: Z + Channel Name: z + Class: rviz_default_plugins/PointCloud2 + Color: 200; 200; 200 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 15 + Min Color: 0; 0; 0 + Min Intensity: -5 + Name: NoGroundPointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/obstacle_segmentation/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Segmentation + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/DetectedObjects + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Input pointcloud topic: /perception/obstacle_segmentation/pointcloud + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: DetectedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + Enabled: false + Name: Detection + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/TrackedObjects + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Input pointcloud topic: /perception/obstacle_segmentation/pointcloud + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: TrackedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/tracking/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + Enabled: false + Name: Tracking + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/PredictedObjects + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: false + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Input pointcloud topic: /perception/obstacle_segmentation/pointcloud + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: PredictedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Maneuver + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/prediction/maneuver + Value: false + Enabled: true + Name: Prediction + Enabled: true + Name: ObjectRecognition + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RecognitionResultOnImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/traffic_light_recognition/debug/rois + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MapBasedDetectionResult + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers + Value: true + Enabled: true + Name: TrafficLight + - Class: rviz_common/Group + Displays: + - Alpha: 0.5 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/occupancy_grid_map/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/occupancy_grid_map/map_updates + Use Timestamp: false + Value: true + Enabled: false + Name: OccupancyGrid + Enabled: true + Name: Perception + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: RouteArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/route_marker + Value: true + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.30000001192092896 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.5 + Name: GoalPose + Shaft Length: 3 + Shaft Radius: 0.20000000298023224 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/echo_back_goal_pose + Value: true + Enabled: true + Name: MissionPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: ScenarioTrajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/trajectory + Value: true + View Path: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: Path + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/path + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Path: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_LaneChange + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/lane_change + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_ExternalRequestLaneChangeRight + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/ext_request_lane_change_right + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_ExternalRequestLaneChangeLeft + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/ext_request_lane_change_left + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_Avoidance + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/avoidance + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_PullOut + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/pull_out + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate_PullOver + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/path_candidate/pull_over + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Bound + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (BlindSpot) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Crosswalk) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (DetectionArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Intersection) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (MergeFromPrivateArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (NoStoppingArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (OcclusionSpot) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (StopLine) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (TrafficLight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (VirtualTrafficLight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (RunOut) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (SpeedBump) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/speed_bump + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Arrow + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Crosswalk + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Intersection + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Blind Spot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: TrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: VirtualTrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: StopLine + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: DetectionArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: OcclusionSpot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: NoStoppingArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: RunOut + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Avoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: LaneChange + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanechange + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: LaneFollowing + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanefollowing + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: PullOver + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullover + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: PullOut + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullout + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: SideShift + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/sideshift + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: SpeedBump + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/speed_bump + Value: false + Enabled: false + Name: DebugMarker + Enabled: true + Name: BehaviorPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/trajectory + Value: false + View Path: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleStop) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (SurroundObstacle) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleAvoidance) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleCruise) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/virtual_wall + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: SurroundObstacleCheck + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Footprint + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 239; 41; 41 + Enabled: false + Name: FootprintOffset + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 10; 21; 255 + Enabled: false + Name: FootprintRecoverOffset + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset + Value: false + Enabled: false + Name: SurroundObstacleChecker + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ObstacleStop + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: CruiseVirtualWall + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: DebugMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker + Value: true + Enabled: false + Name: ObstacleCruise + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ObstacleAvoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker + Value: false + Enabled: false + Name: DebugMarker + Enabled: true + Name: MotionPlanning + Enabled: true + Name: LaneDriving + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Costmap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates + Use Timestamp: false + Value: false + - Alpha: 0.9990000128746033 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PartialPoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (3D) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array + Value: true + - Alpha: 0.9990000128746033 + Arrow Length: 0.5 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 0; 255 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array + Value: true + Enabled: true + Name: Parking + - Class: rviz_plugins/PoseWithUuidStamped + Enabled: true + Length: 1.5 + Name: ModifiedGoal + Radius: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/modified_goal + UUID: + Scale: 0.30000001192092896 + Value: false + Value: true + Enabled: true + Name: ScenarioPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: PlanningErrorMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker + Value: true + Enabled: false + Name: Diagnostic + Enabled: true + Name: Planning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: Predicted Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/lateral/predicted_trajectory + Value: true + View Path: + Alpha: 1 + Color: 255; 255; 255 + Constant Color: true + Value: true + Width: 0.05000000074505806 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/MPC + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/mpc_follower/debug/markers + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/PurePursuit + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/controller_node_exe/debug/markers + Value: false + Enabled: true + Name: Control + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /output/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 10; 10; 10 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/goal + - Acceleration: 0 + Class: rviz_plugins/PedestrianInitialPoseTool + Interactive: false + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 1 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/CarInitialPoseTool + H vehicle height: 2 + Interactive: false + L vehicle length: 4 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 3 + W vehicle width: 1.7999999523162842 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/BusInitialPoseTool + H vehicle height: 3.5 + Interactive: false + L vehicle length: 10.5 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + W vehicle width: 2.5 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Class: rviz_plugins/MissionCheckpointTool + Pose Topic: /planning/mission_planning/checkpoint + Theta std deviation: 0.2617993950843811 + X std deviation: 0.5 + Y std deviation: 0.5 + Z position: 0 + - Class: rviz_plugins/DeleteAllObjectsTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: + - Class: rviz_default_plugins/ThirdPersonFollower + Distance: 18 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 0.20000000298023224 + Target Frame: base_link + Value: ThirdPersonFollower (rviz) + Yaw: 3.141592025756836 + - Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: TopDownOrtho + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 +Window Geometry: + AutowareDateTimePanel: + collapsed: false + AutowareStatePanel: + collapsed: false + Displays: + collapsed: false + Height: 1536 + Hide Left Dock: false + Hide Right Dock: false + InitialPoseButtonPanel: + collapsed: false + QMainWindow State: 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 + RecognitionResultOnImage: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2486 + X: 74 + Y: 27 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 cd46ff5f9636..3a5c3113bfea 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 @@ -29,6 +29,8 @@ DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("det void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) { clear_markers(); + objects_frame_id_ = msg->header.frame_id; + objs_buffer.clear(); int id = 0; for (const auto & object : msg->objects) { // TODO(Satoshi Tanaka): fixing from string label to one string @@ -78,8 +80,107 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) twist_marker_ptr->id = id++; add_marker(twist_marker_ptr); } + + // add objects to buffer for pointcloud filtering + objs_buffer.push_back(object); } } +void DetectedObjectsDisplay::onInitialize() +{ + ObjectPolygonDisplayBase::onInitialize(); + // get access to rivz node to sub and to pub to topics + 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(&DetectedObjectsDisplay::pointCloudCallback, + this, + std::placeholders::_1)); +} + +void DetectedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) +{ + sensor_msgs::msg::PointCloud2 transformed_pointcloud; + if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) + { + geometry_msgs::msg::TransformStamped transform; + transform = tf_buffer_->lookupTransform( + input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + + tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); + + } else { + transformed_pointcloud = input_pointcloud_msg; + } + + pcl::PCLPointCloud2 input_pcl_cloud; + pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); + + // Create a new point cloud with RGB color information and copy data from input cloudb + pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); + pcl::copyPointCloud(*temp_cloud, *colored_cloud); + + pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); + + if (objs_buffer.size() > 0) { + for (auto object : objs_buffer) { + + if (object.shape.type == 0) + { + pcl::PointCloud filtered_cloud; + pcl::CropBox cropBoxFilter (true); + cropBoxFilter.setInputCloud(colored_cloud); + float trans_x = object.kinematics.pose_with_covariance.pose.position.x; + float trans_y = object.kinematics.pose_with_covariance.pose.position.y; + float trans_z = object.kinematics.pose_with_covariance.pose.position.z; + float max_x = trans_x + object.shape.dimensions.x / 2.0; + float min_x = trans_x - object.shape.dimensions.x / 2.0; + float max_y = trans_y + object.shape.dimensions.y / 2.0; + float min_y = trans_y - object.shape.dimensions.y / 2.0; + float max_z = trans_z + object.shape.dimensions.z / 2.0; + float min_z = trans_z - object.shape.dimensions.z / 2.0; + + Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); + Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); + cropBoxFilter.setMin(min_pt); + cropBoxFilter.setMax(max_pt); + cropBoxFilter.filter(filtered_cloud); + + // Define a custom color for the box polygons + const uint8_t r = 30, g = 44, b = 255; + + for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) + { + cloud_it->r = r; + cloud_it->g = g; + cloud_it->b = b; + } + + *out_cloud += filtered_cloud; + + } + + } + + } + + sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); + // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); + + pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); + output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; + output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; + + // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map + // change pointcloud frame to map + // update color of the points and remove all points outside deceted objects + + + // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); + publisher_->publish(*output_pointcloud_msg_ptr); +} } // namespace object_detection } // namespace rviz_plugins 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 edf3353db9c0..3112517bb313 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 @@ -53,6 +53,8 @@ std::vector PredictedObjectsDisplay: update_id_map(msg); std::vector markers; + objects_frame_id_ = msg->header.frame_id; + objs_buffer.clear(); for (const auto & object : msg->objects) { // Get marker for shape @@ -177,6 +179,9 @@ std::vector PredictedObjectsDisplay: markers.push_back(path_confidence_marker_ptr); } } + + // add objects to buffer for pointcloud filtering + objs_buffer.push_back(object); } return markers; @@ -210,6 +215,103 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) wall_dt, ros_dt); } +void PredictedObjectsDisplay::onInitialize() +{ + ObjectPolygonDisplayBase::onInitialize(); + // get access to rivz node to sub and to pub to topics + 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(&PredictedObjectsDisplay::pointCloudCallback, + this, + std::placeholders::_1)); +} + +void PredictedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) +{ + sensor_msgs::msg::PointCloud2 transformed_pointcloud; + if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) + { + geometry_msgs::msg::TransformStamped transform; + transform = tf_buffer_->lookupTransform( + input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + + tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); + + } else { + transformed_pointcloud = input_pointcloud_msg; + } + + pcl::PCLPointCloud2 input_pcl_cloud; + pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); + + // Create a new point cloud with RGB color information and copy data from input cloudb + pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); + pcl::copyPointCloud(*temp_cloud, *colored_cloud); + + pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); + + if (objs_buffer.size() > 0) { + for (auto object : objs_buffer) { + + if (object.shape.type == 0) + { + pcl::PointCloud filtered_cloud; + pcl::CropBox cropBoxFilter (true); + cropBoxFilter.setInputCloud(colored_cloud); + float trans_x = object.kinematics.initial_pose_with_covariance.pose.position.x; + float trans_y = object.kinematics.initial_pose_with_covariance.pose.position.y; + float trans_z = object.kinematics.initial_pose_with_covariance.pose.position.z; + float max_x = trans_x + object.shape.dimensions.x / 2.0; + float min_x = trans_x - object.shape.dimensions.x / 2.0; + float max_y = trans_y + object.shape.dimensions.y / 2.0; + float min_y = trans_y - object.shape.dimensions.y / 2.0; + float max_z = trans_z + object.shape.dimensions.z / 2.0; + float min_z = trans_z - object.shape.dimensions.z / 2.0; + + Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); + Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); + cropBoxFilter.setMin(min_pt); + cropBoxFilter.setMax(max_pt); + cropBoxFilter.filter(filtered_cloud); + + // Define a custom color for the box polygons + const uint8_t r = 30, g = 44, b = 255; + + for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) + { + cloud_it->r = r; + cloud_it->g = g; + cloud_it->b = b; + } + + *out_cloud += filtered_cloud; + + } + + } + + } + + sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); + // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); + + pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); + output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; + output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; + + // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map + // change pointcloud frame to map + // update color of the points and remove all points outside deceted objects + + + // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); + publisher_->publish(*output_pointcloud_msg_ptr); +} + } // namespace object_detection } // namespace rviz_plugins } // namespace autoware 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 1e739ea48977..e1ad2aa4a334 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 @@ -30,6 +30,8 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) { clear_markers(); update_id_map(msg); + objects_frame_id_ = msg->header.frame_id; + objs_buffer.clear(); for (const auto & object : msg->objects) { // Get marker for shape @@ -118,7 +120,107 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) twist_marker_ptr->id = uuid_to_marker_id(object.object_id); add_marker(twist_marker_ptr); } + + // add objects to buffer for pointcloud filtering + objs_buffer.push_back(object); + } +} + +void TrackedObjectsDisplay::onInitialize() +{ + ObjectPolygonDisplayBase::onInitialize(); + // get access to rivz node to sub and to pub to topics + 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(&TrackedObjectsDisplay::pointCloudCallback, + this, + std::placeholders::_1)); +} + +void TrackedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) +{ + sensor_msgs::msg::PointCloud2 transformed_pointcloud; + if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) + { + geometry_msgs::msg::TransformStamped transform; + transform = tf_buffer_->lookupTransform( + input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + + tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); + + } else { + transformed_pointcloud = input_pointcloud_msg; + } + + pcl::PCLPointCloud2 input_pcl_cloud; + pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); + + // Create a new point cloud with RGB color information and copy data from input cloudb + pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); + pcl::copyPointCloud(*temp_cloud, *colored_cloud); + + pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); + + if (objs_buffer.size() > 0) { + for (auto object : objs_buffer) { + + if (object.shape.type == 0) + { + pcl::PointCloud filtered_cloud; + pcl::CropBox cropBoxFilter (true); + cropBoxFilter.setInputCloud(colored_cloud); + float trans_x = object.kinematics.pose_with_covariance.pose.position.x; + float trans_y = object.kinematics.pose_with_covariance.pose.position.y; + float trans_z = object.kinematics.pose_with_covariance.pose.position.z; + float max_x = trans_x + object.shape.dimensions.x / 2.0; + float min_x = trans_x - object.shape.dimensions.x / 2.0; + float max_y = trans_y + object.shape.dimensions.y / 2.0; + float min_y = trans_y - object.shape.dimensions.y / 2.0; + float max_z = trans_z + object.shape.dimensions.z / 2.0; + float min_z = trans_z - object.shape.dimensions.z / 2.0; + + Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); + Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); + cropBoxFilter.setMin(min_pt); + cropBoxFilter.setMax(max_pt); + cropBoxFilter.filter(filtered_cloud); + + // Define a custom color for the box polygons + const uint8_t r = 30, g = 44, b = 255; + + for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) + { + cloud_it->r = r; + cloud_it->g = g; + cloud_it->b = b; + } + + *out_cloud += filtered_cloud; + + } + + } + } + + sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); + // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); + + pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); + output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; + output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; + + // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map + // change pointcloud frame to map + // update color of the points and remove all points outside deceted objects + + + // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); + publisher_->publish(*output_pointcloud_msg_ptr); } } // namespace object_detection From 39e15a34e39e5d538c0e39b77e927238482ecf38 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Wed, 1 Mar 2023 23:38:02 +0800 Subject: [PATCH 04/58] feat(add_perception_objects_pointcloud_publisher) wip Signed-off-by: Alexey Panferov --- .../detected_objects_display.hpp | 6 +- .../object_polygon_display_base.hpp | 100 ++++++++++- .../predicted_objects_display.hpp | 6 +- .../tracked_objects_display.hpp | 6 +- .../launch/rviz.launch.xml | 6 + .../detected_objects_display.cpp | 169 +++++++++--------- .../predicted_objects_display.cpp | 165 ++++++++--------- .../tracked_objects_display.cpp | 167 ++++++++--------- 8 files changed, 366 insertions(+), 259 deletions(-) create mode 100644 common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index c1904a8a1f75..bc76a363a26e 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -40,11 +40,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay private: void processMessage(DetectedObjects::ConstSharedPtr msg) override; void onInitialize() override; - void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; + // void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; - std::string objects_frame_id_; - std::vector objs_buffer; + // std::string objects_frame_id_; + // std::vector objs_buffer; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 2228e8ec964d..63866c7ed858 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -63,6 +63,16 @@ namespace rviz_plugins { namespace object_detection { + + // struct for creating objects buffer +struct object_info +{ + autoware_auto_perception_msgs::msg::Shape shape; + geometry_msgs::msg::Point position; + geometry_msgs::msg::Quaternion orientation; + autoware_auto_perception_msgs::msg::ObjectClassification classification; +}; + /// \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. @@ -428,7 +438,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase double get_line_width() { return m_line_width_property.getFloat(); } - // std::vector objs_buffer; // object buffer to filtrate input pointcloud + // std::vector objs_buffer; // object buffer to filtrate input bcloud // std::string objects_frame_id_; // void transformPointCloud() @@ -442,6 +452,12 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // Default pointcloud topic; rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic; + + + // buffer for restoring information about objects to filtrate input pointcloud + std::vector objs_buffer; + std::string objects_frame_id_; + private: // All rviz plugins should have this. Should be initialized with pointer to this class MarkerCommon m_marker_common; @@ -477,7 +493,87 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::vector predicted_path_colors; - virtual void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) = 0; + void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) + { + sensor_msgs::msg::PointCloud2 transformed_pointcloud; + if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) + { + geometry_msgs::msg::TransformStamped transform; + transform = tf_buffer_.lookupTransform( + input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + + tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); + + } else { + transformed_pointcloud = input_pointcloud_msg; + } + + pcl::PCLPointCloud2 input_pcl_cloud; + pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); + + // Create a new point cloud with RGB color information and copy data from input cloudb + pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); + pcl::copyPointCloud(*temp_cloud, *colored_cloud); + + pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); + + if (objs_buffer.size() > 0) { + for (auto object : objs_buffer) { + + if (object.shape.type == 0) + { + pcl::PointCloud filtered_cloud; + pcl::CropBox cropBoxFilter (true); + cropBoxFilter.setInputCloud(colored_cloud); + float trans_x = object.kinematics.pose_with_covariance.pose.position.x; + float trans_y = object.kinematics.pose_with_covariance.pose.position.y; + float trans_z = object.kinematics.pose_with_covariance.pose.position.z; + float max_x = trans_x + object.shape.dimensions.x / 2.0; + float min_x = trans_x - object.shape.dimensions.x / 2.0; + float max_y = trans_y + object.shape.dimensions.y / 2.0; + float min_y = trans_y - object.shape.dimensions.y / 2.0; + float max_z = trans_z + object.shape.dimensions.z / 2.0; + float min_z = trans_z - object.shape.dimensions.z / 2.0; + + Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); + Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); + cropBoxFilter.setMin(min_pt); + cropBoxFilter.setMax(max_pt); + cropBoxFilter.filter(filtered_cloud); + + // Define a custom color for the box polygons + const uint8_t r = 30, g = 44, b = 255; + + for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) + { + cloud_it->r = r; + cloud_it->g = g; + cloud_it->b = b; + } + + *out_cloud += filtered_cloud; + + } + + } + + } + + sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); + // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); + + pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); + output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; + output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; + + RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); + publisher_->publish(*output_pointcloud_msg_ptr); + + + publisher_->publish(input_pointcloud_msg); + }; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index 8e2fc180fc38..ebf76dced0b2 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -49,7 +49,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay private: void processMessage(PredictedObjects::ConstSharedPtr msg) override; void onInitialize() override; - void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; + // void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) { @@ -116,8 +116,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay std::condition_variable condition; std::vector markers; - std::string objects_frame_id_; - std::vector objs_buffer; + // std::string objects_frame_id_; + // std::vector objs_buffer; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 0099c1f732f7..50b19e92be0a 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -47,7 +47,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay private: void processMessage(TrackedObjects::ConstSharedPtr msg) override; void onInitialize() override; - void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; + // void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) { @@ -100,8 +100,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay std::list unused_marker_ids; int32_t marker_id = 0; - std::string objects_frame_id_; - std::vector objs_buffer; + // std::string objects_frame_id_; + // std::vector objs_buffer; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml b/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml new file mode 100644 index 000000000000..282b14739685 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file 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 3a5c3113bfea..e781502a1b28 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 @@ -29,7 +29,7 @@ DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("det void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) { clear_markers(); - objects_frame_id_ = msg->header.frame_id; + // objects_frame_id_ = msg->header.frame_id; objs_buffer.clear(); int id = 0; for (const auto & object : msg->objects) { @@ -82,105 +82,108 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) } // add objects to buffer for pointcloud filtering - objs_buffer.push_back(object); + objs_buffer.push_back({object.shape, object.kinematics.pose_with_covariance.pose.position, + object.kinematics.pose_with_covariance.pose.orientation, object.classification}); } } -void DetectedObjectsDisplay::onInitialize() + +void DetectedObjectsDisplay::onInitialize() override { ObjectPolygonDisplayBase::onInitialize(); // get access to rivz node to sub and to pub to topics 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(&DetectedObjectsDisplay::pointCloudCallback, - this, - std::placeholders::_1)); + publisher_ = raw_node->create_publisher("output/detected_objects_pointcloud", 10); + // pointcloud_subscription_ = raw_node->create_subscription( + // m_default_pointcloud_topic->getTopicStd(), + // rclcpp::SensorDataQoS(), + // std::bind(&DetectedObjectsDisplay::pointCloudCallback, + // this, + // std::placeholders::_1)); } -void DetectedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) -{ - sensor_msgs::msg::PointCloud2 transformed_pointcloud; - if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) - { - geometry_msgs::msg::TransformStamped transform; - transform = tf_buffer_->lookupTransform( - input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - - tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); +// void DetectedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) +// { +// sensor_msgs::msg::PointCloud2 transformed_pointcloud; +// if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) +// { +// geometry_msgs::msg::TransformStamped transform; +// transform = tf_buffer_->lookupTransform( +// input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + +// tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); - } else { - transformed_pointcloud = input_pointcloud_msg; - } +// } else { +// transformed_pointcloud = input_pointcloud_msg; +// } - pcl::PCLPointCloud2 input_pcl_cloud; - pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); - pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); - pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); +// pcl::PCLPointCloud2 input_pcl_cloud; +// pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); +// pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); +// pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); - // Create a new point cloud with RGB color information and copy data from input cloudb - pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); - pcl::copyPointCloud(*temp_cloud, *colored_cloud); - - pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - - if (objs_buffer.size() > 0) { - for (auto object : objs_buffer) { - - if (object.shape.type == 0) - { - pcl::PointCloud filtered_cloud; - pcl::CropBox cropBoxFilter (true); - cropBoxFilter.setInputCloud(colored_cloud); - float trans_x = object.kinematics.pose_with_covariance.pose.position.x; - float trans_y = object.kinematics.pose_with_covariance.pose.position.y; - float trans_z = object.kinematics.pose_with_covariance.pose.position.z; - float max_x = trans_x + object.shape.dimensions.x / 2.0; - float min_x = trans_x - object.shape.dimensions.x / 2.0; - float max_y = trans_y + object.shape.dimensions.y / 2.0; - float min_y = trans_y - object.shape.dimensions.y / 2.0; - float max_z = trans_z + object.shape.dimensions.z / 2.0; - float min_z = trans_z - object.shape.dimensions.z / 2.0; - - Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); - Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); - cropBoxFilter.setMin(min_pt); - cropBoxFilter.setMax(max_pt); - cropBoxFilter.filter(filtered_cloud); - - // Define a custom color for the box polygons - const uint8_t r = 30, g = 44, b = 255; - - for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) - { - cloud_it->r = r; - cloud_it->g = g; - cloud_it->b = b; - } - - *out_cloud += filtered_cloud; - - } - - } +// // Create a new point cloud with RGB color information and copy data from input cloudb +// pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); +// pcl::copyPointCloud(*temp_cloud, *colored_cloud); + +// pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); + +// if (objs_buffer.size() > 0) { +// for (auto object : objs_buffer) { + +// if (object.shape.type == 0) +// { +// pcl::PointCloud filtered_cloud; +// pcl::CropBox cropBoxFilter (true); +// cropBoxFilter.setInputCloud(colored_cloud); +// float trans_x = object.kinematics.pose_with_covariance.pose.position.x; +// float trans_y = object.kinematics.pose_with_covariance.pose.position.y; +// float trans_z = object.kinematics.pose_with_covariance.pose.position.z; +// float max_x = trans_x + object.shape.dimensions.x / 2.0; +// float min_x = trans_x - object.shape.dimensions.x / 2.0; +// float max_y = trans_y + object.shape.dimensions.y / 2.0; +// float min_y = trans_y - object.shape.dimensions.y / 2.0; +// float max_z = trans_z + object.shape.dimensions.z / 2.0; +// float min_z = trans_z - object.shape.dimensions.z / 2.0; + +// Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); +// Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); +// cropBoxFilter.setMin(min_pt); +// cropBoxFilter.setMax(max_pt); +// cropBoxFilter.filter(filtered_cloud); + +// // Define a custom color for the box polygons +// const uint8_t r = 30, g = 44, b = 255; + +// for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) +// { +// cloud_it->r = r; +// cloud_it->g = g; +// cloud_it->b = b; +// } + +// *out_cloud += filtered_cloud; + +// } + +// } - } +// } - sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); - // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); +// sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); +// // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); - pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); - output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; - output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; +// pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); +// output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; +// output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; - // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map - // change pointcloud frame to map - // update color of the points and remove all points outside deceted objects +// // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map +// // change pointcloud frame to map +// // update color of the points and remove all points outside deceted objects - // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); - publisher_->publish(*output_pointcloud_msg_ptr); -} +// // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); +// publisher_->publish(*output_pointcloud_msg_ptr); +// } } // namespace object_detection } // namespace rviz_plugins 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 3112517bb313..9ab7c4b7719a 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 @@ -181,7 +181,7 @@ std::vector PredictedObjectsDisplay: } // add objects to buffer for pointcloud filtering - objs_buffer.push_back(object); + // objs_buffer.push_back(object); } return markers; @@ -215,102 +215,103 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) wall_dt, ros_dt); } -void PredictedObjectsDisplay::onInitialize() +void PredictedObjectsDisplay::onInitialize() override { ObjectPolygonDisplayBase::onInitialize(); // get access to rivz node to sub and to pub to topics 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(&PredictedObjectsDisplay::pointCloudCallback, - this, - std::placeholders::_1)); + publisher_ = raw_node->create_publisher("output/predicted_objects_pointcloud", 10); + // pointcloud_subscription_ = raw_node->create_subscription( + // m_default_pointcloud_topic->getTopicStd(), + // rclcpp::SensorDataQoS(), + // std::bind(&PredictedObjectsDisplay::pointCloudCallback, + // this, + // std::placeholders::_1)); } -void PredictedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) -{ - sensor_msgs::msg::PointCloud2 transformed_pointcloud; - if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) - { - geometry_msgs::msg::TransformStamped transform; - transform = tf_buffer_->lookupTransform( - input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - - tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); +// void PredictedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) +// { +// sensor_msgs::msg::PointCloud2 transformed_pointcloud; +// if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) +// { +// geometry_msgs::msg::TransformStamped transform; +// transform = tf_buffer_->lookupTransform( +// input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + +// tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); - } else { - transformed_pointcloud = input_pointcloud_msg; - } +// } else { +// transformed_pointcloud = input_pointcloud_msg; +// } - pcl::PCLPointCloud2 input_pcl_cloud; - pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); - pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); - pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); +// pcl::PCLPointCloud2 input_pcl_cloud; +// pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); +// pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); +// pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); - // Create a new point cloud with RGB color information and copy data from input cloudb - pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); - pcl::copyPointCloud(*temp_cloud, *colored_cloud); - - pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - - if (objs_buffer.size() > 0) { - for (auto object : objs_buffer) { - - if (object.shape.type == 0) - { - pcl::PointCloud filtered_cloud; - pcl::CropBox cropBoxFilter (true); - cropBoxFilter.setInputCloud(colored_cloud); - float trans_x = object.kinematics.initial_pose_with_covariance.pose.position.x; - float trans_y = object.kinematics.initial_pose_with_covariance.pose.position.y; - float trans_z = object.kinematics.initial_pose_with_covariance.pose.position.z; - float max_x = trans_x + object.shape.dimensions.x / 2.0; - float min_x = trans_x - object.shape.dimensions.x / 2.0; - float max_y = trans_y + object.shape.dimensions.y / 2.0; - float min_y = trans_y - object.shape.dimensions.y / 2.0; - float max_z = trans_z + object.shape.dimensions.z / 2.0; - float min_z = trans_z - object.shape.dimensions.z / 2.0; - - Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); - Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); - cropBoxFilter.setMin(min_pt); - cropBoxFilter.setMax(max_pt); - cropBoxFilter.filter(filtered_cloud); - - // Define a custom color for the box polygons - const uint8_t r = 30, g = 44, b = 255; - - for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) - { - cloud_it->r = r; - cloud_it->g = g; - cloud_it->b = b; - } - - *out_cloud += filtered_cloud; - - } - - } +// // Create a new point cloud with RGB color information and copy data from input cloudb +// pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); +// pcl::copyPointCloud(*temp_cloud, *colored_cloud); + +// pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); + +// if (objs_buffer.size() > 0) { +// for (auto object : objs_buffer) { + +// if (object.shape.type == 0) +// { +// pcl::PointCloud filtered_cloud; +// pcl::CropBox cropBoxFilter (true); +// cropBoxFilter.setInputCloud(colored_cloud); +// float trans_x = object.kinematics.initial_pose_with_covariance.pose.position.x; +// float trans_y = object.kinematics.initial_pose_with_covariance.pose.position.y; +// float trans_z = object.kinematics.initial_pose_with_covariance.pose.position.z; +// float max_x = trans_x + object.shape.dimensions.x / 2.0; +// float min_x = trans_x - object.shape.dimensions.x / 2.0; +// float max_y = trans_y + object.shape.dimensions.y / 2.0; +// float min_y = trans_y - object.shape.dimensions.y / 2.0; +// float max_z = trans_z + object.shape.dimensions.z / 2.0; +// float min_z = trans_z - object.shape.dimensions.z / 2.0; + +// Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); +// Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); +// cropBoxFilter.setMin(min_pt); +// cropBoxFilter.setMax(max_pt); +// cropBoxFilter.filter(filtered_cloud); + +// // Define a custom color for the box polygons +// const uint8_t r = 30, g = 44, b = 255; + +// for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) +// { +// cloud_it->r = r; +// cloud_it->g = g; +// cloud_it->b = b; +// } + +// *out_cloud += filtered_cloud; + +// } + +// } - } +// } - sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); - // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); +// sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); +// // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); - pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); - output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; - output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; +// pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); +// output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; +// output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; - // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map - // change pointcloud frame to map - // update color of the points and remove all points outside deceted objects +// // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map +// // change pointcloud frame to map +// // update color of the points and remove all points outside deceted objects - // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); - publisher_->publish(*output_pointcloud_msg_ptr); -} +// // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); +// publisher_->publish(*output_pointcloud_msg_ptr); +// } } // namespace object_detection } // namespace rviz_plugins 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 e1ad2aa4a334..8b8e129b295c 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 @@ -30,7 +30,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) { clear_markers(); update_id_map(msg); - objects_frame_id_ = msg->header.frame_id; + // objects_frame_id_ = msg->header.frame_id; objs_buffer.clear(); for (const auto & object : msg->objects) { @@ -122,106 +122,107 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) } // add objects to buffer for pointcloud filtering - objs_buffer.push_back(object); + // objs_buffer.push_back(object); } } -void TrackedObjectsDisplay::onInitialize() +void TrackedObjectsDisplay::onInitialize() override { ObjectPolygonDisplayBase::onInitialize(); // get access to rivz node to sub and to pub to topics 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(&TrackedObjectsDisplay::pointCloudCallback, - this, - std::placeholders::_1)); + publisher_ = raw_node->create_publisher("output/tracked_objects_pointcloud", 10); + // pointcloud_subscription_ = raw_node->create_subscription( + // m_default_pointcloud_topic->getTopicStd(), + // rclcpp::SensorDataQoS(), + // std::bind(&TrackedObjectsDisplay::pointCloudCallback, + // this, + // std::placeholders::_1)); } -void TrackedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) -{ - sensor_msgs::msg::PointCloud2 transformed_pointcloud; - if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) - { - geometry_msgs::msg::TransformStamped transform; - transform = tf_buffer_->lookupTransform( - input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - - tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); +// void TrackedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) +// { +// sensor_msgs::msg::PointCloud2 transformed_pointcloud; +// if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) +// { +// geometry_msgs::msg::TransformStamped transform; +// transform = tf_buffer_->lookupTransform( +// input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + +// tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); - } else { - transformed_pointcloud = input_pointcloud_msg; - } +// } else { +// transformed_pointcloud = input_pointcloud_msg; +// } - pcl::PCLPointCloud2 input_pcl_cloud; - pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); - pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); - pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); +// pcl::PCLPointCloud2 input_pcl_cloud; +// pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); +// pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); +// pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); - // Create a new point cloud with RGB color information and copy data from input cloudb - pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); - pcl::copyPointCloud(*temp_cloud, *colored_cloud); - - pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - - if (objs_buffer.size() > 0) { - for (auto object : objs_buffer) { - - if (object.shape.type == 0) - { - pcl::PointCloud filtered_cloud; - pcl::CropBox cropBoxFilter (true); - cropBoxFilter.setInputCloud(colored_cloud); - float trans_x = object.kinematics.pose_with_covariance.pose.position.x; - float trans_y = object.kinematics.pose_with_covariance.pose.position.y; - float trans_z = object.kinematics.pose_with_covariance.pose.position.z; - float max_x = trans_x + object.shape.dimensions.x / 2.0; - float min_x = trans_x - object.shape.dimensions.x / 2.0; - float max_y = trans_y + object.shape.dimensions.y / 2.0; - float min_y = trans_y - object.shape.dimensions.y / 2.0; - float max_z = trans_z + object.shape.dimensions.z / 2.0; - float min_z = trans_z - object.shape.dimensions.z / 2.0; - - Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); - Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); - cropBoxFilter.setMin(min_pt); - cropBoxFilter.setMax(max_pt); - cropBoxFilter.filter(filtered_cloud); - - // Define a custom color for the box polygons - const uint8_t r = 30, g = 44, b = 255; - - for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) - { - cloud_it->r = r; - cloud_it->g = g; - cloud_it->b = b; - } - - *out_cloud += filtered_cloud; - - } - - } +// // Create a new point cloud with RGB color information and copy data from input cloudb +// pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); +// pcl::copyPointCloud(*temp_cloud, *colored_cloud); + +// pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); + +// if (objs_buffer.size() > 0) { +// for (auto object : objs_buffer) { + +// if (object.shape.type == 0) +// { +// pcl::PointCloud filtered_cloud; +// pcl::CropBox cropBoxFilter (true); +// cropBoxFilter.setInputCloud(colored_cloud); +// float trans_x = object.kinematics.pose_with_covariance.pose.position.x; +// float trans_y = object.kinematics.pose_with_covariance.pose.position.y; +// float trans_z = object.kinematics.pose_with_covariance.pose.position.z; +// float max_x = trans_x + object.shape.dimensions.x / 2.0; +// float min_x = trans_x - object.shape.dimensions.x / 2.0; +// float max_y = trans_y + object.shape.dimensions.y / 2.0; +// float min_y = trans_y - object.shape.dimensions.y / 2.0; +// float max_z = trans_z + object.shape.dimensions.z / 2.0; +// float min_z = trans_z - object.shape.dimensions.z / 2.0; + +// Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); +// Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); +// cropBoxFilter.setMin(min_pt); +// cropBoxFilter.setMax(max_pt); +// cropBoxFilter.filter(filtered_cloud); + +// // Define a custom color for the box polygons +// const uint8_t r = 30, g = 44, b = 255; + +// for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) +// { +// cloud_it->r = r; +// cloud_it->g = g; +// cloud_it->b = b; +// } + +// *out_cloud += filtered_cloud; + +// } + +// } - } +// } - sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); - // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); +// sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); +// // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); - pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); - output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; - output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; +// pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); +// output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; +// output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; - // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map - // change pointcloud frame to map - // update color of the points and remove all points outside deceted objects +// // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map +// // change pointcloud frame to map +// // update color of the points and remove all points outside deceted objects - // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); - publisher_->publish(*output_pointcloud_msg_ptr); -} +// // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); +// publisher_->publish(*output_pointcloud_msg_ptr); +// } } // namespace object_detection } // namespace rviz_plugins From d52e2497f482186389da88941bcfba3aa6c238de Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Wed, 8 Mar 2023 13:35:30 +0800 Subject: [PATCH 05/58] feat(add_perception_objects_pointcloud_publisher) update autoware_auto_rviz plugin to publish pointclouds Signed-off-by: Alexey Panferov --- .../object_polygon_display_base.hpp | 254 +++++++++++++----- .../predicted_objects_display.hpp | 5 + .../package.xml | 6 + .../detected_objects_display.cpp | 24 +- .../predicted_objects_display.cpp | 139 ++++------ .../tracked_objects_display.cpp | 23 +- .../detected_objects_pointcloud_publisher | 1 - 7 files changed, 285 insertions(+), 167 deletions(-) delete mode 160000 perception/detected_objects_pointcloud_publisher diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 63866c7ed858..8a20cacc9e89 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -41,15 +41,23 @@ #include #include #include +#include +#include +#include +#include +#include #include #include #include -#include +#include #include +#include #include +#include +#include #include #include #include @@ -64,15 +72,31 @@ namespace rviz_plugins namespace object_detection { +using Polygon2d = tier4_autoware_utils::Polygon2d; + // struct for creating objects buffer struct object_info { autoware_auto_perception_msgs::msg::Shape shape; - geometry_msgs::msg::Point position; - geometry_msgs::msg::Quaternion orientation; + geometry_msgs::msg::Pose position; autoware_auto_perception_msgs::msg::ObjectClassification 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); +} + + /// \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. @@ -438,13 +462,72 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase double get_line_width() { return m_line_width_property.getFloat(); } - // std::vector objs_buffer; // object buffer to filtrate input bcloud - // std::string objects_frame_id_; + // 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("perception_utils"), ex.what()); // rename + return boost::none; + } + } + + // transfrom 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; + } - // void transformPointCloud() - - std::shared_ptr tf_listener_{nullptr}; - std::unique_ptr tf_buffer_; + // 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); + } else 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 = max_dist < dist ? dist : max_dist; + } + return max_dist; + } else { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "unknown shape type"); + return std::nullopt; + } + } + + std::shared_ptr tf_listener_{nullptr} // !! different type in prototype, + std::unique_ptr tf_buffer_; // !! different type in prototype // add pointcloud subscription rclcpp::Subscription::SharedPtr pointcloud_subscription_; rclcpp::Publisher::SharedPtr publisher_; @@ -452,11 +535,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // Default pointcloud topic; rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic; - - - // buffer for restoring information about objects to filtrate input pointcloud + // variables for transfer detected objects information between callbacks std::vector objs_buffer; std::string objects_frame_id_; + std::string pointcloud_frame_id_; private: // All rviz plugins should have this. Should be initialized with pointer to this class @@ -492,88 +574,116 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::vector predicted_path_colors; - void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) { - sensor_msgs::msg::PointCloud2 transformed_pointcloud; - if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) - { - geometry_msgs::msg::TransformStamped transform; - transform = tf_buffer_.lookupTransform( - input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - - tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); - - } else { - transformed_pointcloud = input_pointcloud_msg; - } + // RCLCPP_INFO(this->get_logger(), "Get new pointcloud"); + pointcloud_frame_id_ = input_pointcloud_msg.header.frame_id; + + pcl::PointXYZ minPt, maxPt; + pcl::PointCloud measured_cloud; + pcl::fromROSMsg(input_pointcloud_msg, measured_cloud); + pcl::getMinMax3D(measured_cloud, minPt, maxPt); + + // RCLCPP_INFO(this->get_logger(), "before translation max X is '%f' max Y is '%f'", maxPt.x, maxPt.y); + // RCLCPP_INFO(this->get_logger(), "before translation min X is '%f' min Y is '%f'", minPt.x, minPt.y); - pcl::PCLPointCloud2 input_pcl_cloud; - pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); + // convert to pcl pointcloud pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); - pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); + // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); + pcl::fromROSMsg(input_pointcloud_msg, *temp_cloud); + + // pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); + // pcl::fromROSMsg(transformed_pointcloud, *colored_cloud); // Create a new point cloud with RGB color information and copy data from input cloudb 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()); if (objs_buffer.size() > 0) { + // RCLCPP_INFO(this->get_logger(), "Filtering pointcloud"); for (auto object : objs_buffer) { - - if (object.shape.type == 0) - { - pcl::PointCloud filtered_cloud; - pcl::CropBox cropBoxFilter (true); - cropBoxFilter.setInputCloud(colored_cloud); - float trans_x = object.kinematics.pose_with_covariance.pose.position.x; - float trans_y = object.kinematics.pose_with_covariance.pose.position.y; - float trans_z = object.kinematics.pose_with_covariance.pose.position.z; - float max_x = trans_x + object.shape.dimensions.x / 2.0; - float min_x = trans_x - object.shape.dimensions.x / 2.0; - float max_y = trans_y + object.shape.dimensions.y / 2.0; - float min_y = trans_y - object.shape.dimensions.y / 2.0; - float max_z = trans_z + object.shape.dimensions.z / 2.0; - float min_z = trans_z - object.shape.dimensions.z / 2.0; - - Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); - Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); - cropBoxFilter.setMin(min_pt); - cropBoxFilter.setMax(max_pt); - cropBoxFilter.filter(filtered_cloud); - - // Define a custom color for the box polygons - const uint8_t r = 30, g = 44, b = 255; - - for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) - { - cloud_it->r = r; - cloud_it->g = g; - cloud_it->b = b; - } - - *out_cloud += filtered_cloud; - - } - - } - + // RCLCPP_INFO(this->get_logger(), "object"); + + const auto search_radius = getMaxRadius(object); + // Search neighbor pointcloud to reduce cost. + pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); + std::vector indices; + std::vector distances; + kdtree->radiusSearch( + toPCL(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, object); + } + } else { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); + return; } + sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); - // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); - + // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); + // *output_pointcloud_msg_ptr = transformed_pointcloud; + output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; - output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; + output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; // remove - RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); + // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); publisher_->publish(*output_pointcloud_msg_ptr); + } + + 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 = + tier4_autoware_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); + + // Define a custom color for the box polygons + const uint8_t r = 30, g = 44, b = 255; + + for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) + { + cloud_it->r = r; + cloud_it->g = g; + cloud_it->b = b; + } + + *out_cloud += filtered_cloud; + } - publisher_->publish(input_pointcloud_msg); - }; +// virtual void objectsCallback(typename MsgT::ConstSharedPtr msg) = 0; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index ebf76dced0b2..1cc3844773b2 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -104,6 +104,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay void update(float wall_dt, float ros_dt) override; + bool transformObjects( + const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, + const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + autoware_auto_perception_msgs::msg::PredictedObjects & output_msg); + std::unordered_map> id_map; // std::unordered_map id_map; std::list unused_marker_ids; diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index 81d4425df9b0..d1ea3fc9d9d5 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -28,6 +28,12 @@ tf2_sensor_msgs tf2_geometry_msgs tf2_eigen + tier4_autoware_utils + tier4_autoware_utils/math + perception_utils + pcl_common + + libqt5-widgets rviz2 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 e781502a1b28..1c548b3fccfb 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 @@ -29,8 +29,6 @@ DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("det void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) { clear_markers(); - // objects_frame_id_ = msg->header.frame_id; - objs_buffer.clear(); int id = 0; for (const auto & object : msg->objects) { // TODO(Satoshi Tanaka): fixing from string label to one string @@ -81,9 +79,25 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(twist_marker_ptr); } - // add objects to buffer for pointcloud filtering - objs_buffer.push_back({object.shape, object.kinematics.pose_with_covariance.pose.position, - object.kinematics.pose_with_covariance.pose.orientation, object.classification}); + // Transform to pointcloud frame + autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; + if (!transformObjects( + *msg, pointcloud_frame_id_, tf_buffer_, + transformed_objects)) { + // objects_pub_->publish(*input_objects); + return; + } + + objects_frame_id_ = transformed_objects.header.frame_id; // remove + + objs_buffer.clear(); + for (const auto & object : transformed_objects.objects) + { + std::vector labels = object.classification; + object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + objs_buffer.push_back(info); + } + // RCLCPP_INFO(this->get_logger(), "Update objects buffer"); } } 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 9ab7c4b7719a..425f8d73ed92 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 @@ -53,8 +53,6 @@ std::vector PredictedObjectsDisplay: update_id_map(msg); std::vector markers; - objects_frame_id_ = msg->header.frame_id; - objs_buffer.clear(); for (const auto & object : msg->objects) { // Get marker for shape @@ -182,6 +180,30 @@ std::vector PredictedObjectsDisplay: // add objects to buffer for pointcloud filtering // objs_buffer.push_back(object); + + autoware_auto_perception_msgs::msg::PredictedObjects transformed_objects; + if (!transformObjects( + *msg, pointcloud_frame_id_, tf_buffer_, + transformed_objects)) { + // objects_pub_->publish(*input_objects); + // RCLCPP_INFO(this->get_logger(), "Did NOT transform objects"); + return; + } + // RCLCPP_INFO(this->get_logger(), "Transform DONE"); + // RCLCPP_INFO(this->get_logger(), "After transform objects frame is '%s'", transformed_objects.header.frame_id.c_str()); + // RCLCPP_INFO(this->get_logger(), "First object X is '%f' Y is '%f'", + // transformed_objects.objects.at(0).kinematics.initial_pose_with_covariance.pose.position.x, + // transformed_objects.objects.at(0).kinematics.initial_pose_with_covariance.pose.position.y); + + objects_frame_id_ = transformed_objects.header.frame_id; + objs_buffer.clear(); + for (const auto & object : transformed_objects.objects) + { + std::vector labels = object.classification; + object_info info = {object.shape, object.kinematics.initial_pose_with_covariance.pose, object.classification}; + objs_buffer.push_back(info); + } + // RCLCPP_INFO(this->get_logger(), "Update objects buffer"); } return markers; @@ -229,89 +251,36 @@ void PredictedObjectsDisplay::onInitialize() override // std::placeholders::_1)); } -// void PredictedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) -// { -// sensor_msgs::msg::PointCloud2 transformed_pointcloud; -// if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) -// { -// geometry_msgs::msg::TransformStamped transform; -// transform = tf_buffer_->lookupTransform( -// input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - -// tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); - -// } else { -// transformed_pointcloud = input_pointcloud_msg; -// } - -// pcl::PCLPointCloud2 input_pcl_cloud; -// pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); -// pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); -// pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); - -// // Create a new point cloud with RGB color information and copy data from input cloudb -// pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); -// pcl::copyPointCloud(*temp_cloud, *colored_cloud); - -// pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - -// if (objs_buffer.size() > 0) { -// for (auto object : objs_buffer) { - -// if (object.shape.type == 0) -// { -// pcl::PointCloud filtered_cloud; -// pcl::CropBox cropBoxFilter (true); -// cropBoxFilter.setInputCloud(colored_cloud); -// float trans_x = object.kinematics.initial_pose_with_covariance.pose.position.x; -// float trans_y = object.kinematics.initial_pose_with_covariance.pose.position.y; -// float trans_z = object.kinematics.initial_pose_with_covariance.pose.position.z; -// float max_x = trans_x + object.shape.dimensions.x / 2.0; -// float min_x = trans_x - object.shape.dimensions.x / 2.0; -// float max_y = trans_y + object.shape.dimensions.y / 2.0; -// float min_y = trans_y - object.shape.dimensions.y / 2.0; -// float max_z = trans_z + object.shape.dimensions.z / 2.0; -// float min_z = trans_z - object.shape.dimensions.z / 2.0; - -// Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); -// Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); -// cropBoxFilter.setMin(min_pt); -// cropBoxFilter.setMax(max_pt); -// cropBoxFilter.filter(filtered_cloud); - -// // Define a custom color for the box polygons -// const uint8_t r = 30, g = 44, b = 255; - -// for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) -// { -// cloud_it->r = r; -// cloud_it->g = g; -// cloud_it->b = b; -// } - -// *out_cloud += filtered_cloud; - -// } - -// } - -// } - -// sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); -// // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); - -// pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); -// output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; -// output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; - -// // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map -// // change pointcloud frame to map -// // update color of the points and remove all points outside deceted objects - - -// // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); -// publisher_->publish(*output_pointcloud_msg_ptr); -// } +bool transformObjects( + const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, + const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + autoware_auto_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; +} + } // namespace object_detection } // namespace rviz_plugins 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 8b8e129b295c..b0797fe46144 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 @@ -30,8 +30,6 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) { clear_markers(); update_id_map(msg); - // objects_frame_id_ = msg->header.frame_id; - objs_buffer.clear(); for (const auto & object : msg->objects) { // Get marker for shape @@ -121,8 +119,25 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(twist_marker_ptr); } - // add objects to buffer for pointcloud filtering - // objs_buffer.push_back(object); + // Transform to pointcloud frame + autoware_auto_perception_msgs::msg::TrackedObjects transformed_objects; + if (!transformObjects( + *msg, pointcloud_frame_id_, tf_buffer_, + transformed_objects)) { + // objects_pub_->publish(*input_objects); + return; + } + + objects_frame_id_ = transformed_objects.header.frame_id; + + objs_buffer.clear(); + for (const auto & object : transformed_objects.objects) + { + std::vector labels = object.classification; + object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + objs_buffer.push_back(info); + } + // RCLCPP_INFO(this->get_logger(), "Update objects buffer");; } } diff --git a/perception/detected_objects_pointcloud_publisher b/perception/detected_objects_pointcloud_publisher deleted file mode 160000 index 97dff7b66bda..000000000000 --- a/perception/detected_objects_pointcloud_publisher +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 97dff7b66bdadf4c16697045474ff3bebd3b1444 From 7d88491bc1fa4b6a750d3d905475aaa5f07371de Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Wed, 8 Mar 2023 20:45:43 +0800 Subject: [PATCH 06/58] feat(add_perception_objects_pointcloud_publisher) update qos policy, use objectsCallback to update objects Signed-off-by: Alexey Panferov --- .../detected_objects_display.hpp | 1 + .../object_polygon_display_base.hpp | 129 ++++++++++-------- .../predicted_objects_display.hpp | 2 + .../tracked_objects_display.hpp | 2 + .../detected_objects_display.cpp | 122 ++++------------- .../predicted_objects_display.cpp | 69 ++++++---- .../tracked_objects_display.cpp | 123 ++++------------- 7 files changed, 160 insertions(+), 288 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index bc76a363a26e..15d839c0fbde 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -40,6 +40,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay private: void processMessage(DetectedObjects::ConstSharedPtr msg) override; void onInitialize() override; + void objectsCallback(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objs_msg) override; // void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 8a20cacc9e89..e06420be37db 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -73,13 +73,15 @@ namespace object_detection { using Polygon2d = tier4_autoware_utils::Polygon2d; +using Shape = autoware_auto_perception_msgs::msg::Shape; // struct for creating objects buffer struct object_info { - autoware_auto_perception_msgs::msg::Shape shape; + Shape shape; geometry_msgs::msg::Pose position; - autoware_auto_perception_msgs::msg::ObjectClassification classification; + // autoware_auto_perception_msgs::msg::ObjectClassification classification; + std::vector classification; }; inline pcl::PointXYZRGB toPCL(const double x, const double y, const double z) @@ -185,7 +187,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher("output/pointcloud", 10); + publisher_ = raw_node->create_publisher("output/pointcloud", rclcpp::SensorDataQoS()); pointcloud_subscription_ = raw_node->create_subscription( m_default_pointcloud_topic->getTopicStd(), rclcpp::SensorDataQoS(), @@ -224,8 +226,62 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase { m_marker_common.addMessage(markers_ptr); } + + // transfrom 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("perception_utils"), ex.what()); // rename + return boost::none; + } + } + + // std::string objects_frame_id_; + // variables for transfer detected objects information between callbacks + std::vector objs_buffer; std::string objects_frame_id_; + std::string pointcloud_frame_id_; + std::shared_ptr tf_listener_{nullptr}; // !! different type in prototype, + std::unique_ptr tf_buffer_; // !! different type in prototype + rclcpp::Node::SharedPtr raw_node; protected: /// \brief Convert given shape msg into a Marker @@ -462,51 +518,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase double get_line_width() { return m_line_width_property.getFloat(); } - // 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("perception_utils"), ex.what()); // rename - return boost::none; - } - } - // transfrom 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; - } // helper function to get radius for kd-search std::optional getMaxRadius(object_info & object) @@ -521,13 +534,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } return max_dist; } else { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "unknown shape type"); + // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "unknown shape type"); return std::nullopt; } } + + virtual void objectsCallback(typename MsgT::ConstSharedPtr msg) = 0; - std::shared_ptr tf_listener_{nullptr} // !! different type in prototype, - std::unique_ptr tf_buffer_; // !! different type in prototype + // add pointcloud subscription rclcpp::Subscription::SharedPtr pointcloud_subscription_; rclcpp::Publisher::SharedPtr publisher_; @@ -535,10 +549,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // Default pointcloud topic; rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic; - // variables for transfer detected objects information between callbacks - std::vector objs_buffer; - std::string objects_frame_id_; - std::string pointcloud_frame_id_; + private: // All rviz plugins should have this. Should be initialized with pointer to this class @@ -579,10 +590,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // RCLCPP_INFO(this->get_logger(), "Get new pointcloud"); pointcloud_frame_id_ = input_pointcloud_msg.header.frame_id; - pcl::PointXYZ minPt, maxPt; - pcl::PointCloud measured_cloud; - pcl::fromROSMsg(input_pointcloud_msg, measured_cloud); - pcl::getMinMax3D(measured_cloud, minPt, maxPt); + // pcl::PointXYZ minPt, maxPt; + // pcl::PointCloud measured_cloud; + // pcl::fromROSMsg(input_pointcloud_msg, measured_cloud); + // pcl::getMinMax3D(measured_cloud, minPt, maxPt); // RCLCPP_INFO(this->get_logger(), "before translation max X is '%f' max Y is '%f'", maxPt.x, maxPt.y); // RCLCPP_INFO(this->get_logger(), "before translation min X is '%f' min Y is '%f'", minPt.x, minPt.y); @@ -625,7 +636,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase filterPolygon(neighbor_pointcloud, out_cloud, object); } } else { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); + // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); return; } @@ -636,7 +647,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // *output_pointcloud_msg_ptr = transformed_pointcloud; output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; - output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; // remove + // output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; // remove // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); publisher_->publish(*output_pointcloud_msg_ptr); diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index 1cc3844773b2..c45b5dcd32b0 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -108,6 +108,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, autoware_auto_perception_msgs::msg::PredictedObjects & output_msg); + + void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_objs_msg) override; std::unordered_map> id_map; // std::unordered_map id_map; diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 50b19e92be0a..31db4695be73 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -96,6 +96,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay return id_map.at(uuid); } + void objectsCallback(const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objs_msg) override; + std::map id_map; std::list unused_marker_ids; int32_t marker_id = 0; 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 1c548b3fccfb..399aa3105b1e 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 @@ -80,33 +80,17 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) } // Transform to pointcloud frame - autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; - if (!transformObjects( - *msg, pointcloud_frame_id_, tf_buffer_, - transformed_objects)) { - // objects_pub_->publish(*input_objects); - return; - } - - objects_frame_id_ = transformed_objects.header.frame_id; // remove - - objs_buffer.clear(); - for (const auto & object : transformed_objects.objects) - { - std::vector labels = object.classification; - object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; - objs_buffer.push_back(info); - } // RCLCPP_INFO(this->get_logger(), "Update objects buffer"); } + objectsCallback(msg); } -void DetectedObjectsDisplay::onInitialize() override +void DetectedObjectsDisplay::onInitialize() { ObjectPolygonDisplayBase::onInitialize(); // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher("output/detected_objects_pointcloud", 10); + publisher_ = raw_node->create_publisher("output/detected_objects_pointcloud", rclcpp::SensorDataQoS()); // pointcloud_subscription_ = raw_node->create_subscription( // m_default_pointcloud_topic->getTopicStd(), // rclcpp::SensorDataQoS(), @@ -115,89 +99,29 @@ void DetectedObjectsDisplay::onInitialize() override // std::placeholders::_1)); } -// void DetectedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) -// { -// sensor_msgs::msg::PointCloud2 transformed_pointcloud; -// if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) -// { -// geometry_msgs::msg::TransformStamped transform; -// transform = tf_buffer_->lookupTransform( -// input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - -// tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); - -// } else { -// transformed_pointcloud = input_pointcloud_msg; -// } - -// pcl::PCLPointCloud2 input_pcl_cloud; -// pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); -// pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); -// pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); - -// // Create a new point cloud with RGB color information and copy data from input cloudb -// pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); -// pcl::copyPointCloud(*temp_cloud, *colored_cloud); - -// pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - -// if (objs_buffer.size() > 0) { -// for (auto object : objs_buffer) { - -// if (object.shape.type == 0) -// { -// pcl::PointCloud filtered_cloud; -// pcl::CropBox cropBoxFilter (true); -// cropBoxFilter.setInputCloud(colored_cloud); -// float trans_x = object.kinematics.pose_with_covariance.pose.position.x; -// float trans_y = object.kinematics.pose_with_covariance.pose.position.y; -// float trans_z = object.kinematics.pose_with_covariance.pose.position.z; -// float max_x = trans_x + object.shape.dimensions.x / 2.0; -// float min_x = trans_x - object.shape.dimensions.x / 2.0; -// float max_y = trans_y + object.shape.dimensions.y / 2.0; -// float min_y = trans_y - object.shape.dimensions.y / 2.0; -// float max_z = trans_z + object.shape.dimensions.z / 2.0; -// float min_z = trans_z - object.shape.dimensions.z / 2.0; - -// Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); -// Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); -// cropBoxFilter.setMin(min_pt); -// cropBoxFilter.setMax(max_pt); -// cropBoxFilter.filter(filtered_cloud); - -// // Define a custom color for the box polygons -// const uint8_t r = 30, g = 44, b = 255; + void DetectedObjectsDisplay::objectsCallback(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objs_msg) + { -// for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) -// { -// cloud_it->r = r; -// cloud_it->g = g; -// cloud_it->b = b; -// } - -// *out_cloud += filtered_cloud; - -// } - -// } - -// } - -// sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); -// // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); + // Transform to pointcloud frame + autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; + if (!transformObjects( + *input_objs_msg, pointcloud_frame_id_, *tf_buffer_, + transformed_objects)) { + // objects_pub_->publish(*input_objects); + return; + } -// pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); -// output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; -// output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; + objects_frame_id_ = transformed_objects.header.frame_id; -// // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map -// // change pointcloud frame to map -// // update color of the points and remove all points outside deceted objects - - -// // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); -// publisher_->publish(*output_pointcloud_msg_ptr); -// } + objs_buffer.clear(); + for (const auto & object : transformed_objects.objects) + { + std::vector labels = object.classification; + object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + objs_buffer.push_back(info); + } + // RCLCPP_INFO(this->get_logger(), "Update objects buffer"); + } } // namespace object_detection } // namespace rviz_plugins 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 425f8d73ed92..4d83b340bf99 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 @@ -177,34 +177,9 @@ std::vector PredictedObjectsDisplay: markers.push_back(path_confidence_marker_ptr); } } - - // add objects to buffer for pointcloud filtering - // objs_buffer.push_back(object); - - autoware_auto_perception_msgs::msg::PredictedObjects transformed_objects; - if (!transformObjects( - *msg, pointcloud_frame_id_, tf_buffer_, - transformed_objects)) { - // objects_pub_->publish(*input_objects); - // RCLCPP_INFO(this->get_logger(), "Did NOT transform objects"); - return; - } - // RCLCPP_INFO(this->get_logger(), "Transform DONE"); - // RCLCPP_INFO(this->get_logger(), "After transform objects frame is '%s'", transformed_objects.header.frame_id.c_str()); - // RCLCPP_INFO(this->get_logger(), "First object X is '%f' Y is '%f'", - // transformed_objects.objects.at(0).kinematics.initial_pose_with_covariance.pose.position.x, - // transformed_objects.objects.at(0).kinematics.initial_pose_with_covariance.pose.position.y); - - objects_frame_id_ = transformed_objects.header.frame_id; - objs_buffer.clear(); - for (const auto & object : transformed_objects.objects) - { - std::vector labels = object.classification; - object_info info = {object.shape, object.kinematics.initial_pose_with_covariance.pose, object.classification}; - objs_buffer.push_back(info); - } - // RCLCPP_INFO(this->get_logger(), "Update objects buffer"); } + + objectsCallback(msg); return markers; } @@ -237,12 +212,12 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) wall_dt, ros_dt); } -void PredictedObjectsDisplay::onInitialize() override +void PredictedObjectsDisplay::onInitialize() { ObjectPolygonDisplayBase::onInitialize(); // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher("output/predicted_objects_pointcloud", 10); + publisher_ = raw_node->create_publisher("output/predicted_objects_pointcloud", rclcpp::SensorDataQoS()); // pointcloud_subscription_ = raw_node->create_subscription( // m_default_pointcloud_topic->getTopicStd(), // rclcpp::SensorDataQoS(), @@ -251,7 +226,7 @@ void PredictedObjectsDisplay::onInitialize() override // std::placeholders::_1)); } -bool transformObjects( +bool PredictedObjectsDisplay::transformObjects( const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, autoware_auto_perception_msgs::msg::PredictedObjects & output_msg) @@ -281,6 +256,40 @@ bool transformObjects( return true; } +void PredictedObjectsDisplay::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_objs_msg) +{ + // RCLCPP_INFO(this->get_logger(), "New objects"); + // RCLCPP_INFO(this->get_logger(), "Befor transform objects frame is '%s'", input_objs_msg->header.frame_id.c_str()); + // RCLCPP_INFO(this->get_logger(), "First object X is '%f' Y is '%f'", + // input_objs_msg->objects.at(0).kinematics.initial_pose_with_covariance.pose.position.x, + // input_objs_msg->objects.at(0).kinematics.initial_pose_with_covariance.pose.position.y); + // Transform to pointcloud frame + autoware_auto_perception_msgs::msg::PredictedObjects transformed_objects; + if (!transformObjects( + *input_objs_msg, pointcloud_frame_id_, *tf_buffer_, + transformed_objects)) { + // objects_pub_->publish(*input_objects); + // RCLCPP_INFO(this->get_logger(), "Did NOT transform objects"); + return; + } + // RCLCPP_INFO(this->get_logger(), "Transform DONE"); + // RCLCPP_INFO(this->get_logger(), "After transform objects frame is '%s'", transformed_objects.header.frame_id.c_str()); + // RCLCPP_INFO(this->get_logger(), "First object X is '%f' Y is '%f'", + // transformed_objects.objects.at(0).kinematics.initial_pose_with_covariance.pose.position.x, + // transformed_objects.objects.at(0).kinematics.initial_pose_with_covariance.pose.position.y); + + // objects_frame_id_ = transformed_objects.header.frame_id; + objs_buffer.clear(); + for (const auto & object : transformed_objects.objects) + { + std::vector labels = object.classification; + object_info info = {object.shape, object.kinematics.initial_pose_with_covariance.pose, object.classification}; + objs_buffer.push_back(info); + } + // RCLCPP_INFO(this->get_logger(), "Update objects buffer"); +} + + } // namespace object_detection } // namespace rviz_plugins 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 b0797fe46144..1b5fb8670549 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 @@ -119,34 +119,18 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(twist_marker_ptr); } - // Transform to pointcloud frame - autoware_auto_perception_msgs::msg::TrackedObjects transformed_objects; - if (!transformObjects( - *msg, pointcloud_frame_id_, tf_buffer_, - transformed_objects)) { - // objects_pub_->publish(*input_objects); - return; - } - objects_frame_id_ = transformed_objects.header.frame_id; - - objs_buffer.clear(); - for (const auto & object : transformed_objects.objects) - { - std::vector labels = object.classification; - object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; - objs_buffer.push_back(info); - } // RCLCPP_INFO(this->get_logger(), "Update objects buffer");; } + objectsCallback(msg); } -void TrackedObjectsDisplay::onInitialize() override +void TrackedObjectsDisplay::onInitialize() { ObjectPolygonDisplayBase::onInitialize(); // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher("output/tracked_objects_pointcloud", 10); + publisher_ = raw_node->create_publisher("output/tracked_objects_pointcloud", rclcpp::SensorDataQoS()); // pointcloud_subscription_ = raw_node->create_subscription( // m_default_pointcloud_topic->getTopicStd(), // rclcpp::SensorDataQoS(), @@ -155,89 +139,28 @@ void TrackedObjectsDisplay::onInitialize() override // std::placeholders::_1)); } -// void TrackedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) -// { -// sensor_msgs::msg::PointCloud2 transformed_pointcloud; -// if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_) -// { -// geometry_msgs::msg::TransformStamped transform; -// transform = tf_buffer_->lookupTransform( -// input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); - -// tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform); - -// } else { -// transformed_pointcloud = input_pointcloud_msg; -// } - -// pcl::PCLPointCloud2 input_pcl_cloud; -// pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud); -// pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); -// pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud); - -// // Create a new point cloud with RGB color information and copy data from input cloudb -// pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); -// pcl::copyPointCloud(*temp_cloud, *colored_cloud); - -// pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - -// if (objs_buffer.size() > 0) { -// for (auto object : objs_buffer) { - -// if (object.shape.type == 0) -// { -// pcl::PointCloud filtered_cloud; -// pcl::CropBox cropBoxFilter (true); -// cropBoxFilter.setInputCloud(colored_cloud); -// float trans_x = object.kinematics.pose_with_covariance.pose.position.x; -// float trans_y = object.kinematics.pose_with_covariance.pose.position.y; -// float trans_z = object.kinematics.pose_with_covariance.pose.position.z; -// float max_x = trans_x + object.shape.dimensions.x / 2.0; -// float min_x = trans_x - object.shape.dimensions.x / 2.0; -// float max_y = trans_y + object.shape.dimensions.y / 2.0; -// float min_y = trans_y - object.shape.dimensions.y / 2.0; -// float max_z = trans_z + object.shape.dimensions.z / 2.0; -// float min_z = trans_z - object.shape.dimensions.z / 2.0; - -// Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f); -// Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f); -// cropBoxFilter.setMin(min_pt); -// cropBoxFilter.setMax(max_pt); -// cropBoxFilter.filter(filtered_cloud); - -// // Define a custom color for the box polygons -// const uint8_t r = 30, g = 44, b = 255; - -// for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) -// { -// cloud_it->r = r; -// cloud_it->g = g; -// cloud_it->b = b; -// } - -// *out_cloud += filtered_cloud; - -// } - -// } - -// } - -// sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); -// // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); + void TrackedObjectsDisplay::objectsCallback(const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objs_msg) + { + // Transform to pointcloud frame + autoware_auto_perception_msgs::msg::TrackedObjects transformed_objects; + if (!transformObjects( + *input_objs_msg, pointcloud_frame_id_, *tf_buffer_, + transformed_objects)) { + // objects_pub_->publish(*input_objects); + return; + } -// pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); -// output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; -// output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; - -// // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map -// // change pointcloud frame to map -// // update color of the points and remove all points outside deceted objects + objects_frame_id_ = transformed_objects.header.frame_id; - -// // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); -// publisher_->publish(*output_pointcloud_msg_ptr); -// } + objs_buffer.clear(); + for (const auto & object : transformed_objects.objects) + { + std::vector labels = object.classification; + object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + objs_buffer.push_back(info); + } + // RCLCPP_INFO(this->get_logger(), "Update objects buffer"); + } } // namespace object_detection } // namespace rviz_plugins From 2b2cd6af1749b945f9b72025fc37d3c74ef708a3 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 14 Mar 2023 01:17:02 +0800 Subject: [PATCH 07/58] feat(add_perception_objects_pointcloud_publisher) integrate sync callback to plugin hardcoded topic names, one color for all classes of objects Signed-off-by: Alexey Panferov --- .../detected_objects_display.hpp | 10 +- .../object_polygon_display_base.hpp | 182 ++++++------------ .../predicted_objects_display.hpp | 10 +- .../tracked_objects_display.hpp | 10 +- .../detected_objects_display.cpp | 103 +++++++--- .../predicted_objects_display.cpp | 89 ++++++--- .../tracked_objects_display.cpp | 103 +++++++--- 7 files changed, 297 insertions(+), 210 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index 15d839c0fbde..a7688ed1cbdb 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -40,7 +40,15 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay private: void processMessage(DetectedObjects::ConstSharedPtr msg) override; void onInitialize() override; - void objectsCallback(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objs_msg) override; + void onObjectsAndObstaclePointCloud(const DetectedObjects::ConstSharedPtr & input_objs_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); + + + message_filters::Subscriber percepted_objects_subscription_; + message_filters::Subscriber pointcloud_subscription_; + using SyncPolicy = message_filters::sync_policies::ApproximateTime; + using Sync = message_filters::Synchronizer; + std::shared_ptr sync_ptr_; // void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index e06420be37db..088b1c044627 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -52,6 +52,10 @@ #include #include +#include +#include +#include + #include #include #include @@ -84,6 +88,13 @@ struct object_info std::vector classification; }; +struct color +{ + uint8_t r; + uint8_t g; + uint8_t b; +}; + inline pcl::PointXYZRGB toPCL(const double x, const double y, const double z) { pcl::PointXYZRGB pcl_point; @@ -171,9 +182,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_polygon_properties.emplace( std::piecewise_construct, std::forward_as_tuple(map_property_it.first), std::forward_as_tuple( - QColor{color[0], color[1], color[2]}, class_property_values.alpha, &parent_property)); + QColor{color[0], color[1], color[2]}, class_property_values.alpha, & parent_property)); } init_color_list(predicted_path_colors); + point_color_ ={255, 255, 255}; // defaul white color } void onInitialize() override @@ -188,17 +200,12 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); publisher_ = raw_node->create_publisher("output/pointcloud", rclcpp::SensorDataQoS()); - pointcloud_subscription_ = raw_node->create_subscription( - m_default_pointcloud_topic->getTopicStd(), - rclcpp::SensorDataQoS(), - std::bind(&ObjectPolygonDisplayBase::pointCloudCallback, - this, - std::placeholders::_1)); tf_buffer_ = std::make_unique(raw_node->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); + } void load(const rviz_common::Config & config) override @@ -274,14 +281,15 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } - // std::string objects_frame_id_; - // variables for transfer detected objects information between callbacks + // variables for transfer detected objects information between callbacks std::vector objs_buffer; std::string objects_frame_id_; std::string pointcloud_frame_id_; std::shared_ptr tf_listener_{nullptr}; // !! different type in prototype, std::unique_ptr tf_buffer_; // !! different type in prototype - rclcpp::Node::SharedPtr raw_node; + + color point_color_; + protected: /// \brief Convert given shape msg into a Marker @@ -539,11 +547,47 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - virtual void objectsCallback(typename MsgT::ConstSharedPtr msg) = 0; - + 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 = + tier4_autoware_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); + + // Define a custom color for the box polygons + const uint8_t r = 30, g = 44, b = 255; + + for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) + { + cloud_it->r = r; + cloud_it->g = g; + cloud_it->b = b; + } + + *out_cloud += filtered_cloud; + } - // add pointcloud subscription - rclcpp::Subscription::SharedPtr pointcloud_subscription_; rclcpp::Publisher::SharedPtr publisher_; // Default pointcloud topic; @@ -585,116 +629,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::vector predicted_path_colors; - void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) - { - // RCLCPP_INFO(this->get_logger(), "Get new pointcloud"); - pointcloud_frame_id_ = input_pointcloud_msg.header.frame_id; - - // pcl::PointXYZ minPt, maxPt; - // pcl::PointCloud measured_cloud; - // pcl::fromROSMsg(input_pointcloud_msg, measured_cloud); - // pcl::getMinMax3D(measured_cloud, minPt, maxPt); - - // RCLCPP_INFO(this->get_logger(), "before translation max X is '%f' max Y is '%f'", maxPt.x, maxPt.y); - // RCLCPP_INFO(this->get_logger(), "before translation min X is '%f' min Y is '%f'", minPt.x, minPt.y); - - // convert to pcl pointcloud - pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); - // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); - pcl::fromROSMsg(input_pointcloud_msg, *temp_cloud); - - // pcl::PointCloud::Ptr colored_cloud(new pcl::PointCloud()); - // pcl::fromROSMsg(transformed_pointcloud, *colored_cloud); - - // Create a new point cloud with RGB color information and copy data from input cloudb - 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()); - - if (objs_buffer.size() > 0) { - // RCLCPP_INFO(this->get_logger(), "Filtering pointcloud"); - for (auto object : objs_buffer) { - // RCLCPP_INFO(this->get_logger(), "object"); - - const auto search_radius = getMaxRadius(object); - // Search neighbor pointcloud to reduce cost. - pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); - std::vector indices; - std::vector distances; - kdtree->radiusSearch( - toPCL(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, object); - } - } else { - // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); - return; - } - - - sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); - // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr); - pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); - // *output_pointcloud_msg_ptr = transformed_pointcloud; - - output_pointcloud_msg_ptr->header = input_pointcloud_msg.header; - // output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; // remove - - // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud"); - publisher_->publish(*output_pointcloud_msg_ptr); - } - - 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 = - tier4_autoware_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); - - // Define a custom color for the box polygons - const uint8_t r = 30, g = 44, b = 255; - - for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) - { - cloud_it->r = r; - cloud_it->g = g; - cloud_it->b = b; - } - - *out_cloud += filtered_cloud; - } - -// virtual void objectsCallback(typename MsgT::ConstSharedPtr msg) = 0; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index c45b5dcd32b0..0fafbf926005 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -109,7 +109,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, autoware_auto_perception_msgs::msg::PredictedObjects & output_msg); - void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_objs_msg) override; + void onObjectsAndObstaclePointCloud(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; @@ -122,6 +123,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay std::mutex mutex; std::condition_variable condition; std::vector markers; + + + message_filters::Subscriber percepted_objects_subscription_; + message_filters::Subscriber pointcloud_subscription_; + typedef message_filters::sync_policies::ApproximateTimeSyncPolicy; + typedef message_filters::Synchronizer Sync; + typename std::shared_ptr sync_ptr_; // std::string objects_frame_id_; // std::vector objs_buffer; diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 31db4695be73..0bf5ae1e689b 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -96,12 +96,20 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay return id_map.at(uuid); } - void objectsCallback(const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objs_msg) override; + void onObjectsAndObstaclePointCloud(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; + + message_filters::Subscriber percepted_objects_subscription_; + message_filters::Subscriber pointcloud_subscription_; + typedef message_filters::sync_policies::ApproximateTimeSyncPolicy; + typedef message_filters::Synchronizer Sync; + typename std::shared_ptr sync_ptr_; + // std::string objects_frame_id_; // std::vector objs_buffer; }; 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 399aa3105b1e..664587a3c0ed 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 @@ -82,7 +82,6 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) // Transform to pointcloud frame // RCLCPP_INFO(this->get_logger(), "Update objects buffer"); } - objectsCallback(msg); } void DetectedObjectsDisplay::onInitialize() @@ -91,38 +90,88 @@ void DetectedObjectsDisplay::onInitialize() // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); publisher_ = raw_node->create_publisher("output/detected_objects_pointcloud", rclcpp::SensorDataQoS()); - // pointcloud_subscription_ = raw_node->create_subscription( - // m_default_pointcloud_topic->getTopicStd(), - // rclcpp::SensorDataQoS(), - // std::bind(&DetectedObjectsDisplay::pointCloudCallback, - // this, - // std::placeholders::_1)); + sync_ptr_ = std::make_shared(SyncPolicy(10), percepted_objects_subscription_, pointcloud_subscription_); + + using std::placeholders::_1; + using std::placeholders::_2; + percepted_objects_subscription_.subscribe(raw_node, "/perception/object_recognition/detection/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), + pointcloud_subscription_.subscribe( + raw_node, "/perception/obstacle_segmentation/pointcloud", + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + sync_ptr_->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); + // sync_ptr_->registerCallback([this](const auto& objects, const auto& cloud) {onObjectsAndObstaclePointCloud(objects, cloud);}); + + } - void DetectedObjectsDisplay::objectsCallback(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objs_msg) +void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( + const DetectedObjects::ConstSharedPtr & input_objs_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) +{ + point_color_ = {255, 5, 5}; // red color + // Transform to pointcloud frame + autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; + if (!transformObjects( + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer_, + transformed_objects)) { + // objects_pub_->publish(*input_objects); + return; + } + + objs_buffer.clear(); + for (const auto & object : transformed_objects.objects) { + std::vector labels = object.classification; + object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + objs_buffer.push_back(info); + } + + // convert to pcl pointcloud + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); + pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud); - // Transform to pointcloud frame - autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; - if (!transformObjects( - *input_objs_msg, pointcloud_frame_id_, *tf_buffer_, - transformed_objects)) { - // objects_pub_->publish(*input_objects); - return; - } - - objects_frame_id_ = transformed_objects.header.frame_id; - - objs_buffer.clear(); - for (const auto & object : transformed_objects.objects) - { - std::vector labels = object.classification; - object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; - objs_buffer.push_back(info); - } - // RCLCPP_INFO(this->get_logger(), "Update objects buffer"); + // Create a new point cloud with RGB color information and copy data from input cloudb + 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()); + + + if (objs_buffer.size() > 0) { + for (auto object : objs_buffer) { + + const auto search_radius = getMaxRadius(object); + // Search neighbor pointcloud to reduce cost. + pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); + std::vector indices; + std::vector distances; + kdtree->radiusSearch( + toPCL(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, object); + } + } else { + // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); + return; } + 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; + + publisher_->publish(*output_pointcloud_msg_ptr); +} + } // namespace object_detection } // namespace rviz_plugins } // namespace autoware 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 4d83b340bf99..bc50ef197ede 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 @@ -178,8 +178,6 @@ std::vector PredictedObjectsDisplay: } } } - - objectsCallback(msg); return markers; } @@ -218,12 +216,15 @@ void PredictedObjectsDisplay::onInitialize() // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); publisher_ = raw_node->create_publisher("output/predicted_objects_pointcloud", rclcpp::SensorDataQoS()); - // pointcloud_subscription_ = raw_node->create_subscription( - // m_default_pointcloud_topic->getTopicStd(), - // rclcpp::SensorDataQoS(), - // std::bind(&PredictedObjectsDisplay::pointCloudCallback, - // this, - // std::placeholders::_1)); + sync_ptr_ = std::make_shared(SyncPolicy(10), percepted_objects_subscription_, pointcloud_subscription_); + + using std::placeholders::_1; + using std::placeholders::_2; + percepted_objects_subscription_.subscribe(raw_node, "/perception/object_recognition/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), + pointcloud_subscription_.subscribe( + raw_node, "/perception/obstacle_segmentation/pointcloud", + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + sync_ptr_->registerCallback(&PredictedObjectsDisplay::onObjectsAndObstaclePointCloud, this); } bool PredictedObjectsDisplay::transformObjects( @@ -256,29 +257,20 @@ bool PredictedObjectsDisplay::transformObjects( return true; } -void PredictedObjectsDisplay::objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_objs_msg) -{ - // RCLCPP_INFO(this->get_logger(), "New objects"); - // RCLCPP_INFO(this->get_logger(), "Befor transform objects frame is '%s'", input_objs_msg->header.frame_id.c_str()); - // RCLCPP_INFO(this->get_logger(), "First object X is '%f' Y is '%f'", - // input_objs_msg->objects.at(0).kinematics.initial_pose_with_covariance.pose.position.x, - // input_objs_msg->objects.at(0).kinematics.initial_pose_with_covariance.pose.position.y); +void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( + const PredictedObjects::ConstSharedPtr & input_objs_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) +{ + point_color_ ={5, 5, 255}; // blue color // Transform to pointcloud frame autoware_auto_perception_msgs::msg::PredictedObjects transformed_objects; if (!transformObjects( - *input_objs_msg, pointcloud_frame_id_, *tf_buffer_, - transformed_objects)) { - // objects_pub_->publish(*input_objects); - // RCLCPP_INFO(this->get_logger(), "Did NOT transform objects"); + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer_, + transformed_objects)) { + // objects_pub_->publish(*input_objects); return; } - // RCLCPP_INFO(this->get_logger(), "Transform DONE"); - // RCLCPP_INFO(this->get_logger(), "After transform objects frame is '%s'", transformed_objects.header.frame_id.c_str()); - // RCLCPP_INFO(this->get_logger(), "First object X is '%f' Y is '%f'", - // transformed_objects.objects.at(0).kinematics.initial_pose_with_covariance.pose.position.x, - // transformed_objects.objects.at(0).kinematics.initial_pose_with_covariance.pose.position.y); - // objects_frame_id_ = transformed_objects.header.frame_id; objs_buffer.clear(); for (const auto & object : transformed_objects.objects) { @@ -286,9 +278,52 @@ void PredictedObjectsDisplay::objectsCallback(const autoware_auto_perception_msg object_info info = {object.shape, object.kinematics.initial_pose_with_covariance.pose, object.classification}; objs_buffer.push_back(info); } - // RCLCPP_INFO(this->get_logger(), "Update objects buffer"); -} + + // convert to pcl pointcloud + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); + pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud); + + // Create a new point cloud with RGB color information and copy data from input cloudb + 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()); + + + if (objs_buffer.size() > 0) { + for (auto object : objs_buffer) { + + const auto search_radius = getMaxRadius(object); + // Search neighbor pointcloud to reduce cost. + pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); + std::vector indices; + std::vector distances; + kdtree->radiusSearch( + toPCL(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, object); + } + } else { + // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); + return; + } + + 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; + + publisher_->publish(*output_pointcloud_msg_ptr); +} } // namespace object_detection 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 1b5fb8670549..2bfbe0a4ba7d 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 @@ -119,10 +119,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(twist_marker_ptr); } - - // RCLCPP_INFO(this->get_logger(), "Update objects buffer");; } - objectsCallback(msg); } void TrackedObjectsDisplay::onInitialize() @@ -131,36 +128,84 @@ void TrackedObjectsDisplay::onInitialize() // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); publisher_ = raw_node->create_publisher("output/tracked_objects_pointcloud", rclcpp::SensorDataQoS()); - // pointcloud_subscription_ = raw_node->create_subscription( - // m_default_pointcloud_topic->getTopicStd(), - // rclcpp::SensorDataQoS(), - // std::bind(&TrackedObjectsDisplay::pointCloudCallback, - // this, - // std::placeholders::_1)); + sync_ptr_ = std::make_shared(SyncPolicy(10), percepted_objects_subscription_, pointcloud_subscription_); + + using std::placeholders::_1; + using std::placeholders::_2; + percepted_objects_subscription_.subscribe(raw_node, "/perception/object_recognition/tracking/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), + pointcloud_subscription_.subscribe( + raw_node, "/perception/obstacle_segmentation/pointcloud", + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + sync_ptr_->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); } - void TrackedObjectsDisplay::objectsCallback(const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objs_msg) +void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( + const TrackedObjects::ConstSharedPtr & input_objs_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) +{ + point_color_ ={5, 255, 5}; // green color + // Transform to pointcloud frame + autoware_auto_perception_msgs::msg::TrackedObjects transformed_objects; + if (!transformObjects( + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer_, + transformed_objects)) { + // objects_pub_->publish(*input_objects); + return; + } + + objs_buffer.clear(); + for (const auto & object : transformed_objects.objects) { - // Transform to pointcloud frame - autoware_auto_perception_msgs::msg::TrackedObjects transformed_objects; - if (!transformObjects( - *input_objs_msg, pointcloud_frame_id_, *tf_buffer_, - transformed_objects)) { - // objects_pub_->publish(*input_objects); - return; - } - - objects_frame_id_ = transformed_objects.header.frame_id; - - objs_buffer.clear(); - for (const auto & object : transformed_objects.objects) - { - std::vector labels = object.classification; - object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; - objs_buffer.push_back(info); - } - // RCLCPP_INFO(this->get_logger(), "Update objects buffer"); + std::vector labels = object.classification; + object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + objs_buffer.push_back(info); } + + // convert to pcl pointcloud + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); + pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud); + + // Create a new point cloud with RGB color information and copy data from input cloudb + 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()); + + + if (objs_buffer.size() > 0) { + for (auto object : objs_buffer) { + + const auto search_radius = getMaxRadius(object); + // Search neighbor pointcloud to reduce cost. + pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); + std::vector indices; + std::vector distances; + kdtree->radiusSearch( + toPCL(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, object); + } + } else { + // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); + return; + } + + 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; + + publisher_->publish(*output_pointcloud_msg_ptr); +} } // namespace object_detection } // namespace rviz_plugins From d87a6545026b258474f50271e606c0e4e8a8f397 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 14 Mar 2023 19:46:51 +0800 Subject: [PATCH 08/58] feat(add_perception_objects_pointcloud_publisher) add colors to different object classes Signed-off-by: Alexey Panferov --- .../object_polygon_display_base.hpp | 21 ++++++++++++------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 088b1c044627..94944debb2ba 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -68,6 +68,7 @@ #include #include #include +#include namespace autoware { @@ -201,10 +202,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); publisher_ = raw_node->create_publisher("output/pointcloud", rclcpp::SensorDataQoS()); - tf_buffer_ = - std::make_unique(raw_node->get_clock()); - tf_listener_ = - std::make_shared(*tf_buffer_); + tf_buffer_ = std::make_unique(raw_node->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); } @@ -576,13 +575,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase crop_hull_filter.filter(filtered_cloud); // Define a custom color for the box polygons - const uint8_t r = 30, g = 44, b = 255; + // const uint8_t r = 30, g = 44, b = 255; + + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(object.classification); for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it) { - cloud_it->r = r; - cloud_it->g = g; - cloud_it->b = b; + // cloud_it->r = color_rgba.r; + // cloud_it->g = color_rgba.g; + // cloud_it->b = color_rgba.b; + + cloud_it->r = std::max(0, std::min(255, (int)floor(color_rgba.r * 256.0))); + cloud_it->g = std::max(0, std::min(255, (int)floor(color_rgba.g * 256.0))); + cloud_it->b = std::max(0, std::min(255, (int)floor(color_rgba.b * 256.0))); } *out_cloud += filtered_cloud; From 7efbec4b8930b443adece5cad7267e460d3ceb0f Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Wed, 15 Mar 2023 23:23:37 +0800 Subject: [PATCH 09/58] feat(add_perception_objects_pointcloud_publisher) add property enable/disable publish poincloud Signed-off-by: Alexey Panferov --- .../detected_objects_display.hpp | 6 ---- .../object_polygon_display_base.hpp | 28 ++++++------------- .../predicted_objects_display.hpp | 3 -- .../tracked_objects_display.hpp | 3 -- .../detected_objects_display.cpp | 20 ++++++------- .../predicted_objects_display.cpp | 16 +++++------ .../tracked_objects_display.cpp | 16 +++++------ 7 files changed, 33 insertions(+), 59 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index a7688ed1cbdb..b66d3b47cd42 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -43,17 +43,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay void onObjectsAndObstaclePointCloud(const DetectedObjects::ConstSharedPtr & input_objs_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); - - message_filters::Subscriber percepted_objects_subscription_; - message_filters::Subscriber pointcloud_subscription_; using SyncPolicy = message_filters::sync_policies::ApproximateTime; using Sync = message_filters::Synchronizer; std::shared_ptr sync_ptr_; // void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; - - // std::string objects_frame_id_; - // std::vector objs_buffer; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 94944debb2ba..888dfe5a7fe7 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -89,13 +89,6 @@ struct object_info std::vector classification; }; -struct color -{ - uint8_t r; - uint8_t g; - uint8_t b; -}; - inline pcl::PointXYZRGB toPCL(const double x, const double y, const double z) { pcl::PointXYZRGB pcl_point; @@ -131,7 +124,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase explicit ObjectPolygonDisplayBase(const std::string & default_topic, const std::string & default_pointcloud_topic) : m_marker_common(this), - // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this}, m_display_label_property{"Display Label", true, "Enable/disable label visualization", this}, m_display_uuid_property{"Display UUID", true, "Enable/disable uuid visualization", this}, m_display_pose_with_covariance_property{ @@ -160,6 +152,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_simple_visualize_mode_property = new rviz_common::properties::EnumProperty( "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this, SLOT(updatePalette())); + m_publish_objs_pointcloud = new rviz_common::properties::BoolProperty("Publish Objects Pointcloud", true, + "Enable/disable objects pointcloud publishing", this); m_default_pointcloud_topic = new rviz_common::properties::RosTopicProperty( "Input pointcloud topic", QString::fromStdString(default_pointcloud_topic), @@ -186,7 +180,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase QColor{color[0], color[1], color[2]}, class_property_values.alpha, & parent_property)); } init_color_list(predicted_path_colors); - point_color_ ={255, 255, 255}; // defaul white color } void onInitialize() override @@ -200,11 +193,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher("output/pointcloud", rclcpp::SensorDataQoS()); tf_buffer_ = std::make_unique(raw_node->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); - + } void load(const rviz_common::Config & config) override @@ -287,7 +279,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::shared_ptr tf_listener_{nullptr}; // !! different type in prototype, std::unique_ptr tf_buffer_; // !! different type in prototype - color point_color_; protected: @@ -574,17 +565,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase crop_hull_filter.filter(filtered_cloud); - // Define a custom color for the box polygons - // const uint8_t r = 30, g = 44, b = 255; - const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(object.classification); + 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 = color_rgba.r; - // cloud_it->g = color_rgba.g; - // cloud_it->b = color_rgba.b; - cloud_it->r = std::max(0, std::min(255, (int)floor(color_rgba.r * 256.0))); cloud_it->g = std::max(0, std::min(255, (int)floor(color_rgba.g * 256.0))); cloud_it->b = std::max(0, std::min(255, (int)floor(color_rgba.b * 256.0))); @@ -597,7 +582,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // Default pointcloud topic; rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic; + // Property to enable/disable objects pointcloud publishing + rviz_common::properties::BoolProperty * m_publish_objs_pointcloud; + message_filters::Subscriber percepted_objects_subscription_; + message_filters::Subscriber pointcloud_subscription_; private: @@ -635,6 +624,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::vector predicted_path_colors; + }; } // namespace object_detection } // namespace rviz_plugins diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index 0fafbf926005..5dc48c96e838 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -124,9 +124,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay std::condition_variable condition; std::vector markers; - - message_filters::Subscriber percepted_objects_subscription_; - message_filters::Subscriber pointcloud_subscription_; typedef message_filters::sync_policies::ApproximateTimeSyncPolicy; typedef message_filters::Synchronizer Sync; typename std::shared_ptr sync_ptr_; diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 0bf5ae1e689b..14118b5c4aa6 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -103,9 +103,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay std::list unused_marker_ids; int32_t marker_id = 0; - - message_filters::Subscriber percepted_objects_subscription_; - message_filters::Subscriber pointcloud_subscription_; typedef message_filters::sync_policies::ApproximateTimeSyncPolicy; typedef message_filters::Synchronizer Sync; typename std::shared_ptr sync_ptr_; 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 664587a3c0ed..e2502c952972 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 @@ -24,7 +24,7 @@ namespace rviz_plugins { namespace object_detection { -DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("detected_objects", "/perception/obstacle_segmentation/pointcloud") {} +DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("/perception/object_recognition/detection/objects", "/perception/obstacle_segmentation/pointcloud") {} void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) { @@ -89,26 +89,22 @@ void DetectedObjectsDisplay::onInitialize() ObjectPolygonDisplayBase::onInitialize(); // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher("output/detected_objects_pointcloud", rclcpp::SensorDataQoS()); - sync_ptr_ = std::make_shared(SyncPolicy(10), percepted_objects_subscription_, pointcloud_subscription_); + publisher_ = raw_node->create_publisher("~/output/detected_objects_pointcloud", rclcpp::SensorDataQoS()); - using std::placeholders::_1; - using std::placeholders::_2; + sync_ptr_ = std::make_shared(SyncPolicy(10), percepted_objects_subscription_, pointcloud_subscription_); + sync_ptr_->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); percepted_objects_subscription_.subscribe(raw_node, "/perception/object_recognition/detection/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription_.subscribe( - raw_node, "/perception/obstacle_segmentation/pointcloud", + pointcloud_subscription_.subscribe(raw_node, m_default_pointcloud_topic->getTopic().toStdString(), rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); - sync_ptr_->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - // sync_ptr_->registerCallback([this](const auto& objects, const auto& cloud) {onObjectsAndObstaclePointCloud(objects, cloud);}); - - } void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( const DetectedObjects::ConstSharedPtr & input_objs_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) { - point_color_ = {255, 5, 5}; // red color + if (!m_publish_objs_pointcloud->getBool()) { + return; + } // Transform to pointcloud frame autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; if (!transformObjects( 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 bc50ef197ede..5e26089c0bfb 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 @@ -22,7 +22,7 @@ namespace rviz_plugins { namespace object_detection { -PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("tracks", "/perception/obstacle_segmentation/pointcloud") +PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("predictions", "/perception/obstacle_segmentation/pointcloud") { std::thread worker(&PredictedObjectsDisplay::workerThread, this); worker.detach(); @@ -215,16 +215,14 @@ void PredictedObjectsDisplay::onInitialize() ObjectPolygonDisplayBase::onInitialize(); // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher("output/predicted_objects_pointcloud", rclcpp::SensorDataQoS()); + publisher_ = raw_node->create_publisher("~/output/predicted_objects_pointcloud", rclcpp::SensorDataQoS()); + sync_ptr_ = std::make_shared(SyncPolicy(10), percepted_objects_subscription_, pointcloud_subscription_); + sync_ptr_->registerCallback(&PredictedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - using std::placeholders::_1; - using std::placeholders::_2; percepted_objects_subscription_.subscribe(raw_node, "/perception/object_recognition/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription_.subscribe( - raw_node, "/perception/obstacle_segmentation/pointcloud", + pointcloud_subscription_.subscribe(raw_node, m_default_pointcloud_topic->getTopic().toStdString(), rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); - sync_ptr_->registerCallback(&PredictedObjectsDisplay::onObjectsAndObstaclePointCloud, this); } bool PredictedObjectsDisplay::transformObjects( @@ -261,7 +259,9 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( const PredictedObjects::ConstSharedPtr & input_objs_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) { - point_color_ ={5, 5, 255}; // blue color + if (!m_publish_objs_pointcloud->getBool()) { + return; + } // Transform to pointcloud frame autoware_auto_perception_msgs::msg::PredictedObjects transformed_objects; if (!transformObjects( 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 2bfbe0a4ba7d..b174cfd8d7fa 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 @@ -24,7 +24,7 @@ namespace rviz_plugins { namespace object_detection { -TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("tracks", "/perception/obstacle_segmentation/pointcloud") {} +TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("/perception/object_recognition/tracking/objects", "/perception/obstacle_segmentation/pointcloud") {} void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) { @@ -127,23 +127,23 @@ void TrackedObjectsDisplay::onInitialize() ObjectPolygonDisplayBase::onInitialize(); // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher("output/tracked_objects_pointcloud", rclcpp::SensorDataQoS()); + publisher_ = raw_node->create_publisher("~/output/tracked_objects_pointcloud", rclcpp::SensorDataQoS()); + sync_ptr_ = std::make_shared(SyncPolicy(10), percepted_objects_subscription_, pointcloud_subscription_); + sync_ptr_->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - using std::placeholders::_1; - using std::placeholders::_2; percepted_objects_subscription_.subscribe(raw_node, "/perception/object_recognition/tracking/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription_.subscribe( - raw_node, "/perception/obstacle_segmentation/pointcloud", + pointcloud_subscription_.subscribe(raw_node, m_default_pointcloud_topic->getTopic().toStdString(), rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); - sync_ptr_->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); } void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( const TrackedObjects::ConstSharedPtr & input_objs_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) { - point_color_ ={5, 255, 5}; // green color + if (!m_publish_objs_pointcloud->getBool()) { + return; + } // Transform to pointcloud frame autoware_auto_perception_msgs::msg::TrackedObjects transformed_objects; if (!transformObjects( From c3cfa577f5174480891a8e7819f2fcd433606e4e Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Thu, 16 Mar 2023 00:06:53 +0800 Subject: [PATCH 10/58] feat(add_perception_objects_pointcloud_publisher) disable editing of input pointcloud Signed-off-by: Alexey Panferov --- .../include/object_detection/object_polygon_display_base.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 888dfe5a7fe7..31a8f710a2ac 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -152,6 +152,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_simple_visualize_mode_property = new rviz_common::properties::EnumProperty( "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this, SLOT(updatePalette())); + m_simple_visualize_mode_property->addOption("Normal", 0); + m_simple_visualize_mode_property->addOption("Simple", 1); m_publish_objs_pointcloud = new rviz_common::properties::BoolProperty("Publish Objects Pointcloud", true, "Enable/disable objects pointcloud publishing", this); m_default_pointcloud_topic = new rviz_common::properties::RosTopicProperty( @@ -161,8 +163,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase "Input for pointcloud visualization of Objectcs detection pipeline", this, SLOT(updatePalette())); - m_simple_visualize_mode_property->addOption("Normal", 0); - m_simple_visualize_mode_property->addOption("Simple", 1); + m_default_pointcloud_topic->setReadOnly(true); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { const auto & class_property_values = map_property_it.second; From e31460b0beee4b70d7530cd8c0f2eb0e4fa62e1b Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Thu, 16 Mar 2023 15:11:39 +0800 Subject: [PATCH 11/58] feat(add_perception_objects_pointcloud_publisher) code style formatting Signed-off-by: Alexey Panferov --- .../detected_objects_display.hpp | 14 +- .../object_polygon_detail.hpp | 36 ++--- .../object_polygon_display_base.hpp | 152 +++++++++--------- .../predicted_objects_display.hpp | 17 +- .../tracked_objects_display.hpp | 11 +- .../detected_objects_display.cpp | 68 ++++---- .../object_polygon_detail.cpp | 80 ++++----- .../predicted_objects_display.cpp | 77 +++++---- .../tracked_objects_display.cpp | 70 ++++---- 9 files changed, 284 insertions(+), 241 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index b66d3b47cd42..32b52bb9bc85 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -28,7 +28,7 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize DetectedObjects class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay -: public ObjectPolygonDisplayBase + : public ObjectPolygonDisplayBase { Q_OBJECT @@ -40,14 +40,16 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay private: void processMessage(DetectedObjects::ConstSharedPtr msg) override; void onInitialize() override; - void onObjectsAndObstaclePointCloud(const DetectedObjects::ConstSharedPtr & input_objs_msg, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); + void onObjectsAndObstaclePointCloud( + const DetectedObjects::ConstSharedPtr & input_objs_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); - using SyncPolicy = message_filters::sync_policies::ApproximateTime; - using Sync = message_filters::Synchronizer; + using SyncPolicy = message_filters::sync_policies::ApproximateTime; + using Sync = message_filters::Synchronizer; std::shared_ptr sync_ptr_; // void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; - + }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp index 5d99b8a46371..4e796c06ab16 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp @@ -56,20 +56,20 @@ struct ObjectPropertyValues // Map defining colors according to value of label field in ObjectClassification msg const std::map< autoware_auto_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> - // Color map is based on cityscapes color - kDefaultObjectPropertyValues = { - {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, - {"UNKNOWN", {255, 255, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, - {"PEDESTRIAN", {255, 192, 203}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - {"MOTORCYCLE", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER, - {"TRAILER", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; +// Color map is based on cityscapes color +kDefaultObjectPropertyValues = { + {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, + {"UNKNOWN", {255, 255, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + {"PEDESTRIAN", {255, 192, 203}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + {"MOTORCYCLE", {119, 11, 32}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER, + {"TRAILER", {30, 144, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param shape_msg Shape msg to be converted. Corners should be in object-local frame @@ -209,14 +209,14 @@ inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose init /// \param labels List of ObjectClassificationMsg objects /// \param logger_name Name to use for logger in case of a warning (if labels is empty) /// \return Id of the best classification, Unknown if there is no best label -template +template AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC - autoware_auto_perception_msgs::msg::ObjectClassification::_label_type - get_best_label(ClassificationContainerT labels, const std::string & logger_name) +autoware_auto_perception_msgs::msg::ObjectClassification::_label_type +get_best_label(ClassificationContainerT labels, const std::string & logger_name) { const auto best_class_label = std::max_element( labels.begin(), labels.end(), - [](const auto & a, const auto & b) -> bool { return a.probability < b.probability; }); + [](const auto & a, const auto & b) -> bool {return a.probability < b.probability;}); if (best_class_label == labels.end()) { RCLCPP_WARN( rclcpp::get_logger(logger_name), diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 31a8f710a2ac..b10bc533e2c1 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include @@ -68,7 +68,7 @@ #include #include #include -#include +#include namespace autoware { @@ -80,11 +80,11 @@ namespace object_detection using Polygon2d = tier4_autoware_utils::Polygon2d; using Shape = autoware_auto_perception_msgs::msg::Shape; - // struct for creating objects buffer +// struct for creating objects buffer struct object_info { Shape shape; - geometry_msgs::msg::Pose position; + geometry_msgs::msg::Pose position; // autoware_auto_perception_msgs::msg::ObjectClassification classification; std::vector classification; }; @@ -108,9 +108,9 @@ inline pcl::PointXYZRGB toPCL(const geometry_msgs::msg::Point & point) /// for the plugin and also defines common helper functions that can be used by its derived /// classes. /// \tparam MsgT PredictedObjects or TrackedObjects or DetectedObjects type -template +template class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase -: public rviz_common::RosTopicDisplay + : public rviz_common::RosTopicDisplay { public: using Color = std::array; @@ -122,7 +122,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase using PolygonPropertyMap = std::unordered_map; - explicit ObjectPolygonDisplayBase(const std::string & default_topic, const std::string & default_pointcloud_topic) + explicit ObjectPolygonDisplayBase( + const std::string & default_topic, + const std::string & default_pointcloud_topic) : 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}, @@ -154,14 +156,15 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase SLOT(updatePalette())); m_simple_visualize_mode_property->addOption("Normal", 0); m_simple_visualize_mode_property->addOption("Simple", 1); - m_publish_objs_pointcloud = new rviz_common::properties::BoolProperty("Publish Objects Pointcloud", true, - "Enable/disable objects pointcloud publishing", this); + m_publish_objs_pointcloud = new rviz_common::properties::BoolProperty( + "Publish Objects Pointcloud", true, + "Enable/disable objects pointcloud publishing", this); m_default_pointcloud_topic = new rviz_common::properties::RosTopicProperty( - "Input pointcloud topic", - QString::fromStdString(default_pointcloud_topic), - "", + "Input pointcloud topic", + QString::fromStdString(default_pointcloud_topic), + "", "Input for pointcloud visualization of Objectcs detection pipeline", - this, + this, SLOT(updatePalette())); m_default_pointcloud_topic->setReadOnly(true); // iterate over default values to create and initialize the properties. @@ -178,7 +181,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_polygon_properties.emplace( std::piecewise_construct, std::forward_as_tuple(map_property_it.first), std::forward_as_tuple( - QColor{color[0], color[1], color[2]}, class_property_values.alpha, & parent_property)); + QColor{color[0], color[1], color[2]}, class_property_values.alpha, &parent_property)); } init_color_list(predicted_path_colors); } @@ -192,8 +195,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase this->topic_property_->setValue(m_default_topic.c_str()); this->topic_property_->setDescription("Topic to subscribe to."); - // get access to rivz node to sub and to pub to topics - rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); + // get access to rivz 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_); @@ -206,7 +210,7 @@ class AUTOWARE_AUTO_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);} void reset() override { @@ -214,7 +218,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_marker_common.clearMarkers(); } - void clear_markers() { m_marker_common.clearMarkers(); } + void clear_markers() {m_marker_common.clearMarkers();} void add_marker(visualization_msgs::msg::Marker::ConstSharedPtr marker_ptr) { @@ -226,7 +230,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_marker_common.addMessage(markers_ptr); } - // transfrom detected object pose to target frame and return bool result + // transfrom 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) @@ -256,7 +260,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase return true; } - // get transformation from tf2 + // 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) @@ -272,16 +276,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - + // variables for transfer detected objects information between callbacks std::vector objs_buffer; std::string objects_frame_id_; std::string pointcloud_frame_id_; - std::shared_ptr tf_listener_{nullptr}; // !! different type in prototype, + std::shared_ptr tf_listener_{nullptr}; // !! different type in prototype, std::unique_ptr tf_buffer_; // !! different type in prototype - - protected: /// \brief Convert given shape msg into a Marker /// \tparam ClassificationContainerT List type with ObjectClassificationMsg @@ -291,7 +293,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase /// \param labels List of ObjectClassificationMsg objects /// \param line_width Line thickness around the object /// \return Marker ptr. Id and header will have to be set by the caller - template + template std::optional get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, @@ -308,7 +310,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - template + template visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, @@ -319,7 +321,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase /// \param centroid Centroid position of the shape in Object.header.frame_id frame /// \param labels List of ObjectClassificationMsg objects /// \return Marker ptr. Id and header will have to be set by the caller - template + template std::optional get_label_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const ClassificationContainerT & labels) const @@ -333,7 +335,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - template + template std::optional get_uuid_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, const geometry_msgs::msg::Point & centroid, const ClassificationContainerT & labels) const @@ -357,7 +359,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - template + template std::optional get_velocity_text_marker_ptr( const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, const ClassificationContainerT & labels) const @@ -370,7 +372,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - template + template std::optional get_acceleration_text_marker_ptr( const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, const ClassificationContainerT & labels) const @@ -428,7 +430,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase /// \param labels list of classifications /// \return Color and alpha for the best class in the given list. Unknown class is used in /// degenerate cases - template + template std_msgs::msg::ColorRGBA get_color_rgba(const ClassificationContainerT & labels) const { static const std::string kLoggerName("ObjectPolygonDisplayBase"); @@ -444,7 +446,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase /// \tparam ClassificationContainerT Container of ObjectClassification /// \param labels list of classifications /// \return best label string - template + template std::string get_best_label(const ClassificationContainerT & labels) const { static const std::string kLoggerName("ObjectPolygonDisplayBase"); @@ -469,7 +471,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase get_color_from_uuid(const std::string & uuid) const { int i = (static_cast(uuid.at(0)) * 4 + static_cast(uuid.at(1))) % - static_cast(predicted_path_colors.size()); + static_cast(predicted_path_colors.size()); std_msgs::msg::ColorRGBA color; color.r = predicted_path_colors.at(i).r; @@ -515,9 +517,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase colors.push_back(sample_color); // spring green } - double get_line_width() { return m_line_width_property.getFloat(); } - - + double get_line_width() {return m_line_width_property.getFloat();} // helper function to get radius for kd-search @@ -538,58 +538,59 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - 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 = - tier4_autoware_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)); - } + 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 = + tier4_autoware_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, (int)floor(color_rgba.r * 256.0))); - cloud_it->g = std::max(0, std::min(255, (int)floor(color_rgba.g * 256.0))); - cloud_it->b = std::max(0, std::min(255, (int)floor(color_rgba.b * 256.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); - *out_cloud += filtered_cloud; + 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, (int)floor(color_rgba.r * 256.0))); + cloud_it->g = std::max(0, std::min(255, (int)floor(color_rgba.g * 256.0))); + cloud_it->b = std::max(0, std::min(255, (int)floor(color_rgba.b * 256.0))); + } + + *out_cloud += filtered_cloud; } rclcpp::Publisher::SharedPtr publisher_; // Default pointcloud topic; rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic; - // Property to enable/disable objects pointcloud publishing + // Property to enable/disable objects pointcloud publishing rviz_common::properties::BoolProperty * m_publish_objs_pointcloud; message_filters::Subscriber percepted_objects_subscription_; message_filters::Subscriber pointcloud_subscription_; - private: // All rviz plugins should have this. Should be initialized with pointer to this class MarkerCommon m_marker_common; @@ -625,7 +626,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::vector predicted_path_colors; - }; } // namespace object_detection } // namespace rviz_plugins diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index 5dc48c96e838..69238e676f7d 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -37,7 +37,7 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize PredictedObjects class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay -: public ObjectPolygonDisplayBase + : public ObjectPolygonDisplayBase { Q_OBJECT @@ -73,7 +73,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay auto itr = id_map.begin(); while (itr != id_map.end()) { if ( - std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) { + std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) + { unused_marker_ids.push_back(itr->second); itr = id_map.erase(itr); } else { @@ -105,11 +106,12 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay void update(float wall_dt, float ros_dt) override; bool transformObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, + const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, autoware_auto_perception_msgs::msg::PredictedObjects & output_msg); - - void onObjectsAndObstaclePointCloud(const PredictedObjects::ConstSharedPtr & input_objs_msg, + + void onObjectsAndObstaclePointCloud( + const PredictedObjects::ConstSharedPtr & input_objs_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); std::unordered_map> id_map; @@ -124,10 +126,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay std::condition_variable condition; std::vector markers; - typedef message_filters::sync_policies::ApproximateTimeSyncPolicy; + typedef message_filters::sync_policies::ApproximateTime SyncPolicy; typedef message_filters::Synchronizer Sync; typename std::shared_ptr sync_ptr_; - + // std::string objects_frame_id_; // std::vector objs_buffer; }; diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 14118b5c4aa6..771602982caf 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -35,7 +35,7 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize TrackedObjects class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay -: public ObjectPolygonDisplayBase + : public ObjectPolygonDisplayBase { Q_OBJECT @@ -71,7 +71,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay auto itr = id_map.begin(); while (itr != id_map.end()) { if ( - std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) { + std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) + { unused_marker_ids.push_back(itr->second); itr = id_map.erase(itr); } else { @@ -96,14 +97,16 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay return id_map.at(uuid); } - void onObjectsAndObstaclePointCloud(const TrackedObjects::ConstSharedPtr & input_objs_msg, + void onObjectsAndObstaclePointCloud( + 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; - typedef message_filters::sync_policies::ApproximateTimeSyncPolicy; + typedef message_filters::sync_policies::ApproximateTime SyncPolicy; typedef message_filters::Synchronizer Sync; typename std::shared_ptr sync_ptr_; 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 e2502c952972..f4149d80e519 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 @@ -24,7 +24,9 @@ namespace rviz_plugins { namespace object_detection { -DetectedObjectsDisplay::DetectedObjectsDisplay() : ObjectPolygonDisplayBase("/perception/object_recognition/detection/objects", "/perception/obstacle_segmentation/pointcloud") {} +DetectedObjectsDisplay::DetectedObjectsDisplay() +: ObjectPolygonDisplayBase("/perception/object_recognition/detection/objects", + "/perception/obstacle_segmentation/pointcloud") {} void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) { @@ -86,22 +88,29 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) void DetectedObjectsDisplay::onInitialize() { - ObjectPolygonDisplayBase::onInitialize(); - // get access to rivz node to sub and to pub to topics - rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher("~/output/detected_objects_pointcloud", rclcpp::SensorDataQoS()); - - sync_ptr_ = std::make_shared(SyncPolicy(10), percepted_objects_subscription_, pointcloud_subscription_); - sync_ptr_->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - percepted_objects_subscription_.subscribe(raw_node, "/perception/object_recognition/detection/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription_.subscribe(raw_node, m_default_pointcloud_topic->getTopic().toStdString(), - rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + ObjectPolygonDisplayBase::onInitialize(); + // get access to rivz node to sub and to pub to topics + rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); + publisher_ = raw_node->create_publisher( + "~/output/detected_objects_pointcloud", rclcpp::SensorDataQoS()); + + sync_ptr_ = std::make_shared( + SyncPolicy( + 10), percepted_objects_subscription_, pointcloud_subscription_); + sync_ptr_->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); + percepted_objects_subscription_.subscribe( + raw_node, + "/perception/object_recognition/detection/objects", + rclcpp::QoS{1}.get_rmw_qos_profile()), + pointcloud_subscription_.subscribe( + raw_node, m_default_pointcloud_topic->getTopic().toStdString(), + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( const DetectedObjects::ConstSharedPtr & input_objs_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) -{ +{ if (!m_publish_objs_pointcloud->getBool()) { return; } @@ -109,19 +118,21 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; if (!transformObjects( *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer_, - transformed_objects)) { - // objects_pub_->publish(*input_objects); + transformed_objects)) + { + // objects_pub_->publish(*input_objects); return; } objs_buffer.clear(); - for (const auto & object : transformed_objects.objects) - { - std::vector labels = object.classification; - object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + for (const auto & object : transformed_objects.objects) { + std::vector labels = + object.classification; + object_info info = + {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; objs_buffer.push_back(info); } - + // convert to pcl pointcloud pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); @@ -133,12 +144,12 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( // Create Kd-tree to search neighbor pointcloud to reduce cost. pcl::search::Search::Ptr kdtree = - pcl::make_shared>(false); + pcl::make_shared>(false); kdtree->setInputCloud(colored_cloud); pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - + if (objs_buffer.size() > 0) { for (auto object : objs_buffer) { @@ -146,25 +157,26 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( // Search neighbor pointcloud to reduce cost. pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); std::vector indices; - std::vector distances; + std::vector distances; kdtree->radiusSearch( - toPCL(object.position.position), search_radius.value(), indices, distances); + toPCL(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, object); - } + } } else { // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); return; } - sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); + 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; - + publisher_->publish(*output_pointcloud_msg_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 73a524e13077..74b2783f7e80 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 @@ -487,23 +487,23 @@ void calc_cylinder_line_list( for (int i = 0; i < n; ++i) { geometry_msgs::msg::Point point; point.x = std::cos( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius; + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; point.y = std::sin( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius; + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; point.z = shape.dimensions.z * 0.5; points.push_back(point); point.x = std::cos( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius; + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; point.y = std::sin( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius; + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; point.z = -shape.dimensions.z * 0.5; points.push_back(point); } @@ -532,27 +532,27 @@ void calc_circle_line_list( for (int i = 0; i < n; ++i) { geometry_msgs::msg::Point point; point.x = std::cos( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius + - center.x; + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.x; point.y = std::sin( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius + - center.y; + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.y; point.z = center.z; points.push_back(point); point.x = std::cos( - (static_cast(i + 1.0) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius + - center.x; + (static_cast(i + 1.0) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.x; point.y = std::sin( - (static_cast(i + 1.0) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius + - center.y; + (static_cast(i + 1.0) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.y; point.z = center.z; points.push_back(point); } @@ -575,11 +575,11 @@ void calc_polygon_line_list( point.z = shape.dimensions.z / 2.0; points.push_back(point); point.x = shape.footprint.points - .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) - .x; + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; point.y = shape.footprint.points - .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) - .y; + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; point.z = shape.dimensions.z / 2.0; points.push_back(point); } @@ -590,11 +590,11 @@ void calc_polygon_line_list( point.z = -shape.dimensions.z / 2.0; points.push_back(point); point.x = shape.footprint.points - .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) - .x; + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; point.y = shape.footprint.points - .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) - .y; + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; point.z = -shape.dimensions.z / 2.0; points.push_back(point); } @@ -628,11 +628,11 @@ void calc_2d_polygon_bottom_line_list( point.z = -shape.dimensions.z / 2.0; points.push_back(point); point.x = shape.footprint.points - .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) - .x; + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; point.y = shape.footprint.points - .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) - .y; + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; point.z = -shape.dimensions.z / 2.0; points.push_back(point); } 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 5e26089c0bfb..f2186edf7f16 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 @@ -22,7 +22,8 @@ namespace rviz_plugins { namespace object_detection { -PredictedObjectsDisplay::PredictedObjectsDisplay() : ObjectPolygonDisplayBase("predictions", "/perception/obstacle_segmentation/pointcloud") +PredictedObjectsDisplay::PredictedObjectsDisplay() +: ObjectPolygonDisplayBase("predictions", "/perception/obstacle_segmentation/pointcloud") { std::thread worker(&PredictedObjectsDisplay::workerThread, this); worker.detach(); @@ -32,7 +33,7 @@ void PredictedObjectsDisplay::workerThread() { while (true) { std::unique_lock lock(mutex); - condition.wait(lock, [this] { return this->msg; }); + condition.wait(lock, [this] {return this->msg;}); auto tmp_msg = this->msg; this->msg.reset(); @@ -210,23 +211,29 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) wall_dt, ros_dt); } -void PredictedObjectsDisplay::onInitialize() +void PredictedObjectsDisplay::onInitialize() { - ObjectPolygonDisplayBase::onInitialize(); - // get access to rivz node to sub and to pub to topics - rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher("~/output/predicted_objects_pointcloud", rclcpp::SensorDataQoS()); - - sync_ptr_ = std::make_shared(SyncPolicy(10), percepted_objects_subscription_, pointcloud_subscription_); - sync_ptr_->registerCallback(&PredictedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - - percepted_objects_subscription_.subscribe(raw_node, "/perception/object_recognition/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription_.subscribe(raw_node, m_default_pointcloud_topic->getTopic().toStdString(), - rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + ObjectPolygonDisplayBase::onInitialize(); + // get access to rivz node to sub and to pub to topics + rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); + publisher_ = raw_node->create_publisher( + "~/output/predicted_objects_pointcloud", rclcpp::SensorDataQoS()); + + sync_ptr_ = std::make_shared( + SyncPolicy( + 10), percepted_objects_subscription_, pointcloud_subscription_); + sync_ptr_->registerCallback(&PredictedObjectsDisplay::onObjectsAndObstaclePointCloud, this); + + percepted_objects_subscription_.subscribe( + raw_node, "/perception/object_recognition/objects", + rclcpp::QoS{1}.get_rmw_qos_profile()), + pointcloud_subscription_.subscribe( + raw_node, m_default_pointcloud_topic->getTopic().toStdString(), + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } bool PredictedObjectsDisplay::transformObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, + const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, autoware_auto_perception_msgs::msg::PredictedObjects & output_msg) { @@ -258,27 +265,30 @@ bool PredictedObjectsDisplay::transformObjects( void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( const PredictedObjects::ConstSharedPtr & input_objs_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) -{ - if (!m_publish_objs_pointcloud->getBool()) { +{ + if (!m_publish_objs_pointcloud->getBool()) { return; } // Transform to pointcloud frame autoware_auto_perception_msgs::msg::PredictedObjects transformed_objects; if (!transformObjects( *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer_, - transformed_objects)) { - // objects_pub_->publish(*input_objects); + transformed_objects)) + { + // objects_pub_->publish(*input_objects); return; } objs_buffer.clear(); - for (const auto & object : transformed_objects.objects) - { - std::vector labels = object.classification; - object_info info = {object.shape, object.kinematics.initial_pose_with_covariance.pose, object.classification}; + for (const auto & object : transformed_objects.objects) { + std::vector labels = + object.classification; + object_info info = + {object.shape, object.kinematics.initial_pose_with_covariance.pose, + object.classification}; objs_buffer.push_back(info); } - + // convert to pcl pointcloud pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); @@ -290,12 +300,12 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( // Create Kd-tree to search neighbor pointcloud to reduce cost. pcl::search::Search::Ptr kdtree = - pcl::make_shared>(false); + pcl::make_shared>(false); kdtree->setInputCloud(colored_cloud); pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - + if (objs_buffer.size() > 0) { for (auto object : objs_buffer) { @@ -303,25 +313,26 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( // Search neighbor pointcloud to reduce cost. pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); std::vector indices; - std::vector distances; + std::vector distances; kdtree->radiusSearch( - toPCL(object.position.position), search_radius.value(), indices, distances); + toPCL(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, object); - } + } } else { // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); return; } - sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); + 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; - + publisher_->publish(*output_pointcloud_msg_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 b174cfd8d7fa..c40dc9402cee 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 @@ -24,7 +24,9 @@ namespace rviz_plugins { namespace object_detection { -TrackedObjectsDisplay::TrackedObjectsDisplay() : ObjectPolygonDisplayBase("/perception/object_recognition/tracking/objects", "/perception/obstacle_segmentation/pointcloud") {} +TrackedObjectsDisplay::TrackedObjectsDisplay() +: ObjectPolygonDisplayBase("/perception/object_recognition/tracking/objects", + "/perception/obstacle_segmentation/pointcloud") {} void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) { @@ -124,23 +126,30 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) void TrackedObjectsDisplay::onInitialize() { - ObjectPolygonDisplayBase::onInitialize(); - // get access to rivz node to sub and to pub to topics - rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher("~/output/tracked_objects_pointcloud", rclcpp::SensorDataQoS()); - - sync_ptr_ = std::make_shared(SyncPolicy(10), percepted_objects_subscription_, pointcloud_subscription_); - sync_ptr_->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - - percepted_objects_subscription_.subscribe(raw_node, "/perception/object_recognition/tracking/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription_.subscribe(raw_node, m_default_pointcloud_topic->getTopic().toStdString(), - rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + ObjectPolygonDisplayBase::onInitialize(); + // get access to rivz node to sub and to pub to topics + rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); + publisher_ = raw_node->create_publisher( + "~/output/tracked_objects_pointcloud", rclcpp::SensorDataQoS()); + + sync_ptr_ = std::make_shared( + SyncPolicy( + 10), percepted_objects_subscription_, pointcloud_subscription_); + sync_ptr_->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); + + percepted_objects_subscription_.subscribe( + raw_node, + "/perception/object_recognition/tracking/objects", + rclcpp::QoS{1}.get_rmw_qos_profile()), + pointcloud_subscription_.subscribe( + raw_node, m_default_pointcloud_topic->getTopic().toStdString(), + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( const TrackedObjects::ConstSharedPtr & input_objs_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) -{ +{ if (!m_publish_objs_pointcloud->getBool()) { return; } @@ -148,19 +157,21 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( autoware_auto_perception_msgs::msg::TrackedObjects transformed_objects; if (!transformObjects( *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer_, - transformed_objects)) { - // objects_pub_->publish(*input_objects); + transformed_objects)) + { + // objects_pub_->publish(*input_objects); return; } objs_buffer.clear(); - for (const auto & object : transformed_objects.objects) - { - std::vector labels = object.classification; - object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + for (const auto & object : transformed_objects.objects) { + std::vector labels = + object.classification; + object_info info = + {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; objs_buffer.push_back(info); } - + // convert to pcl pointcloud pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); @@ -172,12 +183,12 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( // Create Kd-tree to search neighbor pointcloud to reduce cost. pcl::search::Search::Ptr kdtree = - pcl::make_shared>(false); + pcl::make_shared>(false); kdtree->setInputCloud(colored_cloud); pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - + if (objs_buffer.size() > 0) { for (auto object : objs_buffer) { @@ -185,25 +196,26 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( // Search neighbor pointcloud to reduce cost. pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); std::vector indices; - std::vector distances; + std::vector distances; kdtree->radiusSearch( - toPCL(object.position.position), search_radius.value(), indices, distances); + toPCL(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, object); - } + } } else { // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); return; } - sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); + 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; - + publisher_->publish(*output_pointcloud_msg_ptr); } From 2a30ed3075dee289eec237b021ca79248954e858 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Thu, 16 Mar 2023 16:18:46 +0800 Subject: [PATCH 12/58] feat(add_perception_objects_pointcloud_publisher) update variable names Signed-off-by: Alexey Panferov --- .../detected_objects_display.hpp | 3 +-- .../object_polygon_display_base.hpp | 19 ++++++++----------- .../predicted_objects_display.hpp | 5 +---- .../tracked_objects_display.hpp | 5 +---- .../detected_objects_display.cpp | 19 ++++++++----------- .../predicted_objects_display.cpp | 17 ++++++++--------- .../tracked_objects_display.cpp | 17 ++++++++--------- 7 files changed, 35 insertions(+), 50 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index 32b52bb9bc85..2469fa54af1d 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -47,8 +47,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay using SyncPolicy = message_filters::sync_policies::ApproximateTime; using Sync = message_filters::Synchronizer; - std::shared_ptr sync_ptr_; - // void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; + std::shared_ptr sync_ptr; }; diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index b10bc533e2c1..7d3eda9d6e76 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -199,8 +199,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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_); + tf_buffer = std::make_unique(raw_node->get_clock()); + tf_listener = std::make_shared(*tf_buffer); } @@ -271,7 +271,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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("perception_utils"), ex.what()); // rename + RCLCPP_WARN_STREAM(rclcpp::get_logger("autoware_auto_perception_plugin"), ex.what()); // rename return boost::none; } } @@ -279,10 +279,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // variables for transfer detected objects information between callbacks std::vector objs_buffer; - std::string objects_frame_id_; - std::string pointcloud_frame_id_; - std::shared_ptr tf_listener_{nullptr}; // !! different type in prototype, - std::unique_ptr tf_buffer_; // !! different type in prototype + std::shared_ptr tf_listener{nullptr}; + std::unique_ptr tf_buffer; protected: /// \brief Convert given shape msg into a Marker @@ -533,7 +531,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } return max_dist; } else { - // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "unknown shape type"); return std::nullopt; } } @@ -581,15 +578,15 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase *out_cloud += filtered_cloud; } - rclcpp::Publisher::SharedPtr publisher_; // Default pointcloud topic; rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic; // Property to enable/disable objects pointcloud publishing rviz_common::properties::BoolProperty * m_publish_objs_pointcloud; - message_filters::Subscriber percepted_objects_subscription_; - message_filters::Subscriber pointcloud_subscription_; + rclcpp::Publisher::SharedPtr publisher; + message_filters::Subscriber percepted_objects_subscription; + message_filters::Subscriber pointcloud_subscription; private: // All rviz plugins should have this. Should be initialized with pointer to this class diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index 69238e676f7d..365cdd30c156 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -49,7 +49,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay private: void processMessage(PredictedObjects::ConstSharedPtr msg) override; void onInitialize() override; - // void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) { @@ -129,10 +128,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay typedef message_filters::sync_policies::ApproximateTime SyncPolicy; typedef message_filters::Synchronizer Sync; - typename std::shared_ptr sync_ptr_; + typename std::shared_ptr sync_ptr; - // std::string objects_frame_id_; - // std::vector objs_buffer; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 771602982caf..1530c9bb461f 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -47,7 +47,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay private: void processMessage(TrackedObjects::ConstSharedPtr msg) override; void onInitialize() override; - // void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) { @@ -108,10 +107,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay typedef message_filters::sync_policies::ApproximateTime SyncPolicy; typedef message_filters::Synchronizer Sync; - typename std::shared_ptr sync_ptr_; + typename std::shared_ptr sync_ptr; - // std::string objects_frame_id_; - // std::vector objs_buffer; }; } // namespace object_detection 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 f4149d80e519..a61c65ab697c 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 @@ -80,9 +80,6 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) twist_marker_ptr->id = id++; add_marker(twist_marker_ptr); } - - // Transform to pointcloud frame - // RCLCPP_INFO(this->get_logger(), "Update objects buffer"); } } @@ -91,18 +88,18 @@ void DetectedObjectsDisplay::onInitialize() ObjectPolygonDisplayBase::onInitialize(); // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher( + publisher = raw_node->create_publisher( "~/output/detected_objects_pointcloud", rclcpp::SensorDataQoS()); - sync_ptr_ = std::make_shared( + sync_ptr = std::make_shared( SyncPolicy( - 10), percepted_objects_subscription_, pointcloud_subscription_); - sync_ptr_->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - percepted_objects_subscription_.subscribe( + 10), percepted_objects_subscription, pointcloud_subscription); + sync_ptr->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); + percepted_objects_subscription.subscribe( raw_node, "/perception/object_recognition/detection/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription_.subscribe( + pointcloud_subscription.subscribe( raw_node, m_default_pointcloud_topic->getTopic().toStdString(), rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } @@ -117,7 +114,7 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( // Transform to pointcloud frame autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; if (!transformObjects( - *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer_, + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { // objects_pub_->publish(*input_objects); @@ -177,7 +174,7 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - publisher_->publish(*output_pointcloud_msg_ptr); + publisher->publish(*output_pointcloud_msg_ptr); } } // namespace object_detection 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 f2186edf7f16..d0cb82b766c2 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 @@ -216,18 +216,18 @@ void PredictedObjectsDisplay::onInitialize() ObjectPolygonDisplayBase::onInitialize(); // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher( + publisher = raw_node->create_publisher( "~/output/predicted_objects_pointcloud", rclcpp::SensorDataQoS()); - sync_ptr_ = std::make_shared( + sync_ptr = std::make_shared( SyncPolicy( - 10), percepted_objects_subscription_, pointcloud_subscription_); - sync_ptr_->registerCallback(&PredictedObjectsDisplay::onObjectsAndObstaclePointCloud, this); + 10), percepted_objects_subscription, pointcloud_subscription); + sync_ptr->registerCallback(&PredictedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - percepted_objects_subscription_.subscribe( + percepted_objects_subscription.subscribe( raw_node, "/perception/object_recognition/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription_.subscribe( + pointcloud_subscription.subscribe( raw_node, m_default_pointcloud_topic->getTopic().toStdString(), rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } @@ -272,10 +272,9 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( // Transform to pointcloud frame autoware_auto_perception_msgs::msg::PredictedObjects transformed_objects; if (!transformObjects( - *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer_, + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { - // objects_pub_->publish(*input_objects); return; } @@ -333,7 +332,7 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - publisher_->publish(*output_pointcloud_msg_ptr); + publisher->publish(*output_pointcloud_msg_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 c40dc9402cee..17936fc05ddc 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 @@ -129,19 +129,19 @@ void TrackedObjectsDisplay::onInitialize() ObjectPolygonDisplayBase::onInitialize(); // get access to rivz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher_ = raw_node->create_publisher( + publisher = raw_node->create_publisher( "~/output/tracked_objects_pointcloud", rclcpp::SensorDataQoS()); - sync_ptr_ = std::make_shared( + sync_ptr = std::make_shared( SyncPolicy( - 10), percepted_objects_subscription_, pointcloud_subscription_); - sync_ptr_->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); + 10), percepted_objects_subscription, pointcloud_subscription); + sync_ptr->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - percepted_objects_subscription_.subscribe( + percepted_objects_subscription.subscribe( raw_node, "/perception/object_recognition/tracking/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription_.subscribe( + pointcloud_subscription.subscribe( raw_node, m_default_pointcloud_topic->getTopic().toStdString(), rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } @@ -156,10 +156,9 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( // Transform to pointcloud frame autoware_auto_perception_msgs::msg::TrackedObjects transformed_objects; if (!transformObjects( - *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer_, + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { - // objects_pub_->publish(*input_objects); return; } @@ -216,7 +215,7 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - publisher_->publish(*output_pointcloud_msg_ptr); + publisher->publish(*output_pointcloud_msg_ptr); } } // namespace object_detection From 1a2c1b7336f0cc37bec47396ba53dc1e956f1f5a Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Thu, 16 Mar 2023 16:26:24 +0800 Subject: [PATCH 13/58] feat(add_perception_objects_pointcloud_publisher) reformat Signed-off-by: Alexey Panferov --- .../detected_objects_display.hpp | 7 +- .../object_polygon_display_base.hpp | 90 ++++++++----------- .../predicted_objects_display.hpp | 8 +- .../tracked_objects_display.hpp | 8 +- .../detected_objects_display.cpp | 31 ++++--- .../predicted_objects_display.cpp | 30 +++---- .../tracked_objects_display.cpp | 32 ++++--- 7 files changed, 92 insertions(+), 114 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index 2469fa54af1d..fef8b9cfcddd 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -16,8 +16,8 @@ #include -#include #include +#include #include namespace autoware @@ -44,11 +44,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay const DetectedObjects::ConstSharedPtr & input_objs_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); - using SyncPolicy = message_filters::sync_policies::ApproximateTime; + using SyncPolicy = + message_filters::sync_policies::ApproximateTime; using Sync = message_filters::Synchronizer; std::shared_ptr sync_ptr; - }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 7d3eda9d6e76..3c5b59d7fb65 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -26,49 +26,48 @@ #include // #include -#include "rclcpp/rclcpp.hpp" #include "rclcpp/clock.hpp" -#include -#include +#include "rclcpp/rclcpp.hpp" #include "rviz_common/properties/ros_topic_property.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 +#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -#include -#include +#include #include #include #include #include #include #include -#include namespace autoware { @@ -103,7 +102,6 @@ inline pcl::PointXYZRGB toPCL(const geometry_msgs::msg::Point & point) return toPCL(point.x, point.y, point.z); } - /// \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. @@ -123,8 +121,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::unordered_map; explicit ObjectPolygonDisplayBase( - const std::string & default_topic, - const std::string & default_pointcloud_topic) + const std::string & default_topic, const std::string & default_pointcloud_topic) : 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}, @@ -157,14 +154,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_simple_visualize_mode_property->addOption("Normal", 0); m_simple_visualize_mode_property->addOption("Simple", 1); m_publish_objs_pointcloud = new rviz_common::properties::BoolProperty( - "Publish Objects Pointcloud", true, - "Enable/disable objects pointcloud publishing", this); + "Publish Objects Pointcloud", true, "Enable/disable objects pointcloud publishing", this); m_default_pointcloud_topic = new rviz_common::properties::RosTopicProperty( - "Input pointcloud topic", - QString::fromStdString(default_pointcloud_topic), - "", - "Input for pointcloud visualization of Objectcs detection pipeline", - this, + "Input pointcloud topic", QString::fromStdString(default_pointcloud_topic), "", + "Input for pointcloud visualization of Objectcs detection pipeline", this, SLOT(updatePalette())); m_default_pointcloud_topic->setReadOnly(true); // iterate over default values to create and initialize the properties. @@ -201,7 +194,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase tf_buffer = std::make_unique(raw_node->get_clock()); tf_listener = std::make_shared(*tf_buffer); - } void load(const rviz_common::Config & config) override @@ -271,16 +263,16 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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_auto_perception_plugin"), ex.what()); // rename + RCLCPP_WARN_STREAM( + rclcpp::get_logger("autoware_auto_perception_plugin"), ex.what()); // rename return boost::none; } } - // variables for transfer detected objects information between callbacks std::vector objs_buffer; - std::shared_ptr tf_listener{nullptr}; - std::unique_ptr tf_buffer; + std::shared_ptr tf_listener{nullptr}; + std::unique_ptr tf_buffer; protected: /// \brief Convert given shape msg into a Marker @@ -517,7 +509,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase double get_line_width() {return m_line_width_property.getFloat();} - // helper function to get radius for kd-search std::optional getMaxRadius(object_info & object) { @@ -543,17 +534,17 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::vector vertices_array; pcl::Vertices vertices; - Polygon2d poly2d = - tier4_autoware_utils::toPolygon2d(object.position, object.shape); - if (boost::geometry::is_empty(poly2d)) {return;} + Polygon2d poly2d = tier4_autoware_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(poly2d.outer().at(i).x()), static_cast(poly2d.outer().at(i).y()), static_cast(0.0)); } @@ -566,8 +557,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase crop_hull_filter.filter(filtered_cloud); - - const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(object.classification); // need to be converted to int + 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, (int)floor(color_rgba.r * 256.0))); @@ -578,7 +569,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase *out_cloud += filtered_cloud; } - // Default pointcloud topic; rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic; // Property to enable/disable objects pointcloud publishing @@ -621,8 +611,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::string m_default_topic; std::vector predicted_path_colors; - - }; } // namespace object_detection } // namespace rviz_plugins diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index 365cdd30c156..fb973b2466b8 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -16,8 +16,8 @@ #include -#include #include +#include #include #include @@ -125,11 +125,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay std::condition_variable condition; std::vector markers; - typedef message_filters::sync_policies::ApproximateTime SyncPolicy; + typedef message_filters::sync_policies::ApproximateTime< + PredictedObjects, sensor_msgs::msg::PointCloud2> + SyncPolicy; typedef message_filters::Synchronizer Sync; typename std::shared_ptr sync_ptr; - }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 1530c9bb461f..24b8bf9621a8 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -16,8 +16,8 @@ #include -#include #include +#include #include #include @@ -104,11 +104,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay std::list unused_marker_ids; int32_t marker_id = 0; - typedef message_filters::sync_policies::ApproximateTime SyncPolicy; + typedef message_filters::sync_policies::ApproximateTime< + TrackedObjects, sensor_msgs::msg::PointCloud2> + SyncPolicy; typedef message_filters::Synchronizer Sync; typename std::shared_ptr sync_ptr; - }; } // namespace object_detection 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 a61c65ab697c..31f20e43a02a 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 @@ -25,8 +25,11 @@ namespace rviz_plugins namespace object_detection { DetectedObjectsDisplay::DetectedObjectsDisplay() -: ObjectPolygonDisplayBase("/perception/object_recognition/detection/objects", - "/perception/obstacle_segmentation/pointcloud") {} +: ObjectPolygonDisplayBase( + "/perception/object_recognition/detection/objects", + "/perception/obstacle_segmentation/pointcloud") +{ +} void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) { @@ -91,13 +94,11 @@ void DetectedObjectsDisplay::onInitialize() publisher = raw_node->create_publisher( "~/output/detected_objects_pointcloud", rclcpp::SensorDataQoS()); - sync_ptr = std::make_shared( - SyncPolicy( - 10), percepted_objects_subscription, pointcloud_subscription); + sync_ptr = + std::make_shared(SyncPolicy(10), percepted_objects_subscription, pointcloud_subscription); sync_ptr->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); percepted_objects_subscription.subscribe( - raw_node, - "/perception/object_recognition/detection/objects", + raw_node, "/perception/object_recognition/detection/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), pointcloud_subscription.subscribe( raw_node, m_default_pointcloud_topic->getTopic().toStdString(), @@ -114,8 +115,7 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( // Transform to pointcloud frame autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; if (!transformObjects( - *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, - transformed_objects)) + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { // objects_pub_->publish(*input_objects); return; @@ -125,8 +125,8 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( for (const auto & object : transformed_objects.objects) { std::vector labels = object.classification; - object_info info = - {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + object_info info = { + object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; objs_buffer.push_back(info); } @@ -146,13 +146,12 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - if (objs_buffer.size() > 0) { for (auto object : objs_buffer) { - const auto search_radius = getMaxRadius(object); // Search neighbor pointcloud to reduce cost. - pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); + pcl::PointCloud::Ptr neighbor_pointcloud( + new pcl::PointCloud); std::vector indices; std::vector distances; kdtree->radiusSearch( @@ -168,8 +167,8 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( return; } - sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr(new sensor_msgs::msg:: - PointCloud2); + 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; 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 d0cb82b766c2..1a30a8eccfb8 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 @@ -219,14 +219,12 @@ void PredictedObjectsDisplay::onInitialize() publisher = raw_node->create_publisher( "~/output/predicted_objects_pointcloud", rclcpp::SensorDataQoS()); - sync_ptr = std::make_shared( - SyncPolicy( - 10), percepted_objects_subscription, pointcloud_subscription); + sync_ptr = + std::make_shared(SyncPolicy(10), percepted_objects_subscription, pointcloud_subscription); sync_ptr->registerCallback(&PredictedObjectsDisplay::onObjectsAndObstaclePointCloud, this); percepted_objects_subscription.subscribe( - raw_node, "/perception/object_recognition/objects", - rclcpp::QoS{1}.get_rmw_qos_profile()), + raw_node, "/perception/object_recognition/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), pointcloud_subscription.subscribe( raw_node, m_default_pointcloud_topic->getTopic().toStdString(), rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); @@ -246,8 +244,8 @@ bool PredictedObjectsDisplay::transformObjects( 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); + 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; } @@ -272,8 +270,7 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( // Transform to pointcloud frame autoware_auto_perception_msgs::msg::PredictedObjects transformed_objects; if (!transformObjects( - *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, - transformed_objects)) + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { return; } @@ -282,9 +279,8 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( for (const auto & object : transformed_objects.objects) { std::vector labels = object.classification; - object_info info = - {object.shape, object.kinematics.initial_pose_with_covariance.pose, - object.classification}; + object_info info = { + object.shape, object.kinematics.initial_pose_with_covariance.pose, object.classification}; objs_buffer.push_back(info); } @@ -304,13 +300,12 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - if (objs_buffer.size() > 0) { for (auto object : objs_buffer) { - const auto search_radius = getMaxRadius(object); // Search neighbor pointcloud to reduce cost. - pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); + pcl::PointCloud::Ptr neighbor_pointcloud( + new pcl::PointCloud); std::vector indices; std::vector distances; kdtree->radiusSearch( @@ -326,8 +321,8 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( return; } - sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr(new sensor_msgs::msg:: - PointCloud2); + 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; @@ -335,7 +330,6 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( publisher->publish(*output_pointcloud_msg_ptr); } - } // namespace object_detection } // namespace rviz_plugins } // namespace autoware 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 17936fc05ddc..a56e1d93bce8 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 @@ -25,8 +25,11 @@ namespace rviz_plugins namespace object_detection { TrackedObjectsDisplay::TrackedObjectsDisplay() -: ObjectPolygonDisplayBase("/perception/object_recognition/tracking/objects", - "/perception/obstacle_segmentation/pointcloud") {} +: ObjectPolygonDisplayBase( + "/perception/object_recognition/tracking/objects", + "/perception/obstacle_segmentation/pointcloud") +{ +} void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) { @@ -120,7 +123,6 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) twist_marker_ptr->id = uuid_to_marker_id(object.object_id); add_marker(twist_marker_ptr); } - } } @@ -132,14 +134,12 @@ void TrackedObjectsDisplay::onInitialize() publisher = raw_node->create_publisher( "~/output/tracked_objects_pointcloud", rclcpp::SensorDataQoS()); - sync_ptr = std::make_shared( - SyncPolicy( - 10), percepted_objects_subscription, pointcloud_subscription); + sync_ptr = + std::make_shared(SyncPolicy(10), percepted_objects_subscription, pointcloud_subscription); sync_ptr->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); percepted_objects_subscription.subscribe( - raw_node, - "/perception/object_recognition/tracking/objects", + raw_node, "/perception/object_recognition/tracking/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), pointcloud_subscription.subscribe( raw_node, m_default_pointcloud_topic->getTopic().toStdString(), @@ -156,8 +156,7 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( // Transform to pointcloud frame autoware_auto_perception_msgs::msg::TrackedObjects transformed_objects; if (!transformObjects( - *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, - transformed_objects)) + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { return; } @@ -166,8 +165,8 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( for (const auto & object : transformed_objects.objects) { std::vector labels = object.classification; - object_info info = - {object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + object_info info = { + object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; objs_buffer.push_back(info); } @@ -187,13 +186,12 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - if (objs_buffer.size() > 0) { for (auto object : objs_buffer) { - const auto search_radius = getMaxRadius(object); // Search neighbor pointcloud to reduce cost. - pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); + pcl::PointCloud::Ptr neighbor_pointcloud( + new pcl::PointCloud); std::vector indices; std::vector distances; kdtree->radiusSearch( @@ -209,8 +207,8 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( return; } - sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr(new sensor_msgs::msg:: - PointCloud2); + 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; From 5f014b27129e12b673c0ee3ea9917d3d042bbff1 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Thu, 16 Mar 2023 16:28:20 +0800 Subject: [PATCH 14/58] feat(add_perception_objects_pointcloud_publisher) remove test launch and config Signed-off-by: Alexey Panferov --- .../launch/rviz.launch.xml | 6 - .../rviz/debug_config.rivz.rviz | 2167 ----------------- 2 files changed, 2173 deletions(-) delete mode 100644 common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml delete mode 100644 common/autoware_auto_perception_rviz_plugin/rviz/debug_config.rivz.rviz diff --git a/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml b/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml deleted file mode 100644 index 282b14739685..000000000000 --- a/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/common/autoware_auto_perception_rviz_plugin/rviz/debug_config.rivz.rviz b/common/autoware_auto_perception_rviz_plugin/rviz/debug_config.rivz.rviz deleted file mode 100644 index 635c4c38a4cb..000000000000 --- a/common/autoware_auto_perception_rviz_plugin/rviz/debug_config.rivz.rviz +++ /dev/null @@ -1,2167 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - - /Perception1 - - /Perception1/ObjectRecognition1 - - /Perception1/ObjectRecognition1/Prediction1/PredictedObjects1 - Splitter Ratio: 0.557669460773468 - Tree Height: 288 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel - - Class: AutowareDateTimePanel - Name: AutowareDateTimePanel - - Class: rviz_plugins::AutowareStatePanel - Name: AutowareStatePanel -Visualization Manager: - Class: "" - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/SteeringAngle - Enabled: true - Left: 95 - Length: 190 - Name: SteeringAngle - Scale: 17 - Text Color: 25; 255; 240 - Top: 95 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/steering_status - Value: true - Value Scale: 0.14999249577522278 - Value height offset: 0 - - Class: rviz_plugins/ConsoleMeter - Enabled: true - Left: 379 - Length: 190 - Name: ConsoleMeter - Text Color: 25; 255; 240 - Top: 95 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/velocity_status - Value: true - Value Scale: 0.14999249577522278 - Value height offset: 0 - - Alpha: 0.9990000128746033 - Class: rviz_plugins/VelocityHistory - Color Border Vel Max: 3 - Constant Color: - Color: 255; 255; 255 - Value: true - Enabled: true - Name: VelocityHistory - Scale: 0.30000001192092896 - Timeout: 10 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/velocity_status - Value: true - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera0/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera0/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera1/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera1/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera2/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera2/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera3/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera3/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera4/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera4/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera5/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera5/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - gnss_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_kit_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - tamagawa/imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - traffic_light_left_camera/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - traffic_light_left_camera/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - traffic_light_right_camera/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - traffic_light_right_camera/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - velodyne_left: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_left_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_rear: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_rear_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_right: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_right_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_top: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_top_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Mass Properties: - Inertia: false - Mass: false - Name: VehicleModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Class: rviz_plugins/MaxVelocity - Enabled: true - Left: 441 - Length: 71 - Name: MaxVelocity - Text Color: 255; 255; 255 - Top: 207 - Topic: /planning/scenario_planning/current_max_velocity - Value: true - Value Scale: 0.25 - - Class: rviz_plugins/TurnSignal - Enabled: true - Height: 190 - Left: 145 - Name: TurnSignal - Top: 259 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /vehicle/status/turn_indicators_status - Value: true - Width: 379 - Enabled: true - Name: Vehicle - Enabled: true - Name: System - - Class: rviz_common/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.71826171875 - Min Value: -7.4224700927734375 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 237 - Min Color: 211; 215; 207 - Min Intensity: 0 - Name: PointCloudMap - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/pointcloud_map - Use Fixed Frame: true - Use rainbow: false - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Lanelet2VectorMap - Namespaces: - center_lane_line: false - center_line_arrows: false - crosswalk_lanelets: true - lane_start_bound: false - lanelet_id: false - left_lane_bound: true - right_lane_bound: true - road_lanelets: false - stop_lines: true - traffic_light: true - traffic_light_id: false - traffic_light_triangle: true - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - Enabled: true - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - Alpha: 0.9990000128746033 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: MeasurementRange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/lidar/crop_box_filter/crop_box_polygon - Value: false - Enabled: true - Name: LiDAR - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 233; 185; 110 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: false - Position: - Alpha: 0.20000000298023224 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Head Length: 0.699999988079071 - Head Radius: 1.2000000476837158 - Name: PoseWithCovariance - Shaft Length: 1 - Shaft Radius: 0.5 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/gnss/pose_with_covariance - Value: true - Enabled: false - Name: GNSS - Enabled: true - Name: Sensing - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 170; 255 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovInitial - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/initial_pose_with_covariance - Value: false - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 255; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovAligned - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose_with_covariance - Value: false - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: false - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /localization/util/downsample/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Aligned - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MonteCarloInitialPose - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/monte_carlo_initial_pose_marker - Value: true - Enabled: true - Name: NDT - - Class: rviz_common/Group - Displays: - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: EKF - Enabled: true - Name: Localization - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 15 - Min Value: -2 - Value: false - Axis: Z - Channel Name: z - Class: rviz_default_plugins/PointCloud2 - Color: 200; 200; 200 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15 - Min Color: 0; 0; 0 - Min Intensity: -5 - Name: NoGroundPointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/obstacle_segmentation/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Segmentation - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/DetectedObjects - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Input pointcloud topic: /perception/obstacle_segmentation/pointcloud - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Detection - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/TrackedObjects - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Input pointcloud topic: /perception/obstacle_segmentation/pointcloud - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: TrackedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Tracking - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_auto_perception_rviz_plugin/PredictedObjects - Display Acceleration: true - Display Label: true - Display PoseWithCovariance: false - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display UUID: true - Display Velocity: true - Enabled: true - Input pointcloud topic: /perception/obstacle_segmentation/pointcloud - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: PredictedObjects - Namespaces: - {} - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Maneuver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/prediction/maneuver - Value: false - Enabled: true - Name: Prediction - Enabled: true - Name: ObjectRecognition - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Image - Enabled: true - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RecognitionResultOnImage - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/debug/rois - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MapBasedDetectionResult - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers - Value: true - Enabled: true - Name: TrafficLight - - Class: rviz_common/Group - Displays: - - Alpha: 0.5 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map_updates - Use Timestamp: false - Value: true - Enabled: false - Name: OccupancyGrid - Enabled: true - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: RouteArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/route_marker - Value: true - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.5 - Name: GoalPose - Shaft Length: 3 - Shaft Radius: 0.20000000298023224 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/echo_back_goal_pose - Value: true - Enabled: true - Name: MissionPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: ScenarioTrajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/trajectory - Value: true - View Path: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: Path - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/path - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Path: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_LaneChange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Path: - Alpha: 0.30000001192092896 - Color: 115; 210; 22 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_ExternalRequestLaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/ext_request_lane_change_right - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Path: - Alpha: 0.30000001192092896 - Color: 115; 210; 22 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_ExternalRequestLaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/ext_request_lane_change_left - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Path: - Alpha: 0.30000001192092896 - Color: 115; 210; 22 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_Avoidance - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/avoidance - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Path: - Alpha: 0.30000001192092896 - Color: 115; 210; 22 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_PullOut - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/pull_out - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Path: - Alpha: 0.30000001192092896 - Color: 115; 210; 22 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_PullOver - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/pull_over - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Path: - Alpha: 0.30000001192092896 - Color: 115; 210; 22 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Bound - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (BlindSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Crosswalk) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DetectionArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Intersection) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MergeFromPrivateArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoStoppingArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OcclusionSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StopLine) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (TrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (VirtualTrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (RunOut) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SpeedBump) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/speed_bump - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Arrow - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Blind Spot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: VirtualTrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StopLine - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: OcclusionSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RunOut - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Avoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanechange - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneFollowing - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanefollowing - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: PullOver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullover - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: PullOut - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullout - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SideShift - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/sideshift - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SpeedBump - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/speed_bump - Value: false - Enabled: false - Name: DebugMarker - Enabled: true - Name: BehaviorPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/trajectory - Value: false - View Path: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SurroundObstacle) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleCruise) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/virtual_wall - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SurroundObstacleCheck - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Footprint - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 239; 41; 41 - Enabled: false - Name: FootprintOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 10; 21; 255 - Enabled: false - Name: FootprintRecoverOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset - Value: false - Enabled: false - Name: SurroundObstacleChecker - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: CruiseVirtualWall - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DebugMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker - Value: true - Enabled: false - Name: ObstacleCruise - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker - Value: false - Enabled: false - Name: DebugMarker - Enabled: true - Name: MotionPlanning - Enabled: true - Name: LaneDriving - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: false - Name: Costmap - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates - Use Timestamp: false - Value: false - - Alpha: 0.9990000128746033 - Arrow Length: 0.30000001192092896 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 255; 25; 0 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PartialPoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (3D) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array - Value: true - - Alpha: 0.9990000128746033 - Arrow Length: 0.5 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 0; 0; 255 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array - Value: true - Enabled: true - Name: Parking - - Class: rviz_plugins/PoseWithUuidStamped - Enabled: true - Length: 1.5 - Name: ModifiedGoal - Radius: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/modified_goal - UUID: - Scale: 0.30000001192092896 - Value: false - Value: true - Enabled: true - Name: ScenarioPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: PlanningErrorMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker - Value: true - Enabled: false - Name: Diagnostic - Enabled: true - Name: Planning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: Predicted Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/lateral/predicted_trajectory - Value: true - View Path: - Alpha: 1 - Color: 255; 255; 255 - Constant Color: true - Value: true - Width: 0.05000000074505806 - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/MPC - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/mpc_follower/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/PurePursuit - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/markers - Value: false - Enabled: true - Name: Control - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: "" - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /output/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 10; 10; 10 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/goal - - Acceleration: 0 - Class: rviz_plugins/PedestrianInitialPoseTool - Interactive: false - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 1 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/CarInitialPoseTool - H vehicle height: 2 - Interactive: false - L vehicle length: 4 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 3 - W vehicle width: 1.7999999523162842 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/BusInitialPoseTool - H vehicle height: 3.5 - Interactive: false - L vehicle length: 10.5 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - W vehicle width: 2.5 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: /planning/mission_planning/checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 - Saved: - - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 18 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 0.20000000298023224 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 3.141592025756836 - - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: TopDownOrtho - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 -Window Geometry: - AutowareDateTimePanel: - collapsed: false - AutowareStatePanel: - collapsed: false - Displays: - collapsed: false - Height: 1536 - Hide Left Dock: false - Hide Right Dock: false - InitialPoseButtonPanel: - collapsed: false - QMainWindow State: 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 - RecognitionResultOnImage: - collapsed: false - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 2486 - X: 74 - Y: 27 From bcc38d0d92f60b35feec6ba2cb5c0ff843bccc87 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Thu, 16 Mar 2023 20:09:30 +0800 Subject: [PATCH 15/58] feat(add_perception_objects_pointcloud_publisher) update readme Signed-off-by: Alexey Panferov --- .../README.md | 35 ++++++++++++++++-- .../images/3d_pointcloud.png | Bin 0 -> 66328 bytes 2 files changed, 32 insertions(+), 3 deletions(-) create mode 100644 common/autoware_auto_perception_rviz_plugin/images/3d_pointcloud.png diff --git a/common/autoware_auto_perception_rviz_plugin/README.md b/common/autoware_auto_perception_rviz_plugin/README.md index cb5deb48f411..efe5797a8dae 100644 --- a/common/autoware_auto_perception_rviz_plugin/README.md +++ b/common/autoware_auto_perception_rviz_plugin/README.md @@ -19,9 +19,16 @@ Example: #### Input Types -| Name | Type | Description | -| ---- | ----------------------------------------------------- | ---------------------- | -| | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection result array | +| Name | Type | Description | +| ---- | ----------------------------------------------------- | ---------------------- | +| | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection result array | +| | `sensor_msgs::msg::PointCloud2` | point cloud for filtering | + +#### Output Types + +| Name | Type | Description | +| ---- | ----------------------------------------------------- | ---------------------- | +| | `sensor_msgs::msg::PointCloud2` | detected objects point cloud | #### Visualization Result @@ -34,6 +41,14 @@ Example: | Name | Type | Description | | ---- | ---------------------------------------------------- | --------------------- | | | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking result array | +| | `sensor_msgs::msg::PointCloud2` | point cloud for filtering | + +#### Output Types + +| Name | Type | Description | +| ---- | ----------------------------------------------------- | ---------------------- | +| | `sensor_msgs::msg::PointCloud2` | tracked objects point cloud | + #### Visualization Result @@ -48,6 +63,14 @@ Overwrite tracking results with detection results. | Name | Type | Description | | ---- | ------------------------------------------------------ | ----------------------- | | | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array | +| | `sensor_msgs::msg::PointCloud2` | pointcloud for filtering | + +#### Output Types + +| Name | Type | Description | +| ---- | ----------------------------------------------------- | ---------------------- | +| | `sensor_msgs::msg::PointCloud2` | predicted objects pointcloud | + #### Visualization Result @@ -55,6 +78,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_auto_perception_rviz_plugin/images/3d_pointcloud.png b/common/autoware_auto_perception_rviz_plugin/images/3d_pointcloud.png new file mode 100644 index 0000000000000000000000000000000000000000..1887bcec33994a973813d06a2c715302cd8aa826 GIT binary patch literal 66328 zcmeFZc|278`v;7YZYnK?Hql+OOsJ3$l1j*K?2NJ-TNwKoOSsdG!kDsV%QD8AvCohs zgzURPcE)bT7&D&J_j|Y8zt{8p`TY0&XJ*cvb3W&s>vJvd>wR6H^ITg~jc?6I}$ z6B{ufXE)#|CZ^kpK5mb#U^broPi*WQT;v5;YY2k;4%YI5hEf{WG~ASJ>>VEZx!dUZ zY3f_~!K`Gg1r-(8Z~Mps1324wKIZpvc5?BM^^q6+&$zO{@9%$$3-bTx5KoxApwjmX z^56GybGPyE5Cl$kx3-nlz4zdsQ-LjcL3>Y6H(7CUZ*OlgZwWD1cRTUxGBPsa*KUa4 zxFHH0A?o4l;`!J|)WzcxFarO7hPY?rVdd`N=IP+-!vB59$4^}0p7Mf%?EL?XY;E;( zU^lqC(|?Awwi35-vT?R?@$?YCE_Pk~*U`WO$^ZG!eYkl1BPKv#;y#bvfJ3i||H;k| zT$=y3wUw-uyUk-ySNHFm|GgJIo2UQT`0tjJgM!R;nQIa<5;w0)-jtM-mbfkc{SJZu z|8vHFHvT!~w)nrtyA9ljtnz=y*LQVwQuvqn_$9@ziCq)5w(+uYa&;4vlmJHlzb63V z`OgJ@lMf*D|NI3={rk_Kvj=SaoPQe^VA9=z*{$Av)5OHY&!l=!N#Dm}dCd54fdRxE zmH52Ue+4G`24Z{;^N^bY;ZSWEc2Z5?Ipf$CRZ>H1*t~Z7Z?^eV=2>`<(t!(&_HTok z{XPXXi;sOi}#C+>i&lmm^|K4%9{`P_YbNZX({7iOF63<-z_vM(FU)%lr zQSR`cVmUbR=#fO|y?^iEXWBgS`?CSJYkfC}DfzjB?aTjjv4fVW|9)1c;75m#2T5FS zKXUTlJA# zf6c*ve%-lp@NoT`*S3eA|9b}@)HDB<-T#F8KcW6lQ~ziF|MRK;cDDcXss9U9{|?Ci zi~j$ERR4oi|3BeWDuYGS-OvI)qj-{v^ia#RfKBfi0=6l;fkY|*BpNX z%`R0;%sUBM%+hYktRcx}RnWq(7tvs$S8;8I*5&3>7=wNk*fSgH3hD7NTP8E#T1RSu z>kIAfwJg5hmmcagoQvbe3Bs3K7k4{K&RB9{j2Zm*G(%4;YQ$u2R z^SPceMB}{1*enSjWgLY|?l`F@V^1zai%Uj_WLV`h9VW^XpXQB3IMM=Z19Viawn_PG zQpBr@fBlZ%rA>mE0y4vV_e}*SSXkx#v)$7|3R84z3VnRQHDx5S{TU>ODtM%(nw3?ay0X2kp5n~Bg*s&9D&6EB=E8b_4dl6MAW3@{ zD>QasIToisum82I87b~^6?_&s9Pt=xJbU-Iv?^Fb;11KceosTYnRoD$iVtHHFn+H( zcAK>&(R`-WU1=VDV}wZWw!_SGi!4w?56;O=v*cury)2V&XcA+cvA8ec`c9)X;~iHY z;y5|jNOSV*iTiQlA{JHmKs(i1mtB98R=o<4SRy@@#~({@Ot-f3?!-^!p+QCMP6vCU zGNat@SLveN{S^u;Wii$WV@Is;a}$NF@B?i0*x9$pw2YxRh3QXiJo>kGAx-7JtdBKG z>cC}gVJ=6bMmBRZs`S+Q-yN<`qQ03J{{7O(k@^ycV~TGpo5j$E zqp)*)pOJcu+7mvv2N$k||H|ZwQ9yEM>VT^p;1st!S`h9+X)n@;GMehD`h}}qzj#0M zqPqKC)x{SS?+0yUa#B~NIvLu9UxiVAprS`DAd$dTH&2G00~0RuWpdooX@jrw`S#>W zMcwBdztn;G;7v3WvyVr6Gj~-|RJe|5daHxoju&Y|m*gj$5 zFKuDIGky;sBAz1Bc-s>skz{q_;Vy>=wo@ zE_1(8cRBUziowf3%3LNfV3>oiNaBT#6%3}u_-E7B`^H{MGe**0ElqEL@>&sA)pR(7muT6}CoUG)9$);J)hfqLU-nl*2mmFr!6s2I8=)?wsf8 zBM?|Yz7#d%Bp>5IULAw%K6m$iHXB2`eSnbd1DB1{;tqZAOI0l;IF1KNZ!Vir{L5B8 z&3MYVCT`pm$>4VKa38zmPQVMzz?MGS9~0K#u}#V{ds$3eJl#0 zECiddo*QZ%S}`Of4;U&r8y6vn(X6GH?U7gah6yb#pqgpF2V9*gsxdrj8SUzp4O`Jg zx$+Dvfj(-~W}tK+o2AO1#$h{O=TKLp+EB6}Q%DVDQ)v*z#^tqYPUu@!Gr2l{Fwckl42nob;nCuH&F(SapXC1MMF)gW*=PFJib3pa59(VZ_mKa}^@BO;?p0?=xFY;~0M znnw9viE3yB*AYI*?d|-jupYGy-quxLebS~@4r-kfVLlY`2<^D zEuphqnmYWB)47B57w2SsAE291Z*Wf!rH72u-BHX?`rkYq@6=NefBAW@l_G0S+Qvz4 z&4Ar;mxmMX%&D_StmEIrY6EZ)9 znCOXoXQ7E!Wz$r((>%L?I{RGTHEXhBEY{RXjnw-QBsbkkXZIuTd@>?)1do0Wb*!UO z7OEOo;Z!DPX0nJ?(gD+GXa6GUzv&h9lUq@<)+LZP-#oRvZ0ZejoLzBshF`1nx)!ry zvtLBQHXXL9i;IB?sUDvqe+9F8@e=@=T<9vUH#oEPSiIzAJUAO-%Ap%ps>l#@c0*Af z8|s)wCNbvu_yS(^-v+PIBFs4*i%^dS|(Pa~~9pEiVjc+$KO5^bzA+I^Oj-7vmNo_+0Ii$`@qf6Imtf8x~>4k04hQm*D+}RYrp&wU>-;OhA|As4iXIj5K zIg%#oG-V))Up$=;hWNrB%u-;cY_8NhDf-yP@=cEuo-sRjoc-(Qv8x)jw-K9)S4S`Y z^h@<#z?8b8-|W5p_8E;L(*C%Q+pFpY^KjG0A=$ok zlf>LMk*e}A>X5nLCsXAfV>WlJ$%AZRnnl$+F0#zK30j?^BPVJU*k%;+2C+U4cS(7M zpe>Sa?ae$Ngi-^8uBL$3THo_6f53z4HQ?GKIaF=7=}N@H9_WscCtRNOP!dnkzBF87 z-~AL2)*tx%xF}TrHqzudJPr5NDc!&@@#Rprr>U5;lJw|K_A`WQrmia_V2;c42=!~q zO3N`sr8n;ZcJFaamN7(fTi4`OAn}kL-$6x$3&2mIy9 zEAL*Hg8wigrXjpLt!*MuSdpC_9)h+DIPE2hNfun_p7)j*v^#cwOTFvc8|i=@vU&!X zuGSCWAkFM%ZR0pG%&gZDYJQ!l9Ov>*{$jxsOyyi85LNF^ex@_ZpCZ~LM*5SGbI3Q3 z+e$=_)ZC2-N!3AqDuX1_yX^`4GGnwaVZTiJP67Z+j1!&DVMy??r|79Z2k5;oM~QhI z9D>`=hsSmuR*b7UE|LyEcwg3+V`lSkeNb6M>=9=8HJQ0e-!-qq&7^RwH+qk`PbJ2l zy3x61>;9;HlI~D%k}@iL^tBVA3{r-zK6{Wc3MHOyi~g%r&D-cvjArS~C$cO|7qVz@ zvQPwHW~!>CDl!wl#hvY_-k=Y0e4kyOF#0inG+$Jluz0bIGJoL}c>5j7n|77p?Gv%j z8sa1lceYrc^5uIU+7QZK{eXUs%Vj^qWG*nPe}V5aNJM0mVMA~lZhAF!OLwqBr5(+P zQ%n^(uaGedBW^WIQsu|9FH{wpE7*?&+CkwK=g|d=wV*)bSLOR!XT*Qm#!gfLI<<(cDRSjaN zNVQkEn^_8Cq=jrgGV}p=wGNgOqFa%%Fo61~AtPt9Ko$oR<{tMU$w;Bc9#ztNf|km}|wG&fsb}*?b`fhxE$Aauxhr%ewqeOT_+q zKMQPSIs`j^ff6Lf>E+-{h07PE<>M)0d_kF1y>se5($`%Iv!+rA+2jw#O`CT5mEYPJ ze^1y?CIjiA9ww`9CDeX2zs>k_!4}o>{4w5jmE+}tPyzdoT`CJ0ELD!yqzp3VC1i_=#0d&2+w{JT!QY6YHA^469jLFDl9Y5>^wa)%Bq= zW1Y{{6RjB_MTEz+cHBB9dnlerQAKKTm!2}zzUBE)sj4|fpn9j4*t{$o4uUEqtem@W zD+|YJ2^(zs-Ixvm0y^`0+MBZrB#R;}QF;|NvlJC^#TGqZpD4q+GXCTNe@21A@ADNu z;QgM$)jgx29_K#dDs3xakS|YUpr!DAee9Np=_vLLy$Oh>!Q0JRDlWEw7MCU zAQ&&VR%uPa%Vv+JMu5csrmdYJug0~Z=&&bpE5gL|x3EEPqv%=9Y)C%_GZw6OyC1t# zqLWu~S!zVrP-o?1$mqiTyfa{X9#A+NS7)+DCwpL#M;!JQN{So2Ei&p`;a;-}8O_~Y zODfQy(Q9z`Odq!x#)*Cn4~^)PT_Ey?PjJp*<))OWFM7k<)z#^sJRs zYpRORDn=4kS$L%lX9^_Ttu$JOlewfW(_BZ8CX0~X0+Cc-bEUjyX?P@B@FtS=))%n! z?n~EOAj7!;Y1^8D5NA}EJ>zfAH#oLIag7c%$UJZ?jW?ON|2o#7FT!q#o4!(<#-*GW zi@rMt{?J9JR9A(nCKjoeNri2euy00irQM#DQ^Rb71C6zp4P2Jv+*vt#mfX$d;`Qf1 zyU?2jSx3v*L1E@{*{#R!OB8`qM>1 zRjBZNv0wCwbm=D_;v@!$VKy-)$wT_F#bhCk%SbOP^?ie`nd!!)VLK>{Znf}APRB1A zE83uuAee2YM0)d$kqkJ_R%k~WtCP~uhsW0fJ{q1bU!b65p-tPg0BlIj33um(k`&hS z1G6{%lm!+p3|}kySZzAeJ=t-QXfC4~E6Fq>elTzdB-=#u3ph?TPj8bEs5lCS;!RsV zo@v_T$&%P~$Fx&c?t{2d#QkR>E4+?c_>8?*;#35I+C@XyIU<+!@yneO(IXn=&B%QF z-Pa!wT00VNLZ3uHxp_O&#ttu@vtu`%6xj(_Q^I4Prp67t;gd?l^GP74dLRl zfb5S26p9dI*PPw^fZu%6zd=7 z59hDd;|+V84RJE2y(`3!49SiRmtW(>RAA-VK4mC`e!WA?oli#qbm zpb+>h96Potg&o09KS2Z0VNd6wV;^Yb!i0pPw)TpiKlz!y>DubKBf}Josa4DF%^hg; z#a~&?s2`xZ*tAenZ?>w8kW_hg@ADAPbr}LrKe^82v&hHv)V~z5iVpjnvgi49E(*cM zuuCd_y8He+IkG&_V9mn(*h4bJ$G1l5K^YucuI=n=?TOEyC{1?wb4lCv1?J{jS6&8r zIN0D~_0~c;+;h-9(Os&!ZUf3{j_PU#Vs-H4S0tLK;<{p<*@})`O}efX#JtHxR2qK3*tM_MPV$NuHlDg^6XvG>ECTZjOR@U}ql8hLYP` za)12tLWv$!$7RP$01Ic`PTXx{>%~|Il;!k|!nKzQ3sKgp?Ujq??g)qp@^E*53vd!I z;0R#aY=aYl#u{5vidl1z;<(WK7$&Q4EvhosUI)u|sj6g5O^xgtJVlQDU3bAv3!QoX;^5I^RJK-?*atlSivA>Ur4gbiQ6Gg;3}=)GwHjV+s1 zH<)pj4tQ8<4aB>lp6F^1d%*J=F`9Clf;?j z#VAf8p5s|`rzXeplWD=3MPpaCw?qn1pnkuAFooMgx<s4I?Ooi?dboZh zAcTnYS$KNK>q0T2+_5lhD63?#v5>O@S34AazqAY)H%PcSe_9{5=ddqZ2;;i8^aLI9 zq#^`=9g2vV({$0dIwi~}6zHVi!kgK8bTr59hK{5*TJCz$n)_QfXA5>Y)MZuyl2OB2 z3gfU~`4aw==gO2vzNA6#t5t1Ey^{I->931TbEpF2atrnXj!M>vs<>rl{3qe*=3jvs zmjJMZu=W8s-c&Bt?wgqrGP;PDbPA6+^0H{^oyyJ*7{|ZdE#cvT2H4J;Y`6K zF@Sttk=SedWEb^XgzoSXYYLKHzxaiv7;+-cR`h6v*9`caM(j!Ccu}3$^UU`qPJ%2& zn$YrtvY5Ag@KJ;3tNLHBzfA2MDFo+(UAkA->8l%H1B>*vyi}?7bptFKv0FJfEqV zfFofNYEIs($~F46upHS3)%uCgAx?9>e$Kjyuv`;9U8^Z6sr=_tZ>Kwt;T93vi#o;M zT=*(S?2vEmHv~lL&-m7!zVgF!yo3WyOa^H|QM0PJgZ<=n6V&zm4j_ZqtHr-9b}gFf zJHz+-10Dh3fvjTxdrzG;*IVP`#lb=p+96{gm`UnX7k0kd>@$(etnwkQ(((ho$`p`y zdKg4ft?k_1rfuV@5zaQ=@uKdBIu$W9plWT>n}-c1${Z47`Kdd>+*ceOi~3QLj%xjL z*|Uje>21>TWzayQY%11qAI{6>8vZLwt)D#!uoQ*VN?-3&(_BZ+zXVfqP~*dQs|6(u z^?}k$$A?#|EavO`Dff;->E)R*Q^U0%(i`2wjuC~xiSlAYu?~exLKD-ibt0n_xIp>+ znNJFZZ%5}aYpMugd0$^@Mm#6!GBm92K)&ZNwY~^LZ@GAMg}t}ZttKC=_;sUA5QNxI zj%`X*bWwdS`eJ-=(3w^-)QI4ni>mlVpbmZkDoO3P<)nAVoj6@zYaP(;-3T^#tQwi5 zHoO!orN3eR_7vai0=4A9rz-u{!ca2OFg@1bI8pY{!ULk`gK;7&yWfL7o%;rGihq$a z)qT|hc@SlelaxJLq^UG;tO+Ms?RvkZ!DXgqbMgEh9M!65s81-^C8qT54&A?x{oHAL zQb6s`M6}Jc!;{y)xQ=ry2Q4pzd9&}-!~R$}5PoUtEC~uLiCn5?f5uuBAVX?(MvI26 zjD71oLY})ZA};i0Y)D*iL#?1Rsq9CN_BEMyZhS3;@fyoMDuP2bR zQIyX&6*5(R;!2?E1@tW5t~%ljX1`UXTM@!Q1a`vxr&w82l-aAjsNq&h6wPvReDR48 zQ-`mjaGJopDyHx)%+BvNq$9sspvv8Na#(d{go8cw-ech+d`y&x!XT!tSsaEaFZ7zo z3abv023U2Y%6EwbP}=#GUv;+$I$-Imt{=h2D}7mTlnU1`(tI4%-Z^6x5z#v$Zj);~ z*5)-yIcbm1J?We68MvCPH%RDYo3LNW3eioAHE2&0E+~FWo^Dg?x9!eeOxI&Dctrvy z-^$9p5IJI?FB+j{>*%f(4Wow%W5o+{XrWkz#qv*SWLjWo1|*CE7J_4Zr_!=?(XorQ>?%l8(u>X<$iyw(rBSWD zfzeczta{{6t8E8kAMNe(`z zvSdomdt_rit0ZCN1ldluXy@%xOhu`^@v?p?eyg95voBP2H+^ZD_3&GIH^b8d*^_k=kc2}|=n3BYKW6OeI4VC?| zyRGhuY5IF5W4d4+H(w1^L#2EbwQv!J-Kd}3(N1NXFr>dp!_1;>($N*RpHpCY?hgP- zc3IzucNRO&4LWu~_q~sWV`1ICr8ClPPTQ};!)lvH);}UlJ1jbcW$CPZKlVdI;P&6! zBI84Os9W8`3%=z@*^BM{0ona_MAU<`CI@nS3QX5Yq3mf@m&;Gxk6G1mdu&W167(8c ze5*X#98eYUdI6Qtb<%-Wdh-CH>6Y-^%sY%D9plgSsNgei5@c<+AS4W)U{2Wovcdkd zb{-4^%J9%j=M}^2+j-m;s@nU{?1o;@134vcCf8DK%vOA|gp9arHg#n9Q7D+DwYMg0 ze($;OyKm!S%irbeWNoX1yE(;Mu~J;C^V*)oV`0s<+1IoK_K&f;e#xV=dG8jjbemLj zRZA4@Sf)*tT$o{OCgJZop(ZQ?j7f^M>J3y-m4`aJMG~rv=@DmP(!Txr^B?Zw&V%oq z$s7Cd=k;u$;`}v+7N0()u7>MaJm?5S|1+ye%%64CsDbwzjZX!zM(gIvkuh7~TVFy!dSS?eYS5SK}}>rSj}V&UZ7ZrB2PRv6mOm zFUQxVnaf)tOh!lY+Nz8=w!?>?7Gzx#mp0Ygfoks9e8KU&{vBJjivZZFK%x?H$CT{E6eVW`Vt#_ zr&ms-AZKgYQ&@=tevbnhi1&%(3QZ&{4X9BxT}cY`S1TrY)}a&@`Ws51iU zaOiqDqOqU!CUQke^H0TJVcty{PzC8{DCzqX{eKMJm~)33$1vJdw!4vms$f0vqT<)0 z;1Xu6g$2asZHBpu_rjN$&fIQjR|>RRPjNp+kGujEXnd@CF{Y$oFEd3bb9{lrTvW)b-V3`5Yt?$O-Ie@Fm~}Sq}@c1O~LmL zPby&oEkoQk^|=3TDd)7n2A|IzOz3PeU6;~S6i75F>535S-_*O(E^I*JU1u zcd^(}1BAf@L0T!Vb|+9rpH7Z=@qZgM071)lY!wzmyGwT_s&)xVK#L#Q3P z>tcnSq_B+W+L<16Zp$WZQ*|vc5*QIL5JZUQy{Cifc3(ATJuatm5Na?G94Iwqq^<|f zFD$?e>FIYvH~OH4&N=OcPN7fB1-$))&HrrwjR&pQ;0B0uYd3{s!RJIRL6#h)+-6Bj z8pMzJ5E%7=Q9pf%(H?@W>j(jA-Hfs7JEpmH-CH7#_t`xrO&{O-62b6qckGFn2BYao zDSV@K7}GJinwiLS+Yb-hBK*0$xi0$?g>;;F^K$ap_m^a3mj1sk?kqepjcLI3`pu2i zNN&};^r;1BjHN`j%CzHeB|CLNj8v}LvxCZL2gE(wZoNGROe_)j| zmd%06h`lRhc(~lh#bOTWV^Cftl9FCQI#6H#BcBRx7YEE~&*LNSpT#x@o!-3XP19c# zrrHelTel_LA33=^)Kg}1n=nt|{#~`)z<_F1+)e4cXL*j};A4Kd7lY)}BR1`2vWMgX zj^iGf_CLdv46o6melLnwz5$?5QeL~=#o{xp$Y@H+3CLxt{8F|TNT{HUn+Mkfb%MPOXzxN{nDQb1K@FCbgJh;mp4IhS2545u?&RZ z{I#`7X~zeQWlNf$xws&lG~X=`ty3ARsX_D<_?4%M#VE*NB(zd=5N}q#4)i5U|5|I} z69L3^HkRNc?1R?D}d$L7pl!Q8Fieuz3RPR ztejq4OCKp*urg(sq3Hlr7Tj5T9E3 z-#?Oz(Srb1)wnZz>G+@%i-E5w1%=459YLNu7|clk0^$R{)7z6p1^TA0{Rev$WJI->SDsx~Guzkg88~DPqi~RR{497(u+~Tq69{=o7dow3n*SbA zDB2!P$S3fjB$ZINm2cpk)TIE57*g5d1Hn0;$|5KBn-PdjCI+SO=rUWcR8X z7tfG%tg9x1use|6b*3{;T+7kfRs(Ras@4%g*)&gQGP z;Bwa8s)N^ISO6^+Pv`i)Y@|_BNh{C zhgXKizF``1h64%01BQ$E$Ni{cr;FuyeWaZP2G%R=#e-N|IlEHoiCugI0lea>QJ@At z{()KGkssji^GiS^%5m-5Cn{4}49?&zhwI8rBNY92ur3`i4p_cL;qFSe*Qd2SL9INT zHtjQ?b3@Hnoi$nrjj|2C`pg9Xez~y%USg4GwBaB2?Jt3DzB5VEKNtDn@&IBcN%Lys z-3i;ngEtnsXb<4cuA}MOeQ-V>@3HbWL)(#doNaXHe4lIWV$DP7W{IjH=f@Vq5JfUE z@R7N)x!Uk^AhXBclP!w3MHU8w(C(@tALkA0t`zM@3 z@y4hyZGxRq_uAxR?4!c@4<0;wHx_ms$5y}5bJbC#f~g3`{Kb7^18TWrF-h8`;HKvu zUkq%j>G_t=mEZd+#|41K#8-Eif@7{)w!ISFF<~7PMm5DZ5HbWwqsKd@;uZUk`i1=dw^5SHuF8 zDdcXnC^Rc%dgS&Ha#D1hqN)@en|%E{j%5Bsh^4|nOWx;+W2bCMyi_$(u`$me?PLwbl`Xu|F#F}p-vBI$on!Tx zt1K0o{D!U6?5}}C?y);Fo!t7^=Lu3@OZRazUnB%=yCnjXocAfPs$~hXLaia2BK)g3 z1{Yb(y7I|SxvwhUf;I~?NiwfVsF=0;C2w3YAa4$>3l-7zUdM}z6N{!H3)r1Y(N`~X zUO2m@>c`e0DJw1{yj=x%hxq)ox8`o^HCkvz zZ-<(4RfC$N*TBM)UhOi-3A|f~&`$hf8y`OaFVPHw(oulJte^~6Di3dO< znd>u=RbVm(E?X)A;ZTcB0e%OF4h;LO$xDrlx&n;HoLkNni^Q*at7*E3yg-dKokih1 zf5QqWx3hW1QX3rGUXa)4W0y@$98c!^sWHa~dx3zgAum6Ao@MA%@n$Gi4W@F|7*sK+ zvqcNXI-pXvlZ3jO(x6-FS=hY3@~c)jN*RST+LaefKfqXi0{a`VRAIx8ysG}K_Nu+1 zkNOd1l;by^{9~(wxbIAdT=|hB!J;CT&1XO!fz&cH*+ZBkBS;<*R)SWg_xl01=@vsl zDWunU#5cl#mM{zrqOqQBeB~__k$ZKyJi- zQzOH!K)4?tv%0B{`R27uxkT6-5T{*AR9Gx|WXkqh?*0$H23G;7kAJ6lrn2P-yU`sI z-_k&EGiR=ycZ}lP0Qqo{IF^==Wu5rWzigmXkzC#W7McusC*k*Dk1=B41a@9l!rrjR z7w#bJB=<$15e2m)Kb-N6%YeOpwU*F7h(iU%LD>;o718-XT$L_eJiDk1WCFvw?c!Dn z0IoQ@zff6paKE(8`!BJXfF_Dsvpq*W_q&?#nmvZqh-LVXBof*ilicmm2)YL56Ll)WlIu40@kdKxr+|dJL7{eE)pBQ!- zqDf{`c?0kE&U`R%VDPWLqP8&q5XSMrXWtd(mhk9Fmcs#unU`W_x+Rb#4C@&GVf{0? zJMWC?v|U8b+?zOwL^+9^VKWUK8V|T?bx33QtUJh^@%i!y->8}EZdvF6&j8{z&Zeth zw7PCQ5VpD=QR!jvt7p$G4xp-}&m_kvGX*CJS_Thx(96S(y~wWkSc=L8h+Tdlv`DJG z)Uim^8F$5`yeZK&Xqy8aS9Fj_osmNnyA(+3LoqadCzb1>=4RbfVfTqqiN8RNeyQDw+y^VX>*kqQ-~KWrwcwpa%0^ov5ixYZ3Wk$%C;kPv{FzD} zXZx*j9p3$pr{+E@Aw`D>bGnNfISogD!pk9mAF=ZoKO@ER=2UULqj~0c)M3*F;6KjN zrxN70jLn%^vAO_NkDS@}e!hJX6W$TV{xQ1Vs*f*jwd0&Pi^HDCskP*P0G|RU@}+5^ z?;?Y2P2VIPJoF1@J^?gIG?hu=0^s3NgGhhivO5iq;xxN}eg*QrNpU%QvDBB+RVkPT zGIecqL8i}ekx-fsLruCvhgKzUhz~eLpUDp)`5usHjcQ-LHo>3w&i4>V0E1uqoM1WZ zCP@?N-oD>u5~mZC2ucuzd#2FRF(8Ugia-M_N)#$@7f-D z+4&RX+_(yu?b@`2=fUGUGc35Pep~4}W80`20H`2x%dZ%6dl}{!(ixN2cowS&ewuCQ z1mgMwgQI~HoENlL$1V6gQ<8yzvV&+cfGBL!^%#$y+`Xp7|3h=!JiaUbK#~@q_@aMW zqJ|rkL(lqJew40jMQ`B!V35a{oIsWsF4M~LZ}H0kmNduUVcGlM$KZ|#U-Pz8ZFtywMaAEUq>1qF zeA;PIS3uj1PZn##=9ptf51oa< zk9y|;L6~G<%9C36cK8ViD^dnY$~$7WApE4@pZj#n{63E+uPG`_!Cwv!URPBixa+6L z$x1QGo+6#PzlRVYyZDX~B|@;nv`>n)lW0|%d{a|$tGo^Ix*xzLmP?jod1aE?3;wKi zbD1Ow^u2jy{q~0#>Z86l`JFZ#2QeSKA!MnZ3YfmbT#6iGTN?U>X44@yw>Lf!%{Zn} zD>>P?X6lcO`zYO4{q2Rf{Z@kxU!t?|_J^GisF*WoYTfP=qHnPJ_xNE}11wn@Che#K zTH?f_^fs$ZFTZbk*a6fB?GP45Wd&H6Kq*A^$onk}$+PZJ;qh(4!szT<38qkrVuHTN zLgz^5J)K>;nP&dLjESfKIgpHRx81$aKQF?!n;8>1cAvH3!GVHX>kxCow66+!-!ZC) zTm0&7WD&$~mAWV|;>oqwJj9p?P_fv~uI6m`rpP>Nme zk~}(^%-KL#E@dwX8;uS=247v&Y;=PTrTyXL|I8yh*=$~aS!9>rxrhiK5Nl6O?t8uC6nE07ffmjz4l_ zM|+gx^Pafdmv|2D=_0@$ae67?zQ3&yVO@0 z0}#<82I`Ke-qPhm^k?~zQ3fVhdDyV($eN7j4-LBQW=&4K<(B1?ae&ZZs8^dnlD{%BH zj%!!htp6|uW`{ne5U4G;GMcVy*CL)~QCpe&$+0oQl1AJ8P`!HG99jrlEEfjiy>88r zrrd+y@K69haQ|#irtTl+F9sbfU?1HpQ`B1#-@kFR9`H9TYlZ|7@4ZTB_40u=aM43` z^;TmhJn80r=uU{FJa$hSpPq$jBUr#-t;i))2PVc=4hT7Qs&${5)~4}DXm^YV@m#HI zrPGPDYh#Sz0WSP*eu!1Gf+y2WLkx@Jx}a7q+@Ki#;uuGItdNd>ohaHH$OJyH5!GDp z^D6T){CivlxpI>Z8zTL_bOB)y34Zo?W3Wglbg&CyG3_q|OwYW*uXn{ZsRd*ljw>cl*r7XNp& z$lfI9j2Kjt*m>4vo00>WZoZ(m^F@Mv+(+3Fv7_0}?bUqYjNU+@*bKYOOq=iiTYzJR#(Q}Z%;!72E^N|ZvmY-snx{e!R)R{42Hxy)s|(7Nm3F0GZy@Zq%p}4yU<} z7P=b_Nq3eCbm_=6SY!^`< zxUjjHpA3IwVYY>&5W(mfTid;o2juFk3TZ4u?CL{Qq-%f;Eu0Q3_#k(Cx#l5lr6Vf5 zNC>07mOeC2yjxLSJ#An}pR4^gpS_f~nAOps3EeI)!r1{i=)!eP|$@c_6nftIGfc}HsqumY*?jom0< zq%*7^%yJ_bYZnQ8mFBMC&uok5mh_tMM3^jzA`92LsRHH!>lyvF1MfW_t8#qfVEF%p zICr>#xNDpY{{#eeSB_hA=PZoNz1%EVp{4W-dTDSUqUGm6C~-F__cN z5ut;=1NxL~!yE%C5BCSB#5L^!%#d-qc3}>E?w@z{{Kmx{b^qXtGCiC9Jap`!+4ktlOLY9)j3#I&zEN1Fc7U=YSTe(X4HK(*3PcTNKx@# zT$?-A#;U2#gWeh@;%CtsrrU#TlOMyYo_bwt*s;~%O$+@;0e><3#|ICxC{1+Iqxg_! zKtS$DLz|lFmBI|q24Wxrmo7!}`P`WVJaWkTx`tcNKp+RVANIy*U5zQ*1-ZA4Y4XdB zrSNDRQ|;}Nf?X4Ha2z->5HY;UN2Z05P78lnD{tJ4`v+bWFMKZ?xu{iM1c=T_mdCmC zr_!|Di3r_t5osv_QE@P1*$&0^g0b3>aHqNC1A%(3stxVBBRcjsXYp4#H@7=6`;-`( zS_%9W&ZZl8xmwl`G8e50l8ZbGWaR)cX>uOg3oxq$!Y?>BcnpY*q-!t!W(J^La*&7G zAvV|8&SX!xu#D%!h9p;!opH)EH;**^#ylsx_Po$Mu5)x2eJ^HI zV(P$C=aP5)hu_QAO_KMLa>0K5p7}fj$rPir(>N{VYfXW54%kElPZ*Uka)( z|HJ687rd`RgCl%|zx{lSst7c>s-Bz*eSyJn zr?zy`>&ryZ>ddzRGOINzDp3zJDKU-|dJV$HXJt8AOu<_xuK7xz8SGtq$xmWDSX2m* zX-M2BQ9z~|$BQ+)Du{)E(~k36)kLfCurqQmb2NI%U>FM9@gZo7SX1+{ZrZcZ0KdOu z71{Grat9&$o%S)|$2b=zzOm7-t7FiH@RbFRL?4F_;?Hr#zm|142>`y!AW?J$z`p?9 zNjgCet|-$RuF@-&Z!|dSQoa{EKTknISElZV5%=CHiVvC(DLb`gV%7X+mbkI6O>*op z?B1)({SD2>@~tWx4PT6u<7_*>ECP)4`+UsDqwLN{B~-OKV}z<^0XW6Is)na>WXRO5NaAi;fP*on2NP+sdg7(xUou5MJJAcv z05mQey5R-9zw08i=@t2#@8-c!ol+q7?N-O813q#vMQ*P1B<>Pw*(ZB#gs4e5={nhV z)@p3C&p+TaPN3VLZyaM#p4~Da*r(WEbT{v9kD>y$Y4e4`ann1ga`HI~137&ruFjLM zJeGbq0CT_rjM!W`!vezDhNe7RjhD_lyg6|}D*tFo;3ANX z`AjoT^r06SPLF>thK@P{bBj%7R^Vekh${?&1UaAC;vvj~>sGo2h=0hQIkT>0guyVD zCnmypxNcLetMB=7K&x~$YgWc*LJL`m=H4XT0XMX3xDAq9>}syISnep)z`5|9ZC#!; zf&R`l83M(sZ+#j5P}Yh4FOyg-|NU{TE}UTn9{11SEbW)!5j|f`#d2tYq}!HDT+Zql zr1`AN>uqUcfGaqmW+>fOO2cCG!XzAI0lKCwVGL@H{wBq(_w@!E0e z(ks%;gQ=j+CiG479eke{gL}XxP%}HNHBmnUs?k5$@=M^Aa;$*d>*2>AZulY}b(^N7 zVJA`ag+<*`kDOTZnt+D}*x7SWsNp8c$_oQkGF8w2Ko-Pe`U)GzMn@}(FU7luNuH*x zE>YS`0cOE|HtLMA;6kdoJP?J(yDZw1le?tfs3t3i+S;bppJVJacW;#T_GD&N40uN$OY;*kS969;i+UNCOG8^}B5R)yTsk`_ zJwe$wIo$#B!253jg_kIK*>`1%3YapjG>8@URdno3m#Gu3>K{@7_l^lPft7I`l>(5| z#o#P`T^e!k9IKw5dl7yyMmut~rZWxf)Rqz2hDL2|JwzbbQ<39o!nL3SYe0;9>ZD6F zO53GSz%HKvW^t}Y+4T6zcpr9B?*HNG8Uyoenr>{fvCYP2W3;i|#4%s9+aKFFHF{Jx!VyBS|Ry-qB>$ zVUZJB75U6p|BqVi*ueyf#?{7+QJ?}#ftkP9d(CLQ1*tl08|7<%MP#}?Omz?$g%FO? z2XRoYpw_5j@USeG(JendKbp7jkgg_N+O{jrZ(+CaD5)GWa%1~dLLVI~DMjAtD})9S zt;ET*oBFtoS2x;-7=;2puFpA9{`e%LvuC^3`uf-$S@f{Vz595i_wudmtY|@lNwlE= zCUbT#o3#9R!7h_P0O7bF)j38NF-$XuhNZ9|>$@0}iU$pvnYU$naVJKx^0M;J@JQ=V zhLoM7lT+YR{XMJ|40vUSUGhtb2kBkvj~_~Y*Wy<6^TU9W5JLp|qM(3rM*G67r@-hj zGC%}&ERqO`Bxq)=m|`?o#*2w-cngk!@}_WB!^~!Q%?_c;=c^&&4}u$$EGq|g1&hc! z%VobR3*wntJ^xkxBu$ohl=_KTuwPu06`Xa?J0}IQn(0V%J3?CfZUR#F)}%h{rpPVR zo=ijt=Kb%h5`llF_javv0@hL$<}2f$(AIx>&Lrw)sVtTN2E}sL#!ywq)-cWsEtf8M)dpY5fU@bvs3o*Q=edK8Is8Pt?`Hh_; z!O14h=B88cGK-wmv@*Zl#HCS7ofK4$59Xo#|}DQ1Y`^L8>>rm+w%9 zw87QuqX}H3_J<8;Q?&l1uTU=)MCfVgru^ffm9rnq;KDqvG@qG1ooM|tKd>W&iWuy! zn>Cj8eLLam<-UG zGa91e)54OlW`Nd;Cs@%2SRYt=i*42&N@9<}F43H}^=t4bq8SLw;`ChRTz=qV?Z50I z4Xpm#Im8k4cIvWD;?M9S31&ic4hC(;XW74G<)MI*k*X zkOdq`QmyXqG3BJ@N7HuO!>5VDXlOlc_&~%s%c{s>aXhGs)7-67R&Uw zRGF%1ER{qlStUAlhBD^J;cRykc3!ziUEE&NA!-wJHOtaea7|fM{->Oj!a5z^#DbZFT0mgCJkXuWO1#g5+%VWE0GrkFSzn$liJ>JzmOf&N(N;^49ovQaZ`F0$BaT35u40=#jk-deF4rugowt0`6 zj+F+~qFPLI_eIvh#WsS(v~Q@gDtacss7uPMnXaoLDP7@Y2{Vpy^u5Y6!L^!p=C1f| zLv9jnDY1aW=v~L!=uZII5CxPUBc41LrDl- zuT%Y8C3W7McDkOm;JkO-4>eW)1UdNsd0+0E_f106)l&fG&{Ae%q&~RM->wc-w`+rr z4tXqbg)ROrBycx|`oq}w*26-r_Kn-e0n= zRPA2B=$bR`m@+_Mk7L$&1|0eyUst^Us_kwqM~-G4)-qx()IV)9Xq&Y;uA`Yd&z&JS z>~_jq2gC!5{$DWTdkbc(tkDFpZ>E(=`4VIm7lDLIWhbq*@OdMOQJkmRQbUB`{{{z5 z2cI2H-QeD^)Fb~IHizlZn_%oSfYWsPiO5jt<0qMH?ep9l<8BlEy(g5N;9XbB#82lr zulowxPv>G~2y@4!-Hxwp51(%wZqCSFZ^;_2E-0Mi1s;brtUXpQ={%yOj2ps}7k^8E zKwp5Fx|Kn;_IW6H$*`N>PLmSIY)#*Owi8PvP2oXuwv~;cf4fHSCg^X!Wuc4oA4fnI z0wuBaf}rM*nge}cK{#bkFp4e!!$<07pz(9)BRE1T6}-jg6~Fw+TuLY3KlmsBkDpUW z;l9gsa;2T|m|y-u(W!Mc=*JB#$LQkuJ^>7NaAR%br7czcXC*wx6)Pu8SF!4*!UYVt z1NclcT=$3@TVSBP{-sc^Dbv^Q6f*y1G9W0qy!0zY2P$ z+lE|TR_hPN>9vds^0h z%_hh-0nps6^WmS?v#f#bdjb&V3 zg2+|Yoz10)yHRwuJdWQiLS+&|)S!YkVcZeOv}u%6DyadHdnog9g*QBD17{R9&)KX= z*Pil6V>I^_!J$zWz2U8KEBxQ4|ED{WhmQu zmmy=`zm#b0;K%AcIWNDGSfV#&rL5IBx&M-`nW!g=cj?sN+W(LXj31^55QpL1P^u8$ zH>V2qOXx_&ayBVPcax#N*o+cwIu@3FN=^8IM7f)Q^P;Y-O#_ewYmrxMqyx= zNW=EFEiE0l$Pd}d=|ZJ1oy5zg)99l$n|7x*4&*#o%fAzBnPj|Zi(qzD|7I5yey6Kv zd*q!J_>-p=i}R!#CR=gxaNO*A<5J`kx|Ck%E-R9(Sv+-VRt6yoEOKC|wV^)s-J!&*Cp%$xHqEXk;i#n;Wu3+55Wsae-i2y`wN7{=TC_ zGVAu~%p>4SM6X;0PRW6cINu7BaM~KQzJPEF5-lEuR-t%WhCg`@$qw0HJRoZQjS=1;nHT9VgzHUW(ZI`hqHigKKpuk5Z+A$B ztmP79>cbNTZG*YmbYe~+@+B z@>j<@lR*qdbL!3Vm#ODeUOqvN(^~bgo2JhCT(Dt~*praKQvU^4rZ7ED4kVK%8{spg z4!Kd%t2;fCsS9*S$^aTFn|*Lf7AaI)E8J`gYOB+rqGEe^aOc6!+!4pIJ0Y>aCd2Cl zS5am2@{6`P7FO1wOL?Mo8BI=poLpZDU#Zx^lJusHwfisO$hlMy307yU9DYV98GA<8 zr#T29hc#?#&a%UpLd6PVYs%BqNW#|mLlx)~*0b^sm^C<%Rph&`|M{~{X;N0h9g^`R z@q3KFS4EFU71F+SPJ*uGW>1JQHOZUH0Rpx`;eT2HQQJJ|I}deOH~fELq2dqhcM#nz z8<0k?cbDXa10@e;!wETcTLUn0#%uxE6S@ zG9xKV$tB}jWr9}SU9C-%SE84Ed<C;&uujj1vBgJN*s| zV%|m>a;E}?gvcI;$JJLp_P-F@Fy7Q&aAN(41qkrhb3+;FS#X+@eg3k({+v!aC3V<$ z>+tG>Mid9pWJadd1Z7AZdp&4WvE7WRkYMbM|5G=FER-=2`*H85)NC;8;Kea;ff^^Y2f;q=aJli>-my&nMjnO zw^cf!i`=N%gwy7PEXuS5;gJnK`jsqVtp5zgw20}NbQRVIHILXDOp7=peJL#17 zdTZpAEOm@P&p;3st^S;8jyv*Ii{@8%U5UJ{Zw=>mW|3_-uMGXD?@pv5287Nc_f&h< z_n#D`LK9Nt?%3JC?n+MlKV*g4expcj_1NIx1n1|s8H1Vnc!gU;I3=Cmx@<6rt7}jx zO>RqbnlKAg298S5wd{RVo69%am973{Q9OFbpI!@J4hK9BoB<#ua(snW}bM;lQY04BzA0pG!z;wd?112wj zHSN;H;x8&5gU`-g;h!&-8B>R|d#KfXKYNA;9YHgRfdl$K`>-0_Dp1XRRntywuBp}v zsL!2gCjl{u7dt`U-f>`HM-e4Zf2auRHgdmT%L~*@4zv71*irsl+lY$Uncn-(%ee~(r7t+?A<41Z9KY)a-|0>e0v&W95z%!KN zx+}2i2djgKPHFtL&vmheur>@`N;6n<^|rr@c||OoTJxWAGPS#vW0>3^EwDwcdE&8- zbUsGtqh4Tr){YamcULgL z5i-`9 zhPN4diV02^lGa z{H9B8*fwqfr=2`cgD9e^Me{vX8NvX;@nfV2$GbuOEMOQZHFji2&yNczVT&pIdYm5u zwY1=YVxD@)KP3j-%A0VpdQ2ou5LwKj)57w;OF(%|apZV*`bAd6!`d=}EOwAHp_>_z zV^uM)YrrP;)9Hk6gtBJg$wX}P;nIZz^TDaVMXtURK_I{9Dr;5fXZOpNp@!k`3}0O@ zyh*BdD3hZuG(9QdAh-tHVTQ?jr{ylI@Y)WXMi7%1d;F=3)h)EO*i`v{=pGALdwjkB zKhkKK`7I0%4XU3h^0LWJv36!ZtxH1CM*C`MBg}KuIP7Moae=ab>;#r=HSuIqLl2h> zJ4!QRP+c;FX_EX(K}P@JRYN{_`G<{2_P$4{RSCbZ=2v{Y9{g+Bn8P+k6SB1-+Knmv z(F?k*A(?M=%Y9$laR#`j!dK0VAYR@IK2z$9dF&kx1O9eGO=}8xgG7S{hwh4C`Oc)1 z7_9y6EG84oH>^O92PD{`U{(2lIE)v9nl5oG2BAAlmb9DtA$wUrBhn#orHB@5P}8E= z1{YbDQGrL1r=5@Y?WBd8YD;mesy2 zY>v+5GnkT9&W+!@S=+NhW5t@xvC?EyFDxZlQy{%K4BZ!}FHT&nUj0`sP*DL__Dp0j znEmGnZPaQFndt<%S&s(Stv?V(NT^I!ou&0op8N0r^XXt9kG2^&UWoilLk+rM8g z-6Xqv{2uBshapIA!Rpg2X&I-mdycq^12O3-HQ(&ET^SgQu#}b@<)x5tPb`J*>+BKI z2sRh=ix8B4PohOkGvZb%%zxLPdX}tfhp;HZW9%ikCmRQA9H+DKQ*gjbO#=POC@`cT zaddOUs52vIJNG(3se%QofA}ZuH@4T7G&F+X8Xb=5J_Viu$MROKByw8N%soqLQm2LS z2d0On3$Xu^CJTTp6A*G!--}qF4j1#S3R<~7G+6UsN@dXUy|5=H44GYkotc3QRn6uw zXXhOfw2+8&H@t%)`(9R?VR`4#z%?wA$PzTf5nl;HXhpD%V;Cyagrl@)2A-5zkleU^af9MP-tNY5WH*jY0RKYm7t9yiVD(OqSCphxql%`; z<>ez?y`Ey2-B{rq&y7@;c~&o5s%CYVsoQ??stqejk9UAsf&(mmZa60%@BI#yfPghc z3}d%-h^w=~AH~OanOQ?A((H{1BG0Uyj-c%Bba_A7nU3*+KObq7sc!Fm)Yx6C{;&`&NU=&N!mes)O*C|9J!< zu1BL)M&Lv@f;RK0iMxzc>(RA=XlN0C4>+N30Y?m3$y)6F=Ok=H-jCt6C&Zd+lPtk! zHVN`Z3JS~2?J_Os9>Z~DeBY&IgvS_wV!kdH<#jS2sS|0RyJQok&^K>g zWphZ_Ne2_b?1;c)I@F+TlT8iRu-k*G_JLX>hBR|%J!1DNB4NFIIr^KIL#WEry&*Id zuv1DQhVjunIop#4aBCwn6R_eW9D(uNr}aEaeQ!cq!jP9ix^Z1kRC9ZKxq6}Y-A75# zrYr}F1~VV@gWH)LM^Tzz4pQ1Ywy1S*p6<2-PB_AoSjtA6%b#F|c_{Z;*YN zF>rK(vu4I@5IAk@Lv8H(B@W8iuFIwf`COEo-7*|Ly5by`8MnU0(ltwV;7)75oTFMx1<40C)8)77;% zhpH&>f8NmSO0ZtDr+kMm2S5endM#+w2mn!l6bL2laOl8Dn;r-Gl#_}lD-#`1Va@bkW5<)=p#>4 zA8&#p` zsK`hs={uER2(KalSH8l%|$bofiy=9xpj-l ziDfoWp#5q49*J19dbi8T>}hb<4Zq$6_2JruZ0Z~?weZBH)yZj2jM10b*S=4<^e}tW zNw{{DlMQi~J-oO}Z8?W5cl^TqP6kJy;hYA*1$=ZrrYKeKiu#bJFNHQ&o3+z^1n`e;{8U%y-@jXiqxA&qW!c-d$SWL0nNe6${ZYj z0eQ2&5cX!pMbybf^l%@PS_^0ZK(gfJWol|>USFTL)83%|?P9zhSzW(p6E$+#3oXUW zG-rN396Jv{sJju@?RW)-`xL`i=K(L&ZyL@; z3(T9f>r;bTB40U;Zld1FXXc{Yju|go@zKtL75LYOkmwU`=~noP{Kvc7Ii22Ik6BMX zGTA(hL82hl8YJ-<25rdEt2Wr;N9L%+C+ZXVGLm=8mEO=@VIKqMyW94QfF(oa|ArXt zM(zgKtWiYOtu6Y%ne|Mn9|g(kT8Cm(tuIY;ENKH@H%3-F;@LZhZmQ1^u>*y1x{}Mz z6J%88?%86qzRZ959%_XtIZ8g^R)B6qU?e$`XWjo{X#P&hG}`cMe~-EIdJ{(}1B?M$ zyE0F`h!UFJXM1sCyIkdziciu~tDTNg@0*@E+^rf;MY)V*?rDz+UGhdB>d_gNji05 zBrsZkztv)rtmK^Mv7t!u$X^|`4-SGzixL#Ch1mL)9{`+B!oK{j@nUVPoDiHJDHtZh ze4;J@4*)#7`3-b&2K@tFC6-{ppeAbqTq-KdU~)+vNZjHHah2mUjM~1q1lt;hEVJ3_ z6wihEnF_yumC!+eHG(?-WL#2)Vl^dY&k!-B_1n1YPQ**vYTP)~**RJBos|2^$cN}+ zedey`yv@ZjLV7L}tT9bk3Yz(FQln_90{1LUBAV2tjFWq_w+vSnW!~u-p{D3oE7|&T z3jkkD9&j46b=V1g!k@q7#zRvKVl4v|8qwVi$YKSdrKi?;>Rj+)uZM;xjCR?Ud)Qb4 zsInk0GO;{$Cy2b9vEr1hlOzl9znZ>v9yl9%al->#k8 zZ2aD;U=#r7th@V=Gt^8cUlztP*9$c#R(O~#M}x0f0LPn<6ARXuT5d7B0^4c{ zKL)k5b=>dRHh9@pVZ(=1Lg8 zfS4})l=}*u6pbN* zgAh|O_&D?+)E~U&%k&pqC78}dIs3lNxC|oWVU#8v`z159;&s#qnTi9c_*%o}mdZ=-8l)?g)Y;fYwez;~qnYEzDx*2T8Y`rxX5UDlDl ziBxuz({+rfqht;G8?f7JCgEF0uO%*WEU|X zhx9Zs^`I&mcYE*MvsXaN+v3OaFWhGdfZU4Sudf8anQPqkf9-xh^jfKok+EHF zJ&yLlf%Bdpa>oUs)q(q`qqC)7rl!V`2m_rrA*T7*X9t_GK+!sT<$9@2sn-F_PK5{* zan!kY%1=6Z8ZTb2SgNmf^Ur~5(#T^&QrrB zL$$h+)wIy=K|0x%vh5x&lI?Ef?SQUa8QT=nKFW8RL1b>vb=Geva8ysyISXA%#LJ$w zVcSrB9ATKeQRZZ6?bCN6@?+%%G4YP|@mWEYG)B>shpb6u*`ru(o$_Q}yH)1IH?Njg z2)lBEJF;ly{(JgvriwVg&Cq&e+Vj5uoRRN94DuBM$z0YCl^^`tvPV7@sHKbK(hb!x zLA?uAeB)fs&m#;bISY-Nd1GoHiL?W$psoj0ErG?|(`fvj)*pbzO@X=PgWR3-Gj2)S zTr@6MQGIStUL#nY^@%+k3hU~QH0rh=#XtzppT zpOWf_ju(KJvYupMamJ<=va%pHtTcts@(q6Kd9u9`^NCd@C9P6sneb%XvyS3^Vd$D` zy9KcLD;03)3-`-k=ibZ*B{|cDDA3`-hk}D&;9j&c^-W2Dex6a|kJeU&LL0aKM#0Pq zZe=^}@H0K}tY`pGlK^THX3p=a7x-)|Q$6R>Y*c&`+yS&u?=iIK=U zf6m^%$NE_p!PtnBce4l60hSnEZGPHeyyTB75TeHNF?F16*shqRQ6gj^ZLxg|dt>c} zJj^h8AXnRflVHnDyMg1%6jE6}30w3N!yir~9n|hA+f8)Iv>E{t4%DMNdgB}tvT1Hr5^gTJEfYF74 zG+sYEr=nZ>JWN%%9D_Z1n|60=ql(jRt3+9tt1|=jl8Da)4q(woJ&q0 zFi2;plqNk1FGDDUIWcJbmLeJiI>;2w&73$GpNrFVi#BX&97~LCdkEA*d64AMh*8fN zPLGMl5hsJO$d2&O_QSu}S3;%_UqL+XhBuw_Q!G1jqcv2mDSi&F@R1?*i8mdKMSQiV z(VC~sVRg%3LMp?56B(JmVZ2&CsYc|tR0mM2$-ase-iP%d$=KF5w?*x7apO%*Wewsn z)RfUjma7PuaKf(#YK^4yLu65U8Id7vcPNalwy;)AwIqo|pA?i(2MHh83-&KRxE(U2 zV>)jb-tM{>t^Wn#(LL6WZqXiUKV}so#*TKx@4de^CH}}~3$~F2TGRK(mV`YFi7H?^ z{xgZ~+-WEE8r-+Lcne+`Z{4}NhH&neNj(y;7qj%!Z|CwVCyqxCbdBT}h3q_vKyVWxAEFY}S z)IJ<6b%dp(F;F@onV;2qjfH2rfGhxWJF8{zbi5{_)8vTG(ageR27zdKKaUl7+1*dh znuwPY5Lbb+aI5uwoi8*rUtM`5aYgY~;&tHVTQP`**a*zg17C`&9iGGQ55jZ8 zFT)^3yL}2=3y{^4R?KgXH;iFfa_|pPBO&s-tw9y2Q?4z zS_j3d`#`Xrs*l#J4QXL{^|Es-Ix-?09wEZ(2(xHWem+*UN-315&YjEMUp~6Ktgkm7 zGU-JWvT@TH9!ZQortj@Z2y~D>oPLl1q#W9qd239)iW{UK*z3DX7P^mHG46B3q}SKZ zHTEE{zp&jNtDkXmh$L}!P?g+Gv?vY6{cz4$xPo9XYepNfKYPc zJfFiLsq}N&aV&e38v4CQQ}w2jv-C8tGWqY$GJ>(K4V8`Kada%y{GjVlhTXx@HxDPo zhag35$ju%1c``B;e5Tojr~BB`6k48V!5l#8iAv^vN}-GDMN?;&q> zjU@RKlL|TfL8g6iR#4b{=AfK>chZ3$0FXmS{kg_m4DLzkwXeGUb9z+nq!AN(!wx6mnxw!tT(0^No^3cSqSIZ$I1Yj?tvublWJz8XH*EW2%VV0RCug9>kniTe$WynuXr z{4X(Wz&3OeYlq4emF>V#pbf>6l%(;&VupF$!IxCAR)7dR6R1DiwC<)sW`q7!-RUB_ zCab5%-pFRgpEID^?_>>7ldV!NfN>uX|A4uqVXv4{&dQ14;_RE_2jJxtj)~JXmwifQ zW}?udn(A~1>VedR}~`jjQIZwxq;@I9%m(N=}YL*vXs8j&A*N?_#+ zY9f0*nb!ke1@z~#Dwm?=i1#SA0+eY(%_9E}Ij!EIjRZ9M`{K50YebM=sD6(MB6^%nKd#nIV%EzjSvXP)2KqOFv8^oi z_@8$b$9#}E4c4k4nEk}NJ8l1BW&h`qR5VXa;+ndH$S++9c!<8M{>A3Z#`Sw~&-BjW zujtq~KlBWL7VwJTQR4PP=5)!?91ix#gt?WlEmgQ`?HOl->7yWaTe!bGR<#3cNmkC`)H1wO>V@vRiT1Z#eqAQMo?u6{-HF=h-kZb^(g4Hmyu(0!rit^_ea)A3 zVE`;qP+4+J@BT}tPoGUSiR#0!OGB@A{M(M`*P;UwoyClb<&3wV0Lz^0TYOm2b_U4> z2vq~S`9ue~I~f?h43MYhQAVhU=n^Wsa9$X$8Ih1=G8hRJ0gI=_G9xc=SS6fUq?@{7->u`c6^dosvD~ZZqRU_1)Af zFhz<>WV`o6Ne4HYZH@L`>}vo^2WQa|K$Z}dtaIf<_mA=wssm(8saBt5Z~o)phU7cU ztb?Bq+mVj%>;N$$S8c5jamVpEx$6Kmd=pZAHrUOs0QB*ZhN<8a$@Jg9Rj0?sdP3XE zo$ZfmQ39_RsT1(0%>UO+L6l zBUs`eOJrkjC@ zGYAQ9w+N?84dzB6A;PDjXzf?#;Xlvg4#h34OfXtlnK=yFS(K<&%4>tSl7e)DtB9Xk zw-U^v@0@=*tfo5fAOWlZ%X1&UPVv63wJpydC_QUKtQ&QBR@+U0wlL6HDMfj?TO*!w zLD;We121tIR%e=e*{*qyo+)wwydUufvwv@q100YYM5@=L{_`Ijq1VN^H+A=&CrGVW zLVQ=_vvWDA=NWOmh?d?H>6MMol0%eyi8p8#r$A4h(*D%zLpr4_pTi()L#?)5O+L^1 zaAfC?aPY6@m>FO!c8Q<_S^g-by{OiajBA=mb9M%Ih@%*F40z{1xaz-H1PA}MMAHdr z<}%?T(EI)P5v0`~TjbUU)%e=FVIKls1TCBH+ME02AY~@jc`S8S7 zoKh2`Ha1xz@3m5U_Vj;R0Eo#_K?4%A=_3QwV`DUcj@MT`4%VFq!5j`E-8dd=l;&a@ zN?CTgd$<`UMzit8QJo_x&GKqp40-;D>f+Amzq;ugE3!iNW7Xs|$| zv#asdn~Q>i-t{^^-T!8ob^OntmX=wSwqb&Hgw{;deI>mh)`g9gQq({NA5PChLNG9e zbVN!~3M!7BkyPVN-R*P!;S$G;(FrM+7a|u!Z%e{p1L7#Wjz*%i?v5Xz90ru2sY>wy zO-qOR6bz{zJNRm01cxh^<)b4CIWaS z2k>nSq#!QA#58{?Vx?xbT8X}`VojLG(>onvb&%p?w{o;}C8*MhACS4M6QCx>AfuLo z%k%U8(KEGKHqVWv)FpJEcBAF3LjW<2$Urb!*~2K(s9}o{{^izXfAL`V4i$${jT43y z^kdY|!}P3}xRx8SSg7iIk~qCa&TBnp~G;d`lMBEuEv&##3Ip zo?ql?3T?TnMJ09aNO<+9ERR*@*aODHX5Fg$lu=jDsMQR7W`!RX@vbBE^Y=5Hl6H{Q z_hdSydiBF=yAHepRx(Td&_OCeP#P|8QI>B3nJisd4_9MdP*2J$_&~SMZmq}^3)y$i zO`E8;GYlYoy0&V?Y`r%aGyn&-RTUIiX!$PzP=Nx|5(5xJy8$w<2RteI3WuyXx|J~2 zroKz_{qhq1XiD;)zZ~3KOIGiif5w+x|11F7Iy!@h zC)|tOar*WVdDOYte@Lz4^4jGb<@5oiGc$imWZf(p`7+B_#%B!G)08>(cIk-mX;N7g zotziOh5?OvB^{X6XXvwXpD4OKHc^XkZGC9dQTS1EpoAkbzApXJMqF>7Kgu886WzXL z{*i$pA7Q5{#GuS4aq0W<0HNuGbRwdQd(xlePS$2=vpEsaIq6QR;R*+KF=mDM^SJFV zy-VK=jx?pn$M}Q(9=>+$=sG@a=O$Q5>_rO(l^Y1EW$}w0ld`(Po$tP4B#o-wcf*$2 zbNWAeV;7?91Wl z7j~nuF{Q42yQv~x4Sa2Mth=BoNGq+;%C!kPGk!V4?nB!u|Cb>LJBN>h^>H~3m5F*- z8KKpCh&#}30*T%k$)C?D`Z@G=LPk0 zoUwhc&wvudJ`wcSLT^)&GUr4-0%P*hqU`&?aK9Ki(v!(=CjK%14C8e0+b2*@xm&xT zYEi4Tuc4TO0V(n#tl*Z?kbOZzlr!YPYsXl}oE_)x$#{*S1DjvI`MwUZ^-p(h;R{4P_OqyMtpDi0dsyu=qx}p z;B-Yj-4A1eXfmS6Q+jVJWY@;oWi!_5X8Q3+;g zfX`tJy(Vkv7}=BqM4McOknO3BJ;E=}pCt{8X?!=BzvVp%MwPMQO)+VO&e8>i^yiq| zF!koP<85{M0*ZyrD3^nfVKCATO(2+Ssg<@lr!H}UW>FBoV{-8f+mH;DuDdk_i%!T! z4^?J@B~aer?@z65Qj>EfxvVq~mhR9SCA;Ls+ca>fg_ge5V;TGE+_`wA_kP6jXqEZ2 z#w_RdUW@C0d&6v18Iec-sTr!Ge6Q)+!A$y)I+)k7utjqg)>%z5p`f^UU^|Hcg)|*I ztU>q-R@k?>v;%L`#y9{Cog*-hbo0dMRP-&a%iij)X`ehUSDwsqLk%D#e<9~#2-%8K zbfCfNna0(rY(*8*;_Q+iG5GtH?$2T{^ORtK?B~#p+JaRR(FV{6 zboVAG0a&nKB|&gpGF&SUN@GVBMbPdFWW2JE$){WWS-lKnTntY86u;%YVvNF;?*i4* zW)b!#q^SpQ87!{^zV?#~`KK+GrzN%BvSE>_nYW%WraR1DTC{p3<9t0oly$!F6GNX*`gC#vcIpK(9WN9)2;! zMnpknzDkHv%i4)O)#EhiW#v=^&?tV(39G@mpzY$OpeI&gM%Es|x4B((jbY_n#(fq> z9>KmL(L~+GF(`tAb_md>pzJOI8W57h^upwEg3x|TZ5=aORNUaNv!6Rl79r=PDNM&M;oD2*gg+DHeAx$L3}!9l_N2(Ki_ zJCEA57+d7kYY*n3`pbKDLz7Ki3upUylUi1Sa$9*;l{nU8CeS4MM-XFF%k3|3rgrcG z>WcW=fPdi|#*czB>LtX-_a%o&*yCKx+Q-SlVxvS4*=3Bdp$_r65m2-6!Pd9n&l0_m zT=_guy>Qb(GFK~sh`rpAE`yZ--9ELWzo%0=(2e##whNePeFH0Sm>ZpvF9M5g3dMOZ&8jD4~*B18{WUxBh11V8?|t(SRyYtq}Q;}9baJq%JKNmDO@ zH{;Rp%gm_-!Q%={jhG33&?4Oe6`ESd4p5(%Aq{fGWx2bsvkV~cAiEg;-GteUIWM*k z7(mrzT7u&*PhKfG-l*nVleurh@cZ=WVhlacH7F-*o25cm5TN11c~v#STx1iZ(F8k) z-Ntfy*vQvhu{CvcSMr4F?9-Mm*4XB3=^s>tHLQg)$wt?5W_+`kyR5M5FwfWkgT2zBp487e_-;9=a|+HzRzG=_@&m+{YApkjf`DU|QHFm~tj94p=nRV0HUH?X0HD!pIMYyf)gjSr5 zB%D!9UG^~Au6rlu!8+d^#ev2|!+oQ0%FJH8LfuV-)7!vyVR>KIR z`c)QaN?uBmRQN!s4^kb6;p9eS(EP)&lmzIswO(O_@)Um1tTxZM45vir{?sAs-;CY- z^|MF|wc(gkoq9@}`G?MzY1EluCJD6t9Vam&x8zw+6~C3gz(fe0&)uNK%24UKD%umj zSASIM=Bz~#T7I)`0RQUXLeGsUodov;;llHRw5Wm#cl#`QdzPoEA0Jl|#PA=FXn|`) zbsgvgwD#2Zv5t!DH$m@2qyz`m8%c`-wjkZXE|*ymW#Tn(71|DfMAP4ni|8F!x5>Df zWf723&PN|=(5b4%P!7R~eqnR}7AqD)m8L%u38J55NrT2-4&f&&pwNNi!q++(l4;q<|bzWG~`;t1Y$zs2SG4*_q zQuhVs(FuT z7k0gV^xCq+TGsY0zSM5~48zNh_EGX_kD$x*5MyxNS;OXs$6Na@O|Y#MI6F5$yLW?T zp1&M;62#74%X9L4p+lX9m?V^^kp;Gwo1)LLj0k8urtk9&B2%{)rnl9N6%e1=0Cg8O z#i7iXMi;t=U&w}6xu-_Jr-Uo6`R<%Ok(^((=!MM65*7o&U7nEe#21W8mj_3>xHv$o zotb!PwaBm=dl$5CiHDP0kCsaktb=u@A;OCq;P0DPyYmKLnztfX-P*~2l@=1V)e+JJ z^Yac?N+#WapRunXCm8!9qSbQ5qOPl2@wYFgz&l&E2{bD9d0!A^b@h&ir8JVt8Lr?q zf_VG|i%Q;>Ibiq5Ot21^xu;G2QMcW}_)nByA_@47+Fzq+x&QRG$)rN16OE0wcz6aj zOF(Z{7|1ED#X}Zu4Aj0_z~>I5OpJjEPGHE?+#NSDZKMp}z`cTvNM@lK@|wAyo~}K< zH+SZeM}o$#`FtCH1^HY&Q@W_}gyape4b}phIase9eA0Vdux5d<#b2roWPKAZ&PY=m zA*eRo{*H5a^pl1B>P#gPP1lgt9vtSkq*FJfC6%fG!fhzTDIYwD>}o$(aQ3asht@|- z4UAWUTgwCBg>_)M65Kb4^13 zoIPe4n>1Q!0>BP4(xmtK`G~TRAlcC8D%rRF1@zLT?^;XLD1duI4RY^c&iUg z^}aw^5l^KZ6S7M-L0gLm1XO!uffp`!ExRNJpTF`h$xlMTAIe{$l{0)zMCa0h@s-hv_@Jl&-7Gr#yp(ll>b9<+%_{GNk4G?N8f zyELyg=;XnL`QXNF?o0Bn`my@5zz9BCwXIQ(WZONHWIca7AL9z=)%0JnchY2=0>CP6 z_k#=n;4i?sMfjR^t=1!t?u>j12(V2_*0))KOQY?=DE=g_5&hkyOhvJawFf`T@c={F z5Vpz&&}ZdxYqF}#nm5j9Bj#xs6#ZNj&F9LW4gb6vg_6gWx9;`ocj>Xv63QEGRo;~< zMIws1agTKOY_{hIay23PJqzcD27FQ(nYI+g7Gv`1b*T@`z6_J?QniK$UW&{+L_-1$MzOUk@%AZHQq!$r*_Lb(A=Vy>$I=xQJ9#6V^-Xvm8N%fW)i@TI8I4fuX- z_jJ1WFiq(YRdwX*d*M3{0nixQ+)nK>f5k_pNxO!>C8-geIQ-DE^`a`vP*icgtTRJ1 zI%RD)Z3Cq0be6u81?+A}`(b0kOqT2Xv~orvrpfJcX-{eq6z&ol*vti!ctleFl+r#48>c;sa2hnPcd#w?lz4#cBw>9Dv}v!BISy7-O&jTgh13 zJp<=S%Nh5{^=>#LZYs=px80eKnS-n#z-;ebn2+DzfpX9v`(hh$0q<95K@uq5IRw}j zs{f(C5=}5`VbIh_AN;a1QIBJGYp=XsoGxCME-TYB&5X~fAUnGTYc53N6B}YQ3Qn$= z49T_I+>4VVU;Oyq1maMi-B^pw%8pHvtMulHf}2qpEL!&zfiW>z>T6ojf9)5!k2w^|E2tOx7S zvD(xEh%DX0_l~-;4(gSDhMBJ@fBX|anHeURZ_py*ZX@KQ<5&I;f?q?8{Hqcybv9|q zq>@$moV4R{oFw4;$;%QSkah?T?y61~0iQmGs{J;%I-1SnwF5k+{)Eaa=@3!zuCwo; zeG?PL3F5dN62ymS1s4DxUV2j>~j&`(GwLX?c_lcpSFPtRvvX_ilT7uT7lr zsj|ikBf5Rgh=Mh?uvgi}OhX(|@BpOu_-<^-1UfzH*($@Dxg86PufSFBdnbn9+6EKP9c-Dbg~SUKy=! zm=>4A)OD}xeD*UBiHMhlJ80-AF9Dk?!tBN zLFDC^O&7*s8wKB%9hjIU^?R<<19L(n*{`p_?!L)CrF<^^NWy^SpY>@Sre`@`G9Ak` z!#{nVu1vr}p6;KC7zU8YBt&DYJ?l-yzEgD6s^bFu&N4ui5M1o!Y+Tutp;r$|CF}XK zKI>X6GleL#GNis=dd8^~@K{CNtQEek<}c68VVJs;0lQ?p68u@&GkOqUU9+_?c2;Jp zaU_0nwPKN=P;E-7u&oi-TlvEPhs;6DYL}U_QpBi4wD0EfxFsoWjM_i2tIBwNkd>uu z_!rirf7UUv-!23UhtBuTodi&tgc`NQVbmBfy$~V_<&@Cg4iVZYdwV=j_CcoT9yzOp zgRk`c4UZsV{i9N~q8`^O?apT!Sf7hrV30Rx6uQF(RQyB-`Sn0FzuP}A`-3IrCDbe4(#ftdksk}MdIcSIW`vA&<;)% zwi)H%)5=)k<_)~EK#c26Cl$-63-mVHa9d~Uxj&PYg%P<*1(GAAjL5G6^j2a*(m>q9T{(tRJGJUiQ`-%0(!6lud*TQK;k z=)sbp6+jeL)6Ej#;}mO92u5BClzJ+|s59eg7IJ5AB;1f4qC;<9RtzYOG8duN3pv5f z>`HxLJH~PNVP!&09zN~;k_T-SVP}`w_woN$t^Y3TptFFq{-r)q=~gpnv15B!n1U}O zmAjT2(HxD)KDtB}*%Y2S6ZG3xNqP3Xth~6}A2aequ;mvpcWNlGO1r!U1mM9;K*BPO zQ=5=)?t0x4aM5zyoo>@P_V26_eK{)Tq&>L^KA!Jz#&_zn!^ErxcY~_RexuCf`rt>@y_~Or+TO|jOJauD z_sim%($|k%_$ES~1i!w^=+rFsT5I13=r#Qv{B-o5S2|LPv;J@T0aWJ{pKrg4tTVE= zNC-?%JGmVKdnON|gaPxq0U?~}Hr(9Z%k@Zr+Wr*qtGT?Yig@TerY&JCyXTutSr3U> z@MVvB-Qoa5XfT%>%9Tbcp{mBzFy?TQzn!|g)2yjuur%#sj;3<^83FwD6V8_pJ}6p! zOviIUXJu6&OQXV;-vl=^LYwKDp=WbGQLdq&%v8NEE27`G`f3oBJPt>`W9d2KxAG>w zN>Obje%_VP>pAb2`+8+d;k%~GQ(fR`QGevd(?8K8wXDDfQcu*?q(z~_+xcUJI{|pw zsLBq2zKuS@>~XEE%{aFE5CcLV$Jin{wh>r$KMs)Q{WG0;-F#budTdFfIZN4(i`1)pC zuII~o$U!Z^95o?8`-!4$xenu6?)~O&?(W<;ui-b^{9|nGN213bAMc8%j^?cffa?Q@ zX^rk(_6t~ma5WbxBXvVwJdvsAp)VmH`Aw8iLAqfSG1VyGyni4op{`L%;s!lp!&I0d-MHcKv7c0kmHO@SL4b_Xh-u zaA|tEy1Q!oT$bpwGjO4yf4p&fj=q1Yxjqa&6?viqR?8gEM*X+Xq(3j^4CL!V%1!YY ztR0Dz>_ezyN?6jY4NDbod{Y43`J+Rw700`t)DY@niqC0*X9)TD+?1Cy+TibYJR-=2 z`|p}N;jJ4&M+*8CqTo+8>+3dgedp=RKjO9Wsw*Vwwj)iM4ZHS~g66nhkVr^QFK+G~ zQ%w57yc_A(c_M;qetdlabrCi~+jh0QK8xYBBuNCnL9#8o_?=)zl&s;*8dBiG(I_N9 zkd;meu8N+Dtgb(KGrNEBle{N=_e2lOpq=)E{in>?DEF>F{6y*su&p-}AZh?hrRhZjeL_y6tIwCO@t=#dw(Twn~gaqL6BYNoP;RhcsxtEZHil^{Op|7)P@2%(X41r=wy>!N`VI-s-n4GwcT1iSA zo}!OpyhQ4w#G81*II!nA#}(Xfzz@SK{A+ZkBR1aCwP)(JgSXNQO-hS+#a;yY;|D-a ziFkr3!CID+xlbPMznFNSqlhCr^%{f8nQJ-6Me#41s|eKTo1*+wwEy|pNW;-|0D7d? zk!2lK5Txd^D8sz>%Y=A%MpZRMrzAD6iGzWbO*@2x6HW?PY1rZnVx}s!oG+P2Yq!Xr zMHQUJu-ko`HQ>nilG~yf9P7;?FI5h zeCeGeeENy+ycb#c1Q`8p$+XjBe=jk=E=9G|I0(qhSTiua=^>Go3`+Go{$_SS;4pJ^ zdFD@OkXfoG@7Z&u!Dg^19(I+Gwi)GCbWQ=`M!NC?-m>=26Mqu%J|@lS^UU^KQC|sK z7C?qG`CwA4BdMBHVkKKks{h41(wwfgKJ$Qf*@AbVh!6ND^LNgL61?0fpm3Jrb}v{ZC`rObABe>DBY4rjoC82?&eXXvClgsaB_w-E6Qj=&xIL6bRh@?)9+J(W z!|{(FXzb0sJ0Uy8P_wT-)We|T;1sRl`kzy9in(jAmY1bFka*4uc+_0|yXY5TT<`SgQT7Dy<;Bx}u%TGD))UnBDm!b=QD5&inDN}36={V?@zo#L z15!K=+VG0EL_(HR9B)rIM2Rxa>*R0sZ4?y{X5 zt%7tsrnFc=15RZ>qQevydpyk5~Hs|`(bth2b)8= z?YX{eMBmt`VrS=@uTVLis@vV#F+47;q)jS7wHUkjPyrp#E5l@QrM>tQI#ygAy5#d& zKe?2@EQ|}x4FaFJM$C4Mo;PgbDn7-MKfHn3i1b`Pf}ronUkde~Pn`ZHoYzce?A*lk zm4#KH;%CT>tuHB%pG9VVfVaN*!d|!^s^uG}-_CHI${B2qjlDeT0oEqkHG8H{gkvfk(Sz+qw^reVEH!;9~SQTPHnRG06R=os>^1ywu5~=CBe4R-=o0+2<~am zS-A_b?=HA&l#jQ}V_scTG^`CV*LiNIOFE6nGk?rSx_zyIp{0hJPrD`VYzFl@#B{4s z|7bE#XPN}<#y2WIQY5%=yXO$*)gCWg5E%cEkOkm!f%*|Z=sh;c_uoU4341$am#FPfJtGcMXAl^X)?w{_;ht z!K+XAqcwX;DXlPaa$f!q6(TR)hDT@$|J1bM{1G+VwvM(sUWi{AqS^tA7`%3-^MfP}FYRhf^U?W%xXw!jEIgC$!J!08!vn4yXow%7s*0$p1M4Y&g7k6pxT6 z^uya7-2*jdoQcV`FRC+Tn)Ydhhbvf?`&ymrc0^7ce=I#x5FU_3R^4`^;ZGinO>6s{ zh0A`8LZ;4!TpJ@nHN;Kb{I|53W{L;Q?4bzRhC2cj_x{yZ5HXW^>(vI501gKbbj|Sx zQ*Pj-;Skj|RE?fr?wUID%Q-kZQ?Ftmbe{hdxAm9x3%Dg&{)syB4*bGd5ve8QWDKMfp>BjAFTAjexlSggl2OGK?cDr;ax@C`aGkPV)5!3&eH% z2GkoF>F38jf>^DG@M%Y>B$HQS4}fvF*B+}Ww9YvX!blBnH>DgplLqufF@JI0R%gZ8 z&LwFtF2c_x$RZVOX?E#c@Ezb)cMDtL}RN-YAcRNtfxAip;ubnBxw7F&fUhvHxBc} zfJ2H}RZ{)qwYzCENk-PpS-FFPAKUX)2Fn7iYlkd(jQ#(CXdzuAq5&bS%|gn#M>5wI zR&YfqDseTYU74Un{}Q-^blR6Lv5({Nc?VFGH!ihKg~Llam(d+VXRV=y>BuaNX;SQs zgiPHpJ(AiNYhriYN-;d^QjCBeb-2n&ZLVUzk0 zeXW?9T<)x-=+%jMiJ|J%8Ot|G=qwyaasgJQGUY>1abLfmURmynsPO| z5N-LRV?N5kXX8z188FeUlqqW+{qJ|c<50-hSXkU>t~9)*0KMuTgbNG%Lyr4n^N)Ro z6Cqm|ufyvp!`8g^3*4luD>SMvxMVksX48;vN2urCAY-Hm7p$Hds|IfGtvny+z6r*hj-8GK^F=%8p#nE^Bf!$gJhMSVKM%pL89LdBzJ0v3sL+Asn z^$T?ioR7gvxpZEtmTBnVk|?tITZg&WR#^A54$-vThoM{GBVfZNK70IFBqX2zZ_G+Y zR=L#H1l=Kj#|-I(qQ-hab1{m@`Dxj(mfJj?v?|SZY(5PUlw4kR^&`vBzY)CA{LydP z;ykMvQUSLMTb1_P6Mw8&)zJ}Is~0ne+zm;Ou7S!vY3V2CkTu5ls;%(^Qv#4+y}WBT zE`9emcgB_7Bj9(3NGZ=d#cpqN5+TXr>&gBQ^8rt^UgaMoWFFQ>$33!6k? zqWQlP>*oc}Qn>)mpiEOThUP z8$>3+625xzR-r$pM%9mZ%3|=yH<7fgueIcG-r&X*e)|dWA`8Sk?X-*N{qa0S3v!mGQnL$DJQw9JHqdO=S9f83q+ zuX=_C*AGlW#0?a!CM#yN=%{)84L1!tKch$EnZBE-hBl776XI86=?=Ue>(;se;q=sG0xFzrom{A#&|Jx z7-O76s~<_ieXT@xPK|6C^53fbayA(cpF4^A$(L@PP&Uq0`;rb`{6*&fgjNFJIW=DrQ9$A9?X((pY0uKs%d9_F4y(|<4Xb2#R} zzeKpB_D8<&J(dxi!s4wEd;dQ0>S|FXp3L$qeLTFgEhF>`E(Ht5Ab`wXAFK(QhHl5M zv^T&&4sAW7^L9pehV{$0u%S)VPDqOht3ML*f$Z`~pELz08P3mz-;F0#?z1^zYZ1-0kN&&cXN3IF?F9*Ui#unp3Dn|Qp2+UKZ!UO>E_^tPlJ)%CdoWlU!`Nz>o+X9!7r;6 zBd=ZGkw+N-`5;jCne0_N9US_rZ&hp+H|%@NH@rOhbF1Rz zo!@?nm}K6kbMW1DyQ}^DZG&a2th{6Y0#Gnx&Ij9#M#d=J*EKM`+H50DL8Us>HE@;$IH+s5`3j&T;% z+Z6FwyMu|f&S7mBLdWWMZfa#Le55>o>IE?MY3t{Zh7*!ojhDjXKG+?8*?qTb_JE{WN5!f{3{K3i0!o9xL6^yGth~#ym0*+yh}}fPfvr4ys+} z9sbltADc1oCITV^Ih-$Edx&1f

krWryZhixn#P&8_(bA0rB< ztqf--*>N})v*u(6m0dfDj#e*{aVg=2h}g$I5n$Zd)Ak;Lk4rZ*uf5n~zQ{gGtjWAS z=P)28AvpkpC%rVIGC|<+=^c))Yg@{gv3>L7N=$g}_0Q6M?p0X~CujNFd~S2^b_Tk(ddwk4f5a^cAH9sYfyU> zDtNM}7I~)0vE7V2jVd7~{UzPoOlv!as|fVE{-i&qSuMYnr>53)3?jE5HE_{GXw2;R$t%@)jc zIwPsg*iLg(Z&l0GA3TkpkkoyO4=d)OYD$>1S{s;iBS-;5lHyB})mip`jh38nfRi{= z`?o#=jYNp(I-ZE+pD}eoV^kM%GQ*rAXM{AxDudZ4(0!{YjFBTNRNyF})eHeUR^0p6 zvLqbKb@A=}+dZ+8y}~?ZWA644sSIB)a$vdx_mbK(OI5bpvWNQbe4K{0qKa9^XD_M_ zCMFJ=G(xC1;eg}fg_YH1{#<%_Qz2Z~p1_d2ztjntJCw5YOYpg1qsT7w%OQuB zL3(+HgtgS2q83Bipyj=&=$MwCy@R029k#HXPHdwFtkd7As~Sq9Oci z*3qb6>(tjHmQa2E8cQrw$2CBxNj?XL=(F1H%u%dU3!ql z_~A*P+0#qI^&zTe)-S3e6c>}_Wi8jsqd8?^8_&kMY9>oEL)r+cLyYPWy}He825wb8 zo(HkKiVHmm3mUIt4fcDLyX$$b^QPawgh7-Y;g zvA@#$CIxpiB!vq}Z&}<%*7L;d#c)Q4C^+JFnuqc^ql+T9Apt`~C(_u~mw$o+NCh%* zvgEfY1xS9IkSw*1L%viQ&GkQOCFSIyP4;*sT&VGS>B>1_i)t_EOuD)4JrGw*QIkBN zypzD=0~5=bp^wC!I&-RvADu1z3xSP2Z zO+^tLd|0E@hPXdW7Jv?B#SfJP{ai!0N$1M590|xsI_?v{%nh3cM|Xwub`o> zYqV)93wnI4vLI);h`zPtq#LeZQc`(5b`L)Z^Bq@?nw4?^v!3m&aGZq`%U#d&Jo>kp z-pgl#@Ri6P?chO*-l}H{#*|RC$m|oR3FIE7ev_|EEUr8a(3TTvFO%sG%;Iyt)gQ~C z1Ts;RQ<=GBJxJy;gh2Uo`MJjmtIN+YhwdYO*igM6>Zss#1x?#BD;ccEm#1aMy~Mx4 ze@eMu$kE6C6dBR7yN#YRiDB%XiPZL`^huXH^SEZ}`-|JsMgbjpBo)KXKi)LY*`8Hi zRJLDy{evIU_~8?l))KKcoz!8c7qXmOzKFXXI7UD5aE97xA+Bq$DpWlU;eImrqP#6> zsMQ2kK+2A-4W_VZ#yzR%)%wN9(N}J%td=iW28py@zn7jhwzzt!^Y|)6%NJ8mkK>7S z|6FiSWk*v66rnuonHKUfb~X95jBBQ@E6rlP|uXw!PVO?;dZ1W z6=FfA!NcMF){dCf^}hVD$(6+4Y=^<+qcleN$ve5FEYNsePS2UNAGJWsp1|7ySd3_P zR^H1+gWf-;!pyto6S7)vFcYO((mX!vxs<+%%6GJr7VvRxaY5j?<5mc8ud_L0_=fzyPFLn>%SxBc}3 z@du~qdVM`q_fZlGrf9CPY4g534Q+`%;N=hW9>Cu8{P9|1v7Pw0UG**K$J?SzZZwqZ z2mIkz#1Cc6(W2p@>r$gbUuiW1=O|q4Rz$b+bE)^H79)P#jiIi7b2NzvF&USGCuqxM z`u0|P$ea8q2xK=|a;xd2a14*;%-J>ZE+UcoqLB?!izD(#1>L1+HB@in0BNa16IJUn zs0$)^Bh44~`?moI=GK*B+x7M2go28Ei}dY)F4sK$F!52ydS>HZ6}@fevYFCGL?4k~Sto^lUEm8(E(<7eJsS_{aw|j5ZLV9mq8%RxcMi z#hafWymLT!qh*>Tncv6+k?%Li!(Ycs_@BwrUtT_<;A41=yM)X2w)f z@Lf!_;w)XVc(fPi6{XzZue9C5fRM4=d*2J87k_=t`ie~zeXNiq0YEQ9l>}Qv^E7=xL5-N$;d`1v8J%&`f%n;}G7=i)&=kkuj!(%J6dxk3F1I$Lc zFwNTCWxg-QeV;}()d4~$RDR!Lfx_Y(>6uL!YsDHAFen&fINyDD_aTUVN7BJ z*Q%;|6>8bUZ)YJb;t1WC@6r&#&RJj8&L`=1;Mm%(Nzodn23fn$_C{lN0fgPnX{t%K z&x_@E*Ir6LC}EdKcT4y29vfsB%e0ng_EYJ3J>8*{6n z|I?sk1_iY2asX6ckp73t?;}p%l>w<53VFOjO8>{-`~)caG_-Ipy(N>^4(thfm>uu$ zU~t<{`JlrJCJS%DmX;^DxO4RK-uo zc;{}45Z9B7dp!7urf}~Y2sChw_27usN+@@e?up$TJq30_fhFMbCILGDJje^TVhrmf!+=zB2 z#NYg2<5`&zNt*%BuVx;IGW4nd{s;`@5BkTY?>ejQzhD6+*-0JP)$malA)EiJ>3oJ0 z^@9VxB=x-lZ$raD)VyvdBJ<;(b=xAIm}v%C?~i7heASt7>oR^#o3hb`E7tW(z3!%X zev;_Bb^HoJB@!^nuC`d%M$T}``zHK`9TjB)nzxYM71z&dt(#D_qsg;2nP*{m5UZ}+n8^Sg%WQNJ0)D6 zecg-5fCQ<2oIViuFK(n@2v;^btr;8Qq=y2T<#&)VpOy2fIlg?nzFx+93?$ktE;p z)1ki{+%*7JiKD}Qox!>9uR*HgOW=p{Ml~47{I8QZK~X7WOGD>wB(jxD5RtG80b9-r zx8H|pF;2&Dj>7O*I|O}6fgReFu`nSlSYzdTg(7MJucwcsLD%ov?B$F=?9d|Cb!m3u z)`om+{De6;pvUuL0xgBAX7ekvTz69 zIUj6OfvCqegouRhWVa?gpGZRMxB7EO!YHw4Zi#iE(Y-H0AmO0hwOq>Yhq*qde3}c9 zkPbT8w{!->2k&;hdxdI#8--7|J%x2Jok6t!iBG6F~KpU;%!9_wnw@)3GX}|*5N2R?C%@r&_Q`ssj1(?=dH=g z^)TE+M`w$L5U#EQx!M^p-ciSss9-(G!g`<1h4a*St9}85|6O8IVT^^;d3k5*`)pbq z$7a%ApbCoq`7!jBt5JRT6D6WFvaK2B1@v}IepW(pIA~J!1pui4NRGi?PKD{+Fb7S)SzeIDCCV=FOivk^8N%&YXi zAC8r*ne-dx#rMYA9-zB64$m?KzHKC69_2yxey|0oSO4f90{1EML=`iCRSmqom!(y9 z)MgQYZG9L?)LxrZhrO$s`j{V_737(R&keN(Q;F8{e z=JD!!y!a5k$@52o zfkYr#?MD(au20n)p6%h}niy4Z>i2CK!lbrpWc3u33Y8bjqC->d&DYrF271U)%x3}- zQ(5;&^?zxYQuMIO3Qk*!I|FOfuuHtpapA@OROU&6Z=wsG96g5Ks6mL<6IsJg;(U0# z{5TIM?**^}pUdujO{1SjXy{1_E0Pt9*j#38$fKE!8@3YXDgyy@!MvsnfVVc^S6zJB z=ia$Ysn~9@<|&H80F*7E2`t8zVk8E)`Xo3~(`HeEXJM@--1ag6vBZbQ3*MGz-|4`z zg|kE~_lXfF(A&W7w9fae!YFRWnc4G=P;YGULj_A1??vvz(&uXsbm@69H2gtzWO4t# z&lf+x$4`mBCVNXrO@|Mso(hb?rS}IGJO2YZOyN-TkoZvh(wb1cPUS{t8-F)-ISP0A z=bR82xs;6N#CAnNda}GlpnIMH`7cOGBaVRr^B@woIE&22f^Q^+(1w9Ut>ZxmNBydX z>!nRED%>T{*mSQZboc1E=THEH#hNV0-G?kN{aO@-=A7%i0w<0>itG;x^oSom#s78S zJeDX*@jcb-Yq7R%%}1d~6YfUj?n3f#BXA7UhF?)lXWI>Xv_lngylHKJ6IIRcWzo%% zC$!ev)lJRWzM5PVP)H}A>}HC~*8|76a2F&cY5+k$;AH)WFH8wQHt{6IXc zd{pbkt z4GB{dL@UdP$x94Z@h6BedJ%=Oa`Af=f-P`DZs7a|X=AJm(w+5*ykwz*K5FF))Q_ar z_X)H0fe1oz4t4U1WDDBptx-CbI@qlTsMX*vpJTh)!c9j#vGvvk>VMYaRf}O1ibK+o zexe(4i0}F6B&iG5UG7I5K2f$MRH5v6d{ur|xvlqTmR3)0q0api-TY=+B>dB#pTzC> zEjd7t@^U=ao73Ke%74~7p9A+8TLrg1-x?Asw=Khf;N+bN0Q?Be#bgw`n<)CINk2%4 z&dEbOS5gBeUY#g(H6pF*v`3KoBOIkmz<{v&HUccEv9r(|?l|EmwZyf#TBPP**+~7_ z)UUBMT5r){Da79XgVjafiRLB!>LoQYq}H^Ve9N|$YYfx%jvo+J1*Hhb?)VI}MP=2> zp6DyyuS$7=aqSgaHw4>G*&Ag8W?KsXt@SM z^CyeCNsrc;ErqOM3tBrd_J1&G_x=U!R(XJ@DsOna=3L7Ey~ag?u-WTQbviJ10R7E5 zIHwRLS;Pwk1vVE0AdgA+>$dagu{wEJ#XvNq)5L1(yY1Pv25s0{B9ldx7Yw4(m~dR= zQa0T(`Gqf?7ODt|0dOE-uGaW;m?d3lPl^fK;Bt{2HKiuv@_>lgTvY?7QAs_Nui^JO zG;So>%vLuJtC#d3U~v*gc|31tgghHX%O^n)isD{^r)P_EzIOACiJ8CnlM7nd%FSz% zK_j;V6bttarNW}r9)laoU68Bi@~m(@xsoao>;AusEP_)#6okQVn{w6C>^d}xN>l010DN_cf zEomI7C&9ST%Em4!c&6nrfdY;Bfe-F{|)b&2i%udp>KuiN9PkdK`U@}1N;LSv`^ z(ge)LyHnCacP&HhLPzg-&BAknmUMfeS#cY92C}(X*D282B-?!?r0D4hp83FkH{{*keZ(Jg7;fZqsG1**9$vZIB zL4%f3nt8*Qr0}jFtmhVJtlnB|{Dt5Kak&>6pm03(VeOBIN+}al%p8Rq&hlhK`n*~IB6tk-XlWiU;vn*g(YLScJ3nj z$H#BEJ9(vE6|@$0Xq>N1Jl(R&M;z^uBe3o?UHGfXzz64_!ufAJ%1x0f67@~97k(Tg355F3V))rM{LChk`s^aIXlv%yt$+|}W2*2$QBPUL~&dqkqa`umV%vYcc z4VC}1f63)!pT zFWn2x(U8b`OsKu3UwgZ(>@-iuhZxB?K*Pvz)o&{h*5CA@i!2+BXONc)d{+PHS?Y4g z|JAcy#QZDUR|{_Kr0E8#6_TMiU=j=_-V2>H#1i_z+Ay{5`b)0?Yavx&4)LqatA^n- zrcng%oWARI^hWCM$KinZo8kD}O3(JnAdTy2EvkBP_Q&Wbjo(rSV(z^szQ4-?X2l;8 zlRj*bJ9+Sn5#Gtlek0&(dc*?*uwOiRfPv9}eG~=UON5tl8-sK|5fWfNeOK-&^IIkq zo1G+3WSPy^fS-Nfg-`W+sV82v;zdGkiez1NKn4gSA19ABlr@79JWXzkvD2RD1$11? z>q&#BeXyxBpEC602io~+PIx_8b$|{67GrS#kUw$`7}#e&0244$H-7Hb&jTBR8p3!A zzuzowzB8^nV)CP>Uq1_Wz52a+V9~n&ayXqhe`ok~CkQfxN&AA;Xoo6UkT1KDX#Nf> zLR$#njo(m4A3JkGo2u`o@bPVbza16jk(=piG^-(Y@2c`A#&j<7^&FKd&SxVID(^&g zufae43UR8c`-foF1tNRP@Lypm5b7Yf&Gia^q%i^$MG>5YI!!u)+hd{p-j&g14S|-q z+htYK__{()j=y;84s)@~eQX=N_!q=0Y{py&yRx4A(l#FzerW~5dsLwTnubhsT}6Qa z8C`mTvU3OSxQWGMT%UG{EQmf!+E%x z&P(s!r5bOg?&u2%3~SM$w9!rhyU$YId2OZ{)K^>~Lc+>_0H~JS4>e~8C;&)GGTaq+ z&1+J39*=2O_H>_|A6@BB11W&q!T*HD>mx8{e4H?t&Eft$+ituK< z5-Jr)Nd&+E_E4a+&Jo9we{Qq?WuAgzP)d`i0D0m(%WG+5rF~VM*hL)@qsc5L%{K#{ z9!J#dLq{P-;Fq=l_D?@@A45Y>l=( z*Ic@m=fj-C0dv^m9Rs1Ehh6ji&V9 zF-BdLdv<4W06;^~RpGpXBvjI@r94im>;nvbcnWv0b>i){RtQrLjSH~0tWc97*Ez{2 z9cfU)U={hJL99TiFra*vhEt5I>jb;p3wFG@bF}Yu{gI5#){YKL{cgj?i9xh=^uSFt zQ%~@rIqo-eScS?N={>_S`5BnFpE&ADJI*}KXVTwqj(>gMmB+GWXwJj}8ew5mZ<77X zEj}RcJ32nsy`vrK)_Ty6P&+xD9J}huxAzhc)6knNvVelg8jY<%M6MzQr z9&SZZS0p&+=&dy^t)4-;vf5FA=>Sa$cea=95oKKy^5Ux1poBYdC#aYBG0oZ!Fk#+y zVThymGvm@`8L*I-?O%NuXRc4t@?kxnjlH0ByV(&WuCPj?Y*^QTBq|sOs!J>rIm+jXaAWiY$=dO@>+PH zzB(|9?hkU_lz69}e0tA|$_UbjH|x`E6yOUyfY#qYEh|8})tzW_Z1WRHv{~|YSm|qrJ`Wy{(f#XnO`roQSr^<`U04av;hwC1w@-VV zEv&rGt95CoGtJiZ%xtzwo~kf}vPWs1+?v3HAajlaJ+;!TTUhGcxBS{pOXO#%NWdr- zvZ(8tZJikZjH>?s<=WyV2k;jZt>^$Znn%cN9!$SKwa9GHp5RPmS&9hs?d=};+U~M% z7pyUFcEFMv$|s{DCTqmye-N6j!jJz{j}6SpO?z6+67NY>+#&ESeFFMmF_dCg`I;1g z@bd3dwu+kScWo&=BfNDJFUHOZZ>NpQCZz8Hp(mx^I=-Vm=CCgjfETZbdA*Grh{m`l z)3GXm!~kYmOBwZntpHpVp>uf-R7;@phNh^-iT>OeLwjHSvSqWo{;Pct{`@6r+$R_B z-)n|uO&Qx!1A;MCADYR+Bt(`jeaJT)#X5WNFqa;Rp}!yEsITXYkw9wC&{XcT{POA5 z5b3|FT5LFNYEfz3(#br+ai0?fmCV{DaQ0#WgY>Q%)zz2csbIfkZebs(>os6~zq^2^ zY2SG5g10x#lU$t19}VzjAQs~(q~c-(76k%EONkHQ928n#01QDMqWbL_4cU91v>$bg znrN=e)MID__nLaR-8nB>D^bT^JfY~Qil{exfRZTE?P8l061$VlouxV6W-IpCu+9@U zbMUptMjSm+R^Vl(qe+RX$~Po*BBM6i)|pvQmN#_O3=Me{p#GHtrx(_&{=NDTx}Lm80}8iLNNZ0Z7j2{8^iME=)ZC*Bj{{ zvCmhO&$5~`=r%2A`7~Rne^~oY8py~hw~d7T-30Y9mQMwsDI`nxF95C?cCJE4c_r1Os$Y|UGyiD(+!g2Z)udBW(RXnX?u4K31zk|^U|_PYeL^2*ln0QX2{9 zlOCWqjPI(r&)*vH-_a5HRo^|{{sd(AmxbSGcTNrJCf(3^P*wuB(UxlaUK}y@O$@tu z^yadw6LckJ>k0;Xr>Z~iE0_bFLnGi~kL{lud8Yx6 z1yXMs$d;onn_ghom-t(*n!`Sy^NLLp3Z7xiU((~8IoDfqoIl|%d8yB4M0#ScD8V-< zCG)O)3$hqAr*G(Kf!nOANR)8+)YGT_^0RK;YXBaXVmB_oq(8V~tg^{0+@DLxkmStm zjRUOnyMUd8^i~Z3>Dci!<}9QVQxdv85rZ4ErGwrT}{bvIT%jCFL+ zMc{AuwAxN5TGrzm#%*;r6zbYLDX!%WOIa zDxbAPd0JLNtIbq>GT1@@U3A}p#hr!1IbN`_>rSIKy$Kypu_n}7B9L5uL=@mjq^<+` z#xdJAz`n;U`$|5mxb4=!o{>vsP9d$p-VYo*@*9E;KtrxTOWoJf)AaOHT#XJ`jJ?O~pH z2KG%WFAQce_*wX5Jd_^38U?AluNOZWRWWJScI~M7^v9BM_6(gvlkalRoeF%hF~%vM zPybrmgkdPvub(J7qzSYyY+PdNXT%QaC-Wq5lfl2pYGHpb&Ffvn_A&p4Ym6nlB}m?% zHry`fwvdPh%LgNaiT0eyC8=n}9Jh1xz+OGYX^ZWq{vghdY%mm|C7O%-4!dg7NxaJXyibk>%gLmf_&H1(8`mEAbKqw#O#K z>&44Ki<7G|3<+t9{brPkvnK{CqCn{= zAkN%LFFp?P-Wwf4-?qnAN*_13eB6C#N775Ti4A;7N8aZKubGbZ`v100frvFAgtgVx znk<%(X9_f`!;HXk#;lrjvoIUUE;-9G3hE;53||ex5>o|_{yzIQl0##(i>|W^1>*+1uz(hNW ze|6?MVy%4pFzzRbERO$mEAs{Y5Ze<_r{Dndk3QvOCnf!H>~%4?TK@?}r&@ zkL2>bAXN}~oi$(_I<0riw4`k$B<-G0{D#LGCo`~l9lp?K&@&bRGf#m+KJOPTzD_=J zYP^l=-l2o7ad|{fkU?{j0Xy)&6aWB(F3>-Z0BHInw_cVfFD$wZ7N2$VX1UzuqpUR& zezj_QhbCc%v|2>lb&zc#z}1T*W2~Mm-vahrS>Ta)AdoWOVFHDFwB!-(cvY#LuFgxNq2$?H>Smd>VJ}407AI z=cTj7j@ZSD)zkcvt^7-2&*$=P+b&sl-jYPfVA2J}-)Ah%EIxM7eYp|6HZ|FpHyJs6 zGt8G6O@jzDz6}P(i$C{>-rM|x$=}X~2aiT+lcP;u&$&UJbMqP1A8}*qo|5I0YLAI^ zpX$b(662msn~k9Qa8d8chbO?_;~Kv3oi*Gdh7&XN(3TFr+B^K>*hT-S-~H*At7Ptl zGe3Fskbv>fH=tGiz^TlF@a*6dnLi%S_tNoKvZ@cX`_<`9d4y2u-3C_89&8}3~E z%Y;tBe#|{~)pe7+v8&Jzp;!BUK$%SL@fk<1HOnd-_H>`kKinHGO=_t!`(2|EgijLw z$zK7$l$$`BR1ED;nbohQr7Ti0xKQilt!@I8tD?CEA~!DIsG6}#o>dtzvNMWI8$MH= z^|CZf*{9Fvdgo#yl08tXY%{qDXCMVsBy&0dgSt(Y?F1{+Dr=PZbWhYV$<2!y+V<>o zNn&ghjgs9xLAla;-<;{@km)a(#bZ&8d7kIpj*OPrrB|}fYtkV{1QJKv2AoJ(0!NmW zhQ&r@b0o6+m_=5V0M=2(>@ZLxn%({NCa@WdAD+PQ>o#7|{~J%?C$#1)>%YaA-rS+4 zd7pAU4-57JflN8c6N~!B4uW#8dv&IH3E0%AM6 zXhg?=Y4F$7)y6{9!6?#i3zGDRMnV8d(%*3&L*?vMt=|G5@8tmivXQ2aWceTq{C3T@7~tmQlC8B{bO&nS zA)<82(k$uK$LAFH6jQgdwjao_d|`3zU#oF^zwbFL5CTyM)~X-~Am;E_*Ri@@aF3p` zuGd%gfs$>TX!bd-Hb$z#-7}l-4It&}eZDV99WW6d;G~01o(DiF6{JkTHr|boh)m{R<|GO`eH@JC2>H+?=``B zPDI=pCIP(}GynEV7CtJ=)W&B<0O-lTA-p<${$im_-zw)=-6u}hTX7kIHcxPj>-J)^ zbTMX>FW&so0vwe0`OyPNC&|rZjXN;oA}C8<1F*N9XA>LU2TEut2v3!G7Qb>r_(BR7c@sW$y@8zDvpvOdSRri&-O!K+&X{ zx97@mLIrbJc1cs&xO0Nl>!rcl5FPksXIq|9?NV!J7uxekt(ur|=NG6hatrR*$SHsU zEz5)gET5<(AQTgf@0j|(6!!k&Pp-Fd3df66Dvp>OnpHn$djAtUO;usq*Zj_P#CYxI zsjKWed=&vtD4twPi=BJ2d`>T*yO|>eB^-7$Y^b)i_vXyJ>K^*i70OhK;~U|DU!;gS z9xK!5(6fAJT>Tn}0M&(5mG#6c=IvW7z9ymm?EZL7@;y=kZ>q}p82zZBn#MUZJ7<}| zM|Gst7O?LF-5M*Vg$v59At0rzGQelG2B}Y$fDit)*9X@glJ#H&JIn43#2G&0^#doN zbU4(+8ZA<}-&$t{+lt~b&<`U~HFp^D^XicXOrZwb(7}Il9g6wvoM}C70z0m+>1_E*zoI9vtH}rNkgNuA^rw`{IB$c}d{&BlU@~GN~#Q zBqTW)t{BQ+lHp&eG=N!Xw)1oHy1whRCz4sMG&vcz_P--fFR$-DU*(42(i4dQymy)h zac}M9Y0KL^8cruh5hYTJw zU75b0x+6v&FQ?W(jW>bJ-;Q4m~}BmL=?|oiBn2{XgY4AcU&| zA{La`VAB)Gp0W8Q8l}*2?2==4N%j28%V-l+WahAojwhG97{tT$F8VoBP?#Yn5Ki=G zebrTXr>Z!e@9+eulos3FlFT?jefDga>o~X}k}w)Oek`&St6I>pX3tFdR54qBdXwVI zwazozbqEb2PVdwweWuGWA?+kLXM%2))1Ypzyur$-6y6 zQ)enTKOR(H2w<%{LA>Fab!HJIdzeTZsUt%yc98-oZBxcGc~_Lw1a3KfL6pcvu=$c) z4P6A0%%e$NJdnHt^+d!^$@Jh+*F2@eu9Zg#&F4kaA9s1lv#nAF?-tg4Jj#KFc-&)x-pXMtB`S}rglLK~?rpaY z`L6pC<#)$;r8zr;*TeG)uUUrzOSZXaiyf8DEVpLkK8?;S%m-87}yT%Jg|ULTeS3EEv=MP>SO76+6FS}W47*tQ8qEw&<80aEd%O_#Mu=O!|rCd*-G-P z2h=NU&Wk)Nfpl3CEV-nb_u6&T3?a%3C#jHK2WO3U3GiXkp2F1UNw6Rr3s9F8uJK@Y zhNK?Zr+B<#%j6~Gtvc+JTBVSllD$3@JWzHTuv%d$8JGR$bf?=&1t8wlwG#bC3{00A9I8`VoCuT0V3)!p*e#Ws!Y_oh% z2ltX;>U3RWW9;|f@DE#nJKyV`nPC0+gJSJ|klKfuX)3@~o{g5CL~BCa`<--!%7t5~ zPUsyGbcCR!4d4d1+Y|h{Hp4h$^gHUdH?mh-KDp7SpH@KvDw`aKDvYQ==|eou51b>hWa!L^-y&+@ zw}{uf7TQRPMAk!HOG^y46RGQuR+vZSZ4&$9z!~Pu|ab_O7Wn)dm|#hvfpoM zTcYVoEge4t)ReZS*W`r6{VfyOj@cD|+hZoL7e$%z6QEOW8emc0XL^4<+G|-OF)B)Xn-S<7 zJ~DOHMzEW!dfXK-+CTQJ^tpyml+a&C7x)`fHcl6(`TW+ST=;7g=CowFnlGKx=Xo&c zk;sgdal!<@oGguyM-jWPEAF?d@mZHFeR&E!8Dd-cK4K~Wz-3`nmdq8t6k}vAvGha*U4?BS;$@a|Xby$?TQ{=U{eW6tPx|IOcd=|v z1;FaW-<8s48Ry-L8uEnX+7p9!Kb_3Lvlm0zISC491p23eLSiY-bG(&tT};U|N4)GG2yz`D(n!#B45Z34j*|H1b2m zaD<%~ubvO+RknUn-)6fJiiE7l<)1S)A=2J^229Ng->Y}&{mdOOgMZensuY0&&c>NY zd5z!Mdgau2QJfd0ww>2mJ7;!1Tb(e~yKTWD!(HfFe*J>4+&dZ!=G+&F!WbW{`=ae9 zdwfw%X;I6bw2p|QU41xD+53?m72Vq^0&~wUhlw~F@=b#h-zAk*2Op;B%wM8fJJ ze*zD&{P}&1-KnZhD2w4wQMf`oT4B^tmOOu-Sp3^Zp2w6IY8Hw{{P}<|4d55T+S-A( z*Q{k59KX$6aFDRKaksH~6I}YD8$rf_Q=XN>-?Kpsf66sw&kWZeuL6E1f%epWHak0E zKt1(P#BX4P6v#&?#}3_oJgzVZkvKv)_C<1_)?bP=GF{W62W;1TaJ#vx*u6-L)mAzAI`1b_anRegK}{K7n!Bg}K>MFRAxG#%>UDf;zHfEr98cwrXmZ8f;euD7 zfUNnF{3}h#MwSzL&=CzlLJMK9Yi7vEMhXDhtbs$ICA43tz) zwY)B4qS=RYDjFT3pE-9E)oKe5tY=UCvPcTBxr~$eCmdSIOCXl@!X^mx0MZP6qiHZN0Rum>c-c9^Cm0DX+$l&($UR+hMJPARkQk@yWZ9XX#}ac;g! z`~W)d;_2x92M^5t@g}NhPKjEHrot)9!mp<;XT`z8)SXX{b?H25qV{39Mn^NguGV!f z+s#h8zLBYp?Dd-BNAt9aZXL7z(i^bJXCaq|KtM9D!Owqu0_(4CG`}NMKnd*|W7D=D zYF$A#3g{0%Da{mFuU?IPYYboD*1S)rj^M%sSTnxuF9aP$h{D=l>9`mBf`b%4JnhTT zY8{ltR^$A2#u3leg@R2r38WDh%_K|@NS2z}>|k0cm`CrP@}B|jEk59Vo3*i|&!o%qbu++NMiEW~1OHS!tSfuSF1F3-}0 zTf|PhVd51C#?>cbp)27L!Tc1|QfjhTwcFlZtA|}!caQZ>Ddc%Z#;Xm6Gr@OsKPU?q7*W#MMoDMpJ|cD9;`lJz$_5IRYWLnI_}<= z(SS)FnyX^FW-5_LQoqAgIHC$Vy6EHU=G^*$bE7 zjKj+26s1HitPRxM!x*D4T00&tpJVg8L~^>0F=BH|q&;KgXR}Lxw^@>Iuy*mX;}N_} zaDug9z{pb4LyV^=wauckbUp6bcE@2sSIdsV!OKUME;H%#t4EfiHhL`?Mt(y3)vIC0 zsnz4#OG9ap06XGSQ+7(3!oP<$<-<5{a+pD(3PE&r?Smiytx^t|z7E2^<*LZqsdk0mOc zN;Y8mq3*nZqvs}Ocza$D zcK0>h#fxxl?1XO)Mtfn64vb2cAW`U|oIeD=9Ba^WT#P5*&E}R{kUJ*>hp)%2VINob z^y|#+im^yx$&cRCtWJimY8Is@Ym+VJCqtagO>mPMZZFtMM}Er!Z2`R#6X9CqWLq~A zr5j36F@!z5ORWCvIG^m9mdIT7Rvup9NyG@L<-5Y-nl$6}8PE zF)No`VTfh;zV74)bIoL^%|}G_w&qyvcAyl`n6~EkbY{hWSsOuop2@;)ggr)%jLsV$ zP5U5S$lIENcCh|3-eYM71l#q*1cU<=qzwa6cY^+UU3*uGcezkBz4pzRBUDOv=N!Y- zBNVq`yr~F>an!OwRV%vmp`MHo2DQn7yrX6mrrof#TxBI=E@+a3h%qH?7+Vb={JH87 z>L>M4Z#=b9f%+aqW<#@VXp(ijQ#P$>b7pgT-ef0VSh7y&>FJf{bKg`ly|=TLhEcs)GyGODT4vVD;yd?cOf; z60Ys-8s*s#GNt-Me5N}w8mfQ2AWFjLjz^jBLJz$TU#y6*Oa-jyLzjI$7qg0B7hl5y z#z7L5Bm+E1`0FH;1c`>P7>R}-w5Tq-e$nwLYp*9U^`2q*x+}qMB)m}7=$&j*n!;Va zuX{D?6GF2_nSEIXWKgnh z+T>aeUZncd)b~>J9Kgvl+hB4GG z%BH5VF-slq`LI`&d*P^wrzkzS_f8Y7NefHv58r~Yv+sJ94LU|NcWlhB9^0#`OQlqr zOL(bRx%?dQ8LX;Z$)2w}XcES_BhFAADjipI9N$@`>t3o7F{^l`o2$gzv(qhj%7b7S zAxl7reGVqPPW5iupM}F+41d^Re;_X!Ul`H)Iuq)WEzlg4b1YEwX1nl6U(`k@Q8wq` z8xl8`Fti%p%6FNo`KabPOmhE@AH|JY0^n_g>;sM$CZElPZ3zz_1fSzTU6)Kkv}^EF z?)Y+t^!aq{wC%L4_1xau%WxUdsWaKfg!&NZdN&E|z02BUcS3&oSknou6-w?RKFMX6 zDy#MC^**_feN^fh3?;!j@21qeoFMbNmRS2m@RMXcuo{)AK!&saLc_fWpFpeub{+76 zFyb)P@nFNjGykz_GxU8n!{}j0Z=8weLpPVTfP`cyZN!MqUcuyGSOiUh8&&6mZN{dJ zWYd@-?g)}xp(J4~6xJK9Wa+i7UnYp=G31kIo)4DZ<3}I|vf)K&$!42KMenV+b7_(< zP6Wnhl68@=c(D2O=tIveJfM$>IlZxsUC6gp!5vmC4WY^et7T6A+?;a?pJSAH=)A3e zrv9MwP`%jZMcc9XJ9YmCBd!aOda=m7U(XH}&cu}-d!@`u{BF#9z*UfS~Gq(a+z@|Ig!qhcj-yPzDD3*X{8i9)e+isLP|nFS0+E<$%dM zaR>E~{JHS!FR@@nw(sYu|NVd9@+%N~UY)tc@}Iw?3v#2ye98a#`o9nQ?~MM(BKvoZ z{%cJC@LHPxx~G5r=zoVG@ZtX>GR4GGR6`&TX=VAF_y3M_|EF+zObzl2$}`ec+TcbI NWd(Kl!dvEn{{=+dY1se( literal 0 HcmV?d00001 From 644255c261e5a4369332928082b314fe48824f1e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 16 Mar 2023 15:58:31 +0000 Subject: [PATCH 16/58] style(pre-commit): autofix --- .../README.md | 40 +++++----- .../detected_objects_display.hpp | 2 +- .../object_polygon_detail.hpp | 36 ++++----- .../object_polygon_display_base.hpp | 28 +++---- .../predicted_objects_display.hpp | 7 +- .../tracked_objects_display.hpp | 7 +- .../package.xml | 14 ++-- .../detected_objects_display.cpp | 9 +-- .../object_polygon_detail.cpp | 80 +++++++++---------- .../predicted_objects_display.cpp | 11 ++- .../tracked_objects_display.cpp | 9 +-- 11 files changed, 117 insertions(+), 126 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/README.md b/common/autoware_auto_perception_rviz_plugin/README.md index efe5797a8dae..5694d856ec3b 100644 --- a/common/autoware_auto_perception_rviz_plugin/README.md +++ b/common/autoware_auto_perception_rviz_plugin/README.md @@ -20,15 +20,15 @@ Example: #### Input Types | Name | Type | Description | -| ---- | ----------------------------------------------------- | ---------------------- | +| ---- | ----------------------------------------------------- | ------------------------- | | | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection result array | | | `sensor_msgs::msg::PointCloud2` | point cloud for filtering | #### Output Types -| Name | Type | Description | -| ---- | ----------------------------------------------------- | ---------------------- | -| | `sensor_msgs::msg::PointCloud2` | detected objects point cloud | +| Name | Type | Description | +| ---- | ------------------------------- | ---------------------------- | +| | `sensor_msgs::msg::PointCloud2` | detected objects point cloud | #### Visualization Result @@ -38,17 +38,16 @@ Example: #### Input Types -| Name | Type | Description | -| ---- | ---------------------------------------------------- | --------------------- | -| | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking result array | -| | `sensor_msgs::msg::PointCloud2` | point cloud for filtering | +| Name | Type | Description | +| ---- | ---------------------------------------------------- | ------------------------- | +| | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking result array | +| | `sensor_msgs::msg::PointCloud2` | point cloud for filtering | #### Output Types -| Name | Type | Description | -| ---- | ----------------------------------------------------- | ---------------------- | -| | `sensor_msgs::msg::PointCloud2` | tracked objects point cloud | - +| Name | Type | Description | +| ---- | ------------------------------- | --------------------------- | +| | `sensor_msgs::msg::PointCloud2` | tracked objects point cloud | #### Visualization Result @@ -60,17 +59,16 @@ Overwrite tracking results with detection results. #### Input Types -| Name | Type | Description | -| ---- | ------------------------------------------------------ | ----------------------- | -| | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array | -| | `sensor_msgs::msg::PointCloud2` | pointcloud for filtering | +| Name | Type | Description | +| ---- | ------------------------------------------------------ | ------------------------ | +| | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array | +| | `sensor_msgs::msg::PointCloud2` | pointcloud for filtering | #### Output Types -| Name | Type | Description | -| ---- | ----------------------------------------------------- | ---------------------- | -| | `sensor_msgs::msg::PointCloud2` | predicted objects pointcloud | - +| Name | Type | Description | +| ---- | ------------------------------- | ---------------------------- | +| | `sensor_msgs::msg::PointCloud2` | predicted objects pointcloud | #### Visualization Result @@ -80,7 +78,7 @@ Overwrite prediction results with tracking results. ### Visualization with active point cloud publishing -Publishing colored point clouds. With colors according to different classes of detected objects, same as polygons. +Publishing colored point clouds. With colors according to different classes of detected objects, same as polygons. ![visualization-with-pointcloud](./images/3d_pointcloud.png) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index fef8b9cfcddd..b225734a7081 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -28,7 +28,7 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize DetectedObjects class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay - : public ObjectPolygonDisplayBase +: public ObjectPolygonDisplayBase { Q_OBJECT diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp index 4e796c06ab16..5d99b8a46371 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp @@ -56,20 +56,20 @@ struct ObjectPropertyValues // Map defining colors according to value of label field in ObjectClassification msg const std::map< autoware_auto_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> -// Color map is based on cityscapes color -kDefaultObjectPropertyValues = { - {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, - {"UNKNOWN", {255, 255, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, - {"PEDESTRIAN", {255, 192, 203}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, - {"MOTORCYCLE", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER, - {"TRAILER", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; + // Color map is based on cityscapes color + kDefaultObjectPropertyValues = { + {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, + {"UNKNOWN", {255, 255, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + {"PEDESTRIAN", {255, 192, 203}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + {"MOTORCYCLE", {119, 11, 32}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER, + {"TRAILER", {30, 144, 255}}}, + {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param shape_msg Shape msg to be converted. Corners should be in object-local frame @@ -209,14 +209,14 @@ inline AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose init /// \param labels List of ObjectClassificationMsg objects /// \param logger_name Name to use for logger in case of a warning (if labels is empty) /// \return Id of the best classification, Unknown if there is no best label -template +template AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC -autoware_auto_perception_msgs::msg::ObjectClassification::_label_type -get_best_label(ClassificationContainerT labels, const std::string & logger_name) + autoware_auto_perception_msgs::msg::ObjectClassification::_label_type + get_best_label(ClassificationContainerT labels, const std::string & logger_name) { const auto best_class_label = std::max_element( labels.begin(), labels.end(), - [](const auto & a, const auto & b) -> bool {return a.probability < b.probability;}); + [](const auto & a, const auto & b) -> bool { return a.probability < b.probability; }); if (best_class_label == labels.end()) { RCLCPP_WARN( rclcpp::get_logger(logger_name), diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 3c5b59d7fb65..d820f5f5f711 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -106,9 +106,9 @@ inline pcl::PointXYZRGB toPCL(const geometry_msgs::msg::Point & point) /// for the plugin and also defines common helper functions that can be used by its derived /// classes. /// \tparam MsgT PredictedObjects or TrackedObjects or DetectedObjects type -template +template class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase - : public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { public: using Color = std::array; @@ -202,7 +202,7 @@ class AUTOWARE_AUTO_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); } void reset() override { @@ -210,7 +210,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_marker_common.clearMarkers(); } - void clear_markers() {m_marker_common.clearMarkers();} + void clear_markers() { m_marker_common.clearMarkers(); } void add_marker(visualization_msgs::msg::Marker::ConstSharedPtr marker_ptr) { @@ -283,7 +283,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase /// \param labels List of ObjectClassificationMsg objects /// \param line_width Line thickness around the object /// \return Marker ptr. Id and header will have to be set by the caller - template + template std::optional get_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, @@ -300,7 +300,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - template + template visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, @@ -311,7 +311,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase /// \param centroid Centroid position of the shape in Object.header.frame_id frame /// \param labels List of ObjectClassificationMsg objects /// \return Marker ptr. Id and header will have to be set by the caller - template + template std::optional get_label_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const ClassificationContainerT & labels) const @@ -325,7 +325,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - template + template std::optional get_uuid_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, const geometry_msgs::msg::Point & centroid, const ClassificationContainerT & labels) const @@ -349,7 +349,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - template + template std::optional get_velocity_text_marker_ptr( const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos, const ClassificationContainerT & labels) const @@ -362,7 +362,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - template + template std::optional get_acceleration_text_marker_ptr( const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos, const ClassificationContainerT & labels) const @@ -420,7 +420,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase /// \param labels list of classifications /// \return Color and alpha for the best class in the given list. Unknown class is used in /// degenerate cases - template + template std_msgs::msg::ColorRGBA get_color_rgba(const ClassificationContainerT & labels) const { static const std::string kLoggerName("ObjectPolygonDisplayBase"); @@ -436,7 +436,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase /// \tparam ClassificationContainerT Container of ObjectClassification /// \param labels list of classifications /// \return best label string - template + template std::string get_best_label(const ClassificationContainerT & labels) const { static const std::string kLoggerName("ObjectPolygonDisplayBase"); @@ -461,7 +461,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase get_color_from_uuid(const std::string & uuid) const { int i = (static_cast(uuid.at(0)) * 4 + static_cast(uuid.at(1))) % - static_cast(predicted_path_colors.size()); + static_cast(predicted_path_colors.size()); std_msgs::msg::ColorRGBA color; color.r = predicted_path_colors.at(i).r; @@ -507,7 +507,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase colors.push_back(sample_color); // spring green } - double get_line_width() {return m_line_width_property.getFloat();} + double get_line_width() { return m_line_width_property.getFloat(); } // helper function to get radius for kd-search std::optional getMaxRadius(object_info & object) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index fb973b2466b8..5f47d12a8da7 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -37,7 +37,7 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize PredictedObjects class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay - : public ObjectPolygonDisplayBase +: public ObjectPolygonDisplayBase { Q_OBJECT @@ -72,8 +72,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay auto itr = id_map.begin(); while (itr != id_map.end()) { if ( - std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) - { + std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) { unused_marker_ids.push_back(itr->second); itr = id_map.erase(itr); } else { @@ -126,7 +125,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay std::vector markers; typedef message_filters::sync_policies::ApproximateTime< - PredictedObjects, sensor_msgs::msg::PointCloud2> + PredictedObjects, sensor_msgs::msg::PointCloud2> SyncPolicy; typedef message_filters::Synchronizer Sync; typename std::shared_ptr sync_ptr; diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 24b8bf9621a8..4aa7cf6d497f 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -35,7 +35,7 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize TrackedObjects class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay - : public ObjectPolygonDisplayBase +: public ObjectPolygonDisplayBase { Q_OBJECT @@ -70,8 +70,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay auto itr = id_map.begin(); while (itr != id_map.end()) { if ( - std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) - { + std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) { unused_marker_ids.push_back(itr->second); itr = id_map.erase(itr); } else { @@ -105,7 +104,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay int32_t marker_id = 0; typedef message_filters::sync_policies::ApproximateTime< - TrackedObjects, sensor_msgs::msg::PointCloud2> + TrackedObjects, sensor_msgs::msg::PointCloud2> SyncPolicy; typedef message_filters::Synchronizer Sync; typename std::shared_ptr sync_ptr; diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index d1ea3fc9d9d5..f9c0a3899348 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -17,23 +17,21 @@ libboost-dev qtbase5-dev + PCL autoware_auto_perception_msgs + pcl_common + pcl_conversions + perception_utils rviz_common rviz_default_plugins sensor_msgs - PCL - pcl_conversions tf2 + tf2_eigen + tf2_geometry_msgs tf2_ros tf2_sensor_msgs - tf2_geometry_msgs - tf2_eigen tier4_autoware_utils tier4_autoware_utils/math - perception_utils - pcl_common - - libqt5-widgets rviz2 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 31f20e43a02a..eb476e9b31b9 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 @@ -100,9 +100,9 @@ void DetectedObjectsDisplay::onInitialize() percepted_objects_subscription.subscribe( raw_node, "/perception/object_recognition/detection/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription.subscribe( - raw_node, m_default_pointcloud_topic->getTopic().toStdString(), - rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + pointcloud_subscription.subscribe( + raw_node, m_default_pointcloud_topic->getTopic().toStdString(), + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( @@ -115,8 +115,7 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( // Transform to pointcloud frame autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; if (!transformObjects( - *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) - { + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { // objects_pub_->publish(*input_objects); return; } 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 74b2783f7e80..73a524e13077 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 @@ -487,23 +487,23 @@ void calc_cylinder_line_list( for (int i = 0; i < n; ++i) { geometry_msgs::msg::Point point; point.x = std::cos( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius; + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; point.y = std::sin( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius; + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; point.z = shape.dimensions.z * 0.5; points.push_back(point); point.x = std::cos( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius; + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; point.y = std::sin( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius; + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; point.z = -shape.dimensions.z * 0.5; points.push_back(point); } @@ -532,27 +532,27 @@ void calc_circle_line_list( for (int i = 0; i < n; ++i) { geometry_msgs::msg::Point point; point.x = std::cos( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius + - center.x; + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.x; point.y = std::sin( - (static_cast(i) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius + - center.y; + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.y; point.z = center.z; points.push_back(point); point.x = std::cos( - (static_cast(i + 1.0) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius + - center.x; + (static_cast(i + 1.0) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.x; point.y = std::sin( - (static_cast(i + 1.0) / static_cast(n)) * 2.0 * M_PI + - M_PI / static_cast(n)) * - radius + - center.y; + (static_cast(i + 1.0) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius + + center.y; point.z = center.z; points.push_back(point); } @@ -575,11 +575,11 @@ void calc_polygon_line_list( point.z = shape.dimensions.z / 2.0; points.push_back(point); point.x = shape.footprint.points - .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) - .x; + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; point.y = shape.footprint.points - .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) - .y; + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; point.z = shape.dimensions.z / 2.0; points.push_back(point); } @@ -590,11 +590,11 @@ void calc_polygon_line_list( point.z = -shape.dimensions.z / 2.0; points.push_back(point); point.x = shape.footprint.points - .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) - .x; + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; point.y = shape.footprint.points - .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) - .y; + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; point.z = -shape.dimensions.z / 2.0; points.push_back(point); } @@ -628,11 +628,11 @@ void calc_2d_polygon_bottom_line_list( point.z = -shape.dimensions.z / 2.0; points.push_back(point); point.x = shape.footprint.points - .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) - .x; + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; point.y = shape.footprint.points - .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) - .y; + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; point.z = -shape.dimensions.z / 2.0; points.push_back(point); } 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 1a30a8eccfb8..a5d79bfd31f3 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 @@ -33,7 +33,7 @@ void PredictedObjectsDisplay::workerThread() { while (true) { std::unique_lock lock(mutex); - condition.wait(lock, [this] {return this->msg;}); + condition.wait(lock, [this] { return this->msg; }); auto tmp_msg = this->msg; this->msg.reset(); @@ -225,9 +225,9 @@ void PredictedObjectsDisplay::onInitialize() percepted_objects_subscription.subscribe( raw_node, "/perception/object_recognition/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription.subscribe( - raw_node, m_default_pointcloud_topic->getTopic().toStdString(), - rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + pointcloud_subscription.subscribe( + raw_node, m_default_pointcloud_topic->getTopic().toStdString(), + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } bool PredictedObjectsDisplay::transformObjects( @@ -270,8 +270,7 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( // Transform to pointcloud frame autoware_auto_perception_msgs::msg::PredictedObjects transformed_objects; if (!transformObjects( - *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) - { + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { return; } 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 a56e1d93bce8..1144c8825aa9 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 @@ -141,9 +141,9 @@ void TrackedObjectsDisplay::onInitialize() percepted_objects_subscription.subscribe( raw_node, "/perception/object_recognition/tracking/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription.subscribe( - raw_node, m_default_pointcloud_topic->getTopic().toStdString(), - rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + pointcloud_subscription.subscribe( + raw_node, m_default_pointcloud_topic->getTopic().toStdString(), + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( @@ -156,8 +156,7 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( // Transform to pointcloud frame autoware_auto_perception_msgs::msg::TrackedObjects transformed_objects; if (!transformObjects( - *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) - { + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { return; } From 888baaf41c45f46a68796efa6ba89d9882319324 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Fri, 17 Mar 2023 14:36:06 +0800 Subject: [PATCH 17/58] feat(add_perception_objects_pointcloud_publisher) fix pre-commit.ci - pr error Signed-off-by: Alexey Panferov --- .../include/object_detection/detected_objects_display.hpp | 2 ++ .../object_detection/object_polygon_display_base.hpp | 6 +++--- .../include/object_detection/predicted_objects_display.hpp | 1 + .../include/object_detection/tracked_objects_display.hpp | 1 + 4 files changed, 7 insertions(+), 3 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index fef8b9cfcddd..468d9d014dac 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -20,6 +20,8 @@ #include #include +#include + namespace autoware { namespace rviz_plugins diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 3c5b59d7fb65..0428549622be 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -561,9 +561,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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, (int)floor(color_rgba.r * 256.0))); - cloud_it->g = std::max(0, std::min(255, (int)floor(color_rgba.g * 256.0))); - cloud_it->b = std::max(0, std::min(255, (int)floor(color_rgba.b * 256.0))); + 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; diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index fb973b2466b8..6376ddcca2c4 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 24b8bf9621a8..f758aba3a879 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include From c55e0e37c036689631b0b9788eca94ca9d520bd6 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Fri, 17 Mar 2023 15:03:12 +0800 Subject: [PATCH 18/58] feat(add_perception_objects_pointcloud_publisher) fix typos Signed-off-by: Alexey Panferov --- .../object_detection/object_polygon_display_base.hpp | 8 ++++---- .../src/object_detection/detected_objects_display.cpp | 6 +++--- .../src/object_detection/predicted_objects_display.cpp | 8 ++++---- .../src/object_detection/tracked_objects_display.cpp | 6 +++--- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 139cac049a18..3f140f743784 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -157,7 +157,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase "Publish Objects Pointcloud", true, "Enable/disable objects pointcloud publishing", this); m_default_pointcloud_topic = new rviz_common::properties::RosTopicProperty( "Input pointcloud topic", QString::fromStdString(default_pointcloud_topic), "", - "Input for pointcloud visualization of Objectcs detection pipeline", this, + "Input for pointcloud visualization of objects detection pipeline.", this, SLOT(updatePalette())); m_default_pointcloud_topic->setReadOnly(true); // iterate over default values to create and initialize the properties. @@ -188,7 +188,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase this->topic_property_->setValue(m_default_topic.c_str()); this->topic_property_->setDescription("Topic to subscribe to."); - // get access to rivz node to sub and to pub to topics + // get access to rviz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); @@ -222,7 +222,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_marker_common.addMessage(markers_ptr); } - // transfrom detected object pose to target frame and return bool result + // 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) @@ -575,7 +575,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rviz_common::properties::BoolProperty * m_publish_objs_pointcloud; rclcpp::Publisher::SharedPtr publisher; - message_filters::Subscriber percepted_objects_subscription; + message_filters::Subscriber perception_objects_subscription; message_filters::Subscriber pointcloud_subscription; private: 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 eb476e9b31b9..23ed5a9f5caf 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 @@ -95,9 +95,9 @@ void DetectedObjectsDisplay::onInitialize() "~/output/detected_objects_pointcloud", rclcpp::SensorDataQoS()); sync_ptr = - std::make_shared(SyncPolicy(10), percepted_objects_subscription, pointcloud_subscription); + std::make_shared(SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); sync_ptr->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - percepted_objects_subscription.subscribe( + perception_objects_subscription.subscribe( raw_node, "/perception/object_recognition/detection/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), pointcloud_subscription.subscribe( @@ -134,7 +134,7 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud); - // Create a new point cloud with RGB color information and copy data from input cloudb + // 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); 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 a5d79bfd31f3..6c1a5131f68a 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 @@ -214,16 +214,16 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) void PredictedObjectsDisplay::onInitialize() { ObjectPolygonDisplayBase::onInitialize(); - // get access to rivz node to sub and to pub to topics + // get access to rviz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); publisher = raw_node->create_publisher( "~/output/predicted_objects_pointcloud", rclcpp::SensorDataQoS()); sync_ptr = - std::make_shared(SyncPolicy(10), percepted_objects_subscription, pointcloud_subscription); + std::make_shared(SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); sync_ptr->registerCallback(&PredictedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - percepted_objects_subscription.subscribe( + perception_objects_subscription.subscribe( raw_node, "/perception/object_recognition/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), pointcloud_subscription.subscribe( raw_node, m_default_pointcloud_topic->getTopic().toStdString(), @@ -288,7 +288,7 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud); - // Create a new point cloud with RGB color information and copy data from input cloudb + // 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); 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 1144c8825aa9..412aabe075fb 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 @@ -135,10 +135,10 @@ void TrackedObjectsDisplay::onInitialize() "~/output/tracked_objects_pointcloud", rclcpp::SensorDataQoS()); sync_ptr = - std::make_shared(SyncPolicy(10), percepted_objects_subscription, pointcloud_subscription); + std::make_shared(SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); sync_ptr->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - percepted_objects_subscription.subscribe( + perception_objects_subscription.subscribe( raw_node, "/perception/object_recognition/tracking/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), pointcloud_subscription.subscribe( @@ -174,7 +174,7 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud); - // Create a new point cloud with RGB color information and copy data from input cloudb + // 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); From 313d9bd94322cc141448c9b171d7381b689347cb Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Fri, 17 Mar 2023 15:19:21 +0800 Subject: [PATCH 19/58] feat(add_perception_objects_pointcloud_publisher) dependence fix Signed-off-by: Alexey Panferov --- common/autoware_auto_perception_rviz_plugin/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index f9c0a3899348..14219cbcfe35 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -31,7 +31,8 @@ tf2_ros tf2_sensor_msgs tier4_autoware_utils - tier4_autoware_utils/math + + libqt5-widgets rviz2 From 38e11040a77eba7a007ef0c80ab4ef108dcd0f24 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 17 Mar 2023 07:40:16 +0000 Subject: [PATCH 20/58] style(pre-commit): autofix --- common/autoware_auto_perception_rviz_plugin/package.xml | 2 -- .../src/object_detection/detected_objects_display.cpp | 4 ++-- .../src/object_detection/predicted_objects_display.cpp | 4 ++-- .../src/object_detection/tracked_objects_display.cpp | 4 ++-- 4 files changed, 6 insertions(+), 8 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index 14219cbcfe35..06c9d0e2f6f8 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -32,8 +32,6 @@ tf2_sensor_msgs tier4_autoware_utils - - libqt5-widgets rviz2 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 23ed5a9f5caf..2b9030dc90bf 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 @@ -94,8 +94,8 @@ void DetectedObjectsDisplay::onInitialize() publisher = raw_node->create_publisher( "~/output/detected_objects_pointcloud", rclcpp::SensorDataQoS()); - sync_ptr = - std::make_shared(SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); + sync_ptr = std::make_shared( + SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); sync_ptr->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); perception_objects_subscription.subscribe( raw_node, "/perception/object_recognition/detection/objects", 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 6c1a5131f68a..bbbefe0c1d55 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 @@ -219,8 +219,8 @@ void PredictedObjectsDisplay::onInitialize() publisher = raw_node->create_publisher( "~/output/predicted_objects_pointcloud", rclcpp::SensorDataQoS()); - sync_ptr = - std::make_shared(SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); + sync_ptr = std::make_shared( + SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); sync_ptr->registerCallback(&PredictedObjectsDisplay::onObjectsAndObstaclePointCloud, this); perception_objects_subscription.subscribe( 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 412aabe075fb..01620c8a229c 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 @@ -134,8 +134,8 @@ void TrackedObjectsDisplay::onInitialize() publisher = raw_node->create_publisher( "~/output/tracked_objects_pointcloud", rclcpp::SensorDataQoS()); - sync_ptr = - std::make_shared(SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); + sync_ptr = std::make_shared( + SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); sync_ptr->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); perception_objects_subscription.subscribe( From f5a4e2e666a4a5ca5ac6aedb5cc4aca2d8ba9a6e Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Fri, 17 Mar 2023 17:10:44 +0800 Subject: [PATCH 21/58] feat(add_perception_objects_pointcloud_publisher) dependency fix Signed-off-by: Alexey Panferov --- common/autoware_auto_perception_rviz_plugin/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index 14219cbcfe35..fd57baf99375 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -19,7 +19,7 @@ PCL autoware_auto_perception_msgs - pcl_common + pcl_conversions perception_utils rviz_common From 50477ebcf3d7f9ad73e81b48cd7e937884c3e9a0 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Fri, 17 Mar 2023 17:13:44 +0800 Subject: [PATCH 22/58] feat(add_perception_objects_pointcloud_publisher) typo fix Signed-off-by: Alexey Panferov --- .../src/object_detection/detected_objects_display.cpp | 2 +- .../src/object_detection/tracked_objects_display.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 2b9030dc90bf..cde1e194f18f 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 @@ -89,7 +89,7 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) void DetectedObjectsDisplay::onInitialize() { ObjectPolygonDisplayBase::onInitialize(); - // get access to rivz node to sub and to pub to topics + // get access to rviz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); publisher = raw_node->create_publisher( "~/output/detected_objects_pointcloud", rclcpp::SensorDataQoS()); 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 01620c8a229c..1cf6470c33e0 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 @@ -129,7 +129,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) void TrackedObjectsDisplay::onInitialize() { ObjectPolygonDisplayBase::onInitialize(); - // get access to rivz node to sub and to pub to topics + // get access to rviz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); publisher = raw_node->create_publisher( "~/output/tracked_objects_pointcloud", rclcpp::SensorDataQoS()); From c62b202a3065b78e9b22e4de64b0c80ba7c309a3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 17 Mar 2023 09:17:05 +0000 Subject: [PATCH 23/58] style(pre-commit): autofix --- common/autoware_auto_perception_rviz_plugin/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index c9506d2f5d21..9ca00aea68fa 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -19,7 +19,6 @@ PCL autoware_auto_perception_msgs - pcl_conversions perception_utils rviz_common @@ -31,6 +30,7 @@ tf2_ros tf2_sensor_msgs tier4_autoware_utils + libqt5-widgets rviz2 From 9bd2019a3a5052eb975b6791dba1ee343e9bd90a Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Fri, 17 Mar 2023 18:44:19 +0800 Subject: [PATCH 24/58] feat(add_perception_objects_pointcloud_publisher) pcl depend fix Signed-off-by: Alexey Panferov --- common/autoware_auto_perception_rviz_plugin/CMakeLists.txt | 7 +++++++ common/autoware_auto_perception_rviz_plugin/package.xml | 2 -- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt b/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt index dd5f88d4caae..8f8c465f1fc9 100644 --- a/common/autoware_auto_perception_rviz_plugin/CMakeLists.txt +++ b/common/autoware_auto_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_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index c9506d2f5d21..f4297b5842d8 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -17,9 +17,7 @@ libboost-dev qtbase5-dev - PCL autoware_auto_perception_msgs - pcl_conversions perception_utils rviz_common From 708178647018a6c85a86fa5a468df164690b59cd Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 21 Mar 2023 00:22:56 +0800 Subject: [PATCH 25/58] feat(add_perception_objects_pointcloud_publisher) wip add point_cloud_common added point_cloud_common member like in pointcloud_plugin to render pointclouds, add test launch file Signed-off-by: Alexey Panferov --- .../object_detection/object_polygon_display_base.hpp | 9 ++++++++- .../launch/rviz.launch.xml | 5 +++++ .../src/object_detection/detected_objects_display.cpp | 3 ++- .../src/object_detection/predicted_objects_display.cpp | 3 ++- .../src/object_detection/tracked_objects_display.cpp | 3 ++- 5 files changed, 19 insertions(+), 4 deletions(-) create mode 100644 common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 3f140f743784..4256d41018e2 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -26,6 +26,8 @@ #include // #include +#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" // added + #include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" #include "rviz_common/properties/ros_topic_property.hpp" @@ -122,7 +124,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase explicit ObjectPolygonDisplayBase( const std::string & default_topic, const std::string & default_pointcloud_topic) - : m_marker_common(this), + : point_cloud_common_(new rviz_default_plugins::PointCloudCommon(this)), // added + 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_pose_with_covariance_property{ @@ -194,6 +197,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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 @@ -578,6 +583,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase message_filters::Subscriber perception_objects_subscription; message_filters::Subscriber pointcloud_subscription; + std::unique_ptr point_cloud_common_; + private: // All rviz plugins should have this. Should be initialized with pointer to this class MarkerCommon m_marker_common; diff --git a/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml b/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml new file mode 100644 index 000000000000..e89506d830c1 --- /dev/null +++ b/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file 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 cde1e194f18f..1da253aae8e6 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 @@ -172,7 +172,8 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - publisher->publish(*output_pointcloud_msg_ptr); + // publisher->publish(*output_pointcloud_msg_ptr); + point_cloud_common_->addMessage(output_pointcloud_msg_ptr); } } // namespace object_detection 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 bbbefe0c1d55..e8e8217b1b8e 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 @@ -326,7 +326,8 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - publisher->publish(*output_pointcloud_msg_ptr); + // publisher->publish(*output_pointcloud_msg_ptr); + point_cloud_common_->addMessage(output_pointcloud_msg_ptr); } } // namespace object_detection 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 1cf6470c33e0..172f028030c0 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 @@ -212,7 +212,8 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - publisher->publish(*output_pointcloud_msg_ptr); + // publisher->publish(*output_pointcloud_msg_ptr); + point_cloud_common_->addMessage(output_pointcloud_msg_ptr); } } // namespace object_detection From 2a5cee79170a5d0a65e9ed4273ab97777f359897 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 21 Mar 2023 15:41:01 +0800 Subject: [PATCH 26/58] feat(add_perception_objects_pointcloud_publisher) update add_pointcloud Signed-off-by: Alexey Panferov --- .../object_detection/object_polygon_display_base.hpp | 11 ++++++++++- .../src/object_detection/detected_objects_display.cpp | 3 ++- .../object_detection/predicted_objects_display.cpp | 2 +- .../src/object_detection/tracked_objects_display.cpp | 3 ++- 4 files changed, 15 insertions(+), 4 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 4256d41018e2..b3799833e97e 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -207,12 +207,16 @@ class AUTOWARE_AUTO_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 { RosTopicDisplay::reset(); m_marker_common.clearMarkers(); + point_cloud_common_->reset(); } void clear_markers() { m_marker_common.clearMarkers(); } @@ -227,6 +231,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_marker_common.addMessage(markers_ptr); } + 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, 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 1da253aae8e6..679abc2f874d 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 @@ -173,7 +173,8 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; // publisher->publish(*output_pointcloud_msg_ptr); - point_cloud_common_->addMessage(output_pointcloud_msg_ptr); + // point_cloud_common_->addMessage(output_pointcloud_mbsg_ptr); + add_pointcloud(output_pointcloud_msg_ptr); } } // namespace object_detection 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 e8e8217b1b8e..0f5bf5e6fdf0 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 @@ -327,7 +327,7 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; // publisher->publish(*output_pointcloud_msg_ptr); - point_cloud_common_->addMessage(output_pointcloud_msg_ptr); + add_pointcloud(output_pointcloud_msg_ptr); } } // namespace object_detection 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 172f028030c0..e3686a56861c 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 @@ -213,7 +213,8 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; // publisher->publish(*output_pointcloud_msg_ptr); - point_cloud_common_->addMessage(output_pointcloud_msg_ptr); + // point_cloud_common_->addMessage(output_pointcloud_msg_ptr); + add_pointcloud(output_pointcloud_msg_ptr); } } // namespace object_detection From e30454004be86932130cbefb6d2d494c94f22f52 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 21 Mar 2023 23:06:14 +0800 Subject: [PATCH 27/58] feat(add_perception_objects_pointcloud_publisher) add proper centerpoint_tiny config Signed-off-by: Alexey Panferov --- perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml index 92db4cd0b853..cff6d8aa7f21 100644 --- a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml @@ -3,7 +3,7 @@ class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] point_feature_size: 4 max_voxel_size: 40000 - point_cloud_range: [-76.8, -76.8, -2.0, 76.8, 76.8, 4.0] + point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0] voxel_size: [0.32, 0.32, 6.0] downsample_factor: 2 encoder_in_feature_size: 9 From 17e5849b041daf2411aeda69202434357e0a02d9 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 21 Mar 2023 15:16:33 +0000 Subject: [PATCH 28/58] style(pre-commit): autofix --- .../object_detection/object_polygon_display_base.hpp | 12 ++++++------ .../launch/rviz.launch.xml | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index b3799833e97e..ee68b364af74 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -26,11 +26,10 @@ #include // #include -#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" // added - #include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" #include "rviz_common/properties/ros_topic_property.hpp" +#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" // added #include #include @@ -124,7 +123,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase explicit ObjectPolygonDisplayBase( const std::string & default_topic, const std::string & default_pointcloud_topic) - : point_cloud_common_(new rviz_default_plugins::PointCloudCommon(this)), // added + : point_cloud_common_(new rviz_default_plugins::PointCloudCommon(this)), // added 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}, @@ -207,8 +206,9 @@ class AUTOWARE_AUTO_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); } @@ -231,7 +231,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_marker_common.addMessage(markers_ptr); } - void add_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud) + void add_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud) { point_cloud_common_->addMessage(cloud); } diff --git a/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml b/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml index e89506d830c1..400035578da9 100644 --- a/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml +++ b/common/autoware_auto_perception_rviz_plugin/launch/rviz.launch.xml @@ -1,5 +1,5 @@ - + - \ No newline at end of file + From c534358bd555149c7cc5739bfb498f3e75eef89c Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Wed, 22 Mar 2023 13:29:27 +0800 Subject: [PATCH 29/58] feat(add_perception_objects_pointcloud_publisher) update readme Signed-off-by: Alexey Panferov --- .../README.md | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/README.md b/common/autoware_auto_perception_rviz_plugin/README.md index 5694d856ec3b..e0adec886379 100644 --- a/common/autoware_auto_perception_rviz_plugin/README.md +++ b/common/autoware_auto_perception_rviz_plugin/README.md @@ -24,11 +24,6 @@ Example: | | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection result array | | | `sensor_msgs::msg::PointCloud2` | point cloud for filtering | -#### Output Types - -| Name | Type | Description | -| ---- | ------------------------------- | ---------------------------- | -| | `sensor_msgs::msg::PointCloud2` | detected objects point cloud | #### Visualization Result @@ -43,11 +38,6 @@ Example: | | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking result array | | | `sensor_msgs::msg::PointCloud2` | point cloud for filtering | -#### Output Types - -| Name | Type | Description | -| ---- | ------------------------------- | --------------------------- | -| | `sensor_msgs::msg::PointCloud2` | tracked objects point cloud | #### Visualization Result @@ -64,11 +54,6 @@ Overwrite tracking results with detection results. | | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array | | | `sensor_msgs::msg::PointCloud2` | pointcloud for filtering | -#### Output Types - -| Name | Type | Description | -| ---- | ------------------------------- | ---------------------------- | -| | `sensor_msgs::msg::PointCloud2` | predicted objects pointcloud | #### Visualization Result From e87256e08e9bd1bc4d97bc320e1323da56d15ecf Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Wed, 22 Mar 2023 14:10:47 +0800 Subject: [PATCH 30/58] feat(add_perception_objects_pointcloud_publisher) code cleaning Signed-off-by: Alexey Panferov --- .../object_polygon_display_base.hpp | 21 ++++++++----------- .../detected_objects_display.cpp | 11 ++-------- .../predicted_objects_display.cpp | 12 ++++------- .../tracked_objects_display.cpp | 13 ++++-------- 4 files changed, 19 insertions(+), 38 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index b3799833e97e..f65f3d46ca51 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -25,8 +25,7 @@ #include #include -// #include -#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" // added +#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" @@ -86,7 +85,6 @@ struct object_info { Shape shape; geometry_msgs::msg::Pose position; - // autoware_auto_perception_msgs::msg::ObjectClassification classification; std::vector classification; }; @@ -124,7 +122,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase explicit ObjectPolygonDisplayBase( const std::string & default_topic, const std::string & default_pointcloud_topic) - : point_cloud_common_(new rviz_default_plugins::PointCloudCommon(this)), // added + : 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}, @@ -198,7 +196,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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_); + point_cloud_common->initialize(this->context_, this->scene_node_); } void load(const rviz_common::Config & config) override @@ -209,14 +207,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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); + point_cloud_common->update(wall_dt, ros_dt); } void reset() override { RosTopicDisplay::reset(); m_marker_common.clearMarkers(); - point_cloud_common_->reset(); + point_cloud_common->reset(); } void clear_markers() { m_marker_common.clearMarkers(); } @@ -233,7 +231,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase void add_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud) { - point_cloud_common_->addMessage(cloud); + point_cloud_common->addMessage(cloud); } // transform detected object pose to target frame and return bool result @@ -278,7 +276,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase return self_transform_stamped.transform; } catch (tf2::TransformException & ex) { RCLCPP_WARN_STREAM( - rclcpp::get_logger("autoware_auto_perception_plugin"), ex.what()); // rename + rclcpp::get_logger("autoware_auto_perception_plugin"), ex.what()); return boost::none; } } @@ -588,11 +586,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // Property to enable/disable objects pointcloud publishing rviz_common::properties::BoolProperty * m_publish_objs_pointcloud; - rclcpp::Publisher::SharedPtr publisher; message_filters::Subscriber perception_objects_subscription; message_filters::Subscriber pointcloud_subscription; - - std::unique_ptr point_cloud_common_; + // plugin to visualize pointclouds + std::unique_ptr point_cloud_common; private: // All rviz plugins should have this. Should be initialized with pointer to this class 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 679abc2f874d..48ac6647b90d 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 @@ -91,16 +91,14 @@ void DetectedObjectsDisplay::onInitialize() ObjectPolygonDisplayBase::onInitialize(); // get access to rviz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher = raw_node->create_publisher( - "~/output/detected_objects_pointcloud", rclcpp::SensorDataQoS()); sync_ptr = std::make_shared( SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); sync_ptr->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); perception_objects_subscription.subscribe( raw_node, "/perception/object_recognition/detection/objects", - rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription.subscribe( + rclcpp::QoS{1}.get_rmw_qos_profile()); + pointcloud_subscription.subscribe( raw_node, m_default_pointcloud_topic->getTopic().toStdString(), rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } @@ -116,7 +114,6 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; if (!transformObjects( *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { - // objects_pub_->publish(*input_objects); return; } @@ -131,7 +128,6 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( // convert to pcl pointcloud pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); - // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud); // Create a new point cloud with RGB color information and copy data from input cloud @@ -162,7 +158,6 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( filterPolygon(neighbor_pointcloud, out_cloud, object); } } else { - // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); return; } @@ -172,8 +167,6 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - // publisher->publish(*output_pointcloud_msg_ptr); - // point_cloud_common_->addMessage(output_pointcloud_mbsg_ptr); add_pointcloud(output_pointcloud_msg_ptr); } 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 0f5bf5e6fdf0..ed6fc4b8fafa 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 @@ -216,18 +216,16 @@ void PredictedObjectsDisplay::onInitialize() ObjectPolygonDisplayBase::onInitialize(); // get access to rviz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher = raw_node->create_publisher( - "~/output/predicted_objects_pointcloud", rclcpp::SensorDataQoS()); sync_ptr = std::make_shared( SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); sync_ptr->registerCallback(&PredictedObjectsDisplay::onObjectsAndObstaclePointCloud, this); perception_objects_subscription.subscribe( - raw_node, "/perception/object_recognition/objects", rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription.subscribe( - raw_node, m_default_pointcloud_topic->getTopic().toStdString(), - rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + raw_node, "/perception/object_recognition/objects", rclcpp::QoS{1}.get_rmw_qos_profile()); + pointcloud_subscription.subscribe( + raw_node, m_default_pointcloud_topic->getTopic().toStdString(), + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } bool PredictedObjectsDisplay::transformObjects( @@ -316,7 +314,6 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( filterPolygon(neighbor_pointcloud, out_cloud, object); } } else { - // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); return; } @@ -326,7 +323,6 @@ void PredictedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - // publisher->publish(*output_pointcloud_msg_ptr); add_pointcloud(output_pointcloud_msg_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 e3686a56861c..7bd1d43d31f5 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 @@ -131,8 +131,6 @@ void TrackedObjectsDisplay::onInitialize() ObjectPolygonDisplayBase::onInitialize(); // get access to rviz node to sub and to pub to topics rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - publisher = raw_node->create_publisher( - "~/output/tracked_objects_pointcloud", rclcpp::SensorDataQoS()); sync_ptr = std::make_shared( SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); @@ -140,10 +138,10 @@ void TrackedObjectsDisplay::onInitialize() perception_objects_subscription.subscribe( raw_node, "/perception/object_recognition/tracking/objects", - rclcpp::QoS{1}.get_rmw_qos_profile()), - pointcloud_subscription.subscribe( - raw_node, m_default_pointcloud_topic->getTopic().toStdString(), - rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + rclcpp::QoS{1}.get_rmw_qos_profile()); + pointcloud_subscription.subscribe( + raw_node, m_default_pointcloud_topic->getTopic().toStdString(), + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( @@ -202,7 +200,6 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( filterPolygon(neighbor_pointcloud, out_cloud, object); } } else { - // RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty"); return; } @@ -212,8 +209,6 @@ void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - // publisher->publish(*output_pointcloud_msg_ptr); - // point_cloud_common_->addMessage(output_pointcloud_msg_ptr); add_pointcloud(output_pointcloud_msg_ptr); } From 94fa7c94a2db486cf23c35b70711caa78ceff5ac Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 22 Mar 2023 06:16:04 +0000 Subject: [PATCH 31/58] style(pre-commit): autofix --- .../README.md | 3 --- .../object_polygon_display_base.hpp | 25 ++++++++----------- .../detected_objects_display.cpp | 4 +-- 3 files changed, 13 insertions(+), 19 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/README.md b/common/autoware_auto_perception_rviz_plugin/README.md index e0adec886379..5162044041c1 100644 --- a/common/autoware_auto_perception_rviz_plugin/README.md +++ b/common/autoware_auto_perception_rviz_plugin/README.md @@ -24,7 +24,6 @@ Example: | | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection result array | | | `sensor_msgs::msg::PointCloud2` | point cloud for filtering | - #### Visualization Result ![detected-object-visualization-description](./images/detected-object-visualization-description.jpg) @@ -38,7 +37,6 @@ Example: | | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking result array | | | `sensor_msgs::msg::PointCloud2` | point cloud for filtering | - #### Visualization Result Overwrite tracking results with detection results. @@ -54,7 +52,6 @@ Overwrite tracking results with detection results. | | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array | | | `sensor_msgs::msg::PointCloud2` | pointcloud for filtering | - #### Visualization Result Overwrite prediction results with tracking results. diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index cf3e61f477d7..0ad9f2a8971f 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -14,26 +14,23 @@ #ifndef OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ #define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" #include "rviz_common/properties/enum_property.hpp" +#include "rviz_common/properties/ros_topic_property.hpp" +#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" +#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" // added #include #include #include +#include #include #include #include #include -#include - -#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" - -#include "rclcpp/clock.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rviz_common/properties/ros_topic_property.hpp" -#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" // added - -#include #include +#include #include #include @@ -206,8 +203,9 @@ class AUTOWARE_AUTO_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); } @@ -276,8 +274,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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_auto_perception_plugin"), ex.what()); + RCLCPP_WARN_STREAM(rclcpp::get_logger("autoware_auto_perception_plugin"), ex.what()); return boost::none; } } 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 48ac6647b90d..0e09f888e000 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 @@ -99,8 +99,8 @@ void DetectedObjectsDisplay::onInitialize() raw_node, "/perception/object_recognition/detection/objects", rclcpp::QoS{1}.get_rmw_qos_profile()); pointcloud_subscription.subscribe( - raw_node, m_default_pointcloud_topic->getTopic().toStdString(), - rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); + raw_node, m_default_pointcloud_topic->getTopic().toStdString(), + rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); } void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( From 16fdef4f258c82b807f41e1baeb3f5f0dccc3053 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Wed, 22 Mar 2023 14:39:07 +0800 Subject: [PATCH 32/58] feat(add_perception_objects_pointcloud_publisher) fix ci/cd Signed-off-by: Alexey Panferov --- .../include/object_detection/object_polygon_display_base.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 0ad9f2a8971f..cfe22263556e 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -14,12 +14,10 @@ #ifndef OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ #define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ -#include "rclcpp/clock.hpp" #include "rclcpp/rclcpp.hpp" #include "rviz_common/properties/enum_property.hpp" #include "rviz_common/properties/ros_topic_property.hpp" #include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" -#include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" // added #include #include From 31af5cd548e9ecf027c897a6c38d1566298cb4f3 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Thu, 23 Mar 2023 17:31:43 +0800 Subject: [PATCH 33/58] feat(add_perception_objects_pointcloud_publisher) return state of centerpoint confing file Signed-off-by: Alexey Panferov --- perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml index cff6d8aa7f21..92db4cd0b853 100644 --- a/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml +++ b/perception/lidar_centerpoint/config/centerpoint_tiny.param.yaml @@ -3,7 +3,7 @@ class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] point_feature_size: 4 max_voxel_size: 40000 - point_cloud_range: [-89.6, -89.6, -3.0, 89.6, 89.6, 5.0] + point_cloud_range: [-76.8, -76.8, -2.0, 76.8, 76.8, 4.0] voxel_size: [0.32, 0.32, 6.0] downsample_factor: 2 encoder_in_feature_size: 9 From 4082d58311d2f7e92455f415ffc9586be6058696 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Thu, 30 Mar 2023 22:26:14 +0800 Subject: [PATCH 34/58] feat(add_perception_objects_pointcloud_publisher) revert child classes Signed-off-by: Alexey Panferov --- .../detected_objects_display.hpp | 18 +- .../object_polygon_display_base.hpp | 3 +- .../predicted_objects_display.hpp | 26 +-- .../tracked_objects_display.hpp | 18 +- .../detected_objects_display.cpp | 166 +++++++------- .../predicted_objects_display.cpp | 210 ++++++++---------- .../tracked_objects_display.cpp | 170 +++++++------- 7 files changed, 298 insertions(+), 313 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index bfe5a0402102..41ada2090e7d 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -41,15 +41,15 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay private: void processMessage(DetectedObjects::ConstSharedPtr msg) override; - void onInitialize() override; - void onObjectsAndObstaclePointCloud( - const DetectedObjects::ConstSharedPtr & input_objs_msg, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); - - using SyncPolicy = - message_filters::sync_policies::ApproximateTime; - using Sync = message_filters::Synchronizer; - std::shared_ptr sync_ptr; + // void onInitialize() override; + // void onObjectsAndObstaclePointCloud( + // const DetectedObjects::ConstSharedPtr & input_objs_msg, + // const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); + + // using SyncPolicy = + // message_filters::sync_policies::ApproximateTime; + // using Sync = message_filters::Synchronizer; + // std::shared_ptr sync_ptr; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index cfe22263556e..d5fb5809aa14 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -156,7 +156,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase "Input pointcloud topic", QString::fromStdString(default_pointcloud_topic), "", "Input for pointcloud visualization of objects detection pipeline.", this, SLOT(updatePalette())); - m_default_pointcloud_topic->setReadOnly(true); + // m_default_pointcloud_topic->setReadOnly(true); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { const auto & class_property_values = map_property_it.second; @@ -212,6 +212,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase RosTopicDisplay::reset(); m_marker_common.clearMarkers(); point_cloud_common->reset(); + m_default_pointcloud_topic->reset(); } void clear_markers() { m_marker_common.clearMarkers(); } diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index 2fc32627a93c..8c0941a57c61 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -49,7 +49,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay private: void processMessage(PredictedObjects::ConstSharedPtr msg) override; - void onInitialize() override; + // void onInitialize() override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) { @@ -104,14 +104,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay void update(float wall_dt, float ros_dt) override; - bool transformObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, - const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, - autoware_auto_perception_msgs::msg::PredictedObjects & output_msg); + // bool transformObjects( + // const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, + // const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + // autoware_auto_perception_msgs::msg::PredictedObjects & output_msg); - void onObjectsAndObstaclePointCloud( - const PredictedObjects::ConstSharedPtr & input_objs_msg, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); + // void onObjectsAndObstaclePointCloud( + // 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; @@ -125,11 +125,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay std::condition_variable condition; std::vector markers; - typedef message_filters::sync_policies::ApproximateTime< - PredictedObjects, sensor_msgs::msg::PointCloud2> - SyncPolicy; - typedef message_filters::Synchronizer Sync; - typename std::shared_ptr sync_ptr; + // typedef message_filters::sync_policies::ApproximateTime< + // PredictedObjects, sensor_msgs::msg::PointCloud2> + // SyncPolicy; + // typedef message_filters::Synchronizer Sync; + // typename std::shared_ptr sync_ptr; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index cae6b6c769a6..23b0eea52917 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -47,7 +47,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay private: void processMessage(TrackedObjects::ConstSharedPtr msg) override; - void onInitialize() override; + // void onInitialize() override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) { @@ -96,19 +96,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay return id_map.at(uuid); } - void onObjectsAndObstaclePointCloud( - const TrackedObjects::ConstSharedPtr & input_objs_msg, - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); + // void onObjectsAndObstaclePointCloud( + // 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; - typedef message_filters::sync_policies::ApproximateTime< - TrackedObjects, sensor_msgs::msg::PointCloud2> - SyncPolicy; - typedef message_filters::Synchronizer Sync; - typename std::shared_ptr sync_ptr; + // typedef message_filters::sync_policies::ApproximateTime< + // TrackedObjects, sensor_msgs::msg::PointCloud2> + // SyncPolicy; + // typedef message_filters::Synchronizer Sync; + // typename std::shared_ptr sync_ptr; }; } // namespace object_detection 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 0e09f888e000..b7b448e7f64d 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 @@ -86,89 +86,89 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) } } -void DetectedObjectsDisplay::onInitialize() -{ - ObjectPolygonDisplayBase::onInitialize(); - // get access to rviz node to sub and to pub to topics - rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - - sync_ptr = std::make_shared( - SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); - sync_ptr->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - perception_objects_subscription.subscribe( - raw_node, "/perception/object_recognition/detection/objects", - rclcpp::QoS{1}.get_rmw_qos_profile()); - pointcloud_subscription.subscribe( - raw_node, m_default_pointcloud_topic->getTopic().toStdString(), - rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); -} - -void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( - 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_auto_perception_msgs::msg::DetectedObjects transformed_objects; - if (!transformObjects( - *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { - return; - } - - objs_buffer.clear(); - for (const auto & object : transformed_objects.objects) { - std::vector labels = - object.classification; - object_info info = { - object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; - objs_buffer.push_back(info); - } - - // 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()); - - if (objs_buffer.size() > 0) { - for (auto object : objs_buffer) { - const auto search_radius = getMaxRadius(object); - // Search neighbor pointcloud to reduce cost. - pcl::PointCloud::Ptr neighbor_pointcloud( - new pcl::PointCloud); - std::vector indices; - std::vector distances; - kdtree->radiusSearch( - toPCL(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, object); - } - } else { - return; - } - - 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); -} +// void DetectedObjectsDisplay::onInitialize() +// { +// ObjectPolygonDisplayBase::onInitialize(); +// // get access to rviz node to sub and to pub to topics +// rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); + +// sync_ptr = std::make_shared( +// SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); +// sync_ptr->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); +// perception_objects_subscription.subscribe( +// raw_node, "/perception/object_recognition/detection/objects", +// rclcpp::QoS{1}.get_rmw_qos_profile()); +// pointcloud_subscription.subscribe( +// raw_node, m_default_pointcloud_topic->getTopic().toStdString(), +// rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); +// } + +// void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( +// 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_auto_perception_msgs::msg::DetectedObjects transformed_objects; +// if (!transformObjects( +// *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { +// return; +// } + +// objs_buffer.clear(); +// for (const auto & object : transformed_objects.objects) { +// std::vector labels = +// object.classification; +// object_info info = { +// object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; +// objs_buffer.push_back(info); +// } + +// // 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()); + +// if (objs_buffer.size() > 0) { +// for (auto object : objs_buffer) { +// const auto search_radius = getMaxRadius(object); +// // Search neighbor pointcloud to reduce cost. +// pcl::PointCloud::Ptr neighbor_pointcloud( +// new pcl::PointCloud); +// std::vector indices; +// std::vector distances; +// kdtree->radiusSearch( +// toPCL(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, object); +// } +// } else { +// return; +// } + +// 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 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 ed6fc4b8fafa..ba459bcbd713 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 @@ -211,120 +211,104 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) wall_dt, ros_dt); } -void PredictedObjectsDisplay::onInitialize() -{ - ObjectPolygonDisplayBase::onInitialize(); - // get access to rviz node to sub and to pub to topics - rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - - sync_ptr = std::make_shared( - SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); - sync_ptr->registerCallback(&PredictedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - - perception_objects_subscription.subscribe( - raw_node, "/perception/object_recognition/objects", rclcpp::QoS{1}.get_rmw_qos_profile()); - pointcloud_subscription.subscribe( - raw_node, m_default_pointcloud_topic->getTopic().toStdString(), - rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); -} - -bool PredictedObjectsDisplay::transformObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, - const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, - autoware_auto_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::onObjectsAndObstaclePointCloud( - 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_auto_perception_msgs::msg::PredictedObjects transformed_objects; - if (!transformObjects( - *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { - return; - } - - objs_buffer.clear(); - for (const auto & object : transformed_objects.objects) { - std::vector labels = - object.classification; - object_info info = { - object.shape, object.kinematics.initial_pose_with_covariance.pose, object.classification}; - objs_buffer.push_back(info); - } - - // convert to pcl pointcloud - pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); - // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); - 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()); - - if (objs_buffer.size() > 0) { - for (auto object : objs_buffer) { - const auto search_radius = getMaxRadius(object); - // Search neighbor pointcloud to reduce cost. - pcl::PointCloud::Ptr neighbor_pointcloud( - new pcl::PointCloud); - std::vector indices; - std::vector distances; - kdtree->radiusSearch( - toPCL(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, object); - } - } else { - return; - } - - 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); -} +// bool PredictedObjectsDisplay::transformObjects( +// const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, +// const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, +// autoware_auto_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::onObjectsAndObstaclePointCloud( +// 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_auto_perception_msgs::msg::PredictedObjects transformed_objects; +// if (!transformObjects( +// *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { +// return; +// } + +// objs_buffer.clear(); +// for (const auto & object : transformed_objects.objects) { +// std::vector labels = +// object.classification; +// object_info info = { +// object.shape, object.kinematics.initial_pose_with_covariance.pose, object.classification}; +// objs_buffer.push_back(info); +// } + +// // convert to pcl pointcloud +// pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); +// // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); +// 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()); + +// if (objs_buffer.size() > 0) { +// for (auto object : objs_buffer) { +// const auto search_radius = getMaxRadius(object); +// // Search neighbor pointcloud to reduce cost. +// pcl::PointCloud::Ptr neighbor_pointcloud( +// new pcl::PointCloud); +// std::vector indices; +// std::vector distances; +// kdtree->radiusSearch( +// toPCL(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, object); +// } +// } else { +// return; +// } + +// 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 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 7bd1d43d31f5..d1424f2aafc2 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 @@ -126,91 +126,91 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) } } -void TrackedObjectsDisplay::onInitialize() -{ - ObjectPolygonDisplayBase::onInitialize(); - // get access to rviz node to sub and to pub to topics - rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - - sync_ptr = std::make_shared( - SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); - sync_ptr->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - - perception_objects_subscription.subscribe( - raw_node, "/perception/object_recognition/tracking/objects", - rclcpp::QoS{1}.get_rmw_qos_profile()); - pointcloud_subscription.subscribe( - raw_node, m_default_pointcloud_topic->getTopic().toStdString(), - rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); -} - -void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( - 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_auto_perception_msgs::msg::TrackedObjects transformed_objects; - if (!transformObjects( - *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { - return; - } - - objs_buffer.clear(); - for (const auto & object : transformed_objects.objects) { - std::vector labels = - object.classification; - object_info info = { - object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; - objs_buffer.push_back(info); - } - - // convert to pcl pointcloud - pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); - // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); - 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()); - - if (objs_buffer.size() > 0) { - for (auto object : objs_buffer) { - const auto search_radius = getMaxRadius(object); - // Search neighbor pointcloud to reduce cost. - pcl::PointCloud::Ptr neighbor_pointcloud( - new pcl::PointCloud); - std::vector indices; - std::vector distances; - kdtree->radiusSearch( - toPCL(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, object); - } - } else { - return; - } - - 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); -} +// void TrackedObjectsDisplay::onInitialize() +// { +// ObjectPolygonDisplayBase::onInitialize(); +// // get access to rviz node to sub and to pub to topics +// rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); + +// sync_ptr = std::make_shared( +// SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); +// sync_ptr->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); + +// perception_objects_subscription.subscribe( +// raw_node, "/perception/object_recognition/tracking/objects", +// rclcpp::QoS{1}.get_rmw_qos_profile()); +// pointcloud_subscription.subscribe( +// raw_node, m_default_pointcloud_topic->getTopic().toStdString(), +// rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); +// } + +// void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( +// 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_auto_perception_msgs::msg::TrackedObjects transformed_objects; +// if (!transformObjects( +// *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { +// return; +// } + +// objs_buffer.clear(); +// for (const auto & object : transformed_objects.objects) { +// std::vector labels = +// object.classification; +// object_info info = { +// object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; +// objs_buffer.push_back(info); +// } + +// // convert to pcl pointcloud +// pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); +// // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); +// 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()); + +// if (objs_buffer.size() > 0) { +// for (auto object : objs_buffer) { +// const auto search_radius = getMaxRadius(object); +// // Search neighbor pointcloud to reduce cost. +// pcl::PointCloud::Ptr neighbor_pointcloud( +// new pcl::PointCloud); +// std::vector indices; +// std::vector distances; +// kdtree->radiusSearch( +// toPCL(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, object); +// } +// } else { +// return; +// } + +// 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 From 4b8f47962fd9cd4c878db7cbb1684beccc46dd7c Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Fri, 31 Mar 2023 00:28:54 +0800 Subject: [PATCH 35/58] feat(add_perception_objects_pointcloud_publisher) subscription on and off, but update of the pointcloud topic doesn't work Signed-off-by: Alexey Panferov --- .../object_polygon_display_base.hpp | 66 +++++++++++++++++-- 1 file changed, 62 insertions(+), 4 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index d5fb5809aa14..30ba0f3f38d8 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -155,7 +156,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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(updatePalette())); + SLOT(updateTopic())); // m_default_pointcloud_topic->setReadOnly(true); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { @@ -212,7 +213,55 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase RosTopicDisplay::reset(); m_marker_common.clearMarkers(); point_cloud_common->reset(); - m_default_pointcloud_topic->reset(); + } + + virtual void subscribe() + { + RosTopicDisplay::subscribe(); + + if (!RosTopicDisplay::isEnabled() && !m_publish_objs_pointcloud) { + 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 sstm; + sstm << "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(sstm.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 (); } void clear_markers() { m_marker_common.clearMarkers(); } @@ -535,6 +584,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } + void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) + { + std::string frame_id = input_pointcloud_msg.header.frame_id; + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "hello from poincloudCallback"); + + // add pointcloud to buffer + } + void filterPolygon( const pcl::PointCloud::ConstPtr & in_cloud, pcl::PointCloud::Ptr out_cloud, const object_info & object) @@ -583,8 +640,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // Property to enable/disable objects pointcloud publishing rviz_common::properties::BoolProperty * m_publish_objs_pointcloud; - message_filters::Subscriber perception_objects_subscription; - message_filters::Subscriber pointcloud_subscription; + // message_filters::Subscriber perception_objects_subscription; + // rclcpp::Subscriber pointcloud_subscription; + rclcpp::Subscription::SharedPtr pointcloud_subscription; // plugin to visualize pointclouds std::unique_ptr point_cloud_common; From 2f68f39c0aff96ed14ceb823a4a8760b77f560b4 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Fri, 31 Mar 2023 20:23:14 +0800 Subject: [PATCH 36/58] feat(add_perception_objects_pointcloud_publisher) update pointcloud topic works Signed-off-by: Alexey Panferov --- .../object_polygon_display_base.hpp | 48 +++++++++++++++++-- 1 file changed, 43 insertions(+), 5 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 30ba0f3f38d8..f108187a826e 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -137,7 +138,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase "Display Predicted Path Confidence", true, "Enable/disable predicted paths visualization", 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, SLOT(updatePalette())); @@ -152,11 +154,12 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_simple_visualize_mode_property->addOption("Normal", 0); m_simple_visualize_mode_property->addOption("Simple", 1); m_publish_objs_pointcloud = new rviz_common::properties::BoolProperty( - "Publish Objects Pointcloud", true, "Enable/disable objects pointcloud publishing", this); + "Publish Objects Pointcloud", true, "Enable/disable objects pointcloud publishing", this, SLOT(RosTopicDisplay::updateTopic())); 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())); + SLOT(RosTopicDisplay::updateTopic())); + qos_profile_points_property = new rviz_common::properties::QosProfileProperty(m_default_pointcloud_topic, qos_profile_points); // m_default_pointcloud_topic->setReadOnly(true); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { @@ -186,6 +189,20 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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(); @@ -193,6 +210,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase tf_buffer = std::make_unique(raw_node->get_clock()); tf_listener = std::make_shared(*tf_buffer); + // m_default_pointcloud_topic->setValue(m_default_pointcloud_topic->c_str()); + point_cloud_common->initialize(this->context_, this->scene_node_); } @@ -217,9 +236,18 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase virtual void subscribe() { + if (!RosTopicDisplay::isEnabled()) { + return; + } + RosTopicDisplay::subscribe(); - - if (!RosTopicDisplay::isEnabled() && !m_publish_objs_pointcloud) { + points_subscribe(); + } + + + void points_subscribe() + { + if (!m_publish_objs_pointcloud->getBool()) { return; } @@ -258,10 +286,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } + void unsubscribe() { RosTopicDisplay::unsubscribe(); pointcloud_subscription.reset (); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Unsubscribe called"); + } void clear_markers() { m_marker_common.clearMarkers(); } @@ -586,6 +617,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) { + if (!m_publish_objs_pointcloud->getBool()) { + return; + } + std::string frame_id = input_pointcloud_msg.header.frame_id; RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "hello from poincloudCallback"); @@ -645,6 +680,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase rclcpp::Subscription::SharedPtr pointcloud_subscription; // plugin to visualize pointclouds std::unique_ptr point_cloud_common; + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node; private: // All rviz plugins should have this. Should be initialized with pointer to this class @@ -679,6 +715,8 @@ class AUTOWARE_AUTO_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 From a3559d333a69b032c46d0b3ff0edbcaf6813de5b Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Mon, 3 Apr 2023 17:54:04 +0800 Subject: [PATCH 37/58] feat(add_perception_objects_pointcloud_publisher) wip add find nearest pointcloud by timestamp, builds, but crused on runtime Signed-off-by: Alexey Panferov --- .../detected_objects_display.hpp | 6 +- .../object_polygon_display_base.hpp | 58 ++++++++ .../detected_objects_display.cpp | 138 +++++++++--------- 3 files changed, 133 insertions(+), 69 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index 41ada2090e7d..42b72f86ce09 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -42,9 +42,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay private: void processMessage(DetectedObjects::ConstSharedPtr msg) override; // void onInitialize() override; - // void onObjectsAndObstaclePointCloud( - // const DetectedObjects::ConstSharedPtr & input_objs_msg, - // const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); + void onObjectsAndObstaclePointCloud( + const DetectedObjects::ConstSharedPtr & input_objs_msg, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); // using SyncPolicy = // message_filters::sync_policies::ApproximateTime; diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index f108187a826e..0d8e4a1e9f92 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -62,6 +62,7 @@ #include #include +#include #include #include #include @@ -100,6 +101,12 @@ 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 (int(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. @@ -620,6 +627,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase if (!m_publish_objs_pointcloud->getBool()) { return; } + + // Add a pointer to the new message to the front of the deque + pointCloudBuffer.push_front(input_pointcloud_msg); + + // If the deque has more than 5 elements, remove the oldest element from the back of the deque + if (pointCloudBuffer.size() > 5) { + pointCloudBuffer.pop_back(); + } std::string frame_id = input_pointcloud_msg.header.frame_id; RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "hello from poincloudCallback"); @@ -670,6 +685,46 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase *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) + { + // Sort the buffer of pointclouds by timestamp using our custom comparator function + std::sort(buffer.begin(), buffer.end(), comparePointCloudsByTimestamp); + + // Find the first pointcloud with a timestamp greater than or equal to the target timestamp + auto iter = std::lower_bound(buffer.begin(), buffer.end(), timestamp, + [](const sensor_msgs::msg::PointCloud2 & pointcloud, const rclcpp::Time& timestamp) { + return pointcloud.header.stamp.nanosec < timestamp.nanoseconds(); + }); + + // If the target timestamp is less than the timestamp of the first pointcloud in the buffer, return that pointcloud + if (iter == buffer.begin()) { + return *iter; + } + + // If the target timestamp is greater than the timestamp of the last pointcloud in the buffer, return that pointcloud + if (iter == buffer.end()) { + return *(--iter); + } + + // Calculate the difference between the target timestamp and the timestamps of the two adjacent pointclouds + rclcpp::Duration diff1 = timestamp - rclcpp::Time(iter->header.stamp); + // rclcpp::Time diff2 = iter->header.stamp.nanosec - (*(--iter)).header.stamp.nanosec; + rclcpp::Duration diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time((--iter)->header.stamp); + + // rclcpp::Duration diff2; + // if (iter != buffer.begin()) { + // --iter; + // diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time(iter->header.stamp); + // } else { + + // diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time(iter->header.stamp); + // } + + // Return the pointcloud with the nearest timestamp + return (diff1 < diff2) ? *iter : *(++iter); + } + // Default pointcloud topic; rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic; // Property to enable/disable objects pointcloud publishing @@ -681,6 +736,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // plugin to visualize pointclouds std::unique_ptr point_cloud_common; rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node; + std::deque pointCloudBuffer; + private: // All rviz plugins should have this. Should be initialized with pointer to this class @@ -723,3 +780,4 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } // namespace autoware #endif // OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ + 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 b7b448e7f64d..5dd6a475a782 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 @@ -33,6 +33,12 @@ DetectedObjectsDisplay::DetectedObjectsDisplay() void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) { + // find nearest pointcloud + // transfor objs to poincloud frame + // + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + onObjectsAndObstaclePointCloud(msg, closest_pointcloud); + clear_markers(); int id = 0; for (const auto & object : msg->objects) { @@ -103,72 +109,72 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) // rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); // } -// void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( -// 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_auto_perception_msgs::msg::DetectedObjects transformed_objects; -// if (!transformObjects( -// *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { -// return; -// } - -// objs_buffer.clear(); -// for (const auto & object : transformed_objects.objects) { -// std::vector labels = -// object.classification; -// object_info info = { -// object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; -// objs_buffer.push_back(info); -// } - -// // 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()); - -// if (objs_buffer.size() > 0) { -// for (auto object : objs_buffer) { -// const auto search_radius = getMaxRadius(object); -// // Search neighbor pointcloud to reduce cost. -// pcl::PointCloud::Ptr neighbor_pointcloud( -// new pcl::PointCloud); -// std::vector indices; -// std::vector distances; -// kdtree->radiusSearch( -// toPCL(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, object); -// } -// } else { -// return; -// } - -// 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); -// } +void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( + 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_auto_perception_msgs::msg::DetectedObjects transformed_objects; + if (!transformObjects( + *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { + return; + } + + objs_buffer.clear(); + for (const auto & object : transformed_objects.objects) { + std::vector labels = + object.classification; + object_info info = { + object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; + objs_buffer.push_back(info); + } + + // 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()); + + if (objs_buffer.size() > 0) { + for (auto object : objs_buffer) { + const auto search_radius = getMaxRadius(object); + // Search neighbor pointcloud to reduce cost. + pcl::PointCloud::Ptr neighbor_pointcloud( + new pcl::PointCloud); + std::vector indices; + std::vector distances; + kdtree->radiusSearch( + toPCL(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, object); + } + } else { + return; + } + + 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 From 7b15c45480f531b42f16de98628bd4c56c391c2e Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Mon, 3 Apr 2023 23:20:21 +0800 Subject: [PATCH 38/58] feat(add_perception_objects_pointcloud_publisher) add poincloud buffer guard Signed-off-by: Alexey Panferov --- .../object_polygon_display_base.hpp | 8 ++++++- .../detected_objects_display.cpp | 21 +++++++++++++------ 2 files changed, 22 insertions(+), 7 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 0d8e4a1e9f92..86c89c24bfe0 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -690,22 +690,27 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase { // Sort the buffer of pointclouds by timestamp using our custom comparator function std::sort(buffer.begin(), buffer.end(), comparePointCloudsByTimestamp); + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "sort poinclouds"); // Find the first pointcloud with a timestamp greater than or equal to the target timestamp auto iter = std::lower_bound(buffer.begin(), buffer.end(), timestamp, [](const sensor_msgs::msg::PointCloud2 & pointcloud, const rclcpp::Time& timestamp) { return pointcloud.header.stamp.nanosec < timestamp.nanoseconds(); }); + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "lower bond"); // If the target timestamp is less than the timestamp of the first pointcloud in the buffer, return that pointcloud if (iter == buffer.begin()) { return *iter; } + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "not first pointcloud"); // If the target timestamp is greater than the timestamp of the last pointcloud in the buffer, return that pointcloud if (iter == buffer.end()) { return *(--iter); } + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "not Last poincloud"); + // Calculate the difference between the target timestamp and the timestamps of the two adjacent pointclouds rclcpp::Duration diff1 = timestamp - rclcpp::Time(iter->header.stamp); @@ -717,9 +722,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // --iter; // diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time(iter->header.stamp); // } else { - + // diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time(iter->header.stamp); // } + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "calculate diff"); // Return the pointcloud with the nearest timestamp return (diff1 < diff2) ? *iter : *(++iter); 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 5dd6a475a782..2fabb53b5fcc 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 @@ -33,11 +33,6 @@ DetectedObjectsDisplay::DetectedObjectsDisplay() void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) { - // find nearest pointcloud - // transfor objs to poincloud frame - // - sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); - onObjectsAndObstaclePointCloud(msg, closest_pointcloud); clear_markers(); int id = 0; @@ -90,6 +85,15 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(twist_marker_ptr); } } + + if (pointCloudBuffer.empty()) + { + return; + } + // poincloud pub + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( + getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + onObjectsAndObstaclePointCloud(msg, closest_pointcloud); } // void DetectedObjectsDisplay::onInitialize() @@ -116,12 +120,14 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( if (!m_publish_objs_pointcloud->getBool()) { return; } + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "publish pointcloud flag"); // Transform to pointcloud frame autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; if (!transformObjects( *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { return; } + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "transform objects to poincloud frame"); objs_buffer.clear(); for (const auto & object : transformed_objects.objects) { @@ -131,6 +137,7 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; objs_buffer.push_back(info); } + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "convert to objs info"); // convert to pcl pointcloud pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); @@ -172,8 +179,10 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - + add_pointcloud(output_pointcloud_msg_ptr); + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "publish poinclouds"); + } } // namespace object_detection From 0150e97b559ff42698b62ae75c8d2cb960b4a65c Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Mon, 3 Apr 2023 23:30:51 +0800 Subject: [PATCH 39/58] feat(add_perception_objects_pointcloud_publisher) refactor getMaxRadius and onObjectsAndObstaclePointCloud Signed-off-by: Alexey Panferov --- .../object_polygon_display_base.hpp | 10 +++--- .../detected_objects_display.cpp | 34 +++++++++---------- 2 files changed, 23 insertions(+), 21 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 86c89c24bfe0..f6666f394cc1 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -610,16 +610,18 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase { 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); - } else if (object.shape.type == Shape::POLYGON) { + } + + 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 = max_dist < dist ? dist : max_dist; + max_dist = std::max(max_dist, dist); } return max_dist; - } else { - return std::nullopt; } + + return std::nullopt; } void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) 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 2fabb53b5fcc..b6c67689ce15 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 @@ -154,25 +154,25 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - if (objs_buffer.size() > 0) { - for (auto object : objs_buffer) { - const auto search_radius = getMaxRadius(object); - // Search neighbor pointcloud to reduce cost. - pcl::PointCloud::Ptr neighbor_pointcloud( - new pcl::PointCloud); - std::vector indices; - std::vector distances; - kdtree->radiusSearch( - toPCL(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, object); - } - } else { + if (objs_buffer.empty()) { return; } + + for (auto object : objs_buffer) { + const auto search_radius = getMaxRadius(object); + // Search neighbor pointcloud to reduce cost. + pcl::PointCloud::Ptr neighbor_pointcloud( + new pcl::PointCloud); + std::vector indices; + std::vector distances; + kdtree->radiusSearch( + toPCL(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, object); + } sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); From 21fe5a98ec5bb2b7a30401bf334fbf29e84c6e05 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 4 Apr 2023 00:22:23 +0800 Subject: [PATCH 40/58] feat(add_perception_objects_pointcloud_publisher) remove objs_buffer from Signed-off-by: Alexey Panferov --- .../object_polygon_display_base.hpp | 1 - .../detected_objects_display.cpp | 44 +++++-------------- 2 files changed, 10 insertions(+), 35 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index f6666f394cc1..32e136b9dc30 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -366,7 +366,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } // variables for transfer detected objects information between callbacks - std::vector objs_buffer; std::shared_ptr tf_listener{nullptr}; std::unique_ptr tf_buffer; 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 b6c67689ce15..763aa064fde6 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 @@ -96,23 +96,6 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) onObjectsAndObstaclePointCloud(msg, closest_pointcloud); } -// void DetectedObjectsDisplay::onInitialize() -// { -// ObjectPolygonDisplayBase::onInitialize(); -// // get access to rviz node to sub and to pub to topics -// rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - -// sync_ptr = std::make_shared( -// SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); -// sync_ptr->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this); -// perception_objects_subscription.subscribe( -// raw_node, "/perception/object_recognition/detection/objects", -// rclcpp::QoS{1}.get_rmw_qos_profile()); -// pointcloud_subscription.subscribe( -// raw_node, m_default_pointcloud_topic->getTopic().toStdString(), -// rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); -// } - void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( const DetectedObjects::ConstSharedPtr & input_objs_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg) @@ -129,15 +112,6 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( } RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "transform objects to poincloud frame"); - objs_buffer.clear(); - for (const auto & object : transformed_objects.objects) { - std::vector labels = - object.classification; - object_info info = { - object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; - objs_buffer.push_back(info); - } - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "convert to objs info"); // convert to pcl pointcloud pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); @@ -154,24 +128,26 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( pcl::PointCloud::Ptr out_cloud(new pcl::PointCloud()); - if (objs_buffer.empty()) { - return; - } - - for (auto object : objs_buffer) { - const auto search_radius = getMaxRadius(object); + 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(object.position.position), search_radius.value(), indices, distances); + 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, object); + filterPolygon(neighbor_pointcloud, out_cloud, unified_object); + } sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( From 36e74873721f59b81dbcfbdd71be7d5ceceadf8f Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 4 Apr 2023 01:12:33 +0800 Subject: [PATCH 41/58] feat(add_perception_objects_pointcloud_publisher) add processPointCloud to tracked and predicted works but sometime core dumped Signed-off-by: Alexey Panferov --- .../detected_objects_display.hpp | 7 +- .../object_polygon_display_base.hpp | 12 +- .../predicted_objects_display.hpp | 20 +- .../tracked_objects_display.hpp | 12 +- .../detected_objects_display.cpp | 13 +- .../predicted_objects_display.cpp | 188 +++++++++--------- .../tracked_objects_display.cpp | 145 ++++++-------- 7 files changed, 170 insertions(+), 227 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index 42b72f86ce09..a6e8d337a259 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -42,14 +42,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay private: void processMessage(DetectedObjects::ConstSharedPtr msg) override; // void onInitialize() override; - void onObjectsAndObstaclePointCloud( + void processPointCloud( const DetectedObjects::ConstSharedPtr & input_objs_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); - - // using SyncPolicy = - // message_filters::sync_policies::ApproximateTime; - // using Sync = message_filters::Synchronizer; - // std::shared_ptr sync_ptr; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 32e136b9dc30..40abb79a8153 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -161,7 +161,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_simple_visualize_mode_property->addOption("Normal", 0); m_simple_visualize_mode_property->addOption("Simple", 1); m_publish_objs_pointcloud = new rviz_common::properties::BoolProperty( - "Publish Objects Pointcloud", true, "Enable/disable objects pointcloud publishing", this, SLOT(RosTopicDisplay::updateTopic())); + "Publish Objects Pointcloud", true, "Enable/disable objects pointcloud publishing", this, SLOT(updatePalette())); 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, @@ -638,7 +638,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } std::string frame_id = input_pointcloud_msg.header.frame_id; - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "hello from poincloudCallback"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "hello from poincloudCallback"); // add pointcloud to buffer } @@ -691,26 +691,26 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase { // Sort the buffer of pointclouds by timestamp using our custom comparator function std::sort(buffer.begin(), buffer.end(), comparePointCloudsByTimestamp); - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "sort poinclouds"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "sort poinclouds"); // Find the first pointcloud with a timestamp greater than or equal to the target timestamp auto iter = std::lower_bound(buffer.begin(), buffer.end(), timestamp, [](const sensor_msgs::msg::PointCloud2 & pointcloud, const rclcpp::Time& timestamp) { return pointcloud.header.stamp.nanosec < timestamp.nanoseconds(); }); - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "lower bond"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "lower bond"); // If the target timestamp is less than the timestamp of the first pointcloud in the buffer, return that pointcloud if (iter == buffer.begin()) { return *iter; } - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "not first pointcloud"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "not first pointcloud"); // If the target timestamp is greater than the timestamp of the last pointcloud in the buffer, return that pointcloud if (iter == buffer.end()) { return *(--iter); } - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "not Last poincloud"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "not Last poincloud"); // Calculate the difference between the target timestamp and the timestamps of the two adjacent pointclouds diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index 8c0941a57c61..36f19e9a0f30 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -104,14 +104,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay void update(float wall_dt, float ros_dt) override; - // bool transformObjects( - // const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, - // const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, - // autoware_auto_perception_msgs::msg::PredictedObjects & output_msg); + bool transformObjects( + const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, + const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + autoware_auto_perception_msgs::msg::PredictedObjects & output_msg); - // void onObjectsAndObstaclePointCloud( - // const PredictedObjects::ConstSharedPtr & input_objs_msg, - // const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_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; @@ -124,12 +124,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay std::mutex mutex; std::condition_variable condition; std::vector markers; - - // typedef message_filters::sync_policies::ApproximateTime< - // PredictedObjects, sensor_msgs::msg::PointCloud2> - // SyncPolicy; - // typedef message_filters::Synchronizer Sync; - // typename std::shared_ptr sync_ptr; }; } // namespace object_detection diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 23b0eea52917..8bb64ccf7cf3 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -96,19 +96,13 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay return id_map.at(uuid); } - // void onObjectsAndObstaclePointCloud( - // const TrackedObjects::ConstSharedPtr & input_objs_msg, - // const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); + 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; - - // typedef message_filters::sync_policies::ApproximateTime< - // TrackedObjects, sensor_msgs::msg::PointCloud2> - // SyncPolicy; - // typedef message_filters::Synchronizer Sync; - // typename std::shared_ptr sync_ptr; }; } // namespace object_detection 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 763aa064fde6..bfcb30a6a4b1 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 @@ -93,34 +93,28 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) // poincloud pub sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); - onObjectsAndObstaclePointCloud(msg, closest_pointcloud); + processPointCloud(msg, closest_pointcloud); } -void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( +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; } - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "publish pointcloud flag"); // Transform to pointcloud frame autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects; if (!transformObjects( *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { return; } - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "transform objects to poincloud frame"); - - // 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); @@ -147,7 +141,6 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( } filterPolygon(neighbor_pointcloud, out_cloud, unified_object); - } sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( @@ -157,8 +150,6 @@ void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; add_pointcloud(output_pointcloud_msg_ptr); - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "publish poinclouds"); - } } // namespace object_detection 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 ba459bcbd713..13f7401b7bf7 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 @@ -180,6 +180,11 @@ std::vector PredictedObjectsDisplay: } } + // poincloud pub + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( + getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + processPointCloud(msg, closest_pointcloud); + return markers; } @@ -212,103 +217,92 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) } -// bool PredictedObjectsDisplay::transformObjects( -// const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, -// const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, -// autoware_auto_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::onObjectsAndObstaclePointCloud( -// 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_auto_perception_msgs::msg::PredictedObjects transformed_objects; -// if (!transformObjects( -// *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { -// return; -// } - -// objs_buffer.clear(); -// for (const auto & object : transformed_objects.objects) { -// std::vector labels = -// object.classification; -// object_info info = { -// object.shape, object.kinematics.initial_pose_with_covariance.pose, object.classification}; -// objs_buffer.push_back(info); -// } - -// // convert to pcl pointcloud -// pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); -// // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); -// 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()); - -// if (objs_buffer.size() > 0) { -// for (auto object : objs_buffer) { -// const auto search_radius = getMaxRadius(object); -// // Search neighbor pointcloud to reduce cost. -// pcl::PointCloud::Ptr neighbor_pointcloud( -// new pcl::PointCloud); -// std::vector indices; -// std::vector distances; -// kdtree->radiusSearch( -// toPCL(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, object); -// } -// } else { -// return; -// } - -// 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); -// } +bool PredictedObjectsDisplay::transformObjects( + const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, + const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + autoware_auto_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_auto_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); + 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 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 d1424f2aafc2..826af3e9beae 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 @@ -124,93 +124,68 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(twist_marker_ptr); } } + // poincloud pub + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( + getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + processPointCloud(msg, closest_pointcloud); } -// void TrackedObjectsDisplay::onInitialize() -// { -// ObjectPolygonDisplayBase::onInitialize(); -// // get access to rviz node to sub and to pub to topics -// rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); - -// sync_ptr = std::make_shared( -// SyncPolicy(10), perception_objects_subscription, pointcloud_subscription); -// sync_ptr->registerCallback(&TrackedObjectsDisplay::onObjectsAndObstaclePointCloud, this); - -// perception_objects_subscription.subscribe( -// raw_node, "/perception/object_recognition/tracking/objects", -// rclcpp::QoS{1}.get_rmw_qos_profile()); -// pointcloud_subscription.subscribe( -// raw_node, m_default_pointcloud_topic->getTopic().toStdString(), -// rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile()); -// } - -// void TrackedObjectsDisplay::onObjectsAndObstaclePointCloud( -// 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_auto_perception_msgs::msg::TrackedObjects transformed_objects; -// if (!transformObjects( -// *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { -// return; -// } - -// objs_buffer.clear(); -// for (const auto & object : transformed_objects.objects) { -// std::vector labels = -// object.classification; -// object_info info = { -// object.shape, object.kinematics.pose_with_covariance.pose, object.classification}; -// objs_buffer.push_back(info); -// } - -// // convert to pcl pointcloud -// pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); -// // pcl::fromROSMsg(transformed_pointcloud, *temp_cloud); -// 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()); - -// if (objs_buffer.size() > 0) { -// for (auto object : objs_buffer) { -// const auto search_radius = getMaxRadius(object); -// // Search neighbor pointcloud to reduce cost. -// pcl::PointCloud::Ptr neighbor_pointcloud( -// new pcl::PointCloud); -// std::vector indices; -// std::vector distances; -// kdtree->radiusSearch( -// toPCL(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, object); -// } -// } else { -// return; -// } - -// 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); -// } + +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_auto_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 } // namespace rviz_plugins From b7914424a7e3755e02eb1e02bcfc9db83132ddf9 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 4 Apr 2023 16:36:20 +0800 Subject: [PATCH 42/58] feat(add_perception_objects_pointcloud_publisher) wip simple getNearestPointCloud Signed-off-by: Alexey Panferov --- .../object_polygon_display_base.hpp | 75 +++++++++++-------- .../detected_objects_display.cpp | 5 +- .../predicted_objects_display.cpp | 17 ++++- .../tracked_objects_display.cpp | 10 ++- 4 files changed, 70 insertions(+), 37 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 40abb79a8153..cd6914081269 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -690,49 +690,62 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase sensor_msgs::msg::PointCloud2 getNearestPointCloud(std::deque & buffer, const rclcpp::Time & timestamp) { // Sort the buffer of pointclouds by timestamp using our custom comparator function - std::sort(buffer.begin(), buffer.end(), comparePointCloudsByTimestamp); + // std::sort(buffer.begin(), buffer.end(), comparePointCloudsByTimestamp); // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "sort poinclouds"); - // Find the first pointcloud with a timestamp greater than or equal to the target timestamp - auto iter = std::lower_bound(buffer.begin(), buffer.end(), timestamp, - [](const sensor_msgs::msg::PointCloud2 & pointcloud, const rclcpp::Time& timestamp) { - return pointcloud.header.stamp.nanosec < timestamp.nanoseconds(); - }); + // // Find the first pointcloud with a timestamp greater than or equal to the target timestamp + // auto iter = std::lower_bound(buffer.begin(), buffer.end(), timestamp, + // [](const sensor_msgs::msg::PointCloud2 & pointcloud, const rclcpp::Time& timestamp) { + // return pointcloud.header.stamp.nanosec < timestamp.nanoseconds(); + // }); // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "lower bond"); - // If the target timestamp is less than the timestamp of the first pointcloud in the buffer, return that pointcloud - if (iter == buffer.begin()) { - return *iter; - } - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "not first pointcloud"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "First pointcloud"); + // // If the target timestamp is less than the timestamp of the first pointcloud in the buffer, return that pointcloud + // if (iter == buffer.begin()) { + // return *iter; + // } - // If the target timestamp is greater than the timestamp of the last pointcloud in the buffer, return that pointcloud - if (iter == buffer.end()) { - return *(--iter); - } - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "not Last poincloud"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Last poincloud"); + // // If the target timestamp is greater than the timestamp of the last pointcloud in the buffer, return that pointcloud + // if (iter == buffer.end()) { + // return *(--iter); + // } - // Calculate the difference between the target timestamp and the timestamps of the two adjacent pointclouds - rclcpp::Duration diff1 = timestamp - rclcpp::Time(iter->header.stamp); - // rclcpp::Time diff2 = iter->header.stamp.nanosec - (*(--iter)).header.stamp.nanosec; - rclcpp::Duration diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time((--iter)->header.stamp); + // // Calculate the difference between the target timestamp and the timestamps of the two adjacent pointclouds + // rclcpp::Duration diff1 = timestamp - rclcpp::Time(iter->header.stamp); + // // rclcpp::Time diff2 = iter->header.stamp.nanosec - (*(--iter)).header.stamp.nanosec; + // rclcpp::Duration diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time((--iter)->header.stamp); - // rclcpp::Duration diff2; - // if (iter != buffer.begin()) { - // --iter; - // diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time(iter->header.stamp); - // } else { + // // rclcpp::Duration diff2; + // // if (iter != buffer.begin()) { + // // --iter; + // // diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time(iter->header.stamp); + // // } else { - // diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time(iter->header.stamp); - // } - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "calculate diff"); + // // diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time(iter->header.stamp); + // // } + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "calculate diff"); - // Return the pointcloud with the nearest timestamp - return (diff1 < diff2) ? *iter : *(++iter); + // // Return the pointcloud with the nearest timestamp + // return (diff1 < diff2) ? *iter : *(++iter); + + sensor_msgs::msg::PointCloud2 result = buffer.front(); + rclcpp::Duration diff = timestamp - rclcpp::Time(result.header.stamp); + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Set last getted pointcloud as result"); + + for (sensor_msgs::msg::PointCloud2 pointcloud : buffer){ + if (diff > (timestamp - rclcpp::Time(pointcloud.header.stamp))) { + diff = timestamp - rclcpp::Time(pointcloud.header.stamp); + result = pointcloud; + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Update result"); + } + } + + return result; } - // Default pointcloud topic; rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic; // Property to enable/disable objects pointcloud publishing rviz_common::properties::BoolProperty * m_publish_objs_pointcloud; 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 bfcb30a6a4b1..c06fa6c6738c 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 @@ -91,8 +91,9 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) return; } // poincloud pub - sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( - getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + // sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( + // getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); processPointCloud(msg, closest_pointcloud); } 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 13f7401b7bf7..216809d0485b 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 @@ -180,10 +180,17 @@ std::vector PredictedObjectsDisplay: } } + if (pointCloudBuffer.empty()) + { + return markers; + } // poincloud pub - sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( - getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + // sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( + // getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); + processPointCloud(msg, closest_pointcloud); + // processPointCloud(msg, pointCloudBuffer.front()); return markers; } @@ -261,6 +268,8 @@ void PredictedObjectsDisplay::processPointCloud( *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { return; } + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Objects transformed"); + // convert to pcl pointcloud pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud); @@ -294,6 +303,8 @@ void PredictedObjectsDisplay::processPointCloud( filterPolygon(neighbor_pointcloud, out_cloud, unified_object); } + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Pointcloud filtered"); + sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); @@ -302,6 +313,8 @@ void PredictedObjectsDisplay::processPointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; add_pointcloud(output_pointcloud_msg_ptr); + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Published"); + } } // namespace object_detection 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 826af3e9beae..63da293102eb 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 @@ -124,9 +124,15 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) add_marker(twist_marker_ptr); } } + + if (pointCloudBuffer.empty()) + { + return; + } // poincloud pub - sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( - getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + // sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( + // getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); processPointCloud(msg, closest_pointcloud); } From 0285d4dd2970ea2e984f3557629d93ef5f5f655b Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 4 Apr 2023 19:33:29 +0800 Subject: [PATCH 43/58] feat(add_perception_objects_pointcloud_publisher) wip Signed-off-by: Alexey Panferov --- .../object_polygon_display_base.hpp | 24 ++++++++++++------- .../detected_objects_display.cpp | 2 +- .../predicted_objects_display.cpp | 5 ++-- .../tracked_objects_display.cpp | 2 +- 4 files changed, 21 insertions(+), 12 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index cd6914081269..a4bb013f26ba 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -731,18 +731,26 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // // Return the pointcloud with the nearest timestamp // return (diff1 < diff2) ? *iter : *(++iter); + if (buffer.empty()) { + // Handle the case where the buffer is empty + throw std::runtime_error("Buffer is empty"); + } + + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "buffer len is %ld", buffer.size()); + + sensor_msgs::msg::PointCloud2 result = buffer.front(); rclcpp::Duration diff = timestamp - rclcpp::Time(result.header.stamp); RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Set last getted pointcloud as result"); - for (sensor_msgs::msg::PointCloud2 pointcloud : buffer){ - if (diff > (timestamp - rclcpp::Time(pointcloud.header.stamp))) { - diff = timestamp - rclcpp::Time(pointcloud.header.stamp); - result = pointcloud; - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Update result"); - } - } - + // for (sensor_msgs::msg::PointCloud2 pointcloud : buffer){ + // if (diff > (timestamp - rclcpp::Time(pointcloud.header.stamp))) { + // diff = timestamp - rclcpp::Time(pointcloud.header.stamp); + // result = pointcloud; + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Update result"); + // } + // } + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "return result"); return result; } 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 c06fa6c6738c..ab0b2865ba52 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 @@ -91,7 +91,7 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) return; } // poincloud pub - // sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( + // sensor_msgs::msg::PointCloud2::ConstSharedPtr test_closest_pointcloud = std::make_shared( // getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); processPointCloud(msg, closest_pointcloud); 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 216809d0485b..a9cbb1c6d839 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 @@ -181,11 +181,12 @@ std::vector PredictedObjectsDisplay: } if (pointCloudBuffer.empty()) - { + { + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Pointcloud buffer is empty"); return markers; } // poincloud pub - // sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( + // sensor_msgs::msg::PointCloud2::ConstSharedPtr test_closest_pointcloud = std::make_shared( // getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); 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 63da293102eb..e550be553dce 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 @@ -130,7 +130,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) return; } // poincloud pub - // sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( + // sensor_msgs::msg::PointCloud2::ConstSharedPtr test_closest_pointcloud = std::make_shared( // getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); processPointCloud(msg, closest_pointcloud); From 585aa90c0c0a27b834883ed1e7ba1148e75c4e4f Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 4 Apr 2023 21:50:56 +0800 Subject: [PATCH 44/58] feat(add_perception_objects_pointcloud_publisher) simple search for nearest poincloud , works on planning simulator Signed-off-by: Alexey Panferov --- .../object_polygon_display_base.hpp | 62 ++++--------------- .../detected_objects_display.cpp | 6 +- .../predicted_objects_display.cpp | 6 +- .../tracked_objects_display.cpp | 6 +- 4 files changed, 20 insertions(+), 60 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index a4bb013f26ba..fe8c16505cd7 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -635,6 +635,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // If the deque has more than 5 elements, remove the oldest element from the back of the deque if (pointCloudBuffer.size() > 5) { pointCloudBuffer.pop_back(); + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Drop poincloud from buffer"); + } std::string frame_id = input_pointcloud_msg.header.frame_id; @@ -689,48 +691,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // 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) { - // Sort the buffer of pointclouds by timestamp using our custom comparator function - // std::sort(buffer.begin(), buffer.end(), comparePointCloudsByTimestamp); - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "sort poinclouds"); - - // // Find the first pointcloud with a timestamp greater than or equal to the target timestamp - // auto iter = std::lower_bound(buffer.begin(), buffer.end(), timestamp, - // [](const sensor_msgs::msg::PointCloud2 & pointcloud, const rclcpp::Time& timestamp) { - // return pointcloud.header.stamp.nanosec < timestamp.nanoseconds(); - // }); - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "lower bond"); - - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "First pointcloud"); - // // If the target timestamp is less than the timestamp of the first pointcloud in the buffer, return that pointcloud - // if (iter == buffer.begin()) { - // return *iter; - // } - - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Last poincloud"); - // // If the target timestamp is greater than the timestamp of the last pointcloud in the buffer, return that pointcloud - // if (iter == buffer.end()) { - // return *(--iter); - // } - - - // // Calculate the difference between the target timestamp and the timestamps of the two adjacent pointclouds - // rclcpp::Duration diff1 = timestamp - rclcpp::Time(iter->header.stamp); - // // rclcpp::Time diff2 = iter->header.stamp.nanosec - (*(--iter)).header.stamp.nanosec; - // rclcpp::Duration diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time((--iter)->header.stamp); - - // // rclcpp::Duration diff2; - // // if (iter != buffer.begin()) { - // // --iter; - // // diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time(iter->header.stamp); - // // } else { - - // // diff2 = rclcpp::Time(iter->header.stamp) - rclcpp::Time(iter->header.stamp); - // // } - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "calculate diff"); - - // // Return the pointcloud with the nearest timestamp - // return (diff1 < diff2) ? *iter : *(++iter); - if (buffer.empty()) { // Handle the case where the buffer is empty throw std::runtime_error("Buffer is empty"); @@ -738,19 +698,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "buffer len is %ld", buffer.size()); - sensor_msgs::msg::PointCloud2 result = buffer.front(); rclcpp::Duration diff = timestamp - rclcpp::Time(result.header.stamp); RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Set last getted pointcloud as result"); - // for (sensor_msgs::msg::PointCloud2 pointcloud : buffer){ - // if (diff > (timestamp - rclcpp::Time(pointcloud.header.stamp))) { - // diff = timestamp - rclcpp::Time(pointcloud.header.stamp); - // result = pointcloud; - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Update result"); - // } - // } - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "return result"); + for (sensor_msgs::msg::PointCloud2 pointcloud : buffer){ + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Check pointcloud in buffer"); + if (diff > (timestamp - rclcpp::Time(pointcloud.header.stamp))) { + diff = timestamp - rclcpp::Time(pointcloud.header.stamp); + result = pointcloud; + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Update result"); + } + } + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "return result"); return result; } 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 ab0b2865ba52..f6be310c4215 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 @@ -91,9 +91,9 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) return; } // poincloud pub - // sensor_msgs::msg::PointCloud2::ConstSharedPtr test_closest_pointcloud = std::make_shared( - // getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); - sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( + getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + // sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); processPointCloud(msg, closest_pointcloud); } 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 a9cbb1c6d839..e46234983e69 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 @@ -186,9 +186,9 @@ std::vector PredictedObjectsDisplay: return markers; } // poincloud pub - // sensor_msgs::msg::PointCloud2::ConstSharedPtr test_closest_pointcloud = std::make_shared( - // getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); - sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( + getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + // sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); processPointCloud(msg, closest_pointcloud); // processPointCloud(msg, pointCloudBuffer.front()); 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 e550be553dce..e6c61a87014d 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 @@ -130,9 +130,9 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) return; } // poincloud pub - // sensor_msgs::msg::PointCloud2::ConstSharedPtr test_closest_pointcloud = std::make_shared( - // getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); - sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( + getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + // sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); processPointCloud(msg, closest_pointcloud); } From b53b96da70dc3a45f2ecba8b640e895d357ac8ab Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Thu, 6 Apr 2023 16:21:06 +0800 Subject: [PATCH 45/58] feat(add_perception_objects_pointcloud_publisher) add mutex to protect pointcloud buffer Signed-off-by: Alexey Panferov --- .../object_polygon_display_base.hpp | 36 +++++++++++-------- .../predicted_objects_display.cpp | 8 ++--- 2 files changed, 25 insertions(+), 19 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index fe8c16505cd7..96b178e4aabe 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -298,7 +298,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase { RosTopicDisplay::unsubscribe(); pointcloud_subscription.reset (); - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Unsubscribe called"); + RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Unsubscribe called"); } @@ -628,18 +628,20 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase if (!m_publish_objs_pointcloud->getBool()) { return; } - + pointCloudBuffer_mutex.lock(); // Add a pointer to the new message to the front of the deque pointCloudBuffer.push_front(input_pointcloud_msg); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Add poincloud to buffer"); // If the deque has more than 5 elements, remove the oldest element from the back of the deque if (pointCloudBuffer.size() > 5) { pointCloudBuffer.pop_back(); - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Drop poincloud from buffer"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Drop poincloud from buffer"); } - - std::string frame_id = input_pointcloud_msg.header.frame_id; + pointCloudBuffer_mutex.unlock(); + + // std::string frame_id = input_pointcloud_msg.header.frame_id; // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "hello from poincloudCallback"); // add pointcloud to buffer @@ -696,21 +698,24 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase throw std::runtime_error("Buffer is empty"); } - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "buffer len is %ld", buffer.size()); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "buffer len is %ld", buffer.size()); sensor_msgs::msg::PointCloud2 result = buffer.front(); rclcpp::Duration diff = timestamp - rclcpp::Time(result.header.stamp); - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Set last getted pointcloud as result"); - - for (sensor_msgs::msg::PointCloud2 pointcloud : buffer){ - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Check pointcloud in buffer"); - if (diff > (timestamp - rclcpp::Time(pointcloud.header.stamp))) { - diff = timestamp - rclcpp::Time(pointcloud.header.stamp); - result = pointcloud; - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Update result"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Set last getted pointcloud as result"); + + pointCloudBuffer_mutex.lock(); + for (size_t i = 0; i != buffer.size(); i++){ + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Check pointcloud in buffer"); + if (diff.nanoseconds() > (timestamp - rclcpp::Time(buffer[i].header.stamp)).nanoseconds()) { + diff = timestamp - rclcpp::Time(buffer[i].header.stamp); + result = buffer[i]; + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Update result"); } } - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "return result"); + pointCloudBuffer_mutex.unlock(); + + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "return result"); return result; } @@ -725,6 +730,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::unique_ptr point_cloud_common; rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node; std::deque pointCloudBuffer; + std::mutex pointCloudBuffer_mutex; private: 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 e46234983e69..3b6d8cf64260 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 @@ -182,7 +182,7 @@ std::vector PredictedObjectsDisplay: if (pointCloudBuffer.empty()) { - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Pointcloud buffer is empty"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Pointcloud buffer is empty"); return markers; } // poincloud pub @@ -269,7 +269,7 @@ void PredictedObjectsDisplay::processPointCloud( *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { return; } - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Objects transformed"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Objects transformed"); // convert to pcl pointcloud pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); @@ -304,7 +304,7 @@ void PredictedObjectsDisplay::processPointCloud( filterPolygon(neighbor_pointcloud, out_cloud, unified_object); } - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Pointcloud filtered"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Pointcloud filtered"); sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( @@ -314,7 +314,7 @@ void PredictedObjectsDisplay::processPointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; add_pointcloud(output_pointcloud_msg_ptr); - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Published"); + // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Published"); } From 27f08975286427ff3b8ed1520aa9cba1d2853077 Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Thu, 6 Apr 2023 19:55:19 +0800 Subject: [PATCH 46/58] feat(add_perception_objects_pointcloud_publisher) code cleaning Signed-off-by: Alexey Panferov --- .../detected_objects_display.hpp | 1 - .../object_polygon_display_base.hpp | 25 ------------------- .../predicted_objects_display.hpp | 1 - .../tracked_objects_display.hpp | 1 - .../detected_objects_display.cpp | 1 - .../predicted_objects_display.cpp | 10 -------- .../tracked_objects_display.cpp | 1 - 7 files changed, 40 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp index a6e8d337a259..ae0d3bbbbf9c 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp @@ -41,7 +41,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay private: void processMessage(DetectedObjects::ConstSharedPtr msg) override; - // void onInitialize() override; void processPointCloud( const DetectedObjects::ConstSharedPtr & input_objs_msg, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg); diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 96b178e4aabe..00be7a4c011f 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -167,7 +167,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase "Input for pointcloud visualization of objects detection pipeline.", this, SLOT(RosTopicDisplay::updateTopic())); qos_profile_points_property = new rviz_common::properties::QosProfileProperty(m_default_pointcloud_topic, qos_profile_points); - // m_default_pointcloud_topic->setReadOnly(true); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { const auto & class_property_values = map_property_it.second; @@ -217,8 +216,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase tf_buffer = std::make_unique(raw_node->get_clock()); tf_listener = std::make_shared(*tf_buffer); - // m_default_pointcloud_topic->setValue(m_default_pointcloud_topic->c_str()); - point_cloud_common->initialize(this->context_, this->scene_node_); } @@ -629,22 +626,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase return; } pointCloudBuffer_mutex.lock(); - // Add a pointer to the new message to the front of the deque pointCloudBuffer.push_front(input_pointcloud_msg); - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Add poincloud to buffer"); - - // If the deque has more than 5 elements, remove the oldest element from the back of the deque if (pointCloudBuffer.size() > 5) { pointCloudBuffer.pop_back(); - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Drop poincloud from buffer"); - } pointCloudBuffer_mutex.unlock(); - - // std::string frame_id = input_pointcloud_msg.header.frame_id; - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "hello from poincloudCallback"); - - // add pointcloud to buffer } void filterPolygon( @@ -697,34 +683,23 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase // Handle the case where the buffer is empty throw std::runtime_error("Buffer is empty"); } - - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "buffer len is %ld", buffer.size()); - sensor_msgs::msg::PointCloud2 result = buffer.front(); rclcpp::Duration diff = timestamp - rclcpp::Time(result.header.stamp); - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Set last getted pointcloud as result"); - pointCloudBuffer_mutex.lock(); for (size_t i = 0; i != buffer.size(); i++){ - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Check pointcloud in buffer"); if (diff.nanoseconds() > (timestamp - rclcpp::Time(buffer[i].header.stamp)).nanoseconds()) { diff = timestamp - rclcpp::Time(buffer[i].header.stamp); result = buffer[i]; - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Update result"); } } pointCloudBuffer_mutex.unlock(); - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "return result"); 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; - - // message_filters::Subscriber perception_objects_subscription; - // rclcpp::Subscriber pointcloud_subscription; rclcpp::Subscription::SharedPtr pointcloud_subscription; // plugin to visualize pointclouds std::unique_ptr point_cloud_common; diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index 36f19e9a0f30..0345ecaf4fc7 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -49,7 +49,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay private: void processMessage(PredictedObjects::ConstSharedPtr msg) override; - // void onInitialize() override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) { diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp index 8bb64ccf7cf3..c946d54fe067 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp @@ -47,7 +47,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay private: void processMessage(TrackedObjects::ConstSharedPtr msg) override; - // void onInitialize() override; boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg) { 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 f6be310c4215..bfcb30a6a4b1 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 @@ -93,7 +93,6 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) // poincloud pub sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); - // sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); processPointCloud(msg, closest_pointcloud); } 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 3b6d8cf64260..eb014ee72053 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 @@ -182,16 +182,12 @@ std::vector PredictedObjectsDisplay: if (pointCloudBuffer.empty()) { - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Pointcloud buffer is empty"); return markers; } // poincloud pub sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); - // sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); - processPointCloud(msg, closest_pointcloud); - // processPointCloud(msg, pointCloudBuffer.front()); return markers; } @@ -269,8 +265,6 @@ void PredictedObjectsDisplay::processPointCloud( *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) { return; } - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Objects transformed"); - // convert to pcl pointcloud pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud); @@ -304,8 +298,6 @@ void PredictedObjectsDisplay::processPointCloud( filterPolygon(neighbor_pointcloud, out_cloud, unified_object); } - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Pointcloud filtered"); - sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2); @@ -314,8 +306,6 @@ void PredictedObjectsDisplay::processPointCloud( output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; add_pointcloud(output_pointcloud_msg_ptr); - // RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Published"); - } } // namespace object_detection 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 e6c61a87014d..1629d33c8be2 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 @@ -132,7 +132,6 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) // poincloud pub sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); - // sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared(pointCloudBuffer.front()); processPointCloud(msg, closest_pointcloud); } From bd5e4ec4a0497f13d96fb43fab8ede02f4a024fd Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 6 Apr 2023 12:16:05 +0000 Subject: [PATCH 47/58] style(pre-commit): autofix --- .../object_polygon_display_base.hpp | 84 +++++++++---------- .../detected_objects_display.cpp | 15 ++-- .../predicted_objects_display.cpp | 14 ++-- .../tracked_objects_display.cpp | 13 ++- 4 files changed, 58 insertions(+), 68 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 00be7a4c011f..90d181860fe4 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -102,7 +102,8 @@ inline pcl::PointXYZRGB toPCL(const geometry_msgs::msg::Point & point) } // 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) +inline bool comparePointCloudsByTimestamp( + const sensor_msgs::msg::PointCloud2 & pc1, const sensor_msgs::msg::PointCloud2 & pc2) { return (int(pc1.header.stamp.nanosec - pc2.header.stamp.nanosec)) < 0; } @@ -161,12 +162,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_simple_visualize_mode_property->addOption("Normal", 0); m_simple_visualize_mode_property->addOption("Simple", 1); m_publish_objs_pointcloud = new rviz_common::properties::BoolProperty( - "Publish Objects Pointcloud", true, "Enable/disable objects pointcloud publishing", this, SLOT(updatePalette())); + "Publish Objects Pointcloud", true, "Enable/disable objects pointcloud publishing", this, + SLOT(updatePalette())); 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(RosTopicDisplay::updateTopic())); - qos_profile_points_property = new rviz_common::properties::QosProfileProperty(m_default_pointcloud_topic, qos_profile_points); + qos_profile_points_property = new rviz_common::properties::QosProfileProperty( + m_default_pointcloud_topic, qos_profile_points); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { const auto & class_property_values = map_property_it.second; @@ -195,18 +198,17 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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(); - }); - + 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()); + 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 @@ -248,7 +250,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase points_subscribe(); } - void points_subscribe() { if (!m_publish_objs_pointcloud->getBool()) { @@ -257,31 +258,27 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase if (m_default_pointcloud_topic->isEmpty()) { RosTopicDisplay::setStatus( - rviz_common::properties::StatusProperty::Error, - "Topic", + 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 sstm; - sstm << "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(sstm.str().c_str())); - }; - - rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node(); + sub_opts.event_callbacks.message_lost_callback = [&](rclcpp::QOSMessageLostInfo & info) { + std::ostringstream sstm; + sstm << "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(sstm.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)); + 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( @@ -290,13 +287,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } - void unsubscribe() { RosTopicDisplay::unsubscribe(); - pointcloud_subscription.reset (); + pointcloud_subscription.reset(); RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Unsubscribe called"); - } void clear_markers() { m_marker_common.clearMarkers(); } @@ -607,7 +602,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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) { @@ -616,11 +611,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } return max_dist; } - + return std::nullopt; } - void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) + void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) { if (!m_publish_objs_pointcloud->getBool()) { return; @@ -628,7 +623,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase pointCloudBuffer_mutex.lock(); pointCloudBuffer.push_front(input_pointcloud_msg); if (pointCloudBuffer.size() > 5) { - pointCloudBuffer.pop_back(); + pointCloudBuffer.pop_back(); } pointCloudBuffer_mutex.unlock(); } @@ -677,16 +672,17 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } // 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) + 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"); + // Handle the case where the buffer is empty + throw std::runtime_error("Buffer is empty"); } - sensor_msgs::msg::PointCloud2 result = buffer.front(); + 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++){ + 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]; @@ -707,7 +703,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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; @@ -749,4 +744,3 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } // namespace autoware #endif // OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ - 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 c5f70e260713..4401674a2c8e 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 @@ -34,7 +34,6 @@ DetectedObjectsDisplay::DetectedObjectsDisplay() void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) { - clear_markers(); int id = 0; for (const auto & object : msg->objects) { @@ -86,14 +85,14 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) add_marker(twist_marker_ptr); } } - - if (pointCloudBuffer.empty()) - { - return; + + if (pointCloudBuffer.empty()) { + return; } // poincloud pub - sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( - getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = + std::make_shared( + getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); processPointCloud(msg, closest_pointcloud); } @@ -149,7 +148,7 @@ void DetectedObjectsDisplay::processPointCloud( pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - + add_pointcloud(output_pointcloud_msg_ptr); } 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 eb014ee72053..8dcf30a74b84 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 @@ -180,13 +180,13 @@ std::vector PredictedObjectsDisplay: } } - if (pointCloudBuffer.empty()) - { - return markers; + if (pointCloudBuffer.empty()) { + return markers; } // poincloud pub - sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( - getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = + std::make_shared( + getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); processPointCloud(msg, closest_pointcloud); return markers; @@ -220,7 +220,6 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) wall_dt, ros_dt); } - bool PredictedObjectsDisplay::transformObjects( const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, @@ -255,7 +254,6 @@ 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; } @@ -304,7 +302,7 @@ void PredictedObjectsDisplay::processPointCloud( pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - + add_pointcloud(output_pointcloud_msg_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 58e254d7db51..7371a25c4296 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 @@ -126,17 +126,16 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) } } - if (pointCloudBuffer.empty()) - { - return; + if (pointCloudBuffer.empty()) { + return; } // poincloud pub - sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( - getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); + 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) @@ -189,7 +188,7 @@ void TrackedObjectsDisplay::processPointCloud( pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr); output_pointcloud_msg_ptr->header = input_pointcloud_msg->header; - + add_pointcloud(output_pointcloud_msg_ptr); } From 7b7debcb765978fb6e8ede3af61ad4630e4fffde Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Thu, 6 Apr 2023 20:49:44 +0800 Subject: [PATCH 48/58] feat(add_perception_objects_pointcloud_publisher) fix pre-commit.ci - pr Signed-off-by: Alexey Panferov --- .../include/object_detection/object_polygon_display_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 90d181860fe4..90591dc0262f 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -105,7 +105,7 @@ inline pcl::PointXYZRGB toPCL(const geometry_msgs::msg::Point & point) inline bool comparePointCloudsByTimestamp( const sensor_msgs::msg::PointCloud2 & pc1, const sensor_msgs::msg::PointCloud2 & pc2) { - return (int(pc1.header.stamp.nanosec - pc2.header.stamp.nanosec)) < 0; + 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 From 22dd6c7ec3137c93a019cf864fa258d1af8500ea Mon Sep 17 00:00:00 2001 From: Alexey Panferov Date: Tue, 11 Apr 2023 15:09:40 +0800 Subject: [PATCH 49/58] feat(add_perception_objects_pointcloud_publisher) fix pointcloud topic update Signed-off-by: Alexey Panferov --- .../object_detection/object_polygon_display_base.hpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 90591dc0262f..e6870cda7f60 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -167,7 +167,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase 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(RosTopicDisplay::updateTopic())); + SLOT(updateTopic())); qos_profile_points_property = new rviz_common::properties::QosProfileProperty( m_default_pointcloud_topic, qos_profile_points); // iterate over default values to create and initialize the properties. @@ -240,7 +240,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase point_cloud_common->reset(); } - virtual void subscribe() + void subscribe() { if (!RosTopicDisplay::isEnabled()) { return; @@ -250,6 +250,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase points_subscribe(); } + void updateTopic() override + { + unsubscribe(); + reset(); + subscribe(); + this->context_->queueRender(); + } + void points_subscribe() { if (!m_publish_objs_pointcloud->getBool()) { From 647d1e3747a5f8e4cf8f6958c6c7d587b1145f31 Mon Sep 17 00:00:00 2001 From: KhalilSelyan Date: Wed, 22 May 2024 17:14:46 +0300 Subject: [PATCH 50/58] chore: add boost geometry lib for rounded functions on triangle list and line list 2d bounding box Signed-off-by: KhalilSelyan --- .../object_polygon_detail.hpp | 11 + .../object_polygon_detail.cpp | 189 ++++++++++++++++-- 2 files changed, 178 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 4f545d194b2c..d0c69c3bc0f9 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 @@ -32,6 +32,13 @@ #include #include +#include +#include +#include +#include +#include +#include + #include #include #include @@ -198,6 +205,10 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_lin const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_triangle_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & 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 7631acffafdf..6715c7d5ec59 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 @@ -549,15 +549,16 @@ 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_auto_perception_msgs::msg::Shape; if (shape_msg.type == Shape::BOUNDING_BOX) { - marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; - calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); + 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 { - calc_2d_bounding_box_bottom_orientation_line_list(shape_msg, marker_ptr->points); } } else if (shape_msg.type == Shape::CYLINDER) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; @@ -573,12 +574,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_auto_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) @@ -670,6 +741,32 @@ void calc_bounding_box_orientation_line_list( calc_line_list_from_points(point_list, point_pairs, 2, points); } +// void calc_2d_bounding_box_bottom_line_list( +// const autoware_auto_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; +// 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); +// } + void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) @@ -677,23 +774,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( From 79dd50dee49ae67927bf58889503577a0dea8257 Mon Sep 17 00:00:00 2001 From: Khalil Selyan <36904941+KhalilSelyan@users.noreply.github.com> Date: Fri, 24 May 2024 12:22:16 +0300 Subject: [PATCH 51/58] Update common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- .../src/object_detection/predicted_objects_display.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 6e60d7354146..9dec902c76ea 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 @@ -264,7 +264,7 @@ std::vector PredictedObjectsDisplay: if (pointCloudBuffer.empty()) { return markers; } - // poincloud pub + // pointcloud pub sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); From 8d0b5f9241e7544e2cf2a02f0cd1c772941a6022 Mon Sep 17 00:00:00 2001 From: Khalil Selyan <36904941+KhalilSelyan@users.noreply.github.com> Date: Fri, 24 May 2024 12:22:22 +0300 Subject: [PATCH 52/58] Update common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- .../src/object_detection/tracked_objects_display.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 948737aa4a7a..8120b7da795f 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 @@ -211,7 +211,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) if (pointCloudBuffer.empty()) { return; } - // poincloud pub + // pointcloud pub sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); From 255c7f6cdd11096331a61c6d7c9e47263caeef13 Mon Sep 17 00:00:00 2001 From: KhalilSelyan Date: Fri, 24 May 2024 12:29:12 +0300 Subject: [PATCH 53/58] fix: pre-commit ci Signed-off-by: KhalilSelyan --- .../src/object_detection/object_polygon_detail.cpp | 3 +++ 1 file changed, 3 insertions(+) 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 6715c7d5ec59..868914bce1bd 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 @@ -559,6 +559,9 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( 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 { + calc_2d_bounding_box_bottom_orientation_line_list(shape_msg, marker_ptr->points); } } else if (shape_msg.type == Shape::CYLINDER) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; From ec76a55867c246c4b3f490c438b0b1cb7b0df443 Mon Sep 17 00:00:00 2001 From: KhalilSelyan Date: Fri, 24 May 2024 12:29:47 +0300 Subject: [PATCH 54/58] chore: better logs for debugging re-renders Signed-off-by: KhalilSelyan --- .../object_polygon_display_base.hpp | 20 ++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) 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 24a3bce4f164..9d85f4257269 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 @@ -220,6 +220,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase void onInitialize() override { + static int init_count = 0; + RCLCPP_INFO( + rclcpp::get_logger("autoware_auto_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()); @@ -264,6 +269,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase void reset() override { + static int reset_count = 0; + RCLCPP_INFO( + rclcpp::get_logger("autoware_auto_perception_plugin"), "reset called %d times", + reset_count++); + RosTopicDisplay::reset(); m_marker_common.clearMarkers(); point_cloud_common->reset(); @@ -281,6 +291,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase void updateTopic() override { + static int update_count = 0; + RCLCPP_INFO( + rclcpp::get_logger("autoware_auto_perception_plugin"), "updateTopic called %d times", + update_count++); unsubscribe(); reset(); subscribe(); @@ -328,7 +342,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase { RosTopicDisplay::unsubscribe(); pointcloud_subscription.reset(); - RCLCPP_INFO(rclcpp::get_logger("autoware_auto_perception_plugin"), "Unsubscribe called"); + static int unsubscribe_count = 0; + RCLCPP_INFO( + rclcpp::get_logger("autoware_auto_perception_plugin"), "Unsubscribe called %d times", + unsubscribe_count++); } void clear_markers() { m_marker_common.clearMarkers(); } @@ -772,6 +789,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase if (!m_publish_objs_pointcloud->getBool()) { return; } + pointCloudBuffer_mutex.lock(); pointCloudBuffer.push_front(input_pointcloud_msg); if (pointCloudBuffer.size() > 5) { From 0de92037c6bfd9b782e8a8a318e675552332744a Mon Sep 17 00:00:00 2001 From: KhalilSelyan Date: Thu, 30 May 2024 00:52:03 +0300 Subject: [PATCH 55/58] chore: Fix typo Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Signed-off-by: KhalilSelyan --- .../object_detection/object_polygon_display_base.hpp | 2 +- .../src/object_detection/detected_objects_display.cpp | 2 +- 2 files changed, 2 insertions(+), 2 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 9d85f4257269..86d99540e0e5 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 @@ -317,7 +317,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase try { rclcpp::SubscriptionOptions sub_opts; sub_opts.event_callbacks.message_lost_callback = [&](rclcpp::QOSMessageLostInfo & info) { - std::ostringstream sstm; + std::ostringstream sstream; sstm << "Some messages were lost:\n>\tNumber of new lost messages: " << info.total_count_change << " \n>\tTotal number of messages lost: " << info.total_count; 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 7d10898eee9c..8e637fe7fbf8 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 @@ -161,7 +161,7 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) if (pointCloudBuffer.empty()) { return; } - // poincloud pub + // pointcloud pub sensor_msgs::msg::PointCloud2::ConstSharedPtr closest_pointcloud = std::make_shared( getNearestPointCloud(pointCloudBuffer, msg->header.stamp)); From a24b8f635ffb62f758a31b28e1f8b58226f0d9ce Mon Sep 17 00:00:00 2001 From: KhalilSelyan Date: Thu, 30 May 2024 00:52:22 +0300 Subject: [PATCH 56/58] refactor: Remove commented out code for calculating 2D bounding box bottom line list Signed-off-by: KhalilSelyan --- .../object_polygon_detail.cpp | 26 ------------------- 1 file changed, 26 deletions(-) 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 868914bce1bd..4bbf44520ed4 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 @@ -744,32 +744,6 @@ void calc_bounding_box_orientation_line_list( calc_line_list_from_points(point_list, point_pairs, 2, points); } -// void calc_2d_bounding_box_bottom_line_list( -// const autoware_auto_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; -// 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); -// } - void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) From 84db8c54be023138b38bf5f21eed1e764358af5a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 28 Jun 2024 15:42:05 +0000 Subject: [PATCH 57/58] style(pre-commit): autofix --- .../object_detection/object_polygon_display_base.hpp | 2 +- .../object_detection/tracked_objects_display.hpp | 2 +- common/autoware_perception_rviz_plugin/package.xml | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) 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 97192fce3806..fa13ffe21f24 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 @@ -928,4 +928,4 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ \ No newline at end of file +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ 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 96821d021897..379b89576b8d 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 @@ -120,4 +120,4 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay } // namespace rviz_plugins } // namespace autoware -#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ \ No newline at end of file +#endif // AUTOWARE_PERCEPTION_RVIZ_PLUGIN__OBJECT_DETECTION__TRACKED_OBJECTS_DISPLAY_HPP_ diff --git a/common/autoware_perception_rviz_plugin/package.xml b/common/autoware_perception_rviz_plugin/package.xml index 9215c774f24f..92bf2a2431bf 100644 --- a/common/autoware_perception_rviz_plugin/package.xml +++ b/common/autoware_perception_rviz_plugin/package.xml @@ -18,9 +18,10 @@ libboost-dev qtbase5-dev + autoware_perception_msgs + autoware_universe_utils pcl_conversions perception_utils - autoware_perception_msgs rviz_common rviz_default_plugins sensor_msgs @@ -29,7 +30,6 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - autoware_universe_utils libqt5-widgets From 69525aa2d8f4b7b2682da2b88758b227ee82a932 Mon Sep 17 00:00:00 2001 From: KhalilSelyan Date: Mon, 1 Jul 2024 18:55:12 +0300 Subject: [PATCH 58/58] chore: add check for empty pointcloud before calling setInputCloud Signed-off-by: KhalilSelyan --- .../src/object_detection/predicted_objects_display.cpp | 6 ++++++ 1 file changed, 6 insertions(+) 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 d9c49b1026b9..668d9a30f441 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 @@ -361,6 +361,12 @@ void PredictedObjectsDisplay::processPointCloud( // 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());