From 88b3133e1836db20bc47486dd7ae54cbb5e4e287 Mon Sep 17 00:00:00 2001 From: ktro2828 Date: Sat, 17 Aug 2024 08:08:42 +0900 Subject: [PATCH 1/4] feat(awviz_plugin): add support of `autoware_perception_msgs::msg::PredictedObjects` Signed-off-by: ktro2828 --- .../predicted_objects_display.hpp | 45 ++++++++++++ awviz_plugin/plugin_description.xml | 5 ++ .../predicted_objects_display.cpp | 72 +++++++++++++++++++ 3 files changed, 122 insertions(+) create mode 100644 awviz_plugin/include/awviz_plugin/autoware_perception/predicted_objects_display.hpp create mode 100644 awviz_plugin/src/autoware_perception/predicted_objects_display.cpp diff --git a/awviz_plugin/include/awviz_plugin/autoware_perception/predicted_objects_display.hpp b/awviz_plugin/include/awviz_plugin/autoware_perception/predicted_objects_display.hpp new file mode 100644 index 0000000..c57fd3b --- /dev/null +++ b/awviz_plugin/include/awviz_plugin/autoware_perception/predicted_objects_display.hpp @@ -0,0 +1,45 @@ +// Copyright 2024 Kotaro Uetake. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AWVIZ_PLUGIN__AUTOWARE_PERCEPTION__PREDICTED_OBJECTS_DISPLAY_HPP_ +#define AWVIZ_PLUGIN__AUTOWARE_PERCEPTION__PREDICTED_OBJECTS_DISPLAY_HPP_ + +#include +#include + +#include + +namespace awviz_plugin +{ +/** + * @brief Display plugin of `autoware_perception_msgs::msg::PredictedObjects`. + */ +class PredictedObjectsDisplay +: public awviz_common::RosTopicDisplay +{ +public: + /** + * @brief Construct a new object. + */ + PredictedObjectsDisplay(); + +protected: + /** + * @todo Add support of colorizing each predicted path mode. + */ + void log_message(autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) override; +}; +} // namespace awviz_plugin + +#endif // AWVIZ_PLUGIN__AUTOWARE_PERCEPTION__PREDICTED_OBJECTS_DISPLAY_HPP_ diff --git a/awviz_plugin/plugin_description.xml b/awviz_plugin/plugin_description.xml index fa8670d..6d25336 100644 --- a/awviz_plugin/plugin_description.xml +++ b/awviz_plugin/plugin_description.xml @@ -42,4 +42,9 @@ Display from autoware_perception_msgs/msg/TrackedObjects message. autoware_perception_msgs/msg/TrackedObjects + + Display from autoware_perception_msgs/msg/PredictedObjects message. + autoware_perception_msgs/msg/PredictedObjects + diff --git a/awviz_plugin/src/autoware_perception/predicted_objects_display.cpp b/awviz_plugin/src/autoware_perception/predicted_objects_display.cpp new file mode 100644 index 0000000..2f2bce8 --- /dev/null +++ b/awviz_plugin/src/autoware_perception/predicted_objects_display.cpp @@ -0,0 +1,72 @@ +// Copyright 2024 Kotaro Uetake. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "awviz_plugin/autoware_perception/predicted_objects_display.hpp" + +#include + +namespace awviz_plugin +{ +PredictedObjectsDisplay::PredictedObjectsDisplay() +: awviz_common::RosTopicDisplay() +{ +} + +void PredictedObjectsDisplay::log_message( + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) +{ + stream_->set_time_seconds( + TIMELINE_NAME, rclcpp::Time(msg->header.stamp.sec, msg->header.stamp.nanosec).seconds()); + + const auto entity_path = property_.entity(msg->header.frame_id); + if (!entity_path) { + stream_->log(property_.topic(), rerun::TextLog("There is no corresponding entity path")); + return; + } + + std::vector centers; + std::vector sizes; + std::vector rotations; + std::vector class_ids; + std::vector paths; + for (const auto & object : msg->objects) { + const auto & init_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto & dimensions = object.shape.dimensions; + centers.emplace_back(init_pose.position.x, init_pose.position.y, init_pose.position.z); + sizes.emplace_back(dimensions.x, dimensions.y, dimensions.z); + rotations.emplace_back(rerun::Quaternion::from_wxyz( + init_pose.orientation.w, init_pose.orientation.x, init_pose.orientation.y, + init_pose.orientation.z)); + class_ids.emplace_back(static_cast(object.classification.front().label)); + + for (const auto & path : object.kinematics.predicted_paths) { + std::vector waypoints; + for (const auto & point : path.path) { + waypoints.emplace_back(point.position.x, point.position.y, point.position.z); + } + paths.emplace_back(rerun::LineStrip3D(waypoints)); + } + } + + stream_->log( + entity_path.value(), + rerun::Boxes3D::from_centers_and_half_sizes(centers, sizes) + .with_rotations(rotations) + .with_class_ids(class_ids), + rerun::LineStrips3D(paths)); +} +} // namespace awviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS(awviz_plugin::PredictedObjectsDisplay, awviz_common::Display); From 8b9ab10b489848e05876b9865caea803e00c926b Mon Sep 17 00:00:00 2001 From: ktro2828 Date: Sat, 26 Oct 2024 16:50:41 +0900 Subject: [PATCH 2/4] feat: move a file and add new collection adapters Signed-off-by: ktro2828 --- .../{image => }/collection_adapter.hpp | 61 +++++++++++++++++-- .../src/image/compressed_image_display.cpp | 2 +- awviz_plugin/src/image/image_display.cpp | 2 +- 3 files changed, 57 insertions(+), 8 deletions(-) rename awviz_plugin/include/awviz_plugin/{image => }/collection_adapter.hpp (50%) diff --git a/awviz_plugin/include/awviz_plugin/image/collection_adapter.hpp b/awviz_plugin/include/awviz_plugin/collection_adapter.hpp similarity index 50% rename from awviz_plugin/include/awviz_plugin/image/collection_adapter.hpp rename to awviz_plugin/include/awviz_plugin/collection_adapter.hpp index 7267a47..0066987 100644 --- a/awviz_plugin/include/awviz_plugin/image/collection_adapter.hpp +++ b/awviz_plugin/include/awviz_plugin/collection_adapter.hpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AWVIZ_PLUGIN__IMAGE__COLLECTION_ADAPTER_HPP_ -#define AWVIZ_PLUGIN__IMAGE__COLLECTION_ADAPTER_HPP_ +#ifndef AWVIZ_PLUGIN__COLLECTION_ADAPTER_HPP_ +#define AWVIZ_PLUGIN__COLLECTION_ADAPTER_HPP_ +#include #include #include #include +#include #include #include @@ -30,15 +32,25 @@ namespace rerun template struct CollectionAdapter { - /// Borrow for non-temporary. + /** + * @brief Borrow for non-temporary. + * + * @param img OpenCV matrix. + * @return Collection + */ Collection operator()(const cv::Mat & img) { return Collection::borrow( reinterpret_cast(img.data), img.total() * img.channels()); } - // Do a full copy for temporaries (otherwise the data might be deleted when the temporary is - // destroyed). + /** + * @brief Do a full copy for temporaries (otherwise the data might be deleted when the temporary + * is destroyed). + * + * @param img OpenCV matrix. + * @return Collection + */ Collection operator()(cv::Mat && img) { std::vector img_vec(img.total() * img.channels()); @@ -46,6 +58,43 @@ struct CollectionAdapter return Collection::take_ownership(std::move(img_vec)); } }; + +/** + * @brief An adaptor to be able to borrow an Eigen matrix into Rerun 3D position without copying. + */ +template <> +struct CollectionAdapter> +{ + // Sanity check that this is binary compatible. + static_assert( + sizeof(rerun::Position3D) == + sizeof(Eigen::Matrix3Xf::Scalar) * Eigen::Matrix3Xf::RowsAtCompileTime); + + /** + * @brief Borrow for non-temporary. + * + * @param container Eigen 3D float vector. + * @return Collection + */ + Collection operator()(const std::vector & container) + { + return Collection::borrow(container.data(), container.size()); + } + + /** + * @brief Do a full copy for temporaries (otherwise the data might be deleted when the temporary + * is destroyed). + * + * @param container Eigen 3D float vector. + * @return Collection + */ + Collection operator()(std::vector && container) + { + std::vector positions(container.size()); + memcpy(positions.data(), container.data(), container.size() * sizeof(Eigen::Vector3f)); + return Collection::take_ownership(std::move(positions)); + } +}; } // namespace rerun namespace awviz_plugin @@ -64,4 +113,4 @@ inline rerun::Collection tensor_shape(const cv::Mat & im } } // namespace awviz_plugin -#endif // AWVIZ_PLUGIN__IMAGE__IMAGE_COMPRESSED_IMAGE_DISPLAY_HPP_ +#endif diff --git a/awviz_plugin/src/image/compressed_image_display.cpp b/awviz_plugin/src/image/compressed_image_display.cpp index 0fb2f6a..85d6cce 100644 --- a/awviz_plugin/src/image/compressed_image_display.cpp +++ b/awviz_plugin/src/image/compressed_image_display.cpp @@ -14,7 +14,7 @@ #include "awviz_plugin/image/compressed_image_display.hpp" -#include "awviz_plugin/image/collection_adapter.hpp" +#include "awviz_plugin/collection_adapter.hpp" #include diff --git a/awviz_plugin/src/image/image_display.cpp b/awviz_plugin/src/image/image_display.cpp index 41c5af9..7f2df27 100644 --- a/awviz_plugin/src/image/image_display.cpp +++ b/awviz_plugin/src/image/image_display.cpp @@ -14,7 +14,7 @@ #include "awviz_plugin/image/image_display.hpp" -#include "awviz_plugin/image/collection_adapter.hpp" +#include "awviz_plugin/collection_adapter.hpp" #include From 7187852bc08d39291f8e48de693fb2dcdf1b5c22 Mon Sep 17 00:00:00 2001 From: ktro2828 Date: Fri, 15 Nov 2024 03:54:42 +0900 Subject: [PATCH 3/4] feat: update params Signed-off-by: ktro2828 --- .../src/autoware_perception/predicted_objects_display.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/awviz_plugin/src/autoware_perception/predicted_objects_display.cpp b/awviz_plugin/src/autoware_perception/predicted_objects_display.cpp index 2f2bce8..0b06cae 100644 --- a/awviz_plugin/src/autoware_perception/predicted_objects_display.cpp +++ b/awviz_plugin/src/autoware_perception/predicted_objects_display.cpp @@ -14,6 +14,8 @@ #include "awviz_plugin/autoware_perception/predicted_objects_display.hpp" +#include + #include namespace awviz_plugin @@ -37,7 +39,7 @@ void PredictedObjectsDisplay::log_message( std::vector centers; std::vector sizes; - std::vector rotations; + std::vector quaternions; std::vector class_ids; std::vector paths; for (const auto & object : msg->objects) { @@ -45,7 +47,7 @@ void PredictedObjectsDisplay::log_message( const auto & dimensions = object.shape.dimensions; centers.emplace_back(init_pose.position.x, init_pose.position.y, init_pose.position.z); sizes.emplace_back(dimensions.x, dimensions.y, dimensions.z); - rotations.emplace_back(rerun::Quaternion::from_wxyz( + quaternions.emplace_back(rerun::Quaternion::from_wxyz( init_pose.orientation.w, init_pose.orientation.x, init_pose.orientation.y, init_pose.orientation.z)); class_ids.emplace_back(static_cast(object.classification.front().label)); @@ -62,7 +64,7 @@ void PredictedObjectsDisplay::log_message( stream_->log( entity_path.value(), rerun::Boxes3D::from_centers_and_half_sizes(centers, sizes) - .with_rotations(rotations) + .with_quaternions(quaternions) .with_class_ids(class_ids), rerun::LineStrips3D(paths)); } From 502450a93e408425f21cbea305fd42c434b58dee Mon Sep 17 00:00:00 2001 From: ktro2828 Date: Fri, 15 Nov 2024 04:47:34 +0900 Subject: [PATCH 4/4] TODO Signed-off-by: ktro2828 --- .../predicted_objects_display.hpp | 3 - .../predicted_objects_display.cpp | 86 ++++++++++++++----- 2 files changed, 63 insertions(+), 26 deletions(-) diff --git a/awviz_plugin/include/awviz_plugin/autoware_perception/predicted_objects_display.hpp b/awviz_plugin/include/awviz_plugin/autoware_perception/predicted_objects_display.hpp index c57fd3b..ab9f550 100644 --- a/awviz_plugin/include/awviz_plugin/autoware_perception/predicted_objects_display.hpp +++ b/awviz_plugin/include/awviz_plugin/autoware_perception/predicted_objects_display.hpp @@ -35,9 +35,6 @@ class PredictedObjectsDisplay PredictedObjectsDisplay(); protected: - /** - * @todo Add support of colorizing each predicted path mode. - */ void log_message(autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) override; }; } // namespace awviz_plugin diff --git a/awviz_plugin/src/autoware_perception/predicted_objects_display.cpp b/awviz_plugin/src/autoware_perception/predicted_objects_display.cpp index 0b06cae..e0909da 100644 --- a/awviz_plugin/src/autoware_perception/predicted_objects_display.cpp +++ b/awviz_plugin/src/autoware_perception/predicted_objects_display.cpp @@ -15,11 +15,66 @@ #include "awviz_plugin/autoware_perception/predicted_objects_display.hpp" #include +#include + +#include + +#include +#include +#include +#include #include namespace awviz_plugin { +namespace +{ +/** + * @brief Transform pose from `init_pose` origin coordinate to world coordinate. + * + * @param origin Pose origin. + * @param pose Pose which is with respect to `origin` origin coordinate. + * @return rerun::Vec3D + */ +rerun::Vec3D do_transform( + const geometry_msgs::msg::Pose & origin, const geometry_msgs::msg::Pose & pose) +{ + tf2::Quaternion oq; + oq.setValue( + origin.orientation.x, origin.orientation.y, origin.orientation.z, origin.orientation.w); + + tf2::Vector3 op, pp; + op.setValue(origin.position.x, origin.position.y, origin.position.z); + pp.setValue(pose.position.x, pose.position.y, pose.position.z); + + tf2::Matrix3x3 rotation(oq); + + auto ret = rotation * pp + op; + return {static_cast(ret.x()), static_cast(ret.y()), static_cast(ret.z())}; +} + +/** + * @brief Convert predicted paths to the sequence of waypoints. + * + * @param origin Pose origin. + * @param path Sequence of predicted poses. + * @return std::vector + */ +std::vector to_waypoints( + const geometry_msgs::msg::Pose & origin, + const autoware_perception_msgs::msg::PredictedPath & path) +{ + std::vector waypoints; + for (const auto & pose : path.path) { + // waypoints.emplace_back(do_transform(origin, pose)); + waypoints.emplace_back(rerun::Vec3D(pose.position.x, pose.position.y, pose.position.z)); + } + return waypoints; +} + +} // namespace + PredictedObjectsDisplay::PredictedObjectsDisplay() : awviz_common::RosTopicDisplay() { @@ -37,36 +92,21 @@ void PredictedObjectsDisplay::log_message( return; } - std::vector centers; - std::vector sizes; - std::vector quaternions; + std::vector linestrips; std::vector class_ids; - std::vector paths; for (const auto & object : msg->objects) { - const auto & init_pose = object.kinematics.initial_pose_with_covariance.pose; - const auto & dimensions = object.shape.dimensions; - centers.emplace_back(init_pose.position.x, init_pose.position.y, init_pose.position.z); - sizes.emplace_back(dimensions.x, dimensions.y, dimensions.z); - quaternions.emplace_back(rerun::Quaternion::from_wxyz( - init_pose.orientation.w, init_pose.orientation.x, init_pose.orientation.y, - init_pose.orientation.z)); - class_ids.emplace_back(static_cast(object.classification.front().label)); + const auto & origin = object.kinematics.initial_pose_with_covariance.pose; + const auto class_id = static_cast(object.classification.front().label); for (const auto & path : object.kinematics.predicted_paths) { - std::vector waypoints; - for (const auto & point : path.path) { - waypoints.emplace_back(point.position.x, point.position.y, point.position.z); - } - paths.emplace_back(rerun::LineStrip3D(waypoints)); + const auto waypoints = to_waypoints(origin, path); + linestrips.emplace_back(rerun::LineStrip3D(waypoints)); + + class_ids.emplace_back(class_id); } } - stream_->log( - entity_path.value(), - rerun::Boxes3D::from_centers_and_half_sizes(centers, sizes) - .with_quaternions(quaternions) - .with_class_ids(class_ids), - rerun::LineStrips3D(paths)); + stream_->log(entity_path.value(), rerun::LineStrips3D(linestrips).with_class_ids(class_ids)); } } // namespace awviz_plugin