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

AMRNAV-4880 Publish using zero-copy #1

Merged
merged 3 commits into from
Apr 17, 2024
Merged

AMRNAV-4880 Publish using zero-copy #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
55733e2
AMRNAV-4880 Publish using zero-copy
HovorunB Mar 15, 2024
0449463
fix zerocopy + add sleep_on_startup parameter
maksymdidukh Mar 21, 2024
00a0e80
cleanup
HovorunB Apr 17, 2024
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
5 changes: 3 additions & 2 deletions pylon_ros2_camera_component/include/pylon_ros2_camera_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1628,7 +1628,7 @@ class PylonROS2CameraNode : public rclcpp::Node
PylonROS2CameraParameter pylon_camera_parameter_set_;
camera_info_manager::CameraInfoManager* camera_info_manager_;

sensor_msgs::msg::Image img_raw_msg_;
sensor_msgs::msg::Image::UniquePtr img_raw_msg_;

cv_bridge::CvImage* cv_bridge_img_rect_;

Expand Down Expand Up @@ -1656,6 +1656,7 @@ class PylonROS2CameraNode : public rclcpp::Node
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr blaze_depth_map_color_pub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr blaze_confidence_pub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr blaze_cam_info_pub_;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr ros_img_raw_pub_;

// services
rclcpp::Service<GetIntegerSrv>::SharedPtr get_max_num_buffer_srv_;
Expand Down Expand Up @@ -1803,7 +1804,7 @@ class PylonROS2CameraNode : public rclcpp::Node
std::vector<std::size_t> sampling_indices_;
std::array<float, 256> brightness_exp_lut_;

bool is_sleeping_;
bool is_sleeping_, use_intra_process_comms_;

// diagnostics
diagnostic_updater::Updater diagnostics_updater_;
Expand Down
5 changes: 5 additions & 0 deletions pylon_ros2_camera_component/include/pylon_ros2_camera_parameter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -305,6 +305,11 @@ class PylonROS2CameraParameter
* Only supported for GigE cameras. Default: true
*/
bool auto_flash_line_3_;

/**
* Flag that indicates if the camera should be sleeping on startup
*/
bool sleep_on_startup_;

