Skip to content

Commit

Permalink
Added custom CCM configuration option
Browse files Browse the repository at this point in the history
  • Loading branch information
@treideme
treideme committed Jun 8, 2023
1 parent 4560419 commit 65528fc
Show file tree
Hide file tree
Showing 2 changed files with 63 additions and 3 deletions.
12 changes: 10 additions & 2 deletions include/bottlenose_camera_driver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,16 @@ namespace bottlenose_camera_driver {
explicit CameraDriver(const rclcpp::NodeOptions&);
~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.
Expand All @@ -67,7 +73,8 @@ 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;
Expand All @@ -77,7 +84,8 @@ namespace bottlenose_camera_driver {
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
54 changes: 53 additions & 1 deletion 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,45 @@ 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",
Expand Down Expand Up @@ -184,6 +224,7 @@ bool CameraDriver::update_runtime_parameters() {
}
// Cache
m_camera_parameter_cache[param] = val;
RCLCPP_DEBUG_STREAM(get_logger(), "Set parameter " << param << " to " << val);
}

for(auto param : {"blackGainBlue",
Expand Down Expand Up @@ -212,6 +253,7 @@ bool CameraDriver::update_runtime_parameters() {
}
// Cache
m_camera_parameter_cache[param] = val;
RCLCPP_DEBUG_STREAM(get_logger(), "Set parameter " << param << " to " << val);
}
return true;
}
Expand All @@ -229,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 @@ -259,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 @@ -271,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 @@ -330,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 @@ -346,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 @@ -411,6 +458,11 @@ 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();
Expand Down

0 comments on commit 65528fc

Please sign in to comment.