Skip to content

Commit 912768b

Browse files
committed
refactor(image_projection_based_fusion): rename template argument
Signed-off-by: kminoda <koji.minoda@tier4.jp>
1 parent 1f6ed4a commit 912768b

File tree

2 files changed

+33
-32
lines changed

2 files changed

+33
-32
lines changed

perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp

+11-10
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,8 @@ using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
5555
using tier4_perception_msgs::msg::DetectedObjectWithFeature;
5656
using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
5757
using autoware_auto_perception_msgs::msg::ObjectClassification;
58-
template <class Msg, class ObjType, class Msg2D>
58+
59+
template <class TargetMsg3D, class ObjType, class Msg2D>
5960
class FusionNode : public rclcpp::Node
6061
{
6162
public:
@@ -72,24 +73,24 @@ class FusionNode : public rclcpp::Node
7273
const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg,
7374
const std::size_t camera_id);
7475

75-
virtual void preprocess(Msg & output_msg);
76+
virtual void preprocess(TargetMsg3D & output_msg);
7677

7778
// callback for Msg subscription
78-
virtual void subCallback(const typename Msg::ConstSharedPtr input_msg);
79+
virtual void subCallback(const typename TargetMsg3D::ConstSharedPtr input_msg);
7980

8081
// callback for roi subscription
8182

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

8586
virtual void fuseOnSingleImage(
86-
const Msg & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg,
87-
const sensor_msgs::msg::CameraInfo & camera_info, Msg & output_msg) = 0;
87+
const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg,
88+
const sensor_msgs::msg::CameraInfo & camera_info, TargetMsg3D & output_msg) = 0;
8889

8990
// set args if you need
90-
virtual void postprocess(Msg & output_msg);
91+
virtual void postprocess(TargetMsg3D & output_msg);
9192

92-
virtual void publish(const Msg & output_msg);
93+
virtual void publish(const TargetMsg3D & output_msg);
9394

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

112113
/** \brief A vector of subscriber. */
113-
typename rclcpp::Subscription<Msg>::SharedPtr sub_;
114+
typename rclcpp::Subscription<TargetMsg3D>::SharedPtr sub_;
114115
std::vector<typename rclcpp::Subscription<Msg2D>::SharedPtr> rois_subs_;
115116

116117
// offsets between cameras and the lidars
117118
std::vector<double> input_offset_ms_;
118119

119120
// cache for fusion
120121
std::vector<bool> is_fused_;
121-
std::pair<int64_t, typename Msg::SharedPtr>
122+
std::pair<int64_t, typename TargetMsg3D::SharedPtr>
122123
cached_msg_; // first element is the timestamp in nanoseconds, second element is the message
123124
std::vector<std::map<int64_t, typename Msg2D::ConstSharedPtr>> cached_roi_msgs_;
124125
std::mutex mutex_cached_msgs_;
125126

126127
// output publisher
127-
typename rclcpp::Publisher<Msg>::SharedPtr pub_ptr_;
128+
typename rclcpp::Publisher<TargetMsg3D>::SharedPtr pub_ptr_;
128129

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

perception/image_projection_based_fusion/src/fusion_node.cpp

