Skip to content

Commit

Permalink
refactor(image_projection_based_fusion): rename template argument (#6583
Browse files Browse the repository at this point in the history 
)

* refactor(image_projection_based_fusion): rename template argument

Signed-off-by: kminoda <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: kminoda <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@kminoda @pre-commit-ci
kminoda and pre-commit-ci[bot] authored Mar 29, 2024
1 parent dd2ff22 commit 6c75c7b
Show file tree
Hide file tree
Showing 2 changed files with 37 additions and 32 deletions.
21 changes: 11 additions & 10 deletions ...ption/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,8 @@ using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
using tier4_perception_msgs::msg::DetectedObjectWithFeature;
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
using autoware_auto_perception_msgs::msg::ObjectClassification;
template <class Msg, class ObjType, class Msg2D>

template <class TargetMsg3D, class ObjType, class Msg2D>
class FusionNode : public rclcpp::Node
{
public:
Expand All @@ -72,24 +73,24 @@ class FusionNode : public rclcpp::Node
const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg,
const std::size_t camera_id);

virtual void preprocess(Msg & output_msg);
virtual void preprocess(TargetMsg3D & output_msg);

// callback for Msg subscription
virtual void subCallback(const typename Msg::ConstSharedPtr input_msg);
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 Msg & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg,
const sensor_msgs::msg::CameraInfo & camera_info, Msg & output_msg) = 0;
const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg,
const sensor_msgs::msg::CameraInfo & camera_info, TargetMsg3D & output_msg) = 0;

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

virtual void publish(const Msg & output_msg);
virtual void publish(const TargetMsg3D & output_msg);

void timer_callback();
void setPeriod(const int64_t new_period);
Expand All @@ -110,21 +111,21 @@ class FusionNode : public rclcpp::Node
std::vector<std::string> input_camera_topics_;

/** \brief A vector of subscriber. */
typename rclcpp::Subscription<Msg>::SharedPtr sub_;
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_;

// cache for fusion
std::vector<bool> is_fused_;
std::pair<int64_t, typename Msg::SharedPtr>
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_;
std::mutex mutex_cached_msgs_;

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

