Skip to content

Commit

Permalink
refactor(autoware_image_projection_based_fusion): organize 2d-detecti…
Browse files Browse the repository at this point in the history 
…on related members (#9789)

* chore: input_camera_topics_ is only for debug

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* feat: fuse main message with cached roi messages in fusion_node.cpp

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* chore: add comments on each process step, organize methods

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* feat: Export process method in fusion_node.cpp

Export the `exportProcess()` method in `fusion_node.cpp` to handle the post-processing and publishing of the fused messages. This method cancels the timer, performs the necessary post-processing steps, publishes the output message, and resets the flags. It also adds processing time for debugging purposes. This change improves the organization and readability of the code.

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* feat: Refactor fusion_node.hpp and fusion_node.cpp

Refactor the `fusion_node.hpp` and `fusion_node.cpp` files to improve code organization and readability. This includes exporting the `exportProcess()` method in `fusion_node.cpp` to handle post-processing and publishing of fused messages, adding comments on each process step, organizing methods, and fusing the main message with cached ROI messages. These changes enhance the overall quality of the codebase.

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* Refactor fusion_node.cpp and fusion_node.hpp for improved code organization and readability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* feat: Refactor fusion_node.cpp for improved code organization and readability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* Refactor fusion_node.cpp for improved code organization and readability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* feat: implement mutex per 2d detection process

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* revise template, inputs first and output at the last

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* explicit in and out types 1

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* clarify pointcloud message type

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* Refactor publisher types in fusion_node.hpp and node.hpp

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix: resolve cppcheck issue shadowVariable

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* chore: rename Det2dManager to Det2dStatus

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* revert mutex related changes

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* refactor: review member and method's access

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix: resolve shadowVariable of 'det2d'

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix missing line

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* refactor message postprocess and publish methods

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* publish the main message is common

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix: replace pointcloud message type by the typename

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* review member access

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* refactor: fix condition for publishing painted pointcloud message

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

* fix: remove unused variable

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

---------

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
  • Loading branch information
@technolojin
technolojin authored Jan 8, 2025
1 parent 9ce874c commit 14ee08b
Show file tree
Hide file tree
Showing 14 changed files with 603 additions and 579 deletions.
124 changes: 73 additions & 51 deletions ...ge_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,14 +52,33 @@ using autoware_perception_msgs::msg::DetectedObject;
using autoware_perception_msgs::msg::DetectedObjects;
using sensor_msgs::msg::CameraInfo;
using sensor_msgs::msg::Image;
using sensor_msgs::msg::PointCloud2;
using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
using PointCloudMsgType = sensor_msgs::msg::PointCloud2;
using RoiMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature;
using ClusterMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature;
using ClusterObjType = tier4_perception_msgs::msg::DetectedObjectWithFeature;
using tier4_perception_msgs::msg::DetectedObjectWithFeature;
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
using autoware::image_projection_based_fusion::CameraProjection;
using autoware_perception_msgs::msg::ObjectClassification;

template <class TargetMsg3D, class ObjType, class Msg2D>
template <class Msg2D>
struct Det2dStatus
{
// camera index
std::size_t id = 0;
// camera projection
std::unique_ptr<CameraProjection> camera_projector_ptr;
bool project_to_unrectified_image = false;
bool approximate_camera_projection = false;
// process flags
bool is_fused = false;
// timing
double input_offset_ms = 0.0;
// cache
std::map<int64_t, typename Msg2D::ConstSharedPtr> cached_det2d_msgs;
};

template <class Msg3D, class Msg2D, class ExportObj>
class FusionNode : public rclcpp::Node
{
public:
Expand All @@ -71,82 +90,85 @@ class FusionNode : public rclcpp::Node
explicit FusionNode(
const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size);

protected:
private:
// Common process methods
void cameraInfoCallback(
const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg,
const std::size_t camera_id);

virtual void preprocess(TargetMsg3D & output_msg);

// callback for Msg subscription
virtual void subCallback(const typename TargetMsg3D::ConstSharedPtr input_msg);

// callback for roi subscription

virtual void roiCallback(
const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i);

virtual void fuseOnSingleImage(
const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg,
TargetMsg3D & output_msg) = 0;

// set args if you need
virtual void postprocess(TargetMsg3D & output_msg);

virtual void publish(const TargetMsg3D & output_msg);

// callback for timer
void timer_callback();
void setPeriod(const int64_t new_period);
void exportProcess();

// 2d detection management methods
bool checkAllDet2dFused()
{
for (const auto & det2d : det2d_list_) {
if (!det2d.is_fused) {
return false;
}
}
return true;
}

std::size_t rois_number_{1};
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;

std::vector<bool> point_project_to_unrectified_image_;

// camera_info
std::map<std::size_t, sensor_msgs::msg::CameraInfo> camera_info_map_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subs_;
// camera projection
std::vector<CameraProjection> camera_projectors_;
std::vector<bool> approx_camera_projection_;
float approx_grid_cell_w_size_;
float approx_grid_cell_h_size_;

// timer
rclcpp::TimerBase::SharedPtr timer_;
double timeout_ms_{};
double match_threshold_ms_{};
std::vector<std::string> input_rois_topics_;
std::vector<std::string> input_camera_info_topics_;
std::vector<std::string> input_camera_topics_;

/** \brief A vector of subscriber. */
typename rclcpp::Subscription<TargetMsg3D>::SharedPtr sub_;
std::vector<typename rclcpp::Subscription<Msg2D>::SharedPtr> rois_subs_;

// offsets between cameras and the lidars
std::vector<double> input_offset_ms_;
std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subs_;

// cache for fusion
std::vector<bool> is_fused_;
std::pair<int64_t, typename TargetMsg3D::SharedPtr>
cached_msg_; // first element is the timestamp in nanoseconds, second element is the message
std::vector<std::map<int64_t, typename Msg2D::ConstSharedPtr>> cached_roi_msgs_;
int64_t cached_det3d_msg_timestamp_;
typename Msg3D::SharedPtr cached_det3d_msg_ptr_;
std::mutex mutex_cached_msgs_;

// output publisher
typename rclcpp::Publisher<TargetMsg3D>::SharedPtr pub_ptr_;
protected:
void setDet2DStatus(std::size_t rois_number);

// debugger
std::shared_ptr<Debugger> debugger_;
virtual bool out_of_scope(const ObjType & obj) = 0;
// callback for main subscription
void subCallback(const typename Msg3D::ConstSharedPtr input_msg);
// callback for roi subscription
void roiCallback(const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i);

// Custom process methods
virtual void preprocess(Msg3D & output_msg);
virtual void fuseOnSingleImage(
const Msg3D & input_msg, const Det2dStatus<Msg2D> & det2d, const Msg2D & input_roi_msg,
Msg3D & output_msg) = 0;
virtual void postprocess(const Msg3D & processing_msg, ExportObj & output_msg);
virtual void publish(const ExportObj & output_msg);

// Members
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;

// 2d detection management
std::vector<Det2dStatus<Msg2D>> det2d_list_;

/** \brief A vector of subscriber. */
typename rclcpp::Subscription<Msg3D>::SharedPtr sub_;

// parameters for out_of_scope filter
float filter_scope_min_x_;
float filter_scope_max_x_;
float filter_scope_min_y_;
float filter_scope_max_y_;
float filter_scope_min_z_;
float filter_scope_max_z_;

// output publisher
typename rclcpp::Publisher<ExportObj>::SharedPtr pub_ptr_;

// debugger
std::shared_ptr<Debugger> debugger_;

/** \brief processing time publisher. **/
std::unique_ptr<autoware::universe_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
std::unique_ptr<autoware::universe_utils::DebugPublisher> debug_publisher_;
Expand Down
20 changes: 8 additions & 12 deletions ...based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,27 +42,25 @@ inline bool isInsideBbox(
proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc;
}

class PointPaintingFusionNode
: public FusionNode<sensor_msgs::msg::PointCloud2, DetectedObjects, DetectedObjectsWithFeature>
class PointPaintingFusionNode : public FusionNode<PointCloudMsgType, RoiMsgType, DetectedObjects>
{
public:
explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options);

protected:
void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override;
private:
void preprocess(PointCloudMsgType & pointcloud_msg) override;

void fuseOnSingleImage(
const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
const DetectedObjectsWithFeature & input_roi_msg,
sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus<RoiMsgType> & det2d,
const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) override;

void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override;
void postprocess(
const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) override;

rclcpp::Publisher<DetectedObjects>::SharedPtr obj_pub_ptr_;
rclcpp::Publisher<PointCloudMsgType>::SharedPtr point_pub_ptr_;
std::unique_ptr<autoware::universe_utils::DiagnosticsInterface> diagnostics_interface_ptr_;

int omp_num_threads_{1};
float score_threshold_{0.0};
std::vector<std::string> class_names_;
std::map<std::string, float> class_index_;
std::map<std::string, std::function<bool(int)>> isClassTable_;
Expand All @@ -74,8 +72,6 @@ class PointPaintingFusionNode
autoware::lidar_centerpoint::DetectionClassRemapper detection_class_remapper_;

std::unique_ptr<image_projection_based_fusion::PointPaintingTRT> detector_ptr_{nullptr};

bool out_of_scope(const DetectedObjects & obj) override;
};
} // namespace autoware::image_projection_based_fusion
#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_
21 changes: 9 additions & 12 deletions ...n_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,21 +24,19 @@ namespace autoware::image_projection_based_fusion
{
const std::map<std::string, uint8_t> IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}};

