Skip to content

Commit 4082d58

Browse files
lexavtankelexavtanke
committed
feat(add_perception_objects_pointcloud_publisher) revert child classes
Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>
1 parent 61967dd commit 4082d58

7 files changed

+298
-313
lines changed

‎common/autoware_auto_perception_rviz_plugin/include/object_detection/detected_objects_display.hpp

+9-9
Original file line numberDiff line numberDiff line change
@@ -41,15 +41,15 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay
4141

4242
private:
4343
void processMessage(DetectedObjects::ConstSharedPtr msg) override;
44-
void onInitialize() override;
45-
void onObjectsAndObstaclePointCloud(
46-
const DetectedObjects::ConstSharedPtr & input_objs_msg,
47-
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg);
48-
49-
using SyncPolicy =
50-
message_filters::sync_policies::ApproximateTime<DetectedObjects, sensor_msgs::msg::PointCloud2>;
51-
using Sync = message_filters::Synchronizer<SyncPolicy>;
52-
std::shared_ptr<Sync> sync_ptr;
44+
// void onInitialize() override;
45+
// void onObjectsAndObstaclePointCloud(
46+
// const DetectedObjects::ConstSharedPtr & input_objs_msg,
47+
// const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg);
48+
49+
// using SyncPolicy =
50+
// message_filters::sync_policies::ApproximateTime<DetectedObjects, sensor_msgs::msg::PointCloud2>;
51+
// using Sync = message_filters::Synchronizer<SyncPolicy>;
52+
// std::shared_ptr<Sync> sync_ptr;
5353
};
5454

5555
} // namespace object_detection

‎common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -156,7 +156,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
156156
"Input pointcloud topic", QString::fromStdString(default_pointcloud_topic), "",
157157
"Input for pointcloud visualization of objects detection pipeline.", this,
158158
SLOT(updatePalette()));
159-
m_default_pointcloud_topic->setReadOnly(true);
159+
// m_default_pointcloud_topic->setReadOnly(true);
160160
// iterate over default values to create and initialize the properties.
161161
for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) {
162162
const auto & class_property_values = map_property_it.second;
@@ -212,6 +212,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
212212
RosTopicDisplay::reset();
213213
m_marker_common.clearMarkers();
214214
point_cloud_common->reset();
215+
m_default_pointcloud_topic->reset();
215216
}
216217

217218
void clear_markers() { m_marker_common.clearMarkers(); }

‎common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp

+13-13
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
4949

5050
private:
5151
void processMessage(PredictedObjects::ConstSharedPtr msg) override;
52-
void onInitialize() override;
52+
// void onInitialize() override;
5353

5454
boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg)
5555
{
@@ -104,14 +104,14 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
104104

105105
void update(float wall_dt, float ros_dt) override;
106106

107-
bool transformObjects(
108-
const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg,
109-
const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
110-
autoware_auto_perception_msgs::msg::PredictedObjects & output_msg);
107+
// bool transformObjects(
108+
// const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg,
109+
// const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
110+
// autoware_auto_perception_msgs::msg::PredictedObjects & output_msg);
111111

112-
void onObjectsAndObstaclePointCloud(
113-
const PredictedObjects::ConstSharedPtr & input_objs_msg,
114-
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg);
112+
// void onObjectsAndObstaclePointCloud(
113+
// const PredictedObjects::ConstSharedPtr & input_objs_msg,
114+
// const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg);
115115

116116
std::unordered_map<boost::uuids::uuid, int32_t, boost::hash<boost::uuids::uuid>> id_map;
117117
// std::unordered_map<boost::uuids::uuid, int32_t> id_map;
@@ -125,11 +125,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
125125
std::condition_variable condition;
126126
std::vector<visualization_msgs::msg::Marker::SharedPtr> markers;
127127

128-
typedef message_filters::sync_policies::ApproximateTime<
129-
PredictedObjects, sensor_msgs::msg::PointCloud2>
130-
SyncPolicy;
131-
typedef message_filters::Synchronizer<SyncPolicy> Sync;
132-
typename std::shared_ptr<Sync> sync_ptr;
128+
// typedef message_filters::sync_policies::ApproximateTime<
129+
// PredictedObjects, sensor_msgs::msg::PointCloud2>
130+
// SyncPolicy;
131+
// typedef message_filters::Synchronizer<SyncPolicy> Sync;
132+
// typename std::shared_ptr<Sync> sync_ptr;
133133
};
134134

