Skip to content

Commit 83596b6

Browse files
eigendudeeigendude
committed
Use modern image_transport API
1 parent 9c3aec2 commit 83596b6

File tree

3 files changed

+28
-141
lines changed

3 files changed

+28
-141
lines changed

‎CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@ set(PACKAGE_DEPENDENCIES
1818
"sensor_msgs"
1919
"camera_info_manager"
2020
"cv_bridge"
21+
"image_transport"
2122
)
2223
foreach(PKGDEP IN LISTS PACKAGE_DEPENDENCIES)
2324
find_package(${PKGDEP} REQUIRED)
@@ -79,6 +80,7 @@ target_link_libraries(camera_component PUBLIC
7980
${sensor_msgs_TARGETS}
8081
camera_info_manager::camera_info_manager
8182
cv_bridge::cv_bridge
83+
image_transport::image_transport
8284
)
8385
target_include_directories(camera_component PUBLIC ${libcamera_INCLUDE_DIRS})
8486
target_link_libraries(camera_component PUBLIC ${libcamera_LINK_LIBRARIES})

‎package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
<depend>sensor_msgs</depend>
1616
<depend>camera_info_manager</depend>
1717
<depend>cv_bridge</depend>
18+
<depend>image_transport</depend>
1819

1920
<exec_depend>ros2launch</exec_depend>
2021
<exec_depend>ament_index_python</exec_depend>

‎src/CameraNode.cpp‎

Lines changed: 25 additions & 141 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#include <cv_bridge/cv_bridge.h>
1616
#endif
1717
#include <atomic>
18+
#include <image_transport/image_transport.hpp>
1819
#include <iostream>
1920
#include <libcamera/base/shared_fd.h>
2021
#include <libcamera/base/signal.h>
@@ -97,38 +98,18 @@ class CameraNode : public rclcpp::Node
9798

9899
bool use_node_time;
99100

100-
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr pub_image;
101-
rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr pub_image_compressed;
102-
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr pub_ci;
101+
image_transport::CameraPublisher pub_camera;
103102

104103
camera_info_manager::CameraInfoManager cim;
105104

106105
ParameterHandler parameter_handler;
107106
std::string frame_id;
108107

109-
#ifdef RCLCPP_HAS_PARAM_EXT_CB
110-
// use new "post_set" callback to apply parameters
111-
PostSetParametersCallbackHandle::SharedPtr param_cb_change;
112-
#else
113-
OnSetParametersCallbackHandle::SharedPtr param_cb_change;
114-
#endif
115-
116-
// compression quality parameter
117-
std::atomic_uint8_t jpeg_quality;
118-
119108
void
120109
requestComplete(libcamera::Request *const request);
121110

122111
void
123112
process(libcamera::Request *const request);
124-
125-
void
126-
postParameterChange(const std::vector<rclcpp::Parameter> &parameters);
127-
128-
#ifndef RCLCPP_HAS_PARAM_EXT_CB
129-
rcl_interfaces::msg::SetParametersResult
130-
onParameterChange(const std::vector<rclcpp::Parameter> &parameters);
131-
#endif
132113
};
133114

134115
RCLCPP_COMPONENTS_REGISTER_NODE(camera::CameraNode)
@@ -182,42 +163,6 @@ get_sensor_format(const std::string &format_str)
182163
throw std::runtime_error("Invalid sensor_mode. Expected [width]:[height] but got " + format_str);
183164
}
184165

185-
// The following function "compressImageMsg" is adapted from "CvImage::toCompressedImageMsg"
186-
// (https://github.com/ros-perception/vision_opencv/blob/066793a23e5d06d76c78ca3d69824a501c3554fd/cv_bridge/src/cv_bridge.cpp#L512-L535)
187-
// and covered by the BSD-3-Clause licence.
188-
void
189-
compressImageMsg(const sensor_msgs::msg::Image &source,
190-
sensor_msgs::msg::CompressedImage &destination,
191-
const std::vector<int> &params = std::vector<int>())
192-
{
193-
namespace enc = sensor_msgs::image_encodings;
194-
195-
std::shared_ptr<CameraNode> tracked_object;
196-
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(source, tracked_object);
197-
198-
destination.header = source.header;
199-
cv::Mat image;
200-
if (cv_ptr->encoding == enc::BGR8 || cv_ptr->encoding == enc::BGRA8 ||
201-
cv_ptr->encoding == enc::MONO8 || cv_ptr->encoding == enc::MONO16)
202-
{
203-
image = cv_ptr->image;
204-
}
205-
else {
206-
cv_bridge::CvImagePtr tempThis = std::make_shared<cv_bridge::CvImage>(*cv_ptr);
207-
cv_bridge::CvImagePtr temp;
208-
if (enc::hasAlpha(cv_ptr->encoding)) {
209-
temp = cv_bridge::cvtColor(tempThis, enc::BGRA8);
210-
}
211-
else {
212-
temp = cv_bridge::cvtColor(tempThis, enc::BGR8);
213-
}
214-
image = temp->image;
215-
}
216-
217-
destination.format = "jpg";
218-
cv::imencode(".jpg", image, destination.data, params);
219-
}
220-
221166