// debugger
std::shared_ptr<Debugger> debugger_;
Expand Down
48 changes: 26 additions & 22 deletions perception/image_projection_based_fusion/src/fusion_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,8 @@ static double processing_time_ms = 0;
namespace image_projection_based_fusion
{

template <class Msg, class ObjType, class Msg2D>
FusionNode<Msg, ObjType, Msg2D>::FusionNode(
template <class TargetMsg3D, class ObjType, class Msg2D>
FusionNode<TargetMsg3D, ObjType, Msg2D>::FusionNode(
const std::string & node_name, const rclcpp::NodeOptions & options)
: Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_)
{
Expand Down Expand Up @@ -106,12 +106,13 @@ FusionNode<Msg, ObjType, Msg2D>::FusionNode(
}

// subscribers
std::function<void(const typename Msg::ConstSharedPtr msg)> sub_callback =
std::function<void(const typename TargetMsg3D::ConstSharedPtr msg)> sub_callback =
std::bind(&FusionNode::subCallback, this, std::placeholders::_1);
sub_ = this->create_subscription<Msg>("input", rclcpp::QoS(1).best_effort(), sub_callback);
sub_ =
this->create_subscription<TargetMsg3D>("input", rclcpp::QoS(1).best_effort(), sub_callback);

// publisher
pub_ptr_ = this->create_publisher<Msg>("output", rclcpp::QoS{1});
pub_ptr_ = this->create_publisher<TargetMsg3D>("output", rclcpp::QoS{1});

// Set timer
const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
Expand Down Expand Up @@ -145,22 +146,24 @@ FusionNode<Msg, ObjType, Msg2D>::FusionNode(
filter_scope_max_z_ = declare_parameter<double>("filter_scope_max_z");
}

template <class Msg, class Obj, class Msg2D>
void FusionNode<Msg, Obj, Msg2D>::cameraInfoCallback(
template <class TargetMsg3D, class Obj, class Msg2D>
void FusionNode<TargetMsg3D, Obj, Msg2D>::cameraInfoCallback(
const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg,
const std::size_t camera_id)
{
camera_info_map_[camera_id] = *input_camera_info_msg;
}

template <class Msg, class Obj, class Msg2D>
void FusionNode<Msg, Obj, Msg2D>::preprocess(Msg & ouput_msg __attribute__((unused)))
template <class TargetMsg3D, class Obj, class Msg2D>
void FusionNode<TargetMsg3D, Obj, Msg2D>::preprocess(TargetMsg3D & ouput_msg
__attribute__((unused)))
{
// do nothing by default
}

template <class Msg, class Obj, class Msg2D>
void FusionNode<Msg, Obj, Msg2D>::subCallback(const typename Msg::ConstSharedPtr input_msg)
template <class TargetMsg3D, class Obj, class Msg2D>
void FusionNode<TargetMsg3D, Obj, Msg2D>::subCallback(
const typename TargetMsg3D::ConstSharedPtr input_msg)
{
if (cached_msg_.second != nullptr) {
stop_watch_ptr_->toc("processing_time", true);
Expand Down Expand Up @@ -202,7 +205,7 @@ void FusionNode<Msg, Obj, Msg2D>::subCallback(const typename Msg::ConstSharedPtr

stop_watch_ptr_->toc("processing_time", true);

typename Msg::SharedPtr output_msg = std::make_shared<Msg>(*input_msg);
typename TargetMsg3D::SharedPtr output_msg = std::make_shared<TargetMsg3D>(*input_msg);

preprocess(*output_msg);

Expand Down Expand Up @@ -291,8 +294,8 @@ void FusionNode<Msg, Obj, Msg2D>::subCallback(const typename Msg::ConstSharedPtr
}
}

template <class Msg, class Obj, class Msg2D>
void FusionNode<Msg, Obj, Msg2D>::roiCallback(
template <class TargetMsg3D, class Obj, class Msg2D>
void FusionNode<TargetMsg3D, Obj, Msg2D>::roiCallback(
const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i)
{
stop_watch_ptr_->toc("processing_time", true);
Expand Down Expand Up @@ -356,14 +359,15 @@ void FusionNode<Msg, Obj, Msg2D>::roiCallback(
(cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg;
}

template <class Msg, class Obj, class Msg2D>
void FusionNode<Msg, Obj, Msg2D>::postprocess(Msg & output_msg __attribute__((unused)))
template <class TargetMsg3D, class Obj, class Msg2D>
void FusionNode<TargetMsg3D, Obj, Msg2D>::postprocess(TargetMsg3D & output_msg
__attribute__((unused)))
{
// do nothing by default
}

template <class Msg, class Obj, class Msg2D>
void FusionNode<Msg, Obj, Msg2D>::timer_callback()
template <class TargetMsg3D, class Obj, class Msg2D>
void FusionNode<TargetMsg3D, Obj, Msg2D>::timer_callback()
{
using std::chrono_literals::operator""ms;
timer_->cancel();
Expand Down Expand Up @@ -401,8 +405,8 @@ void FusionNode<Msg, Obj, Msg2D>::timer_callback()
}
}

template <class Msg, class Obj, class Msg2D>
void FusionNode<Msg, Obj, Msg2D>::setPeriod(const int64_t new_period)
template <class TargetMsg3D, class Obj, class Msg2D>
void FusionNode<TargetMsg3D, Obj, Msg2D>::setPeriod(const int64_t new_period)
{
if (!timer_) {
return;
Expand All @@ -418,8 +422,8 @@ void FusionNode<Msg, Obj, Msg2D>::setPeriod(const int64_t new_period)
}
}

template <class Msg, class Obj, class Msg2D>
void FusionNode<Msg, Obj, Msg2D>::publish(const Msg & output_msg)
template <class TargetMsg3D, class Obj, class Msg2D>
void FusionNode<TargetMsg3D, Obj, Msg2D>::publish(const TargetMsg3D & output_msg)
{
if (pub_ptr_->get_subscription_count() < 1) {
return;
Expand Down

0 comments on commit 6c75c7b

Please sign in to comment.