Skip to content

Commit

Permalink
fix image publisher remapping (#941)
Browse files Browse the repository at this point in the history
Addresses #940 - fixes the compressed/etc topic remapping for publishers
  • Loading branch information
mikeferguson authored Feb 12, 2024
1 parent a6e9060 commit 14f604a
Show file tree
Hide file tree
Showing 6 changed files with 27 additions and 10 deletions.
8 changes: 7 additions & 1 deletion depth_image_proc/src/register.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -143,9 +143,15 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options)
sub_rgb_info_.subscribe(this, "rgb/camera_info");
}
};
// For compressed topics to remap appropriately, we need to pass a
// fully expanded and remapped topic name to image_transport
auto node_base = this->get_node_base_interface();
std::string topic =
node_base->resolve_topic_or_service_name("depth_registered/image_rect", false);

pub_registered_ =
image_transport::create_camera_publisher(
this, "depth_registered/image_rect",
this, topic,
rmw_qos_profile_default, pub_options);
}

Expand Down
3 changes: 2 additions & 1 deletion image_proc/src/crop_decimate.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,7 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options)
// fully expanded and remapped topic name to image_transport
auto node_base = this->get_node_base_interface();
image_topic_ = node_base->resolve_topic_or_service_name("in/image_raw", false);
std::string pub_topic = node_base->resolve_topic_or_service_name("out/image_raw", false);

queue_size_ = this->declare_parameter("queue_size", 5);
target_frame_id_ = this->declare_parameter("target_frame_id", std::string());
Expand Down Expand Up @@ -156,7 +157,7 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options)

// Create publisher with QoS matched to subscribed topic publisher
auto qos_profile = getTopicQosProfile(this, image_topic_);
pub_ = image_transport::create_camera_publisher(this, "out/image_raw", qos_profile, pub_options);
pub_ = image_transport::create_camera_publisher(this, pub_topic, qos_profile, pub_options);
}

void CropDecimateNode::imageCb(
Expand Down
3 changes: 2 additions & 1 deletion image_proc/src/crop_non_zero.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options)
// fully expanded and remapped topic name to image_transport
auto node_base = this->get_node_base_interface();
image_topic_ = node_base->resolve_topic_or_service_name("image_raw", false);
std::string pub_topic = node_base->resolve_topic_or_service_name("image", false);

// Setup lazy subscriber using publisher connection callback
rclcpp::PublisherOptions pub_options;
Expand All @@ -84,7 +85,7 @@ CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options)

// Create publisher with QoS matched to subscribed topic publisher
auto qos_profile = getTopicQosProfile(this, image_topic_);
pub_ = image_transport::create_publisher(this, "image", qos_profile, pub_options);
pub_ = image_transport::create_publisher(this, pub_topic, qos_profile, pub_options);
}

void CropNonZeroNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)
Expand Down
9 changes: 6 additions & 3 deletions image_proc/src/debayer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -56,10 +56,14 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)
// TransportHints does not actually declare the parameter
this->declare_parameter<std::string>("image_transport", "raw");

debayer_ = this->declare_parameter("debayer", 3);

// For compressed topics to remap appropriately, we need to pass a
// fully expanded and remapped topic name to image_transport
auto node_base = this->get_node_base_interface();
image_topic_ = node_base->resolve_topic_or_service_name("image_raw", false);
std::string mono_topic = node_base->resolve_topic_or_service_name("image_mono", false);
std::string color_topic = node_base->resolve_topic_or_service_name("image_color", false);

// Setup lazy subscriber using publisher connection callback
rclcpp::PublisherOptions pub_options;
Expand All @@ -85,9 +89,8 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)

// Create publisher with QoS matched to subscribed topic publisher
auto qos_profile = getTopicQosProfile(this, image_topic_);
pub_mono_ = image_transport::create_publisher(this, "image_mono", qos_profile, pub_options);
pub_color_ = image_transport::create_publisher(this, "image_color", qos_profile, pub_options);
debayer_ = this->declare_parameter("debayer", 3);
pub_mono_ = image_transport::create_publisher(this, mono_topic, qos_profile, pub_options);
pub_color_ = image_transport::create_publisher(this, color_topic, qos_profile, pub_options);
}

void DebayerNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)
Expand Down
5 changes: 3 additions & 2 deletions image_proc/src/resize.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options)
// fully expanded and remapped topic name to image_transport
auto node_base = this->get_node_base_interface();
image_topic_ = node_base->resolve_topic_or_service_name("image/image_raw", false);
std::string pub_topic = node_base->resolve_topic_or_service_name("resize/image_raw", false);

// Declare parameters before we setup any publishers or subscribers
interpolation_ = this->declare_parameter("interpolation", 1);
Expand Down Expand Up @@ -93,8 +94,8 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options)

// Create publisher with QoS matched to subscribed topic publisher
auto qos_profile = getTopicQosProfile(this, image_topic_);
pub_image_ = image_transport::create_camera_publisher(
this, "resize/image_raw", qos_profile, pub_options);
pub_image_ =
image_transport::create_camera_publisher(this, pub_topic, qos_profile, pub_options);
}

void ResizeNode::imageCb(
Expand Down
9 changes: 7 additions & 2 deletions image_rotate/src/image_rotate_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -338,8 +338,13 @@ void ImageRotateNode::onInit()
}
}
};
img_pub_ = image_transport::create_publisher(
this, "rotated/image", rmw_qos_profile_default, pub_options);

// For compressed topics to remap appropriately, we need to pass a
// fully expanded and remapped topic name to image_transport
auto node_base = this->get_node_base_interface();
std::string topic = node_base->resolve_topic_or_service_name("rotated/image", false);

img_pub_ = image_transport::create_publisher(this, topic, rmw_qos_profile_default, pub_options);
}
} // namespace image_rotate

Expand Down

0 comments on commit 14f604a

Please sign in to comment.