Skip to content

Commit

Permalink
SE-2309 (#2)
Browse files Browse the repository at this point in the history 
* Added most ISP and sensor parameters to ROS interface
* Added custom CCM configuration option
  • Loading branch information
@treideme
treideme committed Jun 8, 2023
1 parent 30d1a8d commit 1a5aba3
Show file tree
Hide file tree
Showing 5 changed files with 296 additions and 9 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
16 changes: 14 additions & 2 deletions include/bottlenose_camera_driver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,12 +46,20 @@ namespace bottlenose_camera_driver {
class CameraDriver : public rclcpp::Node {
public:
explicit CameraDriver(const rclcpp::NodeOptions&);
bool is_streaming();
~CameraDriver();
private:
/**
* Convert the frame to a ROS2 message.
* @param buffer Buffer received on GEV interface
* @return ROS2 message
*/
std::shared_ptr<sensor_msgs::msg::Image> convertFrameToMessage(PvBuffer *buffer);

bool set_interval(); ///< Set camera frame rate.
bool set_format(); ///< Set camera format.
bool apply_ccm(); ///< Apply color correction matrix.
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 @@ -66,15 +74,19 @@ namespace bottlenose_camera_driver {
PvStreamGEV *m_stream; ///< GEV stream handle.

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.
/// Image message to publish.
std::shared_ptr<sensor_msgs::msg::Image> m_image_msg;

/// 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.
std::thread m_management_thread; ///< Management thread handle.

std::shared_ptr<camera_info_manager::CameraInfoManager> m_cinfo_manager;
image_transport::CameraPublisher m_camera_pub; ///< Camera publisher.
/// Camera publisher.
image_transport::CameraPublisher m_camera_pub;
};
} // namespace bottlenose_camera_driver
#endif //__BOTTLENOSE_CAMERA_DRIVER_HPP__
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
130 changes: 128 additions & 2 deletions src/bottlenose_camera_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@

#define BUFFER_COUNT ( 16 )
#define BUFFER_SIZE ( 3840 * 2160 * 3 ) // 4K UHD, YUV422 + ~1 image plane to account for chunk data
#define WAIT_PROPAGATE() usleep(250 * 1000)


namespace bottlenose_camera_driver
Expand Down Expand Up @@ -156,6 +157,107 @@ std::shared_ptr<sensor_msgs::msg::Image> CameraDriver::convertFrameToMessage(PvB
return nullptr;
}

bool CameraDriver::apply_ccm() {
if(get_parameter("custom_ccm").as_bool()) {
// Find CUSTOM CCM profile in sensor
PvGenEnum * custom = static_cast<PvGenEnum *>(m_device->GetParameters()->Get("CCMColorProfile"));
PvResult res = custom->SetValue("Custom");
if(res.IsFailure()) {
RCLCPP_ERROR(get_logger(), "Could not select custom CCM profile");
return false;
}
WAIT_PROPAGATE();
// Set custom CCM values
for(auto param : {"CCMValue00",
"CCMValue01",
"CCMValue02",
"CCMValue10",
"CCMValue11",
"CCMValue12",
"CCMValue20",
"CCMValue21",
"CCMValue22"}) {
PvGenFloat * ccm = static_cast<PvGenFloat *>(m_device->GetParameters()->Get(param));
res = ccm->SetValue(get_parameter(param).as_double());
if(res.IsFailure()) {
RCLCPP_ERROR(get_logger(), "Could not set %s", param);
return false;
}
}
WAIT_PROPAGATE();
PvGenCommand * apply = static_cast<PvGenCommand *>(m_device->GetParameters()->Get("SetCustomProfile"));
res = apply->Execute();
if(!res) {
RCLCPP_ERROR(get_logger(), "Could not apply custom CCM profile");
return false;
}
RCLCPP_DEBUG_STREAM(get_logger(), "Applied custom CCM profile");
}
return true;
}

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;
RCLCPP_DEBUG_STREAM(get_logger(), "Set parameter " << param << " to " << 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;
RCLCPP_DEBUG_STREAM(get_logger(), "Set parameter " << param << " to " << val);
}
return true;
}

bool CameraDriver::set_interval() {
PvGenFloat *interval = dynamic_cast<PvGenFloat *>( m_device->GetParameters()->Get("interval"));
if(interval == nullptr) {
Expand All @@ -169,6 +271,8 @@ bool CameraDriver::set_interval() {
get_parameter("fps").as_double());
return false;
}
RCLCPP_DEBUG_STREAM(get_logger(), "Configured interval to " << 1000.0 / get_parameter("fps").as_double()
<< " ms for " << get_parameter("fps").as_double() << " fps");
return true;
}

Expand Down Expand Up @@ -199,7 +303,7 @@ bool CameraDriver::set_format() {
return false;
}
// Wait for parameter pass-through
usleep(200*2000);
WAIT_PROPAGATE();
// Confirm the format
int64_t width_in;
res = widthParam->GetValue(width_in);
Expand All @@ -211,6 +315,7 @@ bool CameraDriver::set_format() {
RCLCPP_ERROR_STREAM(get_logger(), "Could not configure format to " << format << " actual format is " << width_in << " x " << height);
return false;
}
RCLCPP_DEBUG_STREAM(get_logger(), "Configured format to " << format);

return true;
}
Expand Down Expand Up @@ -270,6 +375,7 @@ bool CameraDriver::connect() {
disconnect();
return false;
}
RCLCPP_DEBUG_STREAM(get_logger(), "Set " << param << " to " << get_parameter(param).as_int());
}

// Stream tweaks, see https://supportcenter.pleora.com/s/article/Recommended-eBUS-Player-Settings-for-Wireless-Connection
Expand All @@ -286,6 +392,7 @@ bool CameraDriver::connect() {
disconnect();
return false;
}
RCLCPP_DEBUG_STREAM(get_logger(), "Set " << param << " to " << get_parameter(param).as_int());
}

return true;
Expand Down Expand Up @@ -351,6 +458,17 @@ void CameraDriver::management_thread() {
done = true;
return;
}
if(!apply_ccm()) {
disconnect();
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 +531,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 All @@ -428,6 +550,10 @@ void CameraDriver::management_thread() {
done = true;
}

bool CameraDriver::is_streaming() {
return !done && m_device != nullptr && m_stream != nullptr && m_stream->IsOpen();
}

} // namespace bottlenose_camera_driver

#include "rclcpp_components/register_node_macro.hpp"
Expand Down
Loading

0 comments on commit 1a5aba3

Please sign in to comment.