Skip to content

Add params for camera info paths and frame #4

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
Tacha-S wants to merge 8 commits into
base: master
Choose a base branch
from
Open

Add params for camera info paths and frame #4

Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
cf81b26
Load camera info
Tacha-S Aug 29, 2022
dd94eea
Add frame params
Tacha-S Aug 29, 2022
c2ce8bf
Create camera_info_manager for each left and right
Oct 28, 2022
1921ea7
Merge pull request #3 from sbgisen/bugfix/camerainfo_manager
Tacha-S Oct 31, 2022
557c0ab
Process when camera_info is subscribed
Aug 18, 2023
c1476ba
Apply format
Aug 18, 2023
b23604d
Removed redundant if statements
Aug 18, 2023
981f438
Merge pull request #5 from sbgisen/bugfix/publish_camera_info
gakutasu Oct 3, 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
221 changes: 106 additions & 115 deletions src/side_by_side_stereo_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,11 +41,12 @@
#include <image_transport/image_transport.h>
#include <camera_info_manager/camera_info_manager.h>


// If non-zero, outputWidth and outputHeight set the size of the output images.
// If zero, the outputWidth is set to 1/2 the width of the input image, and
// outputHeight is the same as the height of the input image.
int outputWidth, outputHeight;
std::string leftCameraInfoURL, rightCameraInfoURL;
std::string leftFrame, rightFrame;

// Input image subscriber.
ros::Subscriber imageSub;
Expand All @@ -57,127 +58,117 @@ image_transport::Publisher leftImagePublisher, rightImagePublisher;
ros::Publisher leftCameraInfoPublisher, rightCameraInfoPublisher;
sensor_msgs::CameraInfo leftCameraInfoMsg, rightCameraInfoMsg;

// Camera info managers.
camera_info_manager::CameraInfoManager *left_cinfo_;
camera_info_manager::CameraInfoManager *right_cinfo_;


// Image capture callback.
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
// Get double camera image.
cv_bridge::CvImagePtr cvImg = cv_bridge::toCvCopy(msg, msg->encoding);
cv::Mat image = cvImg->image;

// If there are any subscribers to either output topic then publish images
// on them.
if (leftImagePublisher.getNumSubscribers() > 0 ||
rightImagePublisher.getNumSubscribers() > 0)
// Get double camera image.
cv_bridge::CvImagePtr cvImg = cv_bridge::toCvCopy(msg, msg->encoding);
cv::Mat image = cvImg->image;

// If there are any subscribers to either output topic then publish images
// on them.
if (leftImagePublisher.getNumSubscribers() > 0 || rightImagePublisher.getNumSubscribers() > 0 ||
leftCameraInfoPublisher.getNumSubscribers() > 0 || rightCameraInfoPublisher.getNumSubscribers() > 0)