class RoiClusterFusionNode
: public FusionNode<
DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>
class RoiClusterFusionNode : public FusionNode<ClusterMsgType, RoiMsgType, ClusterMsgType>
{
public:
explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options);

protected:
void preprocess(DetectedObjectsWithFeature & output_cluster_msg) override;
void postprocess(DetectedObjectsWithFeature & output_cluster_msg) override;
private:
void preprocess(ClusterMsgType & output_cluster_msg) override;

void fuseOnSingleImage(
const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id,
const DetectedObjectsWithFeature & input_roi_msg,
DetectedObjectsWithFeature & output_cluster_msg) override;
const ClusterMsgType & input_cluster_msg, const Det2dStatus<RoiMsgType> & det2d,
const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override;

void postprocess(const ClusterMsgType & output_cluster_msg, ClusterMsgType & output_msg) override;

std::string trust_object_iou_mode_{"iou"};
bool use_cluster_semantic_type_{false};
Expand All @@ -53,12 +51,11 @@ class RoiClusterFusionNode
double trust_object_distance_;
std::string non_trust_object_iou_mode_{"iou_x"};

bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold);
bool out_of_scope(const DetectedObjectWithFeature & obj) override;
bool is_far_enough(const ClusterObjType & obj, const double distance_threshold);
bool out_of_scope(const ClusterObjType & obj);
double cal_iou_by_mode(
const sensor_msgs::msg::RegionOfInterest & roi_1,
const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode);
// bool CheckUnknown(const DetectedObjectsWithFeature & obj);
};

} // namespace autoware::image_projection_based_fusion
Expand Down
16 changes: 7 additions & 9 deletions ...fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,33 +31,31 @@ namespace autoware::image_projection_based_fusion

