Skip to content

Commit 7d88491

Browse files
lexavtankelexavtanke
committed
feat(add_perception_objects_pointcloud_publisher) update qos policy, use objectsCallback to update objects
Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>
1 parent d52e249 commit 7d88491

7 files changed

+160
-288
lines changed

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

+1
Original file line numberDiff line numberDiff line change
@@ -40,6 +40,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay
4040
private:
4141
void processMessage(DetectedObjects::ConstSharedPtr msg) override;
4242
void onInitialize() override;
43+
void objectsCallback(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objs_msg) override;
4344
// void pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg) override;
4445

4546

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

+70-59
Original file line numberDiff line numberDiff line change
@@ -73,13 +73,15 @@ namespace object_detection
7373
{
7474

7575
using Polygon2d = tier4_autoware_utils::Polygon2d;
76+
using Shape = autoware_auto_perception_msgs::msg::Shape;
7677

7778
// struct for creating objects buffer
7879
struct object_info
7980
{
80-
autoware_auto_perception_msgs::msg::Shape shape;
81+
Shape shape;
8182
geometry_msgs::msg::Pose position;
82-
autoware_auto_perception_msgs::msg::ObjectClassification classification;
83+
// autoware_auto_perception_msgs::msg::ObjectClassification classification;
84+
std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> classification;
8385
};
8486

8587
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
185187

186188
// get access to rivz node to sub and to pub to topics
187189
rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node();
188-
publisher_ = raw_node->create_publisher<sensor_msgs::msg::PointCloud2>("output/pointcloud", 10);
190+
publisher_ = raw_node->create_publisher<sensor_msgs::msg::PointCloud2>("output/pointcloud", rclcpp::SensorDataQoS());
189191
pointcloud_subscription_ = raw_node->create_subscription<sensor_msgs::msg::PointCloud2>(
190192
m_default_pointcloud_topic->getTopicStd(),
191193
rclcpp::SensorDataQoS(),
@@ -224,8 +226,62 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
224226
{
225227
m_marker_common.addMessage(markers_ptr);
226228
}
229+
230+
// transfrom detected object pose to target frame and return bool result
231+
bool transformObjects(
232+
const MsgT & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
233+
MsgT & output_msg)
234+
{
235+
output_msg = input_msg;
236+
237+
// transform to world coordinate
238+
if (input_msg.header.frame_id != target_frame_id) {
239+
output_msg.header.frame_id = target_frame_id;
240+
tf2::Transform tf_target2objects_world;
241+
tf2::Transform tf_target2objects;
242+
tf2::Transform tf_objects_world2objects;
243+
{
244+
const auto ros_target2objects_world = getTransform(
245+
tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp);
246+
if (!ros_target2objects_world) {
247+
return false;
248+
}
249+
tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world);
250+
}
251+
for (auto & object : output_msg.objects) {
252+
tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects);
253+
tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
254+
tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose);
255+
}
256+
}
257+
return true;
258+
}
259+
260+
// get transformation from tf2
261+
boost::optional<geometry_msgs::msg::Transform> getTransform(
262+
const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
263+
const std::string & target_frame_id, const rclcpp::Time & time)
264+
{
265+
try {
266+
geometry_msgs::msg::TransformStamped self_transform_stamped;
267+
self_transform_stamped = tf_buffer.lookupTransform(
268+
target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5));
269+
return self_transform_stamped.transform;
270+
} catch (tf2::TransformException & ex) {
271+
RCLCPP_WARN_STREAM(rclcpp::get_logger("perception_utils"), ex.what()); // rename
272+
return boost::none;
273+
}
274+
}
275+
227276

277+
// std::string objects_frame_id_;
278+
// variables for transfer detected objects information between callbacks
279+
std::vector<object_info> objs_buffer;
228280
std::string objects_frame_id_;
281+
std::string pointcloud_frame_id_;
282+
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr}; // !! different type in prototype,
283+
std::unique_ptr<tf2_ros::Buffer> tf_buffer_; // !! different type in prototype
284+
rclcpp::Node::SharedPtr raw_node;
229285