{
// Define the relevant rectangles to crop.
cv::Rect leftROI, rightROI;
leftROI.y = rightROI.y = 0;
leftROI.width = rightROI.width = image.cols / 2;
leftROI.height = rightROI.height = image.rows;
leftROI.x = 0;
rightROI.x = image.cols / 2;

// Crop images.
cv::Mat leftImage = cv::Mat(image, leftROI);
cv::Mat rightImage = cv::Mat(image, rightROI);

// Apply scaling, if specified.
bool use_scaled;
cv::Mat leftScaled, rightScaled;
if (use_scaled = (outputWidth > 0 && outputHeight > 0))
{
// Define the relevant rectangles to crop.
cv::Rect leftROI, rightROI;
leftROI.y = rightROI.y = 0;
leftROI.width = rightROI.width = image.cols / 2;
leftROI.height = rightROI.height = image.rows;
leftROI.x = 0;
rightROI.x = image.cols / 2;

// Crop images.
cv::Mat leftImage = cv::Mat(image, leftROI);
cv::Mat rightImage = cv::Mat(image, rightROI);

// Apply scaling, if specified.
bool use_scaled;
cv::Mat leftScaled, rightScaled;
if (use_scaled = (outputWidth > 0 && outputHeight > 0))
{
cv::Size sz = cv::Size(outputWidth, outputHeight);
cv::resize(leftImage, leftScaled, sz);
cv::resize(rightImage, rightScaled, sz);
}

// Publish.
cv_bridge::CvImage cvImage;
sensor_msgs::ImagePtr img;
cvImage.encoding = msg->encoding;
cvImage.header.frame_id = msg->header.frame_id;
cvImage.header.stamp = msg->header.stamp;
if (leftImagePublisher.getNumSubscribers() > 0
|| leftCameraInfoPublisher.getNumSubscribers() > 0)
{
cvImage.image = use_scaled ? leftScaled : leftImage;
img = cvImage.toImageMsg();
leftImagePublisher.publish(img);
leftCameraInfoMsg.header.stamp = img->header.stamp;
leftCameraInfoMsg.header.frame_id = img->header.frame_id;
leftCameraInfoPublisher.publish(leftCameraInfoMsg);
}
if (rightImagePublisher.getNumSubscribers() > 0
|| rightCameraInfoPublisher.getNumSubscribers() > 0)
{
cvImage.image = use_scaled ? rightScaled : rightImage;
img = cvImage.toImageMsg();
rightImagePublisher.publish(img);
rightCameraInfoMsg.header.stamp = img->header.stamp;
rightCameraInfoMsg.header.frame_id = img->header.frame_id;
rightCameraInfoPublisher.publish(rightCameraInfoMsg);
}
cv::Size sz = cv::Size(outputWidth, outputHeight);
cv::resize(leftImage, leftScaled, sz);
cv::resize(rightImage, rightScaled, sz);
}
}

// Publish images
cv_bridge::CvImage cvImage;
sensor_msgs::ImagePtr img;
cvImage.encoding = msg->encoding;
cvImage.header.stamp = msg->header.stamp;
// left image
if (leftFrame.empty())
leftFrame = msg->header.frame_id;
cvImage.image = use_scaled ? leftScaled : leftImage;
cvImage.header.frame_id = leftFrame;
img = cvImage.toImageMsg();
leftImagePublisher.publish(img);
leftCameraInfoMsg.header.stamp = img->header.stamp;
leftCameraInfoMsg.header.frame_id = leftFrame;
leftCameraInfoPublisher.publish(leftCameraInfoMsg);
// right image
if (rightFrame.empty())
rightFrame = msg->header.frame_id;
cvImage.image = use_scaled ? rightScaled : rightImage;
cvImage.header.frame_id = rightFrame;
img = cvImage.toImageMsg();
rightImagePublisher.publish(img);
rightCameraInfoMsg.header.stamp = img->header.stamp;
rightCameraInfoMsg.header.frame_id = rightFrame;
rightCameraInfoPublisher.publish(rightCameraInfoMsg);
}
}