+22-22
Original file line numberDiff line numberDiff line change
@@ -40,8 +40,8 @@ static double processing_time_ms = 0;
4040
namespace image_projection_based_fusion
4141
{
4242

43-
template <class Msg, class ObjType, class Msg2D>
44-
FusionNode<Msg, ObjType, Msg2D>::FusionNode(
43+
template <class TargetMsg3D, class ObjType, class Msg2D>
44+
FusionNode<TargetMsg3D, ObjType, Msg2D>::FusionNode(
4545
const std::string & node_name, const rclcpp::NodeOptions & options)
4646
: Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_)
4747
{
@@ -106,12 +106,12 @@ FusionNode<Msg, ObjType, Msg2D>::FusionNode(
106106
}
107107

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

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

116116
// Set timer
117117
const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
@@ -145,22 +145,22 @@ FusionNode<Msg, ObjType, Msg2D>::FusionNode(
145145
filter_scope_max_z_ = declare_parameter<double>("filter_scope_max_z");
146146
}
147147

148-
template <class Msg, class Obj, class Msg2D>
149-
void FusionNode<Msg, Obj, Msg2D>::cameraInfoCallback(
148+
template <class TargetMsg3D, class Obj, class Msg2D>
149+
void FusionNode<TargetMsg3D, Obj, Msg2D>::cameraInfoCallback(
150150
const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg,
151151
const std::size_t camera_id)
152152
{
153153
camera_info_map_[camera_id] = *input_camera_info_msg;
154154
}
155155

156-
template <class Msg, class Obj, class Msg2D>
157-
void FusionNode<Msg, Obj, Msg2D>::preprocess(Msg & ouput_msg __attribute__((unused)))
156+
template <class TargetMsg3D, class Obj, class Msg2D>
157+
void FusionNode<TargetMsg3D, Obj, Msg2D>::preprocess(TargetMsg3D & ouput_msg __attribute__((unused)))
158158
{
159159
// do nothing by default
160160
}
161161

162-
template <class Msg, class Obj, class Msg2D>
163-
void FusionNode<Msg, Obj, Msg2D>::subCallback(const typename Msg::ConstSharedPtr input_msg)
162+
template <class TargetMsg3D, class Obj, class Msg2D>
163+
void FusionNode<TargetMsg3D, Obj, Msg2D>::subCallback(const typename TargetMsg3D::ConstSharedPtr input_msg)
164164
{
165165
if (cached_msg_.second != nullptr) {
166166
stop_watch_ptr_->toc("processing_time", true);
@@ -202,7 +202,7 @@ void FusionNode<Msg, Obj, Msg2D>::subCallback(const typename Msg::ConstSharedPtr
202202

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

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

207207
preprocess(*output_msg);
208208

@@ -291,8 +291,8 @@ void FusionNode<Msg, Obj, Msg2D>::subCallback(const typename Msg::ConstSharedPtr
291291
}
292292
}
293293

294-
template <class Msg, class Obj, class Msg2D>
295-
void FusionNode<Msg, Obj, Msg2D>::roiCallback(
294+
template <class TargetMsg3D, class Obj, class Msg2D>
295+
void FusionNode<TargetMsg3D, Obj, Msg2D>::roiCallback(
296296
const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i)
297297
{
298298
stop_watch_ptr_->toc("processing_time", true);
@@ -356,14 +356,14 @@ void FusionNode<Msg, Obj, Msg2D>::roiCallback(
356356
(cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg;
357357
}
358358

359-
template <class Msg, class Obj, class Msg2D>
360-
void FusionNode<Msg, Obj, Msg2D>::postprocess(Msg & output_msg __attribute__((unused)))
359+
template <class TargetMsg3D, class Obj, class Msg2D>
360+
void FusionNode<TargetMsg3D, Obj, Msg2D>::postprocess(TargetMsg3D & output_msg __attribute__((unused)))
361361
{
362362
// do nothing by default
363363
}
364364

365-
template <class Msg, class Obj, class Msg2D>
366-
void FusionNode<Msg, Obj, Msg2D>::timer_callback()
365+
template <class TargetMsg3D, class Obj, class Msg2D>
366+
void FusionNode<TargetMsg3D, Obj, Msg2D>::timer_callback()
367367
{
368368
using std::chrono_literals::operator""ms;
369369
timer_->cancel();
@@ -401,8 +401,8 @@ void FusionNode<Msg, Obj, Msg2D>::timer_callback()
401401
}
402402
}
403403

404-
template <class Msg, class Obj, class Msg2D>
405-
void FusionNode<Msg, Obj, Msg2D>::setPeriod(const int64_t new_period)
404+
template <class TargetMsg3D, class Obj, class Msg2D>
405+
void FusionNode<TargetMsg3D, Obj, Msg2D>::setPeriod(const int64_t new_period)
406406
{
407407
if (!timer_) {
408408
return;
@@ -418,8 +418,8 @@ void FusionNode<Msg, Obj, Msg2D>::setPeriod(const int64_t new_period)
418418
}
419419
}
420420

421-
template <class Msg, class Obj, class Msg2D>
422-
void FusionNode<Msg, Obj, Msg2D>::publish(const Msg & output_msg)
421+
template <class TargetMsg3D, class Obj, class Msg2D>
422+
void FusionNode<TargetMsg3D, Obj, Msg2D>::publish(const TargetMsg3D & output_msg)
423423
{
424424
if (pub_ptr_->get_subscription_count() < 1) {
425425
return;

0 commit comments

Comments
 (0)