135135
} // namespace object_detection

‎common/autoware_auto_perception_rviz_plugin/include/object_detection/tracked_objects_display.hpp

+9-9
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay
4747

4848
private:
4949
void processMessage(TrackedObjects::ConstSharedPtr msg) override;
50-
void onInitialize() override;
50+
// void onInitialize() override;
5151

5252
boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg)
5353
{
@@ -96,19 +96,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay
9696
return id_map.at(uuid);
9797
}
9898

99-
void onObjectsAndObstaclePointCloud(
100-
const TrackedObjects::ConstSharedPtr & input_objs_msg,
101-
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg);
99+
// void onObjectsAndObstaclePointCloud(
100+
// const TrackedObjects::ConstSharedPtr & input_objs_msg,
101+
// const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg);
102102

103103
std::map<boost::uuids::uuid, int32_t> id_map;
104104
std::list<int32_t> unused_marker_ids;
105105
int32_t marker_id = 0;
106106

107-
typedef message_filters::sync_policies::ApproximateTime<
108-
TrackedObjects, sensor_msgs::msg::PointCloud2>
109-
SyncPolicy;
110-
typedef message_filters::Synchronizer<SyncPolicy> Sync;
111-
typename std::shared_ptr<Sync> sync_ptr;
107+
// typedef message_filters::sync_policies::ApproximateTime<
108+
// TrackedObjects, sensor_msgs::msg::PointCloud2>
109+
// SyncPolicy;
110+
// typedef message_filters::Synchronizer<SyncPolicy> Sync;
111+
// typename std::shared_ptr<Sync> sync_ptr;
112112
};
113113

114114
} // namespace object_detection

‎common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp

+83-83
Original file line numberDiff line numberDiff line change
@@ -86,89 +86,89 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg)
8686
}
8787
}
8888

