feat(scene_to_image_projector): init package

perception/scene_to_image_projector/CMakeLists.txt

@@ -0,0 +1,35 @@
+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
+        )

perception/scene_to_image_projector/README.md

@@ -0,0 +1,3 @@
+# scene_to_image_projector
+
+## Purpose

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 <Eigen/Core>
+#include <Eigen/Geometry>
+#include <image_transport/image_transport.hpp>
+#include <image_transport/subscriber_filter.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/detected_object.hpp>
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/tracked_object.hpp>
+#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+
+#include <cv_bridge/cv_bridge.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/time_synchronizer.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/convert.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+namespace scene_to_image_projector
+{
+class SceneToImageProjectorNode : public rclcpp::Node
+{
+public:
+  explicit SceneToImageProjectorNode(const rclcpp::NodeOptions & options);
+
+  template <typename T>
+  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<sensor_msgs::msg::CameraInfo> camera_info_sub_;
+
+  message_filters::Subscriber<autoware_auto_perception_msgs::msg::TrackedObjects>
+    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<ApproximateSyncPolicyTracked>;
+  std::shared_ptr<ApproximateSyncTracked> sync_tracked_;
+
+  message_filters::Subscriber<autoware_auto_perception_msgs::msg::DetectedObjects>
+    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<ApproximateSyncPolicyDetected>;
+  std::shared_ptr<ApproximateSyncDetected> sync_detected_;
+
+  image_transport::Publisher image_pub_;
+
+  rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr trajectory_sub_;
+
+  std::deque<autoware_auto_planning_msgs::msg::TrajectoryPoint> trajectory_points_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  template <typename T>
+  std::optional<std::vector<Eigen::Vector3d>> 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 <typename T>
+  static void calculate_bbox_corners(
+    const T & detected_object, std::vector<Eigen::Vector3d> & corners_out);
+
+  void draw_bounding_box(
+    const std::vector<Eigen::Vector3d> & 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_

perception/scene_to_image_projector/launch/scene_to_image_projector.launch.xml

@@ -0,0 +1,17 @@
+<launch>
+  <arg name="input/image" default="/perception/object_recognition/detection/image_rect_front"/>
+  <arg name="input/camera_info" default="/lucid_vision/camera_1/camera_info"/>
+  <arg name="input/objects" default="/perception/object_recognition/detection/objects"/>
+  <arg name="objects_type" default="detected" description="must be tracked or detected"/>
+  <arg name="use_trajectory" default="false" description=""/>
+  <arg name="use_road_boundaries" default="false" description=""/>
+
+  <node pkg="scene_to_image_projector" exec="scene_to_image_projector_node" name="scene_to_image_projector">
+    <remap from="~/input/image" to="$(var input/image)"/>
+    <remap from="~/input/camera_info" to="$(var input/camera_info)"/>
+    <remap from="~/input/objects" to="$(var input/objects)"/>
+    <param name="objects_type" value="$(var objects_type)"/>
+    <param name="use_trajectory" value="$(var use_trajectory)"/>
+    <param name="use_road_boundaries" value="$(var use_road_boundaries)"/>
+  </node>
+</launch>

perception/scene_to_image_projector/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>scene_to_image_projector</name>
+  <version>0.1.0</version>
+  <description>The scene_to_image_projector package</description>
+  <maintainer email="kcolak@leodrive.ai">Kaan Colak</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>autoware_auto_perception_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>libboost-filesystem-dev</depend>
+  <depend>message_filters</depend>
+  <depend>perception_utils</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>sensor_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>