Skip to content

Commit

Permalink
image_rotate: clean up (ros-perception#862)
Browse files Browse the repository at this point in the history 
This is the first component/node with a cleanup pass to be fully
implemented:

* Fix ros-perception#740 by initializing vectors. Do this by declaring parameters
AFTER we define the callback
 * Implement lazy subscribers (I missed this in the earlier PRs)
* Add image_transport parameter so we can specify that desired transport
of our subscriptions
* Update how we test for connectivity (and update the debug message -
has nothing to do with whether we are remapped, it's really about
whether we are connected)
  • Loading branch information
@mikeferguson
mikeferguson authored and Krzysztof Wojciechowski committed May 27, 2024
1 parent a13d486 commit 682fe00
Show file tree
Hide file tree
Showing 3 changed files with 80 additions and 109 deletions.
4 changes: 0 additions & 4 deletions image_rotate/include/image_rotate/image_rotate_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,10 +82,6 @@ class ImageRotateNode : public rclcpp::Node
void do_work(
const sensor_msgs::msg::Image::ConstSharedPtr & msg,
const std::string input_frame_from_msg);
void subscribe();
void unsubscribe();
void connectCb();
void disconnectCb();
void onInit();

rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_;
Expand Down
7 changes: 0 additions & 7 deletions image_rotate/src/image_rotate.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,13 +38,6 @@ int main(int argc, char ** argv)
// Force flush of the stdout buffer.
setvbuf(stdout, NULL, _IONBF, BUFSIZ);

if (argc <= 1) {
RCUTILS_LOG_WARN(
"Topic 'image' has not been remapped! Typical command-line usage:\n"
"\t$ ros2 run image_rotate image_rotate image:=<image topic>");
return 1;
}

rclcpp::init(argc, argv);
rclcpp::NodeOptions options;
auto node = std::make_shared<image_rotate::ImageRotateNode>(options);
Expand Down
178 changes: 80 additions & 98 deletions image_rotate/src/image_rotate_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,31 +72,9 @@ namespace image_rotate
ImageRotateNode::ImageRotateNode(const rclcpp::NodeOptions & options)
: rclcpp::Node("ImageRotateNode", options)
{
config_.target_frame_id = this->declare_parameter("target_frame_id", std::string(""));
config_.target_x = this->declare_parameter("target_x", 0.0);
config_.target_y = this->declare_parameter("target_y", 0.0);
config_.target_z = this->declare_parameter("target_z", 1.0);

config_.source_frame_id = this->declare_parameter("source_frame_id", std::string(""));
config_.source_x = this->declare_parameter("source_x", 0.0);
config_.source_y = this->declare_parameter("source_y", -1.0);
config_.source_z = this->declare_parameter("source_z", 0.0);

config_.output_frame_id = this->declare_parameter("output_frame_id", std::string(""));
config_.input_frame_id = this->declare_parameter("input_frame_id", std::string(""));
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);

auto reconfigureCallback =
[this](std::vector<rclcpp::Parameter> parameters) -> rcl_interfaces::msg::SetParametersResult
{
RCLCPP_INFO(get_logger(), "reconfigureCallback");

auto result = rcl_interfaces::msg::SetParametersResult();
result.successful = true;
for (auto parameter : parameters) {
Expand Down Expand Up @@ -128,13 +106,31 @@ ImageRotateNode::ImageRotateNode(const rclcpp::NodeOptions & options)
source_vector_.vector.x = config_.source_x;
source_vector_.vector.y = config_.source_y;
source_vector_.vector.z = config_.source_z;
if (subscriber_count_) { // @todo: Could do this without an interruption at some point.
unsubscribe();
subscribe();
}

return result;
};
on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(reconfigureCallback);

// Set parameters AFTER add_on_set_parameters_callback
config_.target_frame_id = this->declare_parameter("target_frame_id", std::string(""));
config_.target_x = this->declare_parameter("target_x", 0.0);
config_.target_y = this->declare_parameter("target_y", 0.0);
config_.target_z = this->declare_parameter("target_z", 1.0);

config_.source_frame_id = this->declare_parameter("source_frame_id", std::string(""));
config_.source_x = this->declare_parameter("source_x", 0.0);
config_.source_y = this->declare_parameter("source_y", -1.0);
config_.source_z = this->declare_parameter("source_z", 0.0);

config_.output_frame_id = this->declare_parameter("output_frame_id", std::string(""));
config_.input_frame_id = this->declare_parameter("input_frame_id", std::string(""));
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);

// TransportHints does not actually declare the parameter
this->declare_parameter<std::string>("image_transport", "raw");

onInit();
}

Expand Down Expand Up @@ -187,13 +183,6 @@ void ImageRotateNode::do_work(
source_frame_id, input_frame_id, tf2_time, tf2_time - prev_stamp_);
tf2::doTransform(source_vector_, source_vector_transformed, transform);

// RCUTILS_LOG_INFO("target: %f %f %f", target_vector_.x, target_vector_.y, target_vector_.z);
// RCUTILS_LOG_INFO("target_transformed: %f %f %f", target_vector_transformed.x, "
// "target_vector_transformed.y, target_vector_transformed.z");
// RCUTILS_LOG_INFO("source: %f %f %f", source_vector_.x, source_vector_.y, source_vector_.z);
// RCUTILS_LOG_INFO("source_transformed: %f %f %f", source_vector_transformed.x, "
// "source_vector_transformed.y, source_vector_transformed.z");

