@@ -5,6 +5,21 @@ using std::placeholders::_1;
55
66ImageFilteringNode::ImageFilteringNode (const rclcpp::NodeOptions& options)
77 : Node(" image_filtering_node" , options) {
8+ declare_parameters ();
9+ check_and_subscribe_to_image_topic ();
10+ set_filter_params ();
11+ initialize_parameter_handler ();
12+
13+ rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
14+ auto qos_sensor_data = rclcpp::QoS (
15+ rclcpp::QoSInitialization (qos_profile.history , 1 ), qos_profile);
16+
17+ std::string pub_topic = this ->get_parameter (" pub_topic" ).as_string ();
18+ image_pub_ = this ->create_publisher <sensor_msgs::msg::Image>(
19+ pub_topic, qos_sensor_data);
20+ }
21+
22+ void ImageFilteringNode::declare_parameters () {
823 this ->declare_parameter <std::string>(" sub_topic" );
924 this ->declare_parameter <std::string>(" pub_topic" );
1025 this ->declare_parameter <std::string>(" output_encoding" );
@@ -27,19 +42,6 @@ ImageFilteringNode::ImageFilteringNode(const rclcpp::NodeOptions& options)
2742 this ->declare_parameter <double >(" filter_params.otsu.gsc_weight_b" );
2843 this ->declare_parameter <int >(" filter_params.otsu.erosion_size" );
2944 this ->declare_parameter <int >(" filter_params.otsu.dilation_size" );
30-
31- rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
32- auto qos_sensor_data = rclcpp::QoS (
33- rclcpp::QoSInitialization (qos_profile.history , 1 ), qos_profile);
34-
35- check_and_subscribe_to_image_topic ();
36- set_filter_params ();
37-
38- initialize_parameter_handler ();
39-
40- std::string pub_topic = this ->get_parameter (" pub_topic" ).as_string ();
41- image_pub_ = this ->create_publisher <sensor_msgs::msg::Image>(
42- pub_topic, qos_sensor_data);
4345}
4446
4547void ImageFilteringNode::set_filter_params () {
0 commit comments