230286
protected:
231287
/// \brief Convert given shape msg into a Marker
@@ -462,51 +518,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
462518

463519
double get_line_width() { return m_line_width_property.getFloat(); }
464520

465-
// get transformation from tf2
466-
boost::optional<geometry_msgs::msg::Transform> getTransform(
467-
const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id,
468-
const std::string & target_frame_id, const rclcpp::Time & time)
469-
{
470-
try {
471-
geometry_msgs::msg::TransformStamped self_transform_stamped;
472-
self_transform_stamped = tf_buffer.lookupTransform(
473-
target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5));
474-
return self_transform_stamped.transform;
475-
} catch (tf2::TransformException & ex) {
476-
RCLCPP_WARN_STREAM(rclcpp::get_logger("perception_utils"), ex.what()); // rename
477-
return boost::none;
478-
}
479-
}
480521

481-
// transfrom detected object pose to target frame and return bool result
482-
bool transformObjects(
483-
const MsgT & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
484-
MsgT & output_msg)
485-
{
486-
output_msg = input_msg;
487522

488-
// transform to world coordinate
489-
if (input_msg.header.frame_id != target_frame_id) {
490-
output_msg.header.frame_id = target_frame_id;
491-
tf2::Transform tf_target2objects_world;
492-
tf2::Transform tf_target2objects;
493-
tf2::Transform tf_objects_world2objects;
494-
{
495-
const auto ros_target2objects_world = getTransform(
496-
tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp);
497-
if (!ros_target2objects_world) {
498-
return false;
499-
}
500-
tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world);
501-
}
502-
for (auto & object : output_msg.objects) {
503-
tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects);
504-
tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
505-
tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose);
506-
}
507-
}
508-
return true;
509-
}
510523

511524
// helper function to get radius for kd-search
512525
std::optional<float> getMaxRadius(object_info & object)
@@ -521,24 +534,22 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
521534
}
522535
return max_dist;
523536
} else {
524-
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "unknown shape type");
537+
// RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "unknown shape type");
525538
return std::nullopt;
526539
}
527540
}
541+
542+
virtual void objectsCallback(typename MsgT::ConstSharedPtr msg) = 0;
528543

529-
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr} // !! different type in prototype,
530-
std::unique_ptr<tf2_ros::Buffer> tf_buffer_; // !! different type in prototype
544+
531545
// add pointcloud subscription
532546
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_subscription_;
533547
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
534548

535549
// Default pointcloud topic;
536550
rviz_common::properties::RosTopicProperty * m_default_pointcloud_topic;
537551

538-
// variables for transfer detected objects information between callbacks
539-
std::vector<object_info> objs_buffer;
540-
std::string objects_frame_id_;
541-
std::string pointcloud_frame_id_;
552+
542553

543554
private:
544555
// 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
579590
// RCLCPP_INFO(this->get_logger(), "Get new pointcloud");
580591
pointcloud_frame_id_ = input_pointcloud_msg.header.frame_id;
581592

582-
pcl::PointXYZ minPt, maxPt;
583-
pcl::PointCloud<pcl::PointXYZ> measured_cloud;
584-
pcl::fromROSMsg(input_pointcloud_msg, measured_cloud);
585-
pcl::getMinMax3D(measured_cloud, minPt, maxPt);
593+
// pcl::PointXYZ minPt, maxPt;
594+
// pcl::PointCloud<pcl::PointXYZ> measured_cloud;
595+
// pcl::fromROSMsg(input_pointcloud_msg, measured_cloud);
596+
// pcl::getMinMax3D(measured_cloud, minPt, maxPt);
586597

587598
// RCLCPP_INFO(this->get_logger(), "before translation max X is '%f' max Y is '%f'", maxPt.x, maxPt.y);
588599
// 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
625636
filterPolygon(neighbor_pointcloud, out_cloud, object);
626637
}
627638
} else {
628-
RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty");
639+
// RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "objects buffer is empty");
629640
return;
630641
}
631642

@@ -636,7 +647,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
636647
// *output_pointcloud_msg_ptr = transformed_pointcloud;
637648

638649
output_pointcloud_msg_ptr->header = input_pointcloud_msg.header;
639-
output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; // remove
650+
// output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_; // remove
640651

