Skip to content

Commit

Permalink
Added most ISP and sensor parameters to ROS interface
Browse files Browse the repository at this point in the history
  • Loading branch information
@treideme
treideme committed Jun 8, 2023
1 parent 30d1a8d commit 4560419
Show file tree
Hide file tree
Showing 4 changed files with 143 additions and 2 deletions.
35 changes: 35 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,39 @@ ros2 run image_view image_view --ros-args --remap /image:=/image_raw
| ```keep_partial``` | Keep partial images (i.e. corrupted in transmission) | ```false``` | :heavy_check_mark: |
| ```format``` | Format of the camera (*) | ```1920x1440``` | :x: |
| ```fps``` | Target frames per second (*) | ```20``` | :x: |
| ***Image Sensor(s) Controls*** | (**) | | |
| ```exposure``` | Exposure time in milliseconds | ```20``` | :heavy_check_mark: |
| ```gain``` | Analog gain | ```1.0``` | :heavy_check_mark: |
| ```dGainBlue``` | Digital gain for blue pixels in Bayer array | ```1024``` | :heavy_check_mark: |
| ```dgainGB``` | Digital gain for green-blue pixels in Bayer array | ```1024``` | :heavy_check_mark: |
| ```dgainGR``` | Digital gain for green-red pixels in Bayer array | ```1024``` | :heavy_check_mark: |
| ```dGainRed``` | Digital gain for red pixels in Bayer array | ```1024``` | :heavy_check_mark: |
| **Image Processing Controls** | | | |
| ```gamma``` | Gamma correction of the image | ```2.2``` | :heavy_check_mark: |
| ```blackBlue``` | Black level for blue pixels in Bayer array | ```255``` | :heavy_check_mark: |
| ```blackGB``` | Black level for green-blue pixels in Bayer array | ```255``` | :heavy_check_mark: |
| ```blackGR``` | Black level for green-red pixels in Bayer array | ```255``` | :heavy_check_mark: |
| ```blackRed``` | Black level for red pixels in Bayer array | ```255``` | :heavy_check_mark: |
| ```blackGainBlue``` | Black gain for blue pixels in Bayer array | ```0.9375``` | :heavy_check_mark: |
| ```blackGainGB``` | Black gain for green-blue pixels in Bayer array | ```0.9375``` | :heavy_check_mark: |
| ```blackGainGR``` | Black gain for green-red pixels in Bayer array | ```0.9375``` | :heavy_check_mark: |
| ```blackGainRed``` | Black gain for red pixels in Bayer array | ```0.9375``` | :heavy_check_mark: |
| ```brightness``` | Brightness of the image | ```-13107``` | :heavy_check_mark: |
| ```linearContrast``` | Linear contrast of the image | ```136``` | :heavy_check_mark: |
| ```wbBlue``` | White balance for blue component | ```1.0``` | :heavy_check_mark: |
| ```wbGreen``` | White balance for green component | ```1.0``` | :heavy_check_mark: |
| ```wbRed``` | White balance for red component | ```1.0``` | :heavy_check_mark: |
| **Color Correction Controls** | | | |
| ```custom_ccm``` | Enable custom color correction matrix | ```false``` | :x: |
| ```CCMValue00``` | Color correction matrix value at row 0, column 0 (only if custom_ccm=true) | ```1.0``` | :x: |
| ```CCMValue01``` | Color correction matrix value at row 0, column 1 (only if custom_ccm=true) | ```0.0``` | :x: |
| ```CCMValue02``` | Color correction matrix value at row 0, column 2 (only if custom_ccm=true) | ```0.0``` | :x: |
| ```CCMValue10``` | Color correction matrix value at row 1, column 0 (only if custom_ccm=true) | ```0.0``` | :x: |
| ```CCMValue11``` | Color correction matrix value at row 1, column 1 (only if custom_ccm=true) | ```1.0``` | :x: |
| ```CCMValue12``` | Color correction matrix value at row 1, column 2 (only if custom_ccm=true) | ```0.0``` | :x: |
| ```CCMValue20``` | Color correction matrix value at row 2, column 0 (only if custom_ccm=true) | ```0.0``` | :x: |
| ```CCMValue21``` | Color correction matrix value at row 2, column 1 (only if custom_ccm=true) | ```0.0``` | :x: |
| ```CCMValue22``` | Color correction matrix value at row 2, column 2 (only if custom_ccm=true) | ```1.0``` | :x: |
| **GigE Vision Stream Parameters** | | | |
| ```AnswerTimeout``` | Time the GigE Vision Device can take for command response. | ```100``` | :x: |
| ```CommandRetryCount``` | Command attempts before it is considered as failed | ```50``` | :x: |
Expand All @@ -54,6 +87,8 @@ ros2 run image_view image_view --ros-args --remap /image:=/image_raw
(*) Note: effective limitations are imposed by available bandwidth for the chosen configuration. If the bandwidth is
exceeded the camera will drop frames.

