Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

image_rotate: clean up #862

Merged
merged 6 commits into from
Jan 19, 2024
Merged

image_rotate: clean up #862

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
ea1af6d
image_rotate: initialize target vectors
mikeferguson Jan 19, 2024
478d8ea
implement lazy subscribers
mikeferguson Jan 19, 2024
29b148a
add parameter for subscriber transport
mikeferguson Jan 19, 2024
652e5b2
remove dead code, make topic check actually work
mikeferguson Jan 19, 2024
ab8409f
linting
mikeferguson Jan 19, 2024
6755ce5
use TransportHints
mikeferguson Jan 19, 2024
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
5 changes: 1 addition & 4 deletions image_rotate/include/image_rotate/image_rotate_node.hpp
View file
Open in desktop
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 transport;
};

class ImageRotateNode : public rclcpp::Node
Expand All @@ -82,10 +83,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
Open in desktop
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;
}
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This debug message didn't work at all - made a more robust version by checking the overlap of our topic name with the actual topics that exist (as many of our depth_image_proc stuff does)


rclcpp::init(argc, argv);
rclcpp::NodeOptions options;
auto node = std::make_shared<image_rotate::ImageRotateNode>(options);
Expand Down
174 changes: 76 additions & 98 deletions image_rotate/src/image_rotate_node.cpp
View file
Open in desktop
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();
}
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is unneeded - none of the parameters that are able to be updated would impact how the subscribers would actually be setup


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);

// Allow overriding the SUBSCRIBER transport
config_.transport = this->declare_parameter("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,14 @@ 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 +283,59 @@ 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 &)
{
//std::lock_guard<std::mutex> lock(connect_mutex_);
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);

// 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());
}

RCLCPP_INFO(get_logger(), "Subscribing to %s topic.", topic_name.c_str());

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),
config_.transport,
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),
config_.transport,
custom_qos);
}
}
};
img_pub_ = image_transport::create_publisher(this, "rotated/image", rmw_qos_profile_default, pub_options);
}
} // namespace image_rotate

Expand Down
Loading