From 298c2445c9b2fc0088d8c0be9e5cbbcd6cf2c735 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Thu, 2 Mar 2023 16:20:14 +0300 Subject: [PATCH 1/2] feat(scene_to_image_projector): init package MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kaan Çolak --- .../scene_to_image_projector/CMakeLists.txt | 36 +++ perception/scene_to_image_projector/README.md | 4 + .../include/scene_to_image_projector/node.hpp | 113 +++++++ .../scene_to_image_projector.launch.xml | 17 ++ .../scene_to_image_projector/package.xml | 30 ++ .../scene_to_image_projector/src/node.cpp | 276 ++++++++++++++++++ 6 files changed, 476 insertions(+) create mode 100644 perception/scene_to_image_projector/CMakeLists.txt create mode 100644 perception/scene_to_image_projector/README.md create mode 100644 perception/scene_to_image_projector/include/scene_to_image_projector/node.hpp create mode 100644 perception/scene_to_image_projector/launch/scene_to_image_projector.launch.xml create mode 100644 perception/scene_to_image_projector/package.xml create mode 100644 perception/scene_to_image_projector/src/node.cpp diff --git a/perception/scene_to_image_projector/CMakeLists.txt b/perception/scene_to_image_projector/CMakeLists.txt new file mode 100644 index 0000000000000..7dcbcba9b8bf3 --- /dev/null +++ b/perception/scene_to_image_projector/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.14) +project(scene_to_image_projector) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(ament_cmake_auto REQUIRED) + +ament_auto_find_build_dependencies() + +find_package(OpenCV REQUIRED) +find_package(Eigen3 REQUIRED) + +include_directories( + ${OpenCV_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +ament_auto_add_library(scene_to_image_projector SHARED + src/node.cpp + ) +target_link_libraries(scene_to_image_projector + ${OpenCV_LIBRARIES} + ${EIGEN3_LIBRARIES} + ) + +rclcpp_components_register_node(scene_to_image_projector + PLUGIN "scene_to_image_projector::SceneToImageProjectorNode" + EXECUTABLE scene_to_image_projector_node + ) + + +ament_auto_package(INSTALL_TO_SHARE + launch + ) + diff --git a/perception/scene_to_image_projector/README.md b/perception/scene_to_image_projector/README.md new file mode 100644 index 0000000000000..f17363a9293b7 --- /dev/null +++ b/perception/scene_to_image_projector/README.md @@ -0,0 +1,4 @@ +# scene_to_image_projector + +## Purpose + diff --git a/perception/scene_to_image_projector/include/scene_to_image_projector/node.hpp b/perception/scene_to_image_projector/include/scene_to_image_projector/node.hpp new file mode 100644 index 0000000000000..0c09a99e1e61b --- /dev/null +++ b/perception/scene_to_image_projector/include/scene_to_image_projector/node.hpp @@ -0,0 +1,113 @@ +// Copyright 2022 LeoDrive.ai +// +// 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 SCENE_TO_IMAGE_PROJECTOR__NODE_HPP_ +#define SCENE_TO_IMAGE_PROJECTOR__NODE_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 + +namespace scene_to_image_projector +{ +class SceneToImageProjectorNode : public rclcpp::Node +{ +public: + explicit SceneToImageProjectorNode(const rclcpp::NodeOptions & options); + + template + void ImageObjectCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_msg, + const typename T::ConstSharedPtr & objects_msg); + + void TrajectoryCallback(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + +private: + image_transport::SubscriberFilter image_sub_; + message_filters::Subscriber camera_info_sub_; + + message_filters::Subscriber + tracked_objects_sub_; + using ApproximateSyncPolicyTracked = message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, + autoware_auto_perception_msgs::msg::TrackedObjects>; + using ApproximateSyncTracked = message_filters::Synchronizer; + std::shared_ptr sync_tracked_; + + message_filters::Subscriber + detected_objects_sub_; + using ApproximateSyncPolicyDetected = message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, + autoware_auto_perception_msgs::msg::DetectedObjects>; + using ApproximateSyncDetected = message_filters::Synchronizer; + std::shared_ptr sync_detected_; + + image_transport::Publisher image_pub_; + + rclcpp::Subscription::SharedPtr trajectory_sub_; + + std::deque trajectory_points_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + template + std::optional> detected_object_corners(const T & detected_object); + + bool out_of_image( + const geometry_msgs::msg::Pose & center_pose, const sensor_msgs::msg::CameraInfo & camera_info, + const Eigen::Matrix4d & projection); + + template + static void calculate_bbox_corners( + const T & detected_object, std::vector & corners_out); + + void draw_bounding_box( + const std::vector & corners, cv::Mat & image, + const Eigen::Matrix4d & projection_matrix); + + Eigen::Matrix4d get_projection_matrix(const sensor_msgs::msg::CameraInfo & camera_info_msg); + + bool project_point( + const Eigen::Vector3d & point, const Eigen::Matrix4d & projection_matrix, + cv::Point2d & projected_point_out); +}; + +} // namespace scene_to_image_projector + +#endif // SCENE_TO_IMAGE_PROJECTOR__NODE_HPP_ diff --git a/perception/scene_to_image_projector/launch/scene_to_image_projector.launch.xml b/perception/scene_to_image_projector/launch/scene_to_image_projector.launch.xml new file mode 100644 index 0000000000000..2dd62b9162ebc --- /dev/null +++ b/perception/scene_to_image_projector/launch/scene_to_image_projector.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/perception/scene_to_image_projector/package.xml b/perception/scene_to_image_projector/package.xml new file mode 100644 index 0000000000000..a6d96efa03296 --- /dev/null +++ b/perception/scene_to_image_projector/package.xml @@ -0,0 +1,30 @@ + + + + scene_to_image_projector + 0.1.0 + The scene_to_image_projector package + Kaan Colak + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + autoware_auto_perception_msgs + autoware_auto_planning_msgs + cv_bridge + image_transport + libboost-filesystem-dev + message_filters + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_ros + perception_utils + + + ament_cmake + + diff --git a/perception/scene_to_image_projector/src/node.cpp b/perception/scene_to_image_projector/src/node.cpp new file mode 100644 index 0000000000000..60531d0b82764 --- /dev/null +++ b/perception/scene_to_image_projector/src/node.cpp @@ -0,0 +1,276 @@ +// Copyright 2022 LeoDrive.ai +// +// 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 "scene_to_image_projector/node.hpp" + +#include "perception_utils/perception_utils.hpp" + +#include + +#include +#include +#include +#include + +namespace +{ +boost::optional getTransformAnonymous( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + // check if the frames are ready + std::string errstr; // This argument prevents error msg from being displayed in the terminal. + if (!tf_buffer.canTransform( + target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { + return boost::none; + } + + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + /*target*/ target_frame_id, /*src*/ 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("multi_object_tracker"), ex.what()); + return boost::none; + } +} + +} // namespace + +namespace scene_to_image_projector +{ +SceneToImageProjectorNode::SceneToImageProjectorNode(const rclcpp::NodeOptions & options) +: Node("scene_to_image_projector_node", options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) +{ + RCLCPP_INFO(this->get_logger(), "SceneToImageProjectorNode::SceneToImageProjectorNode"); + + auto objects_type = declare_parameter("objects_type", "tracked"); + auto use_trajectories = declare_parameter("use_trajectory", false); +// auto sub_road_boundaries = declare_parameter("sub_road_boundaries", false); + + image_sub_.subscribe(this, "~/input/image", "raw", rmw_qos_profile_sensor_data); + camera_info_sub_.subscribe(this, "~/input/camera_info", rmw_qos_profile_sensor_data); + + if (objects_type == "tracked") { + tracked_objects_sub_.subscribe(this, "~/input/objects", rclcpp::QoS{1}.get_rmw_qos_profile()); + + sync_tracked_ = std::make_shared( + ApproximateSyncPolicyTracked(10), image_sub_, camera_info_sub_, tracked_objects_sub_); + + sync_tracked_->registerCallback(std::bind( + &SceneToImageProjectorNode::ImageObjectCallback< + autoware_auto_perception_msgs::msg::TrackedObjects>, + this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + } else if (objects_type == "detected") { + detected_objects_sub_.subscribe(this, "~/input/objects", rclcpp::QoS{1}.get_rmw_qos_profile()); + + sync_detected_ = std::make_shared( + ApproximateSyncPolicyDetected(10), image_sub_, camera_info_sub_, detected_objects_sub_); + + sync_detected_->registerCallback(std::bind( + &SceneToImageProjectorNode::ImageObjectCallback< + autoware_auto_perception_msgs::msg::DetectedObjects>, + this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + } else { + RCLCPP_ERROR(this->get_logger(), "objects_type is not supported"); + } + + if (use_trajectories) { + trajectory_sub_ = this->create_subscription( + "~/input/trajectory", 1, + std::bind(&SceneToImageProjectorNode::TrajectoryCallback, this, std::placeholders::_1)); + } + + image_pub_ = image_transport::create_publisher(this, "~/output/image"); +} + +template +void SceneToImageProjectorNode::ImageObjectCallback( + const sensor_msgs::msg::Image::ConstSharedPtr & input_image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_msg, + const typename T::ConstSharedPtr & objects_msg) +{ + RCLCPP_INFO(this->get_logger(), "SceneToImageProjectorNode::ProjectObjectsToImage"); + const auto self_transform = getTransformAnonymous( + tf_buffer_, objects_msg->header.frame_id, camera_info_msg->header.frame_id, + objects_msg->header.stamp); + + if (!self_transform) { + return; + } + + /* transform to world coordinate */ + T transformed_objects; + if (!perception_utils::transformObjects( + *objects_msg, camera_info_msg->header.frame_id, tf_buffer_, transformed_objects)) { + return; + } + + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(input_image_msg, "bgr8"); + + Eigen::Matrix4d projection = get_projection_matrix(*camera_info_msg); + + auto const & objects = transformed_objects.objects; + for (const auto & object : objects) { + if ( + object.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { + continue; + } + + if (out_of_image(object.kinematics.pose_with_covariance.pose, *camera_info_msg, projection)) { + continue; + } + + auto bbox_corners = detected_object_corners(object); + if (!bbox_corners) { + continue; + } + + draw_bounding_box(*bbox_corners, cv_ptr->image, projection); + } + + image_pub_.publish(cv_ptr->toImageMsg()); +} + +template +std::optional> SceneToImageProjectorNode::detected_object_corners( + const T & detected_object) +{ + std::vector corners; + if (detected_object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + calculate_bbox_corners(detected_object, corners); + return corners; + } + + return std::nullopt; +} + +template +void SceneToImageProjectorNode::calculate_bbox_corners( + const T & detected_object, std::vector & corners_out) +{ + auto const & pose = detected_object.kinematics.pose_with_covariance.pose; + auto const & dimensions = detected_object.shape.dimensions; + + const std::vector> corners_template = { + // down surface + {1, 1, -1}, + {1, -1, -1}, + {-1, -1, -1}, + {-1, 1, -1}, + // up surface + {1, 1, 1}, + {1, -1, 1}, + {-1, -1, 1}, + {-1, 1, 1}, + }; + + const auto position = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z); + const auto orientation = Eigen::Quaterniond( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + + for (const auto & corner : corners_template) { + Eigen::Vector3d corner_point( + dimensions.x * corner.at(0) / 2.0, dimensions.y * corner.at(1) / 2.0, + dimensions.z * corner.at(2) / 2.0); + corners_out.emplace_back(orientation * corner_point + position); + } +} + +bool SceneToImageProjectorNode::out_of_image( + const geometry_msgs::msg::Pose & center_pose, const sensor_msgs::msg::CameraInfo & camera_info, + const Eigen::Matrix4d & projection) +{ + const auto position = + Eigen::Vector3d(center_pose.position.x, center_pose.position.y, center_pose.position.z); + + cv::Point2d center_on_image; + if (!project_point(position, projection, center_on_image)) { + return true; + } + + if ( + center_on_image.x < 0 || center_on_image.x > camera_info.width || center_on_image.y < 0 || + center_on_image.y > camera_info.height) { + return true; + } + + return false; +} + +Eigen::Matrix4d SceneToImageProjectorNode::get_projection_matrix( + const sensor_msgs::msg::CameraInfo & camera_info_msg) +{ + Eigen::Matrix4d projection; + projection << camera_info_msg.p.at(0), camera_info_msg.p.at(1), camera_info_msg.p.at(2), + camera_info_msg.p.at(3), camera_info_msg.p.at(4), camera_info_msg.p.at(5), + camera_info_msg.p.at(6), camera_info_msg.p.at(7), camera_info_msg.p.at(8), + camera_info_msg.p.at(9), camera_info_msg.p.at(10), camera_info_msg.p.at(11); + + return projection; +} + +bool SceneToImageProjectorNode::project_point( + const Eigen::Vector3d & point, const Eigen::Matrix4d & projection_matrix, + cv::Point2d & projected_point_out) +{ + Eigen::Vector4d projected_point = + projection_matrix * Eigen::Vector4d(point.x(), point.y(), point.z(), 1.0); + + if (projected_point.z() < 0.0) { + return false; + } + + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + + projected_point_out.x = normalized_projected_point.x(); + projected_point_out.y = normalized_projected_point.y(); + + return true; +} + +void SceneToImageProjectorNode::draw_bounding_box( + const std::vector & corners, cv::Mat & image, + const Eigen::Matrix4d & projection_matrix) +{ + const std::vector> edges = { + {0, 1}, {1, 2}, {2, 3}, {3, 0}, {4, 5}, {5, 6}, {6, 7}, {7, 4}, {0, 4}, {1, 5}, {2, 6}, {3, 7}, + }; + + for (const auto & edge : edges) { + cv::Point2d point_on_image_1; + project_point(corners.at(edge.at(0)), projection_matrix, point_on_image_1); + cv::Point2d point_on_image_2; + project_point(corners.at(edge.at(1)), projection_matrix, point_on_image_2); + cv::line(image, point_on_image_1, point_on_image_2, cv::Scalar(0, 0, 255), 2); + } +} +void SceneToImageProjectorNode::TrajectoryCallback( + const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) +{ + RCLCPP_INFO(get_logger(), "Trajectory received, header: %s", msg->header.frame_id.c_str()); + msg->points.size(); +} + +} // namespace scene_to_image_projector + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(scene_to_image_projector::SceneToImageProjectorNode) \ No newline at end of file From cd8bced6ad20c22efff736de383307470103cad8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 2 Mar 2023 14:03:59 +0000 Subject: [PATCH 2/2] style(pre-commit): autofix --- .../scene_to_image_projector/CMakeLists.txt | 1 - perception/scene_to_image_projector/README.md | 1 - .../scene_to_image_projector.launch.xml | 28 +++++++++---------- .../scene_to_image_projector/package.xml | 2 +- .../scene_to_image_projector/src/node.cpp | 4 +-- 5 files changed, 17 insertions(+), 19 deletions(-) diff --git a/perception/scene_to_image_projector/CMakeLists.txt b/perception/scene_to_image_projector/CMakeLists.txt index 7dcbcba9b8bf3..44e271e472b98 100644 --- a/perception/scene_to_image_projector/CMakeLists.txt +++ b/perception/scene_to_image_projector/CMakeLists.txt @@ -33,4 +33,3 @@ rclcpp_components_register_node(scene_to_image_projector ament_auto_package(INSTALL_TO_SHARE launch ) - diff --git a/perception/scene_to_image_projector/README.md b/perception/scene_to_image_projector/README.md index f17363a9293b7..446de6b020f74 100644 --- a/perception/scene_to_image_projector/README.md +++ b/perception/scene_to_image_projector/README.md @@ -1,4 +1,3 @@ # scene_to_image_projector ## Purpose - diff --git a/perception/scene_to_image_projector/launch/scene_to_image_projector.launch.xml b/perception/scene_to_image_projector/launch/scene_to_image_projector.launch.xml index 2dd62b9162ebc..b1874f1cb9a51 100644 --- a/perception/scene_to_image_projector/launch/scene_to_image_projector.launch.xml +++ b/perception/scene_to_image_projector/launch/scene_to_image_projector.launch.xml @@ -1,17 +1,17 @@ - - - - - - + + + + + + - - - - - - - - + + + + + + + + diff --git a/perception/scene_to_image_projector/package.xml b/perception/scene_to_image_projector/package.xml index a6d96efa03296..f5dbc526f42f1 100644 --- a/perception/scene_to_image_projector/package.xml +++ b/perception/scene_to_image_projector/package.xml @@ -17,12 +17,12 @@ image_transport libboost-filesystem-dev message_filters + perception_utils rclcpp rclcpp_components sensor_msgs tf2 tf2_ros - perception_utils ament_cmake diff --git a/perception/scene_to_image_projector/src/node.cpp b/perception/scene_to_image_projector/src/node.cpp index 60531d0b82764..f277860a0586e 100644 --- a/perception/scene_to_image_projector/src/node.cpp +++ b/perception/scene_to_image_projector/src/node.cpp @@ -61,7 +61,7 @@ SceneToImageProjectorNode::SceneToImageProjectorNode(const rclcpp::NodeOptions & auto objects_type = declare_parameter("objects_type", "tracked"); auto use_trajectories = declare_parameter("use_trajectory", false); -// auto sub_road_boundaries = declare_parameter("sub_road_boundaries", false); + // auto sub_road_boundaries = declare_parameter("sub_road_boundaries", false); image_sub_.subscribe(this, "~/input/image", "raw", rmw_qos_profile_sensor_data); camera_info_sub_.subscribe(this, "~/input/camera_info", rmw_qos_profile_sensor_data); @@ -273,4 +273,4 @@ void SceneToImageProjectorNode::TrajectoryCallback( #include -RCLCPP_COMPONENTS_REGISTER_NODE(scene_to_image_projector::SceneToImageProjectorNode) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(scene_to_image_projector::SceneToImageProjectorNode)