Skip to content

Commit

Permalink
Merge pull request #1 from pixel-robotics/AMRNAV-4880
Browse files Browse the repository at this point in the history 
AMRNAV-4880 Publish using zero-copy
  • Loading branch information
@HovorunB
HovorunB authored Apr 17, 2024
2 parents 8790cda + 00a0e80 commit b38121a
Show file tree
Hide file tree
Showing 4 changed files with 91 additions and 48 deletions.
5 changes: 3 additions & 2 deletions pylon_ros2_camera_component/include/pylon_ros2_camera_node.hpp
View file
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
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
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
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

0 comments on commit b38121a

Please sign in to comment.