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

FIX: changing resolution in runtime #11

Open
wants to merge 1 commit into
base: kronos_main
Choose a base branch
from
Open

FIX: changing resolution in runtime #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
9 changes: 9 additions & 0 deletions include/cv_camera/driver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,11 @@ class Driver : public rclcpp::Node
*/
void RestartNodeCb(shared_ptr_request_id const request_header, shared_ptr_trigger_request const request,
shared_ptr_trigger_response response);

/**
* @brief Update resolution of the camera when parameter is updated in runtime
*/
void update_resolution();
/**
* @brief callback for pause/resume image publishing
* 1 -> pause
Expand Down Expand Up @@ -115,6 +120,10 @@ class Driver : public rclcpp::Node
* @brief ROS private timer for publishing images.
*/
rclcpp::TimerBase::SharedPtr publish_tmr_;
/**
* @brief ROS private timer for updating resolution.
*/
rclcpp::TimerBase::SharedPtr update_resolution_tmr_;
/**
* @brief ROS Service for triggering re setup of the node.
*/
Expand Down
30 changes: 28 additions & 2 deletions src/driver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -157,6 +157,9 @@ bool Driver::setup()
this->create_wall_timer(std::chrono::milliseconds(int(1000.0 / read_rate_)), std::bind(&Driver::read, this));
publish_tmr_ =
this->create_wall_timer(std::chrono::milliseconds(int(1000.0 / publish_rate_)), std::bind(&Driver::proceed, this));
update_resolution_tmr_ =
this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&Driver::update_resolution, this));
update_resolution_tmr_->cancel();

cam_status_ = std::make_shared<std_msgs::msg::UInt8>();
cam_status_->data = ONLINE;
Expand Down Expand Up @@ -368,14 +371,20 @@ rcl_interfaces::msg::SetParametersResult Driver::parameters_cb(const std::vector
{
if (name == "width")
{
RCLCPP_INFO(get_logger(), "Setting new width to %ld", parameter.as_int());
width_ = parameter.as_int();
this->set_parameter(rclcpp::Parameter("cv_cap_prop_frame_width", (double)width_));
// To set the underlying OpenCV parameter we cant set a parameter inside the setParameters callback
// so we need to reset the timer to update the resolution
update_resolution_tmr_->reset();
camera_->setPropertyFromParam(cv::CAP_PROP_FRAME_WIDTH, "cv_cap_prop_frame_width");
}
else if (name == "height")
{
RCLCPP_INFO(get_logger(), "Setting new height to %ld", parameter.as_int());
height_ = parameter.as_int();
this->set_parameter(rclcpp::Parameter("cv_cap_prop_frame_height", (double)height_));
// To set the underlying OpenCV parameter we cant set a parameter inside the setParameters callback
// so we need to reset the timer to update the resolution
update_resolution_tmr_->reset();
camera_->setPropertyFromParam(cv::CAP_PROP_FRAME_HEIGHT, "cv_cap_prop_frame_height");
}
}
Expand Down Expand Up @@ -420,6 +429,23 @@ void Driver::RestartNodeCb(shared_ptr_request_id const, shared_ptr_trigger_reque
response->message = "Camera setup will be restarted";
}

void Driver::update_resolution()
{
update_resolution_tmr_->cancel();
if (width_ != camera_->getProperty(cv::CAP_PROP_FRAME_WIDTH))
{
this->set_parameter(rclcpp::Parameter("cv_cap_prop_frame_width", (double)width_));
}
else if (height_ != camera_->getProperty(cv::CAP_PROP_FRAME_HEIGHT))
{
this->set_parameter(rclcpp::Parameter("cv_cap_prop_frame_height", (double)height_));
}
else
{
RCLCPP_WARN(get_logger(), "Resolution already set");
}
}

void Driver::GrabFrameCb(shared_ptr_request_id const, shared_ptr_grab_frame_request const,
shared_ptr_grab_frame_response response)
{
Expand Down