int main(int argc, char** argv)
{
ros::init(argc, argv, "sxs_stereo");
ros::NodeHandle nh("sxs_stereo");
ros::NodeHandle nh_left(nh, "left");
ros::NodeHandle nh_right(nh, "right");

image_transport::ImageTransport it(nh);

// Allocate and initialize camera info managers.
left_cinfo_ =
new camera_info_manager::CameraInfoManager(nh_left);
right_cinfo_ =
new camera_info_manager::CameraInfoManager(nh_right);
left_cinfo_->loadCameraInfo("");
right_cinfo_->loadCameraInfo("");

// Pre-fill camera_info messages.
leftCameraInfoMsg = left_cinfo_->getCameraInfo();
rightCameraInfoMsg = right_cinfo_->getCameraInfo();

// Load node settings.
std::string inputImageTopic, leftOutputImageTopic, rightOutputImageTopic,
leftCameraInfoTopic, rightCameraInfoTopic;
nh.param("input_image_topic", inputImageTopic,
std::string("input_image_topic_not_set"));
ROS_INFO("input topic to stereo splitter=%s\n", inputImageTopic.c_str());
nh.param("left_output_image_topic", leftOutputImageTopic,
std::string("/sxs_stereo/left/image_raw"));
nh.param("right_output_image_topic", rightOutputImageTopic,
std::string("/sxs_stereo/right/image_raw"));
nh.param("left_camera_info_topic", leftCameraInfoTopic,
std::string("/sxs_stereo/left/camera_info"));
nh.param("right_camera_info_topic", rightCameraInfoTopic,
std::string("/sxs_stereo/right/camera_info"));
nh.param("output_width", outputWidth, 0); // 0 -> use 1/2 input width.
nh.param("output_height", outputHeight, 0); // 0 -> use input height.


// Register publishers and subscriber.
imageSub = nh.subscribe(inputImageTopic.c_str(), 2, &imageCallback);
leftImagePublisher = it.advertise(leftOutputImageTopic.c_str(), 1);
rightImagePublisher = it.advertise(rightOutputImageTopic.c_str(), 1);
leftCameraInfoPublisher = nh_left.advertise<sensor_msgs::CameraInfo>
(leftCameraInfoTopic.c_str(), 1, true);
rightCameraInfoPublisher = nh_right.advertise<sensor_msgs::CameraInfo>
(rightCameraInfoTopic.c_str(), 1, true);

// Run node until cancelled.
ros::spin();

// De-allocate CameraInfoManagers.
left_cinfo_ -> ~CameraInfoManager();
right_cinfo_ -> ~CameraInfoManager();
ros::init(argc, argv, "sxs_stereo");
ros::NodeHandle nh("~");
image_transport::ImageTransport it(nh);

// load the camera info
nh.param("left_camera_info_url", leftCameraInfoURL, std::string(""));
ROS_INFO("left_camera_info_url=%s\n", leftCameraInfoURL.c_str());
nh.param("left_frame", leftFrame, std::string(""));
nh.param("right_camera_info_url", rightCameraInfoURL, std::string(""));
ROS_INFO("right_camera_info_url=%s\n", rightCameraInfoURL.c_str());
nh.param("right_frame", rightFrame, std::string(""));

// Load node settings.
std::string inputImageTopic, leftOutputImageTopic, rightOutputImageTopic, leftCameraInfoTopic, rightCameraInfoTopic,
leftCameraInfoManager, rightCameraInfoManager;
nh.param("input_image_topic", inputImageTopic, std::string("input_image_topic_not_set"));
ROS_INFO("input topic to stereo splitter=%s\n", inputImageTopic.c_str());
nh.param("left_output_image_topic", leftOutputImageTopic, std::string("left/image_raw"));
nh.param("right_output_image_topic", rightOutputImageTopic, std::string("right/image_raw"));
nh.param("left_camera_info_topic", leftCameraInfoTopic, std::string("left/camera_info"));
nh.param("right_camera_info_topic", rightCameraInfoTopic, std::string("right/camera_info"));
nh.param("left_camera_info_manager_ns", leftCameraInfoManager, std::string("~/left"));
nh.param("right_camera_info_manager_ns", rightCameraInfoManager, std::string("~/right"));
nh.param("output_width", outputWidth, 0); // 0 -> use 1/2 input width.
nh.param("output_height", outputHeight, 0); // 0 -> use input height.

// Register publishers and subscriber.
imageSub = nh.subscribe(inputImageTopic.c_str(), 2, &imageCallback);
leftImagePublisher = it.advertise(leftOutputImageTopic.c_str(), 5);
rightImagePublisher = it.advertise(rightOutputImageTopic.c_str(), 5);
leftCameraInfoPublisher = nh.advertise<sensor_msgs::CameraInfo>(leftCameraInfoTopic.c_str(), 5);
rightCameraInfoPublisher = nh.advertise<sensor_msgs::CameraInfo>(rightCameraInfoTopic.c_str(), 5);

// Camera info managers.
ros::NodeHandle nh_left(leftCameraInfoManager);
ros::NodeHandle nh_right(rightCameraInfoManager);
// Allocate and initialize camera info managers.
camera_info_manager::CameraInfoManager left_cinfo_(nh_left, "camera", leftCameraInfoURL);
camera_info_manager::CameraInfoManager right_cinfo_(nh_right, "camera", rightCameraInfoURL);
left_cinfo_.loadCameraInfo(leftCameraInfoURL);
right_cinfo_.loadCameraInfo(rightCameraInfoURL);

// Pre-fill camera_info messages.
leftCameraInfoMsg = left_cinfo_.getCameraInfo();
rightCameraInfoMsg = right_cinfo_.getCameraInfo();

// Run node until cancelled.
ros::spin();
}