// Calculate the angle of the rotation.
double angle = angle_;
if ((target_vector_transformed.vector.x != 0 || target_vector_transformed.vector.y != 0) &&
Expand Down Expand Up @@ -226,11 +215,9 @@ void ImageRotateNode::do_work(
}
angle_ = fmod(angle_, 2.0 * M_PI);
} catch (const tf2::TransformException & e) {
RCUTILS_LOG_ERROR("Transform error: %s", e.what());
RCLCPP_ERROR(get_logger(), "Transform error: %s", e.what());
}

// RCUTILS_LOG_INFO("angle: %f", 180 * angle_ / M_PI);

// Publish the transform.
geometry_msgs::msg::TransformStamped transform;
transform.transform.translation.x = 0;
Expand Down Expand Up @@ -262,7 +249,6 @@ void ImageRotateNode::do_work(
int step = config_.output_image_size;
out_size = candidates[step] + (candidates[step + 1] - candidates[step]) *
(config_.output_image_size - step);
// RCUTILS_LOG_INFO("out_size: %d", out_size);

// Compute the rotation matrix.
cv::Mat rot_matrix = cv::getRotationMatrix2D(
Expand All @@ -281,63 +267,15 @@ void ImageRotateNode::do_work(
out_img->header.frame_id = transform.child_frame_id;
img_pub_.publish(out_img);
} catch (const cv::Exception & e) {
RCUTILS_LOG_ERROR(
RCLCPP_ERROR(
get_logger(),
"Image processing error: %s %s %s %i",
e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
}

prev_stamp_ = tf2_ros::fromMsg(msg->header.stamp);
}

void ImageRotateNode::subscribe()
{
RCUTILS_LOG_DEBUG("Subscribing to image topic.");
if (config_.use_camera_info && config_.input_frame_id.empty()) {
auto custom_qos = rmw_qos_profile_system_default;
custom_qos.depth = 3;

cam_sub_ = image_transport::create_camera_subscription(
this,
"image",
std::bind(
&ImageRotateNode::imageCallbackWithInfo, this,
std::placeholders::_1, std::placeholders::_2),
"raw",
custom_qos);
} else {
auto custom_qos = rmw_qos_profile_system_default;
custom_qos.depth = 3;
img_sub_ = image_transport::create_subscription(
this,
"image",
std::bind(&ImageRotateNode::imageCallback, this, std::placeholders::_1),
"raw",
custom_qos);
}
}

void ImageRotateNode::unsubscribe()
{
RCUTILS_LOG_DEBUG("Unsubscribing from image topic.");
img_sub_.shutdown();
cam_sub_.shutdown();
}

void ImageRotateNode::connectCb()
{
if (subscriber_count_++ == 0) {
subscribe();
}
}

void ImageRotateNode::disconnectCb()
{
subscriber_count_--;
if (subscriber_count_ == 0) {
unsubscribe();
}
}

void ImageRotateNode::onInit()
{
subscriber_count_ = 0;
Expand All @@ -346,18 +284,62 @@ void ImageRotateNode::onInit()
rclcpp::Clock::SharedPtr clock = this->get_clock();
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(clock);
tf_sub_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
// TODO(yechun1): Implement when SubscriberStatusCallback is available
// image_transport::SubscriberStatusCallback connect_cb =
// boost::bind(&ImageRotateNode::connectCb, this, _1);
// image_transport::SubscriberStatusCallback connect_cb =
// std::bind(&CropForemostNode::connectCb, this);
// image_transport::SubscriberStatusCallback disconnect_cb =
// boost::bind(&ImageRotateNode::disconnectCb, this, _1);
// img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "rotated")).advertise(
// "image", 1, connect_cb, disconnect_cb);
connectCb();
img_pub_ = image_transport::create_publisher(this, "rotated/image");
tf_pub_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);

// Create publisher with connect callback
rclcpp::PublisherOptions pub_options;
pub_options.event_callbacks.matched_callback =
[this](rclcpp::MatchedInfo &)
{
if (img_pub_.getNumSubscribers() == 0) {
RCLCPP_DEBUG(get_logger(), "Unsubscribing from image topic.");
img_sub_.shutdown();
cam_sub_.shutdown();
} else {
// 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_name = node_base->resolve_topic_or_service_name("image", false);
RCLCPP_INFO(get_logger(), "Subscribing to %s topic.", topic_name.c_str());

// Check that remapping appears to be correct
auto topics = this->get_topic_names_and_types();
if (topics.find(topic_name) == topics.end()) {
RCLCPP_WARN(
get_logger(),
"Topic %s is not connected! Typical command-line usage:\n"
"\t$ ros2 run image_rotate image_rotate --ros-args -r image:=<image topic>",
topic_name.c_str());
}

// This will check image_transport parameter to get proper transport
image_transport::TransportHints transport_hint(this, "raw");

if (config_.use_camera_info && config_.input_frame_id.empty()) {
auto custom_qos = rmw_qos_profile_system_default;
custom_qos.depth = 3;
cam_sub_ = image_transport::create_camera_subscription(
this,
topic_name,
std::bind(
&ImageRotateNode::imageCallbackWithInfo, this,
std::placeholders::_1, std::placeholders::_2),
transport_hint.getTransport(),
custom_qos);
} else {
auto custom_qos = rmw_qos_profile_system_default;
custom_qos.depth = 3;
img_sub_ = image_transport::create_subscription(
this,
topic_name,
std::bind(&ImageRotateNode::imageCallback, this, std::placeholders::_1),
transport_hint.getTransport(),
custom_qos);
}
}
};
img_pub_ = image_transport::create_publisher(
this, "rotated/image", rmw_qos_profile_default, pub_options);
}
} // namespace image_rotate

Expand Down

0 comments on commit 682fe00

Please sign in to comment.