Skip to content

Various performance optimizations #262

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

Open
wants to merge 5 commits into
base: humble
Choose a base branch
from
Open

Various performance optimizations #262

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
bc08ae9
Moved initial logging out of loop.
Jan 29, 2025
b217913
Just fail on camera removed.
Jan 29, 2025
3c86ff1
Ignore encoding changes on runtime.
Jan 29, 2025
c3656f2
Disabled status publishers by default.
Jan 29, 2025
226281b
Optimized grab postprocessing.
Jan 29, 2025
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
45 changes: 23 additions & 22 deletions pylon_ros2_camera_component/include/internal/impl/pylon_ros2_camera_base.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -428,14 +428,11 @@ bool PylonROS2CameraImpl<CameraTrait>::grab(std::vector<uint8_t>& image, rclcpp:
}
const uint8_t *pImageBuffer = reinterpret_cast<uint8_t*>(ptr_grab_result->GetBuffer());

// ------------------------------------------------------------------------
// Bit shifting
// ------------------------------------------------------------------------
// In case of 12 bits we need to shift the image bits 4 positions to the left
const std::string ros_enc = this->currentROSEncoding();
const std::string gen_api_encoding(cam_->PixelFormat.ToString().c_str());
if (encodingconversions::is_12_bit_ros_enc(ros_enc) && (gen_api_encoding == "BayerRG12" || gen_api_encoding == "BayerBG12" || gen_api_encoding == "BayerGB12" || gen_api_encoding == "BayerGR12" || gen_api_encoding == "Mono12"))
{
if (this->bit_shift_active_){
// ------------------------------------------------------------------------
// Bit shifting
// ------------------------------------------------------------------------
// In case of 12 bits we need to shift the image bits 4 positions to the left
std::vector<uint16_t> shift_array(img_size_byte_ / 2); // Dynamically allocated to avoid heap size error
const uint16_t *convert_bits = reinterpret_cast<uint16_t*>(ptr_grab_result->GetBuffer());
for (size_t i = 0; i < img_size_byte_ / 2; i++)
Expand All @@ -449,33 +446,33 @@ bool PylonROS2CameraImpl<CameraTrait>::grab(std::vector<uint8_t>& image, rclcpp:
image.assign(pImageBuffer, pImageBuffer + img_size_byte_);
}

bool use_chunk_timestamp = false;
if (this->getChunkModeActive() == 1)
if (this->chunk_mode_active_)
{
bool use_chunk_timestamp = false;
const std::string success = this->setChunkSelector(29); // = ChunkSelector_Timestamp
if (success.find("done") != std::string::npos && this->getChunkEnable() == 1)
{
use_chunk_timestamp = true;
}
}

if (use_chunk_timestamp)
{
try
if (use_chunk_timestamp)
{
if (!ptr_grab_result->ChunkTimestamp.IsReadable())
try
{
RCLCPP_WARN_STREAM(LOGGER_BASE, "Error while trying to get the chunk timestamp. The connected camera may not support this feature");
if (!ptr_grab_result->ChunkTimestamp.IsReadable())
{
RCLCPP_WARN_STREAM(LOGGER_BASE, "Error while trying to get the chunk timestamp. The connected camera may not support this feature");
}
else
{
stamp = rclcpp::Time(static_cast<uint64_t>(ptr_grab_result->ChunkTimestamp.GetValue()));
}
}
else
catch (const GenICam::GenericException &e)
{
stamp = rclcpp::Time(static_cast<uint64_t>(ptr_grab_result->ChunkTimestamp.GetValue()));
RCLCPP_WARN_STREAM(LOGGER_BASE, "An exception while getting the chunk timestamp occurred: " << e.GetDescription());
}
}
catch (const GenICam::GenericException &e)
{
RCLCPP_WARN_STREAM(LOGGER_BASE, "An exception while getting the chunk timestamp occurred: " << e.GetDescription());
}
}

if (!is_ready_)
Expand Down Expand Up @@ -579,6 +576,10 @@ bool PylonROS2CameraImpl<CameraTrait>::grab(Pylon::CBaslerUniversalGrabResultPtr
if (cam_->IsCameraDeviceRemoved())
{
RCLCPP_ERROR(LOGGER_BASE, "Lost connection to the camera . . .");
cam_->StopGrabbing();
RCLCPP_ERROR(LOGGER_BASE, "Reconnect the camera and restart the node!");
rclcpp::shutdown();
exit(1);
}
else
{
Expand Down
6 changes: 6 additions & 0 deletions pylon_ros2_camera_component/include/pylon_ros2_camera.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1261,6 +1261,12 @@ class PylonROS2Camera

virtual ~PylonROS2Camera();

/**
* Internal attributes
*/
bool chunk_mode_active_{false};
bool bit_shift_active_{false};

protected:
/**
* Protected default constructor.
Expand Down
70 changes: 24 additions & 46 deletions pylon_ros2_camera_component/src/pylon_ros2_camera_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@

//#include <functional>

#include "encoding_conversions.hpp"
#include "pylon_ros2_camera_node.hpp"


Expand Down Expand Up @@ -73,6 +74,15 @@ PylonROS2CameraNode::PylonROS2CameraNode(const rclcpp::NodeOptions& options)
if (!this->init())
return;

if (this->camera_info_manager_->isCalibrated())
{
RCLCPP_INFO(LOGGER, "Camera is calibrated");
}
else
{
RCLCPP_INFO(LOGGER, "Camera not calibrated");
}

// starting spinning thread
RCLCPP_INFO_STREAM(LOGGER, "Start image grabbing if node connects to topic with a spinning rate of: " << this->frameRate() << " Hz");
timer_ = this->create_wall_timer(
Expand Down Expand Up @@ -130,6 +140,20 @@ bool PylonROS2CameraNode::init()
return false;
}

// set grabbed image conversion parameters
if (this->pylon_camera_->getChunkModeActive() == 1)
{
RCLCPP_INFO(LOGGER, "Activated chunk mode");
this->pylon_camera_->chunk_mode_active_ = true;
}
const std::string ros_enc = this->pylon_camera_->currentROSEncoding();
const std::string gen_api_encoding(this->pylon_camera_->currentBaslerEncoding());
if (encodingconversions::is_12_bit_ros_enc(ros_enc) && (gen_api_encoding == "BayerRG12" || gen_api_encoding == "BayerBG12" || gen_api_encoding == "BayerGB12" || gen_api_encoding == "BayerGR12" || gen_api_encoding == "Mono12"))
{
RCLCPP_INFO(LOGGER, "Activated bit shifting");
this->pylon_camera_->bit_shift_active_ = true;
}

// starting the grabbing procedure with the desired image-settings
if (!this->startGrabbing())
{
Expand Down Expand Up @@ -888,44 +912,6 @@ bool PylonROS2CameraNode::startGrabbing()

void PylonROS2CameraNode::spin()
{
if (this->camera_info_manager_->isCalibrated())
{
RCLCPP_INFO_ONCE(LOGGER, "Camera is calibrated");
}
else
{
RCLCPP_INFO_ONCE(LOGGER, "Camera not calibrated");
}

if (this->pylon_camera_->isCamRemoved())
{
RCLCPP_ERROR(LOGGER, "Pylon camera has been removed, trying to reset");

this->cm_status_.status_id = pylon_ros2_camera_interfaces::msg::ComponentStatus::ERROR;
this->cm_status_.status_msg = "Pylon camera has been removed, trying to reset";

if (this->pylon_camera_parameter_set_.enable_status_publisher_)
{
this->component_status_pub_->publish(this->cm_status_);
}

if (this->pylon_camera_ != nullptr)
{
this->pylon_camera_.reset();
}

// Possible issue here: ROS2 does not allow to shutdown services
// Services are shutdown in the ROS 1 pylon version at this level
this->set_user_output_srvs_.clear();

rclcpp::Rate r(0.5);
r.sleep();

this->init();

return;
}

if (!this->pylon_camera_->isBlaze())
{
const bool any_subscriber = (this->img_raw_pub_.getNumSubscribers() != 0 || this->getNumSubscribersRectImagePub() != 0);
Expand Down Expand Up @@ -1014,14 +1000,6 @@ void PylonROS2CameraNode::spin()
}
}

// Check if the image encoding changed , then save the new image encoding and restart the image grabbing to fix the ros sensor message type issue.
if (this->pylon_camera_parameter_set_.imageEncoding() != this->pylon_camera_->currentROSEncoding())
{
this->pylon_camera_parameter_set_.setimageEncodingParam(*this, this->pylon_camera_->currentROSEncoding());
this->grabbingStopping();
this->grabbingStarting();
}

if (this->pylon_camera_parameter_set_.enable_status_publisher_)
{
this->component_status_pub_->publish(this->cm_status_);
Expand Down
4 changes: 2 additions & 2 deletions pylon_ros2_camera_wrapper/launch/pylon_ros2_camera.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,13 +108,13 @@ def generate_launch_description():

declare_enable_status_publisher_cmd = DeclareLaunchArgument(
'enable_status_publisher',
default_value='true',
default_value='false',
description='Enable/Disable the status publishing.'
)

declare_enable_current_params_publisher_cmd = DeclareLaunchArgument(
'enable_current_params_publisher',
default_value='true',
default_value='false',
description='Enable/Disable the current parameter publishing.'
)

Expand Down