@@ -40,8 +40,8 @@ static double processing_time_ms = 0;
40
40
namespace image_projection_based_fusion
41
41
{
42
42
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(
45
45
const std::string & node_name, const rclcpp::NodeOptions & options)
46
46
: Node(node_name, options), tf_buffer_(this ->get_clock ()), tf_listener_(tf_buffer_)
47
47
{
@@ -106,12 +106,12 @@ FusionNode<Msg, ObjType, Msg2D>::FusionNode(
106
106
}
107
107
108
108
// subscribers
109
- std::function<void (const typename Msg ::ConstSharedPtr msg)> sub_callback =
109
+ std::function<void (const typename TargetMsg3D ::ConstSharedPtr msg)> sub_callback =
110
110
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);
112
112
113
113
// publisher
114
- pub_ptr_ = this ->create_publisher <Msg >(" output" , rclcpp::QoS{1 });
114
+ pub_ptr_ = this ->create_publisher <TargetMsg3D >(" output" , rclcpp::QoS{1 });
115
115
116
116
// Set timer
117
117
const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
@@ -145,22 +145,22 @@ FusionNode<Msg, ObjType, Msg2D>::FusionNode(
145
145
filter_scope_max_z_ = declare_parameter<double >(" filter_scope_max_z" );
146
146
}
147
147
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(
150
150
const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg,
151
151
const std::size_t camera_id)
152
152
{
153
153
camera_info_map_[camera_id] = *input_camera_info_msg;
154
154
}
155
155
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)))
158
158
{
159
159
// do nothing by default
160
160
}
161
161
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)
164
164
{
165
165
if (cached_msg_.second != nullptr ) {
166
166
stop_watch_ptr_->toc (" processing_time" , true );
@@ -202,7 +202,7 @@ void FusionNode<Msg, Obj, Msg2D>::subCallback(const typename Msg::ConstSharedPtr
202
202
203
203
stop_watch_ptr_->toc (" processing_time" , true );
204
204
205
- typename Msg ::SharedPtr output_msg = std::make_shared<Msg >(*input_msg);
205
+ typename TargetMsg3D ::SharedPtr output_msg = std::make_shared<TargetMsg3D >(*input_msg);
206
206
207
207
preprocess (*output_msg);
208
208
@@ -291,8 +291,8 @@ void FusionNode<Msg, Obj, Msg2D>::subCallback(const typename Msg::ConstSharedPtr
291
291
}
292
292
}
293
293
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(
296
296
const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i)
297
297
{
298
298
stop_watch_ptr_->toc (" processing_time" , true );
@@ -356,14 +356,14 @@ void FusionNode<Msg, Obj, Msg2D>::roiCallback(
356
356
(cached_roi_msgs_.at (roi_i))[timestamp_nsec] = input_roi_msg;
357
357
}
358
358
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)))
361
361
{
362
362
// do nothing by default
363
363
}
364
364
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()
367
367
{
368
368
using std::chrono_literals::operator " " ms;
369
369
timer_->cancel ();
@@ -401,8 +401,8 @@ void FusionNode<Msg, Obj, Msg2D>::timer_callback()
401
401
}
402
402
}
403
403
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)
406
406
{
407
407
if (!timer_) {
408
408
return ;
@@ -418,8 +418,8 @@ void FusionNode<Msg, Obj, Msg2D>::setPeriod(const int64_t new_period)
418
418
}
419
419
}
420
420
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)
423
423
{
424
424
if (pub_ptr_->get_subscription_count () < 1 ) {
425
425
return ;
0 commit comments