using sensor_msgs::msg::RegionOfInterest;

class RoiDetectedObjectFusionNode
: public FusionNode<DetectedObjects, DetectedObject, DetectedObjectsWithFeature>
class RoiDetectedObjectFusionNode : public FusionNode<DetectedObjects, RoiMsgType, DetectedObjects>
{
public:
explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options);

protected:
private:
void preprocess(DetectedObjects & output_msg) override;

void fuseOnSingleImage(
const DetectedObjects & input_object_msg, const std::size_t image_id,
const DetectedObjectsWithFeature & input_roi_msg, DetectedObjects & output_object_msg) override;
const DetectedObjects & input_object_msg, const Det2dStatus<RoiMsgType> & det2d,
const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg) override;

std::map<std::size_t, DetectedObjectWithFeature> generateDetectedObjectRoIs(
const DetectedObjects & input_object_msg, const std::size_t & image_id,
const DetectedObjects & input_object_msg, const Det2dStatus<RoiMsgType> & det2d,
const Eigen::Affine3d & object2camera_affine);

void fuseObjectsOnImage(
const DetectedObjects & input_object_msg,
const std::vector<DetectedObjectWithFeature> & image_rois,
const std::map<std::size_t, DetectedObjectWithFeature> & object_roi_map);

void publish(const DetectedObjects & output_msg) override;
void postprocess(const DetectedObjects & processing_msg, DetectedObjects & output_msg) override;

bool out_of_scope(const DetectedObject & obj) override;
bool out_of_scope(const DetectedObject & obj);

private:
struct
{
std::vector<double> passthrough_lower_bound_probability_thresholds{};
Expand Down
35 changes: 16 additions & 19 deletions ...ased_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,32 +25,29 @@

namespace autoware::image_projection_based_fusion
{
class RoiPointCloudFusionNode
: public FusionNode<PointCloud2, DetectedObjectWithFeature, DetectedObjectsWithFeature>
class RoiPointCloudFusionNode : public FusionNode<PointCloudMsgType, RoiMsgType, ClusterMsgType>
{
public:
explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options);

private:
int min_cluster_size_{1};
int max_cluster_size_{20};
bool fuse_unknown_only_{true};
double cluster_2d_tolerance_;
rclcpp::Publisher<PointCloudMsgType>::SharedPtr point_pub_ptr_;
rclcpp::Publisher<PointCloudMsgType>::SharedPtr cluster_debug_pub_;

rclcpp::Publisher<DetectedObjectsWithFeature>::SharedPtr pub_objects_ptr_;
std::vector<DetectedObjectWithFeature> output_fused_objects_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cluster_debug_pub_;
void fuseOnSingleImage(
const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus<RoiMsgType> & det2d,
const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override;

/* data */
public:
explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options);
void postprocess(const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) override;

protected:
void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override;
void publish(const ClusterMsgType & output_msg) override;

void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override;
int min_cluster_size_{1};
int max_cluster_size_{20};
bool fuse_unknown_only_{true};
double cluster_2d_tolerance_;

void fuseOnSingleImage(
const PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
const DetectedObjectsWithFeature & input_roi_msg, PointCloud2 & output_pointcloud_msg) override;
bool out_of_scope(const DetectedObjectWithFeature & obj) override;
std::vector<ClusterObjType> output_fused_objects_;
};

} // namespace autoware::image_projection_based_fusion
Expand Down
Loading

0 comments on commit 14ee08b

Please sign in to comment.