image_publisher: functional component (#861)

* filename now functions when using a component
* parameter callback gets called at startup when using the component
* cleanup a bit of the logging
* add launch file to test component more easily

image_publisher/include/image_publisher/image_publisher.hpp

@@ -46,7 +46,9 @@ namespace image_publisher
 class ImagePublisher : public rclcpp::Node
 {
 public:
-  explicit ImagePublisher(const rclcpp::NodeOptions & options);
+  ImagePublisher(
+    const rclcpp::NodeOptions & options,
+    const std::string & filename = "");
 
 protected:
   void onInit();

image_publisher/launch/image_publisher_component_file.launch.py

@@ -0,0 +1,73 @@
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Software License Agreement (BSD License 2.0)
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the Willow Garage nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+
+# This is basically the same as image_publsher_file.launch.py - but using the component
+def generate_launch_description():
+    filename = os.path.join(get_package_share_directory('image_publisher'), 'launch',
+                            'splash.png')
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+    return LaunchDescription([
+
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use simulation clock if true'),
+
+        ComposableNodeContainer(
+            name='image_publisher_container',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='image_publisher',
+                    plugin='image_publisher::ImagePublisher',
+                    name='image_publisher',
+                    parameters=[{'filename': filename,
+                                 'use_sim_time': use_sim_time}],
+                    remappings=[('image_raw', '/camera/image_raw'),
+                                ('camera_info', '/camera/camera_info')],
+                )
+            ],
+            output='screen',
+        ),
+    ])

image_publisher/src/image_publisher.cpp

@@ -49,7 +49,9 @@ namespace image_publisher
 
 using namespace std::chrono_literals;
 
-ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options)
+ImagePublisher::ImagePublisher(
+  const rclcpp::NodeOptions & options,
+  const std::string & filename)
 : rclcpp::Node("ImagePublisher", options)
 {
   // For compressed topics to remap appropriately, we need to pass a
@@ -69,8 +71,6 @@ ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options)
   auto param_change_callback =
     [this](std::vector<rclcpp::Parameter> parameters) -> rcl_interfaces::msg::SetParametersResult
     {
-      RCLCPP_INFO(get_logger(), "param_change_callback");
-
       auto result = rcl_interfaces::msg::SetParametersResult();
       result.successful = true;
       for (auto parameter : parameters) {
@@ -97,13 +97,16 @@ ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options)
           RCLCPP_INFO(get_logger(), "Reset publish_rate as '%lf'", publish_rate_);
         } else if (parameter.get_name() == "camera_info_url") {
           camera_info_url_ = parameter.as_string();
-          RCLCPP_INFO(get_logger(), "Reset camera_info_rul as '%s'", camera_info_url_.c_str());
+          RCLCPP_INFO(get_logger(), "Reset camera_info_url as '%s'", camera_info_url_.c_str());
         }
       }
       ImagePublisher::reconfigureCallback();
       return result;
     };
   on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(param_change_callback);
+
+  // Set the filename after we do add_on_set_parameters_callback so the callback triggers
+  filename_ = this->declare_parameter("filename", filename);
 }
 
 void ImagePublisher::reconfigureCallback()
@@ -114,7 +117,7 @@ void ImagePublisher::reconfigureCallback()
 
   camera_info_manager::CameraInfoManager c(this);
   if (!camera_info_url_.empty()) {
-    RCLCPP_INFO(get_logger(), "camera_info_url exist");
+    RCLCPP_INFO(get_logger(), "camera_info_url: %s", camera_info_url_.c_str());
     try {
       c.validateURL(camera_info_url_);
       c.loadCameraInfo(camera_info_url_);
@@ -163,7 +166,7 @@ void ImagePublisher::doWork()
 
 void ImagePublisher::onInit()
 {
-  RCLCPP_INFO(this->get_logger(), "File name for publishing image is : %s", filename_.c_str());
+  RCLCPP_INFO(this->get_logger(), "File name for publishing image is: %s", filename_.c_str());
   try {
     image_ = cv::imread(filename_, cv::IMREAD_COLOR);
     if (image_.empty()) {  // if filename not exist, open video device
@@ -192,10 +195,10 @@ void ImagePublisher::onInit()
 
   RCLCPP_INFO(
     this->get_logger(),
-    "Flip horizontal image is : %s", ((flip_horizontal_) ? "true" : "false"));
+    "Flip horizontal image is: %s", ((flip_horizontal_) ? "true" : "false"));
   RCLCPP_INFO(
     this->get_logger(),
-    "Flip flip_vertical image is : %s", ((flip_vertical_) ? "true" : "false"));
+    "Flip flip_vertical image is: %s", ((flip_vertical_) ? "true" : "false"));
 
   // From http://docs.opencv.org/modules/core/doc/operations_on_arrays.html
   // #void flip(InputArray src, OutputArray dst, int flipCode)

image_publisher/src/image_publisher_node.cpp

@@ -49,8 +49,7 @@ int main(int argc, char ** argv)
   }
 
   rclcpp::NodeOptions options;
-  auto publisher = std::make_shared<image_publisher::ImagePublisher>(options);
-  publisher->declare_parameter("filename", argv[1]);
+  auto publisher = std::make_shared<image_publisher::ImagePublisher>(options, argv[1]);
 
   rclcpp::spin(publisher);
   rclcpp::shutdown();