@@ -90,15 +90,12 @@ class FusionNode : public rclcpp::Node
90
90
explicit FusionNode (
91
91
const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size);
92
92
93
- protected :
93
+ private :
94
94
// Common process methods
95
95
void cameraInfoCallback (
96
96
const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg,
97
97
const std::size_t camera_id);
98
- // callback for main subscription
99
- void subCallback (const typename Msg3D::ConstSharedPtr input_msg);
100
- // callback for roi subscription
101
- void roiCallback (const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i);
98
+
102
99
// callback for timer
103
100
void timer_callback ();
104
101
void setPeriod (const int64_t new_period);
@@ -115,6 +112,29 @@ class FusionNode : public rclcpp::Node
115
112
return true ;
116
113
}
117
114
115
+ // camera projection
116
+ float approx_grid_cell_w_size_;
117
+ float approx_grid_cell_h_size_;
118
+
119
+ // timer
120
+ rclcpp::TimerBase::SharedPtr timer_;
121
+ double timeout_ms_{};
122
+ double match_threshold_ms_{};
123
+
124
+ std::vector<typename rclcpp::Subscription<Msg2D>::SharedPtr> rois_subs_;
125
+ std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subs_;
126
+
127
+ // cache for fusion
128
+ int64_t cached_det3d_msg_timestamp_;
129
+ typename Msg3D::SharedPtr cached_det3d_msg_ptr_;
130
+ std::mutex mutex_cached_msgs_;
131
+
132
+ protected:
133
+ // callback for main subscription
134
+ void subCallback (const typename Msg3D::ConstSharedPtr input_msg);
135
+ // callback for roi subscription
136
+ void roiCallback (const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i);
137
+
118
138
// Custom process methods
119
139
virtual void preprocess (Msg3D & output_msg);
120
140
virtual void fuseOnSingleImage (
@@ -131,24 +151,8 @@ class FusionNode : public rclcpp::Node
131
151
// 2d detection management
132
152
std::vector<Det2dStatus<Msg2D>> det2d_list_;
133
153
134
- // camera projection
135
- float approx_grid_cell_w_size_;
136
- float approx_grid_cell_h_size_;
137
-
138
- // timer
139
- rclcpp::TimerBase::SharedPtr timer_;
140
- double timeout_ms_{};
141
- double match_threshold_ms_{};
142
-
143
154
/* * \brief A vector of subscriber. */
144
155
typename rclcpp::Subscription<Msg3D>::SharedPtr sub_;
145
- std::vector<typename rclcpp::Subscription<Msg2D>::SharedPtr> rois_subs_;
146
- std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subs_;
147
-
148
- // cache for fusion
149
- int64_t cached_det3d_msg_timestamp_;
150
- typename Msg3D::SharedPtr cached_det3d_msg_ptr_;
151
- std::mutex mutex_cached_msgs_;
152
156
153
157
// parameters for out_of_scope filter
154
158
float filter_scope_min_x_;
0 commit comments