Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions image_rotate/include/image_rotate/image_rotate_node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,7 @@ struct ImageRotateConfig
bool use_camera_info;
double max_angular_rate;
double output_image_size;
std::string custom_qos_type;
};

class ImageRotateNode : public rclcpp::Node
Expand Down
14 changes: 10 additions & 4 deletions image_rotate/src/image_rotate_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -127,6 +127,7 @@ ImageRotateNode::ImageRotateNode(const rclcpp::NodeOptions & options)
config_.use_camera_info = this->declare_parameter("use_camera_info", true);
config_.max_angular_rate = this->declare_parameter("max_angular_rate", 10.0);
config_.output_image_size = this->declare_parameter("output_image_size", 2.0);
config_.custom_qos_type = this->declare_parameter("custom_qos_type", std::string("default"));

// TransportHints does not actually declare the parameter
this->declare_parameter<std::string>("image_transport", "raw");
Expand Down Expand Up @@ -315,9 +316,16 @@ void ImageRotateNode::onInit()
image_transport::TransportHints transport_hint(*this,
"raw");

if (config_.use_camera_info && config_.input_frame_id.empty()) {
auto custom_qos = rclcpp::SystemDefaultsQoS();
auto custom_qos = rclcpp::SystemDefaultsQoS();
if (config_.custom_qos_type == "sensor_data") {
custom_qos = rclcpp::SensorDataQoS();
custom_qos.keep_last(3);
} else if (config_.custom_qos_type == "default") {
custom_qos = rclcpp::SystemDefaultsQoS();
custom_qos.keep_last(3);
}

if (config_.use_camera_info && config_.input_frame_id.empty()) {
cam_sub_ = image_transport::create_camera_subscription(
*this,
topic_name,
Expand All @@ -327,8 +335,6 @@ void ImageRotateNode::onInit()
transport_hint.getTransport(),
custom_qos);
} else {
auto custom_qos = rclcpp::SystemDefaultsQoS();
custom_qos.keep_last(3);
img_sub_ = image_transport::create_subscription(
*this,
topic_name,
Expand Down