222167
CameraNode::CameraNode(const rclcpp::NodeOptions &options)
223168
: Node("camera", options),
@@ -234,14 +179,7 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options)
234179
#else
235180
cim(this),
236181
#endif
237-
parameter_handler(this),
238-
param_cb_change(
239-
#ifdef RCLCPP_HAS_PARAM_EXT_CB
240-
add_post_set_parameters_callback(std::bind(&CameraNode::postParameterChange, this, std::placeholders::_1))
241-
#else
242-
add_on_set_parameters_callback(std::bind(&CameraNode::onParameterChange, this, std::placeholders::_1))
243-
#endif
244-
)
182+
parameter_handler(this)
245183
{
246184
// pixel format
247185
rcl_interfaces::msg::ParameterDescriptor param_descr_format;
@@ -301,34 +239,14 @@ CameraNode::CameraNode(const rclcpp::NodeOptions &options)
301239
const rclcpp::ParameterValue &camera_id =
302240
declare_parameter("camera", rclcpp::ParameterValue {}, param_descr_ro.set__dynamic_typing(true));
303241

304-
// we cannot control the compression rate of the libcamera MJPEG stream
305-
// ignore "jpeg_quality" parameter for MJPEG streams
306-
if (libcamera::PixelFormat::fromString(format) != libcamera::formats::MJPEG) {
307-
rcl_interfaces::msg::ParameterDescriptor jpeg_quality_description;
308-
jpeg_quality_description.name = "jpeg_quality";
309-
jpeg_quality_description.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
310-
jpeg_quality_description.description = "Image quality for JPEG format";
311-
jpeg_quality_description.read_only = false;
312-
rcl_interfaces::msg::IntegerRange jpeg_range;
313-
jpeg_range.from_value = 1;
314-
jpeg_range.to_value = 100;
315-
jpeg_range.step = 1;
316-
jpeg_quality_description.integer_range = {jpeg_range};
317-
// default to 95
318-
jpeg_quality = declare_parameter<uint8_t>("jpeg_quality", 95, jpeg_quality_description);
319-
}
320-
321242
// use_node_time parameter
322243
rcl_interfaces::msg::ParameterDescriptor param_descr_use_node_time;
323244
param_descr_use_node_time.description = "use node time instead of sensor timestamp for image messages";
324245
param_descr_use_node_time.read_only = true;
325246
use_node_time = declare_parameter<bool>("use_node_time", false, param_descr_use_node_time);
326247

327-
// publisher for raw and compressed image
328-
pub_image = this->create_publisher<sensor_msgs::msg::Image>("~/image_raw", 1);
329-
pub_image_compressed =
330-
this->create_publisher<sensor_msgs::msg::CompressedImage>("~/image_raw/compressed", 1);
331-
pub_ci = this->create_publisher<sensor_msgs::msg::CameraInfo>("~/camera_info", 1);
248+
// publisher for images and camera info
249+
pub_camera = image_transport::create_camera_publisher(this, "~/image_raw");
332250

333251
// start camera manager and check for cameras
334252
camera_manager.start();
@@ -658,55 +576,44 @@ CameraNode::process(libcamera::Request *const request)
658576
// prepare image messages
659577
const libcamera::StreamConfiguration &cfg = stream->configuration();
660578

661-
auto msg_img = std::make_unique<sensor_msgs::msg::Image>();
662-
auto msg_img_compressed = std::make_unique<sensor_msgs::msg::CompressedImage>();
579+
sensor_msgs::msg::Image msg_img;
580+
sensor_msgs::msg::CompressedImage msg_img_compressed;
663581

664582
if (format_type(cfg.pixelFormat) == FormatType::RAW) {
665583
// raw uncompressed image
666584
assert(buffer_info[buffer].size == bytesused);
667-
msg_img->header = hdr;
668-
msg_img->width = cfg.size.width;
669-
msg_img->height = cfg.size.height;
670-
msg_img->step = cfg.stride;
671-
msg_img->encoding = get_ros_encoding(cfg.pixelFormat);
672-
msg_img->is_bigendian = (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__);
673-
msg_img->data.resize(buffer_info[buffer].size);
674-
memcpy(msg_img->data.data(), buffer_info[buffer].data, buffer_info[buffer].size);
675-
676-
// compress to jpeg
677-
if (pub_image_compressed->get_subscription_count()) {
678-
try {
679-
compressImageMsg(*msg_img, *msg_img_compressed,
680-
{cv::IMWRITE_JPEG_QUALITY, jpeg_quality});
681-
}
682-
catch (const cv_bridge::Exception &e) {
683-
RCLCPP_ERROR_STREAM(get_logger(), e.what());
684-
}
685-
}
585+
msg_img.header = hdr;
586+
msg_img.width = cfg.size.width;
587+
msg_img.height = cfg.size.height;
588+
msg_img.step = cfg.stride;
589+
msg_img.encoding = get_ros_encoding(cfg.pixelFormat);
590+
msg_img.is_bigendian = (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__);
591+
msg_img.data.resize(buffer_info[buffer].size);
592+
memcpy(msg_img.data.data(), buffer_info[buffer].data, buffer_info[buffer].size);
686593
}
687594
else if (format_type(cfg.pixelFormat) == FormatType::COMPRESSED) {
688595
// compressed image
689596
assert(bytesused < buffer_info[buffer].size);
690-
msg_img_compressed->header = hdr;
691-
msg_img_compressed->format = get_ros_encoding(cfg.pixelFormat);
692-
msg_img_compressed->data.resize(bytesused);
693-
memcpy(msg_img_compressed->data.data(), buffer_info[buffer].data, bytesused);
597+
msg_img_compressed.header = hdr;
598+
msg_img_compressed.format = get_ros_encoding(cfg.pixelFormat);
599+
msg_img_compressed.data.resize(bytesused);
600+
memcpy(msg_img_compressed.data.data(), buffer_info[buffer].data, bytesused);
694601

695602
// decompress into raw rgb8 image
696-
if (pub_image->get_subscription_count())
697-
cv_bridge::toCvCopy(*msg_img_compressed, "rgb8")->toImageMsg(*msg_img);
603+
if (pub_camera.getNumSubscribers() > 0)
604+
cv_bridge::toCvCopy(msg_img_compressed, "rgb8")->toImageMsg(msg_img);
698605
}
699606
else {
700607
throw std::runtime_error("unsupported pixel format: " +
701608
stream->configuration().pixelFormat.toString());
702609
}
703610

704-
pub_image->publish(std::move(msg_img));
705-
pub_image_compressed->publish(std::move(msg_img_compressed));
706-
611+
// Get camera info
707612
sensor_msgs::msg::CameraInfo ci = cim.getCameraInfo();
708613
ci.header = hdr;
709-
pub_ci->publish(ci);
614+
615+
// Publish image and camera info
616+
pub_camera.publish(msg_img, ci);
710617
}
711618
else if (request->status() == libcamera::Request::RequestCancelled) {
712619
RCLCPP_ERROR_STREAM(get_logger(), "request '" << request->toString() << "' cancelled");
@@ -727,27 +634,4 @@ CameraNode::process(libcamera::Request *const request)
727634
}
728635
}
729636

730-
void
731-
CameraNode::postParameterChange(const std::vector<rclcpp::Parameter> &parameters)
732-
{
733-
// check non-control parameters
734-
for (const rclcpp::Parameter &parameter : parameters) {
735-
if (parameter.get_name() == "jpeg_quality") {
736-
jpeg_quality = parameter.get_parameter_value().get<uint8_t>();
737-
}
738-
}
739-
}
740-
741-
#ifndef RCLCPP_HAS_PARAM_EXT_CB
742-
rcl_interfaces::msg::SetParametersResult
743-
CameraNode::onParameterChange(const std::vector<rclcpp::Parameter> &parameters)
744-
{
745-
postParameterChange(parameters);
746-
747-
rcl_interfaces::msg::SetParametersResult result;
748-
result.successful = true;
749-
return result;
750-
}
751-
#endif
752-
753637
} // namespace camera

0 commit comments

Comments
 (0)