Skip to content

Commit

Permalink
image_publisher: functional component (#861)
Browse files Browse the repository at this point in the history
* 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
  • Loading branch information
mikeferguson authored Jan 19, 2024
1 parent 5645baa commit bcebee4
Show file tree
Hide file tree
Showing 4 changed files with 88 additions and 11 deletions.
4 changes: 3 additions & 1 deletion image_publisher/include/image_publisher/image_publisher.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
73 changes: 73 additions & 0 deletions image_publisher/launch/image_publisher_component_file.launch.py
Original file line number Diff line number Diff line change
@@ -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',
),
])
19 changes: 11 additions & 8 deletions image_publisher/src/image_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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) {
Expand All @@ -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()
Expand All @@ -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_);
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down
3 changes: 1 addition & 2 deletions image_publisher/src/image_publisher_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down

0 comments on commit bcebee4

Please sign in to comment.