641652
// RCLCPP_INFO(this->get_logger(), "Publishing pointcloud");
642653
publisher_->publish(*output_pointcloud_msg_ptr);

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

+2
Original file line numberDiff line numberDiff line change
@@ -108,6 +108,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
108108
const autoware_auto_perception_msgs::msg::PredictedObjects & input_msg,
109109
const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
110110
autoware_auto_perception_msgs::msg::PredictedObjects & output_msg);
111+
112+
void objectsCallback(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr input_objs_msg) override;
111113

112114
std::unordered_map<boost::uuids::uuid, int32_t, boost::hash<boost::uuids::uuid>> id_map;
113115
// std::unordered_map<boost::uuids::uuid, int32_t> id_map;

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

+2
Original file line numberDiff line numberDiff line change
@@ -96,6 +96,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay
9696
return id_map.at(uuid);
9797
}
9898

99+
void objectsCallback(const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr input_objs_msg) override;
100+
99101
std::map<boost::uuids::uuid, int32_t> id_map;
100102
std::list<int32_t> unused_marker_ids;
101103
int32_t marker_id = 0;

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

+23-99
Original file line numberDiff line numberDiff line change
@@ -80,33 +80,17 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg)
8080
}
8181

8282
// Transform to pointcloud frame
83-
autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
84-
if (!transformObjects(
85-
*msg, pointcloud_frame_id_, tf_buffer_,
86-
transformed_objects)) {
87-
// objects_pub_->publish(*input_objects);
88-
return;
89-
}
90-
91-
objects_frame_id_ = transformed_objects.header.frame_id; // remove
92-
93-
objs_buffer.clear();
94-
for (const auto & object : transformed_objects.objects)
95-
{
96-
std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> labels = object.classification;
97-
object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification};
98-
objs_buffer.push_back(info);
99-
}
10083
// RCLCPP_INFO(this->get_logger(), "Update objects buffer");
10184
}
85+
objectsCallback(msg);
10286
}
10387

104-
void DetectedObjectsDisplay::onInitialize() override
88+
void DetectedObjectsDisplay::onInitialize()
10589
{
10690
ObjectPolygonDisplayBase::onInitialize();
10791
// get access to rivz node to sub and to pub to topics
10892
rclcpp::Node::SharedPtr raw_node = this->context_->getRosNodeAbstraction().lock()->get_raw_node();
109-
publisher_ = raw_node->create_publisher<sensor_msgs::msg::PointCloud2>("output/detected_objects_pointcloud", 10);
93+
publisher_ = raw_node->create_publisher<sensor_msgs::msg::PointCloud2>("output/detected_objects_pointcloud", rclcpp::SensorDataQoS());
11094
// pointcloud_subscription_ = raw_node->create_subscription<sensor_msgs::msg::PointCloud2>(
11195
// m_default_pointcloud_topic->getTopicStd(),
11296
// rclcpp::SensorDataQoS(),
@@ -115,89 +99,29 @@ void DetectedObjectsDisplay::onInitialize() override
11599
// std::placeholders::_1));
116100
}
117101