89-
void DetectedObjectsDisplay::onInitialize()
90-
{
91-
ObjectPolygonDisplayBase::onInitialize();
92-
// get access to rviz node to sub and to pub to topics
93-
rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node();
94-
95-
sync_ptr = std::make_shared<Sync>(
96-
SyncPolicy(10), perception_objects_subscription, pointcloud_subscription);
97-
sync_ptr->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this);
98-
perception_objects_subscription.subscribe(
99-
raw_node, "/perception/object_recognition/detection/objects",
100-
rclcpp::QoS{1}.get_rmw_qos_profile());
101-
pointcloud_subscription.subscribe(
102-
raw_node, m_default_pointcloud_topic->getTopic().toStdString(),
103-
rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile());
104-
}
105-
106-
void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud(
107-
const DetectedObjects::ConstSharedPtr & input_objs_msg,
108-
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg)
109-
{
110-
if (!m_publish_objs_pointcloud->getBool()) {
111-
return;
112-
}
113-
// Transform to pointcloud frame
114-
autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
115-
if (!transformObjects(
116-
*input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) {
117-
return;
118-
}
119-
120-
objs_buffer.clear();
121-
for (const auto & object : transformed_objects.objects) {
122-
std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> labels =
123-
object.classification;
124-
object_info info = {
125-
object.shape, object.kinematics.pose_with_covariance.pose, object.classification};
126-
objs_buffer.push_back(info);
127-
}
128-
129-
// convert to pcl pointcloud
130-
pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
131-
pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud);
132-
133-
// Create a new point cloud with RGB color information and copy data from input cloud
134-
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
135-
pcl::copyPointCloud(*temp_cloud, *colored_cloud);
136-
137-
// Create Kd-tree to search neighbor pointcloud to reduce cost.
138-
pcl::search::Search<pcl::PointXYZRGB>::Ptr kdtree =
139-
pcl::make_shared<pcl::search::KdTree<pcl::PointXYZRGB>>(false);
140-
kdtree->setInputCloud(colored_cloud);
141-
142-
pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
143-
144-
if (objs_buffer.size() > 0) {
145-
for (auto object : objs_buffer) {
146-
const auto search_radius = getMaxRadius(object);
147-
// Search neighbor pointcloud to reduce cost.
148-
pcl::PointCloud<pcl::PointXYZRGB>::Ptr neighbor_pointcloud(
149-
new pcl::PointCloud<pcl::PointXYZRGB>);
150-
std::vector<int> indices;
151-
std::vector<float> distances;
152-
kdtree->radiusSearch(
153-
toPCL(object.position.position), search_radius.value(), indices, distances);
154-
for (const auto & index : indices) {
155-
neighbor_pointcloud->push_back(colored_cloud->at(index));
156-
}
157-
158-
filterPolygon(neighbor_pointcloud, out_cloud, object);
159-
}
160-
} else {
161-
return;
162-
}
163-
164-
sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr(
165-
new sensor_msgs::msg::PointCloud2);
166-
pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr);
167-
168-
output_pointcloud_msg_ptr->header = input_pointcloud_msg->header;
169-
170-
add_pointcloud(output_pointcloud_msg_ptr);
171-
}
89+
// void DetectedObjectsDisplay::onInitialize()
90+
// {
91+
// ObjectPolygonDisplayBase::onInitialize();
92+
// // get access to rviz node to sub and to pub to topics
93+
// rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node();
94+
95+
// sync_ptr = std::make_shared<Sync>(
96+
// SyncPolicy(10), perception_objects_subscription, pointcloud_subscription);
97+
// sync_ptr->registerCallback(&DetectedObjectsDisplay::onObjectsAndObstaclePointCloud, this);
98+
// perception_objects_subscription.subscribe(
99+
// raw_node, "/perception/object_recognition/detection/objects",
100+
// rclcpp::QoS{1}.get_rmw_qos_profile());
101+
// pointcloud_subscription.subscribe(
102+
// raw_node, m_default_pointcloud_topic->getTopic().toStdString(),
103+
// rclcpp::SensorDataQoS{}.keep_last(1).get_rmw_qos_profile());
104+
// }
105+
106+
// void DetectedObjectsDisplay::onObjectsAndObstaclePointCloud(
107+
// const DetectedObjects::ConstSharedPtr & input_objs_msg,
108+
// const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud_msg)
109+
// {
110+
// if (!m_publish_objs_pointcloud->getBool()) {
111+
// return;
112+
// }
113+
// // Transform to pointcloud frame
114+
// autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
115+
// if (!transformObjects(
116+
// *input_objs_msg, input_pointcloud_msg->header.frame_id, *tf_buffer, transformed_objects)) {
117+
// return;
118+
// }
119+
120+
// objs_buffer.clear();
121+
// for (const auto & object : transformed_objects.objects) {
122+
// std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> labels =
123+
// object.classification;
124+
// object_info info = {
125+
// object.shape, object.kinematics.pose_with_covariance.pose, object.classification};
126+
// objs_buffer.push_back(info);
127+
// }
128+
129+
// // convert to pcl pointcloud
130+
// pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
131+
// pcl::fromROSMsg(*input_pointcloud_msg, *temp_cloud);
132+
133+
// // Create a new point cloud with RGB color information and copy data from input cloud
134+
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
135+
// pcl::copyPointCloud(*temp_cloud, *colored_cloud);
136+
137+
// // Create Kd-tree to search neighbor pointcloud to reduce cost.
138+
// pcl::search::Search<pcl::PointXYZRGB>::Ptr kdtree =
139+
// pcl::make_shared<pcl::search::KdTree<pcl::PointXYZRGB>>(false);
140+
// kdtree->setInputCloud(colored_cloud);
141+
142+
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
143+
144+
// if (objs_buffer.size() > 0) {
145+
// for (auto object : objs_buffer) {
146+
// const auto search_radius = getMaxRadius(object);
147+
// // Search neighbor pointcloud to reduce cost.
148+
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr neighbor_pointcloud(
149+
// new pcl::PointCloud<pcl::PointXYZRGB>);
150+
// std::vector<int> indices;
151+
// std::vector<float> distances;
152+
// kdtree->radiusSearch(
153+
// toPCL(object.position.position), search_radius.value(), indices, distances);
154+
// for (const auto & index : indices) {
155+
// neighbor_pointcloud->push_back(colored_cloud->at(index));
156+
// }
157+
158+
// filterPolygon(neighbor_pointcloud, out_cloud, object);
159+
// }
160+
// } else {
161+
// return;
162+
// }
163+
164+
// sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr(
165+
// new sensor_msgs::msg::PointCloud2);
166+
// pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr);
167+
168+
// output_pointcloud_msg_ptr->header = input_pointcloud_msg->header;
169+
170+
// add_pointcloud(output_pointcloud_msg_ptr);
171+
// }
172172

173173
} // namespace object_detection
174174
} // namespace rviz_plugins

0 commit comments

Comments
 (0)