(**) Note: For stereo cameras the controls are applied to both sensors simultaneously.

* Required parameters:
* mac_address: The MAC address of the camera to connect to

Expand Down
3 changes: 3 additions & 0 deletions include/bottlenose_camera_driver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ namespace bottlenose_camera_driver {

bool set_interval(); ///< Set camera frame rate.
bool set_format(); ///< Set camera format.
bool update_runtime_parameters(); ///< Update runtime parameters from ROS2 parameters.
bool connect(); ///< Connect to camera.
void disconnect(); ///< Disconnect from camera.
bool queue_buffers(); ///< Queue buffers for GEV stack.
Expand All @@ -68,6 +69,8 @@ namespace bottlenose_camera_driver {
std::string m_mac_address; ///< Mac address of camera to connect to.
std::shared_ptr<sensor_msgs::msg::Image> m_image_msg; ///< Image message to publish.

/// Camera parameter cache
std::map<std::string, std::variant<int64_t, double, std::string>> m_camera_parameter_cache;
std::list<PvBuffer *> m_buffers; ///< List of buffers for GEV stack.

rclcpp::TimerBase::SharedPtr m_timer; ///< Timer for status callback.
Expand Down
35 changes: 34 additions & 1 deletion include/bottlenose_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,41 @@ const parameter_t bottlenose_parameters[] = {
{"format", rclcpp::ParameterValue("1920x1440")},
{"mac_address", rclcpp::ParameterValue("00:00:00:00:00:00")},
{"keep_partial", rclcpp::ParameterValue(false)},
/* Device paramters */
/* Device parameters */
{"fps", rclcpp::ParameterValue(20.0)},
{"exposure", rclcpp::ParameterValue(20.0)},
{"gain", rclcpp::ParameterValue(1.0)},
{"gamma", rclcpp::ParameterValue(2.2)},
{"dgainBlue", rclcpp::ParameterValue(1024)},
{"dgainGB", rclcpp::ParameterValue(1024)},
{"dgainGR", rclcpp::ParameterValue(1024)},
{"dgainRed", rclcpp::ParameterValue(1024)},
/* ISP parameters */
{"blackBlue", rclcpp::ParameterValue(0xFF)},
{"blackGB", rclcpp::ParameterValue(0xFF)},
{"blackGR", rclcpp::ParameterValue(0xFF)},
{"blackRed", rclcpp::ParameterValue(0xFF)},
{"brightness", rclcpp::ParameterValue(-13107)},
{"linearContrast", rclcpp::ParameterValue(136)},
{"blackGainBlue", rclcpp::ParameterValue(0.9375)},
{"blackGainGB", rclcpp::ParameterValue(0.9375)},
{"blackGainGR", rclcpp::ParameterValue(0.9375)},
{"blackGainRed", rclcpp::ParameterValue(0.9375)},
{"wbBlue", rclcpp::ParameterValue(1.0)},
{"wbGreen", rclcpp::ParameterValue(1.0)},
{"wbRed", rclcpp::ParameterValue(1.0)},
{"custom_ccm", rclcpp::ParameterValue(false)},
{"CCMValue00", rclcpp::ParameterValue(1.0)},
{"CCMValue01", rclcpp::ParameterValue(1.0)},
{"CCMValue02", rclcpp::ParameterValue(1.0)},
{"CCMValue10", rclcpp::ParameterValue(1.0)},
{"CCMValue11", rclcpp::ParameterValue(1.0)},
{"CCMValue12", rclcpp::ParameterValue(1.0)},
{"CCMValue20", rclcpp::ParameterValue(1.0)},
{"CCMValue21", rclcpp::ParameterValue(1.0)},
{"CCMValue22", rclcpp::ParameterValue(1.0)},

/* Lens and extrinsic parameters -> not used FIXME: figure out ROS2 calibration files */
{"Rectification", rclcpp::ParameterValue(false)},
{"Undistortion", rclcpp::ParameterValue(false)},
{"cx0", rclcpp::ParameterValue(1928.27)},
Expand Down
72 changes: 71 additions & 1 deletion src/bottlenose_camera_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -156,6 +156,66 @@ std::shared_ptr<sensor_msgs::msg::Image> CameraDriver::convertFrameToMessage(PvB
return nullptr;
}

bool CameraDriver::update_runtime_parameters() {
// All integer parameters
for(auto param : {"blackBlue",
"blackGB",
"blackGR",
"blackRed",
"brightness",
"linearContrast",
"dgainBlue",
"dgainGB",
"dgainGR",
"dgainRed"}) {
PvGenInteger *intval = static_cast<PvGenInteger *>( m_device->GetParameters()->Get(param));
int64_t val = get_parameter(param).as_int();
try {
auto value = m_camera_parameter_cache.at(param);
if(get<int64_t>(value) == val) {
continue;
}
} catch(std::out_of_range &e) { }

PvResult res = intval->SetValue(val);
if (res.IsFailure()) {
RCLCPP_WARN_STREAM(get_logger(), "Could not set parameter " << param << " to " << val);
return false;
}
// Cache
m_camera_parameter_cache[param] = val;
}

for(auto param : {"blackGainBlue",
"blackGainGB",
"blackGainGR",
"blackGainRed",
"exposure",
"gain",
"gamma",
"wbBlue",
"wbGreen",
"wbRed"}) {
PvGenFloat *floatVal = static_cast<PvGenFloat *>( m_device->GetParameters()->Get(param));
double val = get_parameter(param).as_double();
try {
auto value = m_camera_parameter_cache.at(param);
if(get<double>(value) == val) {
continue;
}
} catch(std::out_of_range &e) { }

PvResult res = floatVal->SetValue(val);
if (res.IsFailure()) {
RCLCPP_WARN_STREAM(get_logger(), "Could not set parameter " << param << " to " << val);
return false;
}
// Cache
m_camera_parameter_cache[param] = val;
}
return true;
}

bool CameraDriver::set_interval() {
PvGenFloat *interval = dynamic_cast<PvGenFloat *>( m_device->GetParameters()->Get("interval"));
if(interval == nullptr) {
Expand Down Expand Up @@ -351,6 +411,12 @@ void CameraDriver::management_thread() {
done = true;
return;
}
// Fail hard on first runtime parameter update issues
if(!update_runtime_parameters()) {
disconnect();
done = true;
return;
}

// Map the GenICam AcquisitionStart and AcquisitionStop commands
PvGenCommand *cmdStart = static_cast<PvGenCommand *>( m_device->GetParameters()->Get("AcquisitionStart") );
Expand Down Expand Up @@ -413,13 +479,17 @@ void CameraDriver::management_thread() {
}
res = m_stream->QueueBuffer( buffer );
if(res.IsFailure()) {
RCLCPP_ERROR_STREAM(get_logger(), "Could not queue GEV buffers");
RCLCPP_ERROR(get_logger(), "Could not queue GEV buffers");
break;
}
} else {
RCLCPP_WARN_STREAM(get_logger(), "Buffer failed with " << res.GetCodeString().GetAscii());
break;
}
// Even if parameter update fails, keep going to keep system alive after streaming is enabled
if(!update_runtime_parameters()) {
RCLCPP_WARN(get_logger(), "Runtime parameter update issue");
}
}
cmdStop->Execute();
m_device->StreamDisable();
Expand Down

0 comments on commit 4560419

Please sign in to comment.