From 9e9ff9d36b75ec236f049c87fe4ae1a6f370294c Mon Sep 17 00:00:00 2001 From: Nicola Battilani Date: Thu, 20 Apr 2017 12:17:22 +0200 Subject: [PATCH] Added camera_info publishing synced with image publishing. --- calib/basler_8mm/ost.txt | 31 ++++++++++++++++++++++++ calib/basler_8mm/ost.yaml | 20 +++++++++++++++ config/default.yaml | 2 +- include/pylon_camera/pylon_camera_node.h | 4 ++- src/pylon_camera/pylon_camera_node.cpp | 25 ++++++++++--------- 5 files changed, 69 insertions(+), 13 deletions(-) create mode 100644 calib/basler_8mm/ost.txt create mode 100644 calib/basler_8mm/ost.yaml diff --git a/calib/basler_8mm/ost.txt b/calib/basler_8mm/ost.txt new file mode 100644 index 00000000..78cd6748 --- /dev/null +++ b/calib/basler_8mm/ost.txt @@ -0,0 +1,31 @@ +# oST version 5.0 parameters + + +[image] + +width +1624 + +height +1234 + +[narrow_stereo] + +camera matrix +1918.412263 0.000000 831.707689 +0.000000 1912.356805 636.479080 +0.000000 0.000000 1.000000 + +distortion +-0.109684 0.374537 -0.003725 0.002179 0.000000 + +rectification +1.000000 0.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 0.000000 1.000000 + +projection +1922.024902 0.000000 833.571480 0.000000 +0.000000 1912.502563 632.790241 0.000000 +0.000000 0.000000 1.000000 0.000000 + diff --git a/calib/basler_8mm/ost.yaml b/calib/basler_8mm/ost.yaml new file mode 100644 index 00000000..3ecddb24 --- /dev/null +++ b/calib/basler_8mm/ost.yaml @@ -0,0 +1,20 @@ +image_width: 1624 +image_height: 1234 +camera_name: narrow_stereo +camera_matrix: + rows: 3 + cols: 3 + data: [1918.412263, 0.000000, 831.707689, 0.000000, 1912.356805, 636.479080, 0.000000, 0.000000, 1.000000] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.109684, 0.374537, -0.003725, 0.002179, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] +projection_matrix: + rows: 3 + cols: 4 + data: [1922.024902, 0.000000, 833.571480, 0.000000, 0.000000, 1912.502563, 632.790241, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/config/default.yaml b/config/default.yaml index cffe01c8..98588f79 100644 --- a/config/default.yaml +++ b/config/default.yaml @@ -10,7 +10,7 @@ device_user_id: "" # from the ROS-CameraInfoManager: # http://docs.ros.org/api/camera_info_manager/html/classcamera__info__manager_ # 1_1CameraInfoManager.html#details -camera_info_url: "" +camera_info_url: "package://pylon_camera/calib/basler_8mm/ost.yaml" # The encoding of the pixels -- channel meaning, ordering, size # taken from the list of strings in include/sensor_msgs/image_encodings.h diff --git a/include/pylon_camera/pylon_camera_node.h b/include/pylon_camera/pylon_camera_node.h index 6e5551a3..22eb8778 100644 --- a/include/pylon_camera/pylon_camera_node.h +++ b/include/pylon_camera/pylon_camera_node.h @@ -332,7 +332,9 @@ class PylonCameraNode image_transport::ImageTransport* it_; image_transport::CameraPublisher img_raw_pub_; - ros::Publisher* img_rect_pub_; + image_transport::ImageTransport* it_rect_; + image_transport::CameraPublisher img_rect_pub_; + image_geometry::PinholeCameraModel* pinhole_model_; GrabImagesAS grab_imgs_raw_as_; diff --git a/src/pylon_camera/pylon_camera_node.cpp b/src/pylon_camera/pylon_camera_node.cpp index eab30743..0be548ee 100644 --- a/src/pylon_camera/pylon_camera_node.cpp +++ b/src/pylon_camera/pylon_camera_node.cpp @@ -65,7 +65,8 @@ PylonCameraNode::PylonCameraNode() pylon_camera_(nullptr), it_(new image_transport::ImageTransport(nh_)), img_raw_pub_(it_->advertiseCamera("image_raw", 1)), - img_rect_pub_(nullptr), + it_rect_(new image_transport::ImageTransport(nh_)), + img_rect_pub_(it_rect_->advertiseCamera("image_rect", 1)), grab_imgs_raw_as_( nh_, "grab_images_raw", @@ -355,9 +356,6 @@ bool PylonCameraNode::startGrabbing() void PylonCameraNode::setupRectification() { - img_rect_pub_ = - new ros::Publisher(nh_.advertise("image_rect", 1)); - grab_imgs_rect_as_ = new GrabImagesAS(nh_, "grab_images_rect", @@ -396,23 +394,26 @@ void PylonCameraNode::spin() { return; } + + sensor_msgs::CameraInfoPtr cam_info (new sensor_msgs::CameraInfo(camera_info_manager_->getCameraInfo())); - if ( img_raw_pub_.getNumSubscribers() > 0 ) + + if ( img_raw_pub_.getNumSubscribers() + getNumSubscribersRect() > 0 ) { // get actual cam_info-object in every frame, because it might have // changed due to a 'set_camera_info'-service call - sensor_msgs::CameraInfoPtr cam_info( - new sensor_msgs::CameraInfo( - camera_info_manager_->getCameraInfo())); cam_info->header.stamp = img_raw_msg_.header.stamp; + } + if ( img_raw_pub_.getNumSubscribers() > 0 ) + { // Publish via image_transport img_raw_pub_.publish(img_raw_msg_, *cam_info); } if ( getNumSubscribersRect() > 0 ) { - img_rect_pub_->publish(*cv_bridge_img_rect_); + img_rect_pub_.publish(*(cv_bridge_img_rect_->toImageMsg()), *cam_info); } } } @@ -818,12 +819,12 @@ const std::string& PylonCameraNode::cameraFrame() const uint32_t PylonCameraNode::getNumSubscribersRect() const { - return camera_info_manager_->isCalibrated() ? img_rect_pub_->getNumSubscribers() : 0; + return camera_info_manager_->isCalibrated() ? img_rect_pub_.getNumSubscribers() : 0; } uint32_t PylonCameraNode::getNumSubscribers() const { - return img_raw_pub_.getNumSubscribers() + img_rect_pub_->getNumSubscribers(); + return img_raw_pub_.getNumSubscribers() + img_rect_pub_.getNumSubscribers(); } void PylonCameraNode::setupInitialCameraInfo(sensor_msgs::CameraInfo& cam_info_msg) @@ -1520,6 +1521,8 @@ PylonCameraNode::~PylonCameraNode() pylon_camera_ = NULL; delete it_; it_ = NULL; + delete it_rect_; + it_rect_ = NULL; } } // namespace pylon_camera