/**
* camera grab timeout in ms
Expand Down
123 changes: 77 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 @@ -53,6 +53,7 @@ PylonROS2CameraNode::PylonROS2CameraNode(const rclcpp::NodeOptions& options)
, brightness_exp_lut_()
, is_sleeping_(false)
, diagnostics_updater_(this)
, use_intra_process_comms_(options.use_intra_process_comms())
{
// information logging severity mode
rcutils_ret_t __attribute__((unused)) res = rcutils_logging_set_logger_level(LOGGER.get_name(), RCUTILS_LOG_SEVERITY_DEBUG);
Expand All @@ -69,6 +70,12 @@ PylonROS2CameraNode::PylonROS2CameraNode(const rclcpp::NodeOptions& options)
if (!this->init())
return;


if (this->pylon_camera_parameter_set_.sleep_on_startup_)
{
RCLCPP_INFO(LOGGER, "Camera is set to sleep on startup");
}
this->is_sleeping_ = this->pylon_camera_parameter_set_.sleep_on_startup_;
// 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 @@ -165,7 +172,15 @@ void PylonROS2CameraNode::initPublishers()
this->component_status_pub_ = this->create_publisher<pylon_ros2_camera_interfaces::msg::ComponentStatus>(msg_name, 5);

msg_name = msg_prefix + "image_raw";
this->img_raw_pub_ = image_transport::create_camera_publisher(this, msg_name);

if (this->use_intra_process_comms_)
{
this->ros_img_raw_pub_ = this->create_publisher<sensor_msgs::msg::Image>(msg_name, 10);
}
else
{
this->img_raw_pub_ = image_transport::create_camera_publisher(this, msg_name);
}

// blaze related topics
msg_name = msg_prefix + "blaze_cloud"; this->blaze_cloud_topic_name_ = msg_name;
Expand Down Expand Up @@ -701,16 +716,18 @@ bool PylonROS2CameraNode::startGrabbing()
});
}
}
this->img_raw_msg_.release();
this->img_raw_msg_ = std::make_unique<sensor_msgs::msg::Image>();

this->img_raw_msg_.header.frame_id = this->pylon_camera_parameter_set_.cameraFrame();
this->img_raw_msg_->header.frame_id = this->pylon_camera_parameter_set_.cameraFrame();
// Encoding of pixels -- channel meaning, ordering, size
// taken from the list of strings in include/sensor_msgs/image_encodings.h
this->img_raw_msg_.encoding = this->pylon_camera_->currentROSEncoding();
this->img_raw_msg_.height = this->pylon_camera_->imageRows();
this->img_raw_msg_.width = this->pylon_camera_->imageCols();
this->img_raw_msg_->encoding = this->pylon_camera_->currentROSEncoding();
this->img_raw_msg_->height = this->pylon_camera_->imageRows();
this->img_raw_msg_->width = this->pylon_camera_->imageCols();
// step = full row length in bytes, img_size = (step * rows), imagePixelDepth
// already contains the number of channels
this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth();
this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth();

if (!this->camera_info_manager_->setCameraName(this->pylon_camera_->deviceUserID()))
{
Expand Down Expand Up @@ -749,7 +766,7 @@ bool PylonROS2CameraNode::startGrabbing()
this->setupRectification();
// set the correct tf frame_id
sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo();
cam_info.header.frame_id = this->img_raw_msg_.header.frame_id;
cam_info.header.frame_id = this->img_raw_msg_->header.frame_id;
this->camera_info_manager_->setCameraInfo(cam_info);
}
else
Expand Down Expand Up @@ -922,36 +939,50 @@ void PylonROS2CameraNode::spin()

if (!this->pylon_camera_->isBlaze())
{
if (!this->isSleeping() && (this->img_raw_pub_.getNumSubscribers() || this->getNumSubscribersRectImagePub()))
if (!this->isSleeping() && (this->use_intra_process_comms_ || this->img_raw_pub_.getNumSubscribers() || this->getNumSubscribersRectImagePub()))
{
if (this->img_raw_pub_.getNumSubscribers() || this->getNumSubscribersRectImagePub())
if (this->use_intra_process_comms_ || this->img_raw_pub_.getNumSubscribers() || this->getNumSubscribersRectImagePub())
{
if (!this->grabImage())
{
return;
}
}

if (this->img_raw_pub_.getNumSubscribers() > 0)
{
// get actual cam_info-object in every frame, because it might have
// changed due to a 'set_camera_info'-service call
sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo();
cam_info.header.stamp = this->img_raw_msg_.header.stamp;
// publish via image_transport
this->img_raw_pub_.publish(this->img_raw_msg_, cam_info);
}

// this->getNumSubscribersRectImagePub() involves that this->camera_info_manager_->isCalibrated() == true
if (this->getNumSubscribersRectImagePub() > 0)
{
this->cv_bridge_img_rect_->header.stamp = this->img_raw_msg_.header.stamp;
this->cv_bridge_img_rect_->header.stamp = this->img_raw_msg_->header.stamp;
assert(this->pinhole_model_->initialized());
cv_bridge::CvImagePtr cv_img_raw = cv_bridge::toCvCopy(this->img_raw_msg_, this->img_raw_msg_.encoding);
cv_bridge::CvImagePtr cv_img_raw = cv_bridge::toCvCopy(*this->img_raw_msg_, this->img_raw_msg_->encoding);
this->pinhole_model_->fromCameraInfo(this->camera_info_manager_->getCameraInfo());
this->pinhole_model_->rectifyImage(cv_img_raw->image, this->cv_bridge_img_rect_->image);
this->img_rect_pub_->publish(this->cv_bridge_img_rect_->toImageMsg());
}

if (this->use_intra_process_comms_ || this->img_raw_pub_.getNumSubscribers() > 0)
{
// get actual cam_info-object in every frame, because it might have
// changed due to a 'set_camera_info'-service call
sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo();
cam_info.header.stamp = this->img_raw_msg_->header.stamp;
if (this->use_intra_process_comms_)
{
// publish directly
this->ros_img_raw_pub_->publish(std::move(this->img_raw_msg_));
this->img_raw_msg_ = std::make_unique<sensor_msgs::msg::Image>();
this->img_raw_msg_->header.frame_id = this->pylon_camera_parameter_set_.cameraFrame();
this->img_raw_msg_->encoding = this->pylon_camera_->currentROSEncoding();
this->img_raw_msg_->height = this->pylon_camera_->imageRows();
this->img_raw_msg_->width = this->pylon_camera_->imageCols();
this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth();
}
else
{
// publish via image_transport
this->img_raw_pub_.publish(*this->img_raw_msg_, cam_info);
}
}
// this->getNumSubscribersRectImagePub() involves that this->camera_info_manager_->isCalibrated() == true
}
}
else
Expand Down Expand Up @@ -1016,11 +1047,11 @@ bool PylonROS2CameraNode::grabImage()
{
// Store current time before the image is transmitted for a more accurate grab time estimation
auto grab_time = rclcpp::Node::now();
if (!this->pylon_camera_->grab(this->img_raw_msg_.data))
if (!this->pylon_camera_->grab(this->img_raw_msg_->data))
{
return false;
}
this->img_raw_msg_.header.stamp = grab_time;
this->img_raw_msg_->header.stamp = grab_time;
}
else
{
Expand Down Expand Up @@ -1430,11 +1461,11 @@ bool PylonROS2CameraNode::setROI(const sensor_msgs::msg::RegionOfInterest target
sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo();
cam_info.roi = this->pylon_camera_->currentROI();
this->camera_info_manager_->setCameraInfo(cam_info);
this->img_raw_msg_.width = this->pylon_camera_->imageCols();
this->img_raw_msg_.height = this->pylon_camera_->imageRows();
this->img_raw_msg_->width = this->pylon_camera_->imageCols();
this->img_raw_msg_->height = this->pylon_camera_->imageRows();
// step = full row length in bytes, img_size = (step * rows), imagePixelDepth
// already contains the number of channels
this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth();
this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth();
return false;
}
r.sleep();
Expand All @@ -1444,11 +1475,11 @@ bool PylonROS2CameraNode::setROI(const sensor_msgs::msg::RegionOfInterest target
sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo();
cam_info.roi = this->pylon_camera_->currentROI();
this->camera_info_manager_->setCameraInfo(cam_info);
this->img_raw_msg_.height = this->pylon_camera_->imageRows();
this->img_raw_msg_.width = this->pylon_camera_->imageCols();
this->img_raw_msg_->height = this->pylon_camera_->imageRows();
this->img_raw_msg_->width = this->pylon_camera_->imageCols();
// step = full row length in bytes, img_size = (step * rows), imagePixelDepth
// already contains the number of channels
this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth();
this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth();
this->setupSamplingIndices(this->sampling_indices_,
this->pylon_camera_->imageRows(),
this->pylon_camera_->imageCols(),
Expand Down Expand Up @@ -1480,10 +1511,10 @@ bool PylonROS2CameraNode::setBinningX(const size_t& target_binning_x,
sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo();
cam_info.binning_x = this->pylon_camera_->currentBinningX();
this->camera_info_manager_->setCameraInfo(cam_info);
this->img_raw_msg_.width = this->pylon_camera_->imageCols();
this->img_raw_msg_->width = this->pylon_camera_->imageCols();
// step = full row length in bytes, img_size = (step * rows), imagePixelDepth
// already contains the number of channels
this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth();
this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth();
return false;
}

Expand All @@ -1494,10 +1525,10 @@ bool PylonROS2CameraNode::setBinningX(const size_t& target_binning_x,
sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo();
cam_info.binning_x = this->pylon_camera_->currentBinningX();
this->camera_info_manager_->setCameraInfo(cam_info);
this->img_raw_msg_.width = this->pylon_camera_->imageCols();
this->img_raw_msg_->width = this->pylon_camera_->imageCols();
// step = full row length in bytes, img_size = (step * rows), imagePixelDepth
// already contains the number of channels
this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth();
this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth();
this->setupSamplingIndices(this->sampling_indices_,
this->pylon_camera_->imageRows(),
this->pylon_camera_->imageCols(),
Expand Down Expand Up @@ -1528,10 +1559,10 @@ bool PylonROS2CameraNode::setBinningY(const size_t& target_binning_y,
sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo();
cam_info.binning_y = this->pylon_camera_->currentBinningY();
this->camera_info_manager_->setCameraInfo(cam_info);
this->img_raw_msg_.height = this->pylon_camera_->imageRows();
this->img_raw_msg_->height = this->pylon_camera_->imageRows();
// step = full row length in bytes, img_size = (step * rows), imagePixelDepth
// already contains the number of channels
this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth();
this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth();
return false;
}
r.sleep();
Expand All @@ -1541,10 +1572,10 @@ bool PylonROS2CameraNode::setBinningY(const size_t& target_binning_y,
sensor_msgs::msg::CameraInfo cam_info = this->camera_info_manager_->getCameraInfo();
cam_info.binning_y = this->pylon_camera_->currentBinningY();
this->camera_info_manager_->setCameraInfo(cam_info);
this->img_raw_msg_.height = this->pylon_camera_->imageRows();
this->img_raw_msg_->height = this->pylon_camera_->imageRows();
// step = full row length in bytes, img_size = (step * rows), imagePixelDepth
// already contains the number of channels
this->img_raw_msg_.step = this->img_raw_msg_.width * this->pylon_camera_->imagePixelDepth();
this->img_raw_msg_->step = this->img_raw_msg_->width * this->pylon_camera_->imagePixelDepth();
this->setupSamplingIndices(this->sampling_indices_,
this->pylon_camera_->imageRows(),
this->pylon_camera_->imageCols(),
Expand Down Expand Up @@ -4579,24 +4610,24 @@ void PylonROS2CameraNode::genSamplingIndicesRec(std::vector<std::size_t>& indice
float PylonROS2CameraNode::calcCurrentBrightness()
{
std::lock_guard<std::recursive_mutex> lock(this->grab_mutex_);
if (this->img_raw_msg_.data.empty())
if (this->img_raw_msg_->data.empty())
{
return 0.0;
}

float sum = 0.0;
if (sensor_msgs::image_encodings::isMono(this->img_raw_msg_.encoding))
if (sensor_msgs::image_encodings::isMono(this->img_raw_msg_->encoding))
{
// Check if image is expected size so indices don't fall out of bounds
if (this->img_raw_msg_.data.size() != this->img_raw_msg_.height * this->img_raw_msg_.width)
if (this->img_raw_msg_->data.size() != this->img_raw_msg_->height * this->img_raw_msg_->width)
{
return 0.0;
}

// The mean brightness is calculated using a subset of all pixels
for (const std::size_t& idx : this->sampling_indices_)
{
sum += this->img_raw_msg_.data.at(idx);
sum += this->img_raw_msg_->data.at(idx);
}
if (sum > 0.0)
{
Expand All @@ -4606,10 +4637,10 @@ float PylonROS2CameraNode::calcCurrentBrightness()
else
{
// The mean brightness is calculated using all pixels and all channels
sum = std::accumulate(this->img_raw_msg_.data.begin(), this->img_raw_msg_.data.end(), 0);
sum = std::accumulate(this->img_raw_msg_->data.begin(), this->img_raw_msg_->data.end(), 0);
if (sum > 0.0)
{
sum /= static_cast<float>(this->img_raw_msg_.data.size());
sum /= static_cast<float>(this->img_raw_msg_->data.size());
}
}

Expand Down Expand Up @@ -4659,8 +4690,8 @@ void PylonROS2CameraNode::setupRectification()
{
this->cv_bridge_img_rect_ = new cv_bridge::CvImage();
}
this->cv_bridge_img_rect_->header = img_raw_msg_.header;
this->cv_bridge_img_rect_->encoding = img_raw_msg_.encoding;
this->cv_bridge_img_rect_->header = img_raw_msg_->header;
this->cv_bridge_img_rect_->encoding = img_raw_msg_->encoding;
}

std::shared_ptr<GrabImagesAction::Result> PylonROS2CameraNode::grabRawImages(const std::shared_ptr<GrabImagesGoalHandle> goal_handle)
Expand Down
6 changes: 6 additions & 0 deletions pylon_ros2_camera_component/src/pylon_ros2_camera_parameter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -405,6 +405,12 @@ void PylonROS2CameraParameter::readFromRosParameterServer(rclcpp::Node& nh)
}
nh.get_parameter("auto_flash_line_3", this->auto_flash_line_3_);

if (!nh.has_parameter("sleep_on_startup"))
{
nh.declare_parameter<bool>("sleep_on_startup", false);
}
nh.get_parameter("sleep_on_startup", this->sleep_on_startup_);

RCLCPP_WARN(LOGGER, "Autoflash: %i, line2: %i, line3: %i", this->auto_flash_, this->auto_flash_line_2_, this->auto_flash_line_3_);

this->validateParameterSet(nh);
Expand Down