118-
// void DetectedObjectsDisplay::pointCloudCallback(const sensor_msgs::msg::PointCloud2 input_pointcloud_msg)
119-
// {
120-
// sensor_msgs::msg::PointCloud2 transformed_pointcloud;
121-
// if (objects_frame_id_ != "" && input_pointcloud_msg.header.frame_id != objects_frame_id_)
122-
// {
123-
// geometry_msgs::msg::TransformStamped transform;
124-
// transform = tf_buffer_->lookupTransform(
125-
// input_pointcloud_msg.header.frame_id, objects_frame_id_, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0));
126-
127-
// tf2::doTransform(input_pointcloud_msg, transformed_pointcloud, transform);
128-
129-
// } else {
130-
// transformed_pointcloud = input_pointcloud_msg;
131-
// }
132-
133-
// pcl::PCLPointCloud2 input_pcl_cloud;
134-
// pcl_conversions::toPCL(transformed_pointcloud, input_pcl_cloud);
135-
// pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
136-
// pcl::fromPCLPointCloud2(input_pcl_cloud, *temp_cloud);
137-
138-
// // Create a new point cloud with RGB color information and copy data from input cloudb
139-
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
140-
// pcl::copyPointCloud(*temp_cloud, *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-
147-
// if (object.shape.type == 0)
148-
// {
149-
// pcl::PointCloud<pcl::PointXYZRGB> filtered_cloud;
150-
// pcl::CropBox<pcl::PointXYZRGB> cropBoxFilter (true);
151-
// cropBoxFilter.setInputCloud(colored_cloud);
152-
// float trans_x = object.kinematics.pose_with_covariance.pose.position.x;
153-
// float trans_y = object.kinematics.pose_with_covariance.pose.position.y;
154-
// float trans_z = object.kinematics.pose_with_covariance.pose.position.z;
155-
// float max_x = trans_x + object.shape.dimensions.x / 2.0;
156-
// float min_x = trans_x - object.shape.dimensions.x / 2.0;
157-
// float max_y = trans_y + object.shape.dimensions.y / 2.0;
158-
// float min_y = trans_y - object.shape.dimensions.y / 2.0;
159-
// float max_z = trans_z + object.shape.dimensions.z / 2.0;
160-
// float min_z = trans_z - object.shape.dimensions.z / 2.0;
161-
162-
// Eigen::Vector4f min_pt (min_x, min_y, min_z, 0.0f);
163-
// Eigen::Vector4f max_pt (max_x, max_y, max_z, 0.0f);
164-
// cropBoxFilter.setMin(min_pt);
165-
// cropBoxFilter.setMax(max_pt);
166-
// cropBoxFilter.filter(filtered_cloud);
167-
168-
// // Define a custom color for the box polygons
169-
// const uint8_t r = 30, g = 44, b = 255;
102+
void DetectedObjectsDisplay::objectsCallback(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objs_msg)
103+
{
170104

171-
// for (auto cloud_it = filtered_cloud.begin(); cloud_it != filtered_cloud.end(); ++cloud_it)
172-
// {
173-
// cloud_it->r = r;
174-
// cloud_it->g = g;
175-
// cloud_it->b = b;
176-
// }
177-
178-
// *out_cloud += filtered_cloud;
179-
180-
// }
181-
182-
// }
183-
184-
// }
185-
186-
// sensor_msgs::msg::PointCloud2::SharedPtr output_pointcloud_msg_ptr( new sensor_msgs::msg::PointCloud2);
187-
// // pcl::toROSMsg(*colored_cloud, *output_pointcloud_msg_ptr);
105+
// Transform to pointcloud frame
106+
autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
107+
if (!transformObjects(
108+
*input_objs_msg, pointcloud_frame_id_, *tf_buffer_,
109+
transformed_objects)) {
110+
// objects_pub_->publish(*input_objects);
111+
return;
112+
}
188113

189-
// pcl::toROSMsg(*out_cloud, *output_pointcloud_msg_ptr);
190-
// output_pointcloud_msg_ptr->header = input_pointcloud_msg.header;
191-
// output_pointcloud_msg_ptr->header.frame_id = objects_frame_id_;
114+
objects_frame_id_ = transformed_objects.header.frame_id;
192115

193-
// // TODO(lexavtanke) get pointcloud in frame base link and detected objects in frame map
194-
// // change pointcloud frame to map
195-
// // update color of the points and remove all points outside deceted objects
196-
197-
198-
// // RCLCPP_INFO(this->get_logger(), "Publishing pointcloud");
199-
// publisher_->publish(*output_pointcloud_msg_ptr);
200-
// }
116+
objs_buffer.clear();
117+
for (const auto & object : transformed_objects.objects)
118+
{
119+
std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> labels = object.classification;
120+
object_info info = {object.shape, object.kinematics.pose_with_covariance.pose, object.classification};
121+
objs_buffer.push_back(info);
122+
}
123+
// RCLCPP_INFO(this->get_logger(), "Update objects buffer");
124+
}
201125

202126
} // namespace object_detection
203127
} // namespace rviz_plugins

0 commit comments

Comments
 (0)