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

Jump to bottom

Make sure the fps change has been accepted by camera before finishing… #33

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

Make sure the fps change has been accepted by camera before finishing… #33

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
7 changes: 7 additions & 0 deletions multisense_ros/include/multisense_ros/camera.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -165,6 +165,13 @@ class Camera : public rclcpp::Node

void initalizeParameters(const crl::multisense::image::Config& config);

//
// Helper function to verify that an fps change has been accepted by the camera.
// FPS Changes can take time on certain versions of camera firmware.
// To verify changes have been accepted by the camera we will poll the value
// up to 0.5s.
crl::multisense::Status verifyFpsChange(const crl::multisense::image::Config& config);

//
// Parameter management

Expand Down
36 changes: 35 additions & 1 deletion multisense_ros/src/camera.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1927,6 +1927,35 @@ void Camera::timerCallback()
}
}

crl::multisense::Status Camera::verifyFpsChange(const crl::multisense::image::Config& config)
Copy link
Collaborator

Choose a reason for hiding this comment

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

@psmithcrl Is FPS the only parameter which needs to be confirmed like this? Out of curiosity what failure were you seeing by setting the FPS once?

Copy link
Collaborator

Choose a reason for hiding this comment

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

Found the thread on slack. Sorry

{
int timeout = 50; //500ms
if (const auto status = driver_->setImageConfig(config) != Status_Ok)
{
RCLCPP_ERROR(get_logger(), "Error: Camera failed to set fps");
return status;
}

do {

image::Config i;
if (const auto status = driver_->getImageConfig(i); status != Status_Ok) {
RCLCPP_ERROR(get_logger(), "Camera: failed to query sensor configuration: %s",
Channel::statusString(status));
return status;
}

if (std::abs(config.fps() - i.fps()) < 1.0f) {
return Status_Ok;
}

std::this_thread::sleep_for(std::chrono::milliseconds(10));
} while((timeout-->0));

return Status_TimedOut;
}


void Camera::initalizeParameters(const image::Config& config)
{
//
Expand Down Expand Up @@ -2741,7 +2770,12 @@ rcl_interfaces::msg::SetParametersResult Camera::parameterCallback(const std::ve
if (image_config.fps() != value)
{
image_config.setFps(value);
update_image_config = true;
if (const auto status = verifyFpsChange(image_config); status != Status_Ok) {
RCLCPP_ERROR(get_logger(), "Error: failed to update fps on the camera!");
return result.set__successful(false).set__reason(Channel::statusString(status));
}

updateConfig(image_config);
}
}
else if(name == "stereo_post_filtering")
Expand Down