diff --git a/CMakeLists.txt b/CMakeLists.txt index b2d547f..d504a00 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,28 +3,28 @@ cmake_minimum_required(VERSION 3.5) project(gscam2) # Default to C99 -if (NOT CMAKE_C_STANDARD) +if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) -endif () +endif() # Default to C++14 -if (NOT CMAKE_CXX_STANDARD) +if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) -endif () +endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # Emulate Colcon in CLion -if ($ENV{CLION_IDE}) +if($ENV{CLION_IDE}) message(STATUS "Running inside CLion") find_package(fastrtps_cmake_module REQUIRED) set(FastRTPS_INCLUDE_DIR "/opt/ros/foxy/include") set(FastRTPS_LIBRARY_RELEASE "/opt/ros/foxy/lib/libfastrtps.so") set(ros2_shared_DIR "${PROJECT_SOURCE_DIR}/../../../install/ros2_shared/share/ros2_shared/cmake") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DRUN_INSIDE_CLION") -endif () +endif() # Gstreamer doesn't provide CMake files find_package(PkgConfig) @@ -129,6 +129,26 @@ ament_target_dependencies( rclcpp ) +#============= +# Test +#============= + +# Load & run linters listed in package.xml +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest) + ament_add_gtest( + smoke_test + test/smoke_test.cpp + ENV GSCAM_CONFIG="videotestsrc pattern=snow ! capsfilter caps=video/x-raw,width=800,height=600 ! videoconvert" + ) + if(TARGET smoke_test) + target_link_libraries(smoke_test gscam_node) + endif() +endif() + #============= # Export # Best practice, see https://discourse.ros.org/t/ament-best-practice-for-sharing-libraries/3602 diff --git a/include/gscam2/gscam_node.hpp b/include/gscam2/gscam_node.hpp index 5ad7b37..8c7f7a6 100644 --- a/include/gscam2/gscam_node.hpp +++ b/include/gscam2/gscam_node.hpp @@ -6,20 +6,19 @@ namespace gscam2 { - class GSCamNode : public rclcpp::Node - { - // Hide implementation - class impl; - std::unique_ptr pImpl_; - - void validate_parameters(); +class GSCamNode : public rclcpp::Node +{ + // Hide implementation + class impl; + std::unique_ptr pImpl_; - public: + void validate_parameters(); - explicit GSCamNode(const rclcpp::NodeOptions &options); +public: + explicit GSCamNode(const rclcpp::NodeOptions & options); - ~GSCamNode() override; - }; + ~GSCamNode() override; +}; } diff --git a/include/gscam2/subscriber_node.hpp b/include/gscam2/subscriber_node.hpp index 752d5b8..b289abd 100644 --- a/include/gscam2/subscriber_node.hpp +++ b/include/gscam2/subscriber_node.hpp @@ -7,16 +7,15 @@ namespace gscam2 { - // Node that subscribes to a topic, used for testing composition and IPC - class ImageSubscriberNode : public rclcpp::Node - { - rclcpp::Subscription::SharedPtr sub_; - - public: +// Node that subscribes to a topic, used for testing composition and IPC +class ImageSubscriberNode : public rclcpp::Node +{ + rclcpp::Subscription::SharedPtr sub_; - explicit ImageSubscriberNode(const rclcpp::NodeOptions &options); - }; +public: + explicit ImageSubscriberNode(const rclcpp::NodeOptions & options); +}; } // namespace gscam2 -#endif //SIMPLE_SUBSCRIBER_HPP \ No newline at end of file +#endif //SIMPLE_SUBSCRIBER_HPP diff --git a/launch/composition_launch.py b/launch/composition_launch.py index c95e5be..7af7341 100644 --- a/launch/composition_launch.py +++ b/launch/composition_launch.py @@ -11,6 +11,7 @@ def generate_launch_description(): + gscam_config = 'videotestsrc pattern=snow ! video/x-raw,width=1280,height=720 ! videoconvert' container = ComposableNodeContainer( name='my_container', @@ -23,7 +24,7 @@ def generate_launch_description(): plugin='gscam2::GSCamNode', name='image_publisher', parameters=[{ - 'gscam_config': 'videotestsrc pattern=snow ! video/x-raw,width=1280,height=720 ! videoconvert', + 'gscam_config': gscam_config, }], extra_arguments=[{'use_intra_process_comms': True}], ), @@ -37,4 +38,4 @@ def generate_launch_description(): output='screen', ) - return LaunchDescription([container]) \ No newline at end of file + return LaunchDescription([container]) diff --git a/launch/container_param_launch.py b/launch/container_param_launch.py index 5be596e..0dcfc58 100644 --- a/launch/container_param_launch.py +++ b/launch/container_param_launch.py @@ -20,7 +20,7 @@ print(config_dir) # Parameters file, see https://github.com/ros2/launch_ros/issues/156 -params_file = os.path.join(config_dir, "workaround_params.yaml") +params_file = os.path.join(config_dir, 'workaround_params.yaml') print(params_file) # Camera calibration file diff --git a/launch/node_param_launch.py b/launch/node_param_launch.py index 2082b24..c7e7b18 100644 --- a/launch/node_param_launch.py +++ b/launch/node_param_launch.py @@ -1,6 +1,4 @@ -""" -Launch a Node with parameters and remappings. -""" +"""Launch a Node with parameters and remappings.""" import os @@ -16,7 +14,7 @@ print(config_dir) # Parameters file -params_file = os.path.join(config_dir, "params.yaml") +params_file = os.path.join(config_dir, 'params.yaml') print(params_file) # Camera calibration file @@ -37,7 +35,7 @@ def generate_launch_description(): params_file, # A few more parameters { - 'camera_name': camera_name, # Camera Name + 'camera_name': camera_name, # Camera Name 'camera_info_url': camera_config, # Camera calibration information }, ], diff --git a/package.xml b/package.xml index 222e3f5..8b91486 100644 --- a/package.xml +++ b/package.xml @@ -22,6 +22,24 @@ ament_cmake + ament_cmake_gtest + ament_lint_auto + + + + + + ament_cmake_cppcheck + ament_cmake_flake8 + ament_cmake_lint_cmake + ament_cmake_pep257 + ament_cmake_uncrustify + ament_cmake_xmllint + camera_calibration_parsers camera_info_manager class_loader diff --git a/src/gscam_main.cpp b/src/gscam_main.cpp index 8697278..afbce3b 100644 --- a/src/gscam_main.cpp +++ b/src/gscam_main.cpp @@ -2,7 +2,7 @@ #include "rclcpp/rclcpp.hpp" -int main(int argc, char **argv) +int main(int argc, char ** argv) { // Force flush of the stdout buffer setvbuf(stdout, nullptr, _IONBF, BUFSIZ); @@ -17,7 +17,8 @@ int main(int argc, char **argv) auto node = std::make_shared(options); // Set logging level - auto result = rcutils_logging_set_logger_level(node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_INFO); + auto result = rcutils_logging_set_logger_level( + node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_INFO); (void) result; // Spin until rclcpp::ok() returns false diff --git a/src/gscam_node.cpp b/src/gscam_node.cpp index b1fa9a0..da3ae14 100644 --- a/src/gscam_node.cpp +++ b/src/gscam_node.cpp @@ -14,9 +14,9 @@ extern "C" { namespace gscam2 { - //============================================================================= - // Parameters - //============================================================================= +//============================================================================= +// Parameters +//============================================================================= #define GSCAM_ALL_PARAMS \ CXT_MACRO_MEMBER(gscam_config, std::string, "") /* Stream config */ \ @@ -32,443 +32,459 @@ namespace gscam2 #undef CXT_MACRO_MEMBER #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) - struct GSCamContext - { - CXT_MACRO_DEFINE_MEMBERS(GSCAM_ALL_PARAMS) // NOLINT - }; +struct GSCamContext +{ + CXT_MACRO_DEFINE_MEMBERS(GSCAM_ALL_PARAMS) // NOLINT +}; - //============================================================================= - // GSCamNode::impl - //============================================================================= +//============================================================================= +// GSCamNode::impl +//============================================================================= - class GSCamNode::impl - { - // ROS node - rclcpp::Node *node_; +class GSCamNode::impl +{ + // ROS node + rclcpp::Node * node_; - // Manage camera info - camera_info_manager::CameraInfoManager camera_info_manager_; + // Manage camera info + camera_info_manager::CameraInfoManager camera_info_manager_; - // Gstreamer structures - GstElement *pipeline_; - GstElement *sink_; + // Gstreamer structures + GstElement * pipeline_; + GstElement * sink_; - // We need to poll GStreamer to get data - // Move this to its own thread to avoid blocking or slowing down the rclcpp::spin() thread - std::thread pipeline_thread_; + // We need to poll GStreamer to get data + // Move this to its own thread to avoid blocking or slowing down the rclcpp::spin() thread + std::thread pipeline_thread_; - // Used to stop the pipeline thread - std::atomic stop_signal_; + // Used to stop the pipeline thread + std::atomic stop_signal_; - // Discover width and height from the incoming data - int width_, height_; + // Discover width and height from the incoming data + int width_, height_; - // Calibration between ros::Time and gst timestamps - GstClockTime time_offset_; + // Calibration between ros::Time and gst timestamps + GstClockTime time_offset_; - // Publish images... - rclcpp::Publisher::SharedPtr camera_pub_; + // Publish images... + rclcpp::Publisher::SharedPtr camera_pub_; - // ... or compressed images - rclcpp::Publisher::SharedPtr jpeg_pub_; + // ... or compressed images + rclcpp::Publisher::SharedPtr jpeg_pub_; - // Publish camera info - rclcpp::Publisher::SharedPtr cinfo_pub_; + // Publish camera info + rclcpp::Publisher::SharedPtr cinfo_pub_; - // Create gstreamer pipeline, return true if successful - bool create_pipeline(); + // Create gstreamer pipeline, return true if successful + bool create_pipeline(); - // Delete gstreamer pipeline - void delete_pipeline(); + // Delete gstreamer pipeline + void delete_pipeline(); - // Process one frame - void process_frame(); + // Process one frame + void process_frame(); - public: +public: + // Parameters + GSCamContext cxt_; + + // Constructor + explicit impl(rclcpp::Node * node) + : node_(node), + camera_info_manager_(node), + pipeline_(nullptr), + sink_(nullptr), + stop_signal_(false), + width_(0), + height_(0), + time_offset_(0) + { + } - // Parameters - GSCamContext cxt_; + // Destructor + ~impl() + { + if (pipeline_) { + // Stop thread + stop_signal_ = true; + pipeline_thread_.join(); - // Constructor - explicit impl(rclcpp::Node *node) : - node_(node), - camera_info_manager_(node), - pipeline_(nullptr), - sink_(nullptr), - stop_signal_(false), - width_(0), - height_(0), - time_offset_(0) - { + // Delete pipeline + delete_pipeline(); } + } - // Destructor - ~impl() - { - if (pipeline_) { - // Stop thread - stop_signal_ = true; - pipeline_thread_.join(); - - // Delete pipeline - delete_pipeline(); - } - } + // Start or re-start pipeline + void restart(); +}; - // Start or re-start pipeline - void restart(); - }; +bool GSCamNode::impl::create_pipeline() +{ + if (!gst_is_initialized()) { + // Only need to do this once + gst_init(nullptr, nullptr); + RCLCPP_INFO(node_->get_logger(), "Gstreamer initialized"); + } - bool GSCamNode::impl::create_pipeline() - { - if (!gst_is_initialized()) { - // Only need to do this once - gst_init(nullptr, nullptr); - RCLCPP_INFO(node_->get_logger(), "Gstreamer initialized"); - } + RCLCPP_INFO(node_->get_logger(), "Gstreamer version: %s", gst_version_string()); - RCLCPP_INFO(node_->get_logger(), "Gstreamer version: %s", gst_version_string()); + GError * error = nullptr; + pipeline_ = gst_parse_launch(cxt_.gscam_config_.c_str(), &error); + if (!pipeline_) { + RCLCPP_FATAL(node_->get_logger(), error->message); + return false; + } - GError *error = nullptr; - pipeline_ = gst_parse_launch(cxt_.gscam_config_.c_str(), &error); - if (!pipeline_) { - RCLCPP_FATAL(node_->get_logger(), error->message); - return false; - } + // Create RGB sink + sink_ = gst_element_factory_make("appsink", nullptr); + GstCaps * caps = gst_app_sink_get_caps(GST_APP_SINK(sink_)); + + // http://gstreamer.freedesktop.org/data/doc/gstreamer/head/pwg/html/section-types-definitions.html + if (cxt_.image_encoding_ == sensor_msgs::image_encodings::RGB8) { + caps = gst_caps_new_simple( + "video/x-raw", + "format", G_TYPE_STRING, "RGB", + nullptr); + } else if (cxt_.image_encoding_ == sensor_msgs::image_encodings::MONO8) { + caps = gst_caps_new_simple( + "video/x-raw", + "format", G_TYPE_STRING, "GRAY8", + nullptr); + } else if (cxt_.image_encoding_ == "jpeg") { + caps = gst_caps_new_simple("image/jpeg", nullptr, nullptr); + } - // Create RGB sink - sink_ = gst_element_factory_make("appsink", nullptr); - GstCaps *caps = gst_app_sink_get_caps(GST_APP_SINK(sink_)); + gst_app_sink_set_caps(GST_APP_SINK(sink_), caps); + gst_caps_unref(caps); - // http://gstreamer.freedesktop.org/data/doc/gstreamer/head/pwg/html/section-types-definitions.html - if (cxt_.image_encoding_ == sensor_msgs::image_encodings::RGB8) { - caps = gst_caps_new_simple("video/x-raw", - "format", G_TYPE_STRING, "RGB", - nullptr); - } else if (cxt_.image_encoding_ == sensor_msgs::image_encodings::MONO8) { - caps = gst_caps_new_simple("video/x-raw", - "format", G_TYPE_STRING, "GRAY8", - nullptr); - } else if (cxt_.image_encoding_ == "jpeg") { - caps = gst_caps_new_simple("image/jpeg", nullptr, nullptr); - } + // Set whether the sink should sync + // Sometimes setting this to true can cause a large number of frames to be dropped + gst_base_sink_set_sync( + GST_BASE_SINK(sink_), + (cxt_.sync_sink_) ? TRUE : FALSE); - gst_app_sink_set_caps(GST_APP_SINK(sink_), caps); - gst_caps_unref(caps); + if (GST_IS_PIPELINE(pipeline_)) { + GstPad * outpad = gst_bin_find_unlinked_pad(GST_BIN(pipeline_), GST_PAD_SRC); + g_assert(outpad); - // Set whether the sink should sync - // Sometimes setting this to true can cause a large number of frames to be dropped - gst_base_sink_set_sync( - GST_BASE_SINK(sink_), - (cxt_.sync_sink_) ? TRUE : FALSE); + GstElement * outelement = gst_pad_get_parent_element(outpad); + g_assert(outelement); + gst_object_unref(outpad); - if (GST_IS_PIPELINE(pipeline_)) { - GstPad *outpad = gst_bin_find_unlinked_pad(GST_BIN(pipeline_), GST_PAD_SRC); - g_assert(outpad); + if (!gst_bin_add(GST_BIN(pipeline_), sink_)) { + RCLCPP_FATAL(node_->get_logger(), "gst_bin_add() failed"); + gst_object_unref(outelement); + return false; + } - GstElement *outelement = gst_pad_get_parent_element(outpad); - g_assert(outelement); - gst_object_unref(outpad); + if (!gst_element_link(outelement, sink_)) { + RCLCPP_FATAL( + node_->get_logger(), "Cannot link outelement(\"%s\") -> sink\n", + gst_element_get_name(outelement)); + gst_object_unref(outelement); + return false; + } - if (!gst_bin_add(GST_BIN(pipeline_), sink_)) { - RCLCPP_FATAL(node_->get_logger(), "gst_bin_add() failed"); - gst_object_unref(outelement); - return false; - } + gst_object_unref(outelement); + } else { + GstElement * launchpipe = pipeline_; + pipeline_ = gst_pipeline_new(nullptr); + g_assert(pipeline_); - if (!gst_element_link(outelement, sink_)) { - RCLCPP_FATAL(node_->get_logger(), "Cannot link outelement(\"%s\") -> sink\n", - gst_element_get_name(outelement)); - gst_object_unref(outelement); - return false; - } + gst_object_unparent(GST_OBJECT(launchpipe)); - gst_object_unref(outelement); - } else { - GstElement *launchpipe = pipeline_; - pipeline_ = gst_pipeline_new(nullptr); - g_assert(pipeline_); + gst_bin_add_many(GST_BIN(pipeline_), launchpipe, sink_, nullptr); - gst_object_unparent(GST_OBJECT(launchpipe)); + if (!gst_element_link(launchpipe, sink_)) { + RCLCPP_FATAL(node_->get_logger(), "Cannot link launchpipe -> sink"); + return false; + } + } - gst_bin_add_many(GST_BIN(pipeline_), launchpipe, sink_, nullptr); + // Calibration between rclcpp::Time and gst timestamps + GstClock * clock = gst_system_clock_obtain(); + GstClockTime ct = gst_clock_get_time(clock); + gst_object_unref(clock); + time_offset_ = node_->now().nanoseconds() - ct; + RCLCPP_INFO(node_->get_logger(), "Time offset: %ld", time_offset_); - if (!gst_element_link(launchpipe, sink_)) { - RCLCPP_FATAL(node_->get_logger(), "Cannot link launchpipe -> sink"); - return false; - } - } + gst_element_set_state(pipeline_, GST_STATE_PAUSED); - // Calibration between rclcpp::Time and gst timestamps - GstClock *clock = gst_system_clock_obtain(); - GstClockTime ct = gst_clock_get_time(clock); - gst_object_unref(clock); - time_offset_ = node_->now().nanoseconds() - ct; - RCLCPP_INFO(node_->get_logger(), "Time offset: %ld", time_offset_); + if (gst_element_get_state(pipeline_, nullptr, nullptr, -1) == GST_STATE_CHANGE_FAILURE) { + RCLCPP_FATAL(node_->get_logger(), "Failed to pause stream, check gscam_config"); + return false; + } else { + RCLCPP_INFO(node_->get_logger(), "Stream is paused"); + } - gst_element_set_state(pipeline_, GST_STATE_PAUSED); + cinfo_pub_ = node_->create_publisher("camera_info", 1); + if (cxt_.image_encoding_ == "jpeg") { + jpeg_pub_ = + node_->create_publisher("image_raw/compressed", 1); + } else { + camera_pub_ = node_->create_publisher("image_raw", 1); + } + // Pre-roll camera if needed + if (cxt_.preroll_) { + // The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll + // I am told this is needed and am erring on the side of caution. + gst_element_set_state(pipeline_, GST_STATE_PLAYING); if (gst_element_get_state(pipeline_, nullptr, nullptr, -1) == GST_STATE_CHANGE_FAILURE) { - RCLCPP_FATAL(node_->get_logger(), "Failed to pause stream, check gscam_config"); + RCLCPP_ERROR(node_->get_logger(), "Failed to play in preroll"); return false; } else { - RCLCPP_INFO(node_->get_logger(), "Stream is paused"); + RCLCPP_INFO(node_->get_logger(), "Stream is playing in preroll"); } - cinfo_pub_ = node_->create_publisher("camera_info", 1); - if (cxt_.image_encoding_ == "jpeg") { - jpeg_pub_ = node_->create_publisher("image_raw/compressed", 1); + gst_element_set_state(pipeline_, GST_STATE_PAUSED); + if (gst_element_get_state(pipeline_, nullptr, nullptr, -1) == GST_STATE_CHANGE_FAILURE) { + RCLCPP_ERROR(node_->get_logger(), "failed to pause in preroll"); + return false; } else { - camera_pub_ = node_->create_publisher("image_raw", 1); + RCLCPP_INFO(node_->get_logger(), "Stream is paused in preroll"); } + } - // Pre-roll camera if needed - if (cxt_.preroll_) { - // The PAUSE, PLAY, PAUSE, PLAY cycle is to ensure proper pre-roll - // I am told this is needed and am erring on the side of caution. - gst_element_set_state(pipeline_, GST_STATE_PLAYING); - if (gst_element_get_state(pipeline_, nullptr, nullptr, -1) == GST_STATE_CHANGE_FAILURE) { - RCLCPP_ERROR(node_->get_logger(), "Failed to play in preroll"); - return false; - } else { - RCLCPP_INFO(node_->get_logger(), "Stream is playing in preroll"); - } - - gst_element_set_state(pipeline_, GST_STATE_PAUSED); - if (gst_element_get_state(pipeline_, nullptr, nullptr, -1) == GST_STATE_CHANGE_FAILURE) { - RCLCPP_ERROR(node_->get_logger(), "failed to pause in preroll"); - return false; - } else { - RCLCPP_INFO(node_->get_logger(), "Stream is paused in preroll"); - } - } + if (gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) { + RCLCPP_ERROR(node_->get_logger(), "Could not start stream!"); + return false; + } - if (gst_element_set_state(pipeline_, GST_STATE_PLAYING) == GST_STATE_CHANGE_FAILURE) { - RCLCPP_ERROR(node_->get_logger(), "Could not start stream!"); - return false; - } + RCLCPP_INFO(node_->get_logger(), "Pipeline running"); + return true; +} - RCLCPP_INFO(node_->get_logger(), "Pipeline running"); - return true; +void GSCamNode::impl::delete_pipeline() +{ + // Stop a running stream, or cleanup a stream that failed to fully start + if (pipeline_) { + gst_element_set_state(pipeline_, GST_STATE_NULL); + gst_object_unref(pipeline_); + pipeline_ = nullptr; + RCLCPP_INFO(node_->get_logger(), "Pipeline deleted"); } +} - void GSCamNode::impl::delete_pipeline() - { - // Stop a running stream, or cleanup a stream that failed to fully start - if (pipeline_) { - gst_element_set_state(pipeline_, GST_STATE_NULL); - gst_object_unref(pipeline_); - pipeline_ = nullptr; - RCLCPP_INFO(node_->get_logger(), "Pipeline deleted"); - } +void GSCamNode::impl::process_frame() +{ + // This should block until a new frame is awake, this way, we'll run at the + // actual capture framerate of the device. + // TODO use timeout to handle the case where there's no data + // RCLCPP_DEBUG(get_logger(), "Getting data..."); + GstSample * sample = gst_app_sink_pull_sample(GST_APP_SINK(sink_)); + if (!sample) { + RCLCPP_ERROR(node_->get_logger(), "Could not get sample, pause for 1s"); + using namespace std::chrono_literals; + std::this_thread::sleep_for(1s); + return; + } + GstBuffer * buf = gst_sample_get_buffer(sample); + GstMemory * memory = gst_buffer_get_memory(buf, 0); + GstMapInfo info; + + gst_memory_map(memory, &info, GST_MAP_READ); + gsize & buf_size = info.size; + guint8 * & buf_data = info.data; + GstClockTime bt = gst_element_get_base_time(pipeline_); + // RCLCPP_INFO(get_logger(), "New buffer: timestamp %.6f %lu %lu %.3f", + // GST_TIME_AS_USECONDS(buf->timestamp+bt)/1e6+time_offset_, buf->timestamp, bt, time_offset_); + + // Stop on end of stream + if (!buf) { + RCLCPP_INFO(node_->get_logger(), "Stream ended, pause for 1s"); + using namespace std::chrono_literals; + std::this_thread::sleep_for(1s); + return; } - void GSCamNode::impl::process_frame() - { - // This should block until a new frame is awake, this way, we'll run at the - // actual capture framerate of the device. - // TODO use timeout to handle the case where there's no data - // RCLCPP_DEBUG(get_logger(), "Getting data..."); - GstSample *sample = gst_app_sink_pull_sample(GST_APP_SINK(sink_)); - if (!sample) { - RCLCPP_ERROR(node_->get_logger(), "Could not get sample, pause for 1s"); - using namespace std::chrono_literals; - std::this_thread::sleep_for(1s); - return; - } - GstBuffer *buf = gst_sample_get_buffer(sample); - GstMemory *memory = gst_buffer_get_memory(buf, 0); - GstMapInfo info; - - gst_memory_map(memory, &info, GST_MAP_READ); - gsize &buf_size = info.size; - guint8 *&buf_data = info.data; - GstClockTime bt = gst_element_get_base_time(pipeline_); - // RCLCPP_INFO(get_logger(), "New buffer: timestamp %.6f %lu %lu %.3f", - // GST_TIME_AS_USECONDS(buf->timestamp+bt)/1e6+time_offset_, buf->timestamp, bt, time_offset_); - - // Stop on end of stream - if (!buf) { - RCLCPP_INFO(node_->get_logger(), "Stream ended, pause for 1s"); - using namespace std::chrono_literals; - std::this_thread::sleep_for(1s); - return; + // RCLCPP_DEBUG(get_logger(), "Got data."); + + // Get the image width and height + GstPad * pad = gst_element_get_static_pad(sink_, "sink"); + const GstCaps * caps = gst_pad_get_current_caps(pad); + GstStructure * structure = gst_caps_get_structure(caps, 0); + gst_structure_get_int(structure, "width", &width_); + gst_structure_get_int(structure, "height", &height_); + + // Update header information + camera_info_manager::CameraInfo cur_cinfo = camera_info_manager_.getCameraInfo(); + auto cinfo = std::make_unique(cur_cinfo); + if (cxt_.use_gst_timestamps_) { + cinfo->header.stamp = rclcpp::Time(static_cast(buf->pts + bt + time_offset_)); + } else { + cinfo->header.stamp = node_->now(); + } + // RCLCPP_INFO(get_logger(), "Image time stamp: %.3f",cinfo->header.stamp.toSec()); + cinfo->header.frame_id = cxt_.frame_id_; + if (cxt_.image_encoding_ == "jpeg") { + auto img = std::make_unique(); + img->header = cinfo->header; + img->format = "jpeg"; + img->data.resize(buf_size); + std::copy(buf_data, (buf_data) + (buf_size), img->data.begin()); + jpeg_pub_->publish(std::move(img)); + cinfo_pub_->publish(std::move(cinfo)); + } else { + // Complain if the returned buffer is smaller than we expect + const unsigned int expected_frame_size = + cxt_.image_encoding_ == sensor_msgs::image_encodings::RGB8 ? + width_ * height_ * 3 : + width_ * height_; + + if (buf_size < expected_frame_size) { + RCLCPP_WARN( + node_->get_logger(), + "Image buffer underflow: expected frame to be %d bytes but got only %lu" + " bytes (make sure frames are correctly encoded)", expected_frame_size, (buf_size)); } - // RCLCPP_DEBUG(get_logger(), "Got data."); + // Construct Image message + auto img = std::make_unique(); + + img->header = cinfo->header; - // Get the image width and height - GstPad *pad = gst_element_get_static_pad(sink_, "sink"); - const GstCaps *caps = gst_pad_get_current_caps(pad); - GstStructure *structure = gst_caps_get_structure(caps, 0); - gst_structure_get_int(structure, "width", &width_); - gst_structure_get_int(structure, "height", &height_); + // Image data and metadata + img->width = width_; + img->height = height_; + img->encoding = cxt_.image_encoding_; + img->is_bigendian = false; + img->data.resize(expected_frame_size); - // Update header information - camera_info_manager::CameraInfo cur_cinfo = camera_info_manager_.getCameraInfo(); - auto cinfo = std::make_unique(cur_cinfo); - if (cxt_.use_gst_timestamps_) { - cinfo->header.stamp = rclcpp::Time(static_cast(buf->pts + bt + time_offset_)); + // Copy the image, so we can free the buffer allocated by gstreamer + if (cxt_.image_encoding_ == sensor_msgs::image_encodings::RGB8) { + img->step = width_ * 3; } else { - cinfo->header.stamp = node_->now(); + img->step = width_; } - // RCLCPP_INFO(get_logger(), "Image time stamp: %.3f",cinfo->header.stamp.toSec()); - cinfo->header.frame_id = cxt_.frame_id_; - if (cxt_.image_encoding_ == "jpeg") { - auto img = std::make_unique(); - img->header = cinfo->header; - img->format = "jpeg"; - img->data.resize(buf_size); - std::copy(buf_data, (buf_data) + (buf_size), img->data.begin()); - jpeg_pub_->publish(std::move(img)); - cinfo_pub_->publish(std::move(cinfo)); - } else { - // Complain if the returned buffer is smaller than we expect - const unsigned int expected_frame_size = - cxt_.image_encoding_ == sensor_msgs::image_encodings::RGB8 - ? width_ * height_ * 3 - : width_ * height_; - - if (buf_size < expected_frame_size) { - RCLCPP_WARN(node_->get_logger(), - "Image buffer underflow: expected frame to be %d bytes but got only %lu" - " bytes (make sure frames are correctly encoded)", expected_frame_size, (buf_size)); - } - - // Construct Image message - auto img = std::make_unique(); - - img->header = cinfo->header; - - // Image data and metadata - img->width = width_; - img->height = height_; - img->encoding = cxt_.image_encoding_; - img->is_bigendian = false; - img->data.resize(expected_frame_size); - - // Copy the image, so we can free the buffer allocated by gstreamer - if (cxt_.image_encoding_ == sensor_msgs::image_encodings::RGB8) { - img->step = width_ * 3; - } else { - img->step = width_; - } - std::copy( - buf_data, - (buf_data) + (buf_size), - img->data.begin()); + std::copy( + buf_data, + (buf_data) + (buf_size), + img->data.begin()); #undef SHOW_ADDRESS #ifdef SHOW_ADDRESS - static int count = 0; - RCLCPP_INFO(node_->get_logger(), "%d, %p", count++, reinterpret_cast(img.get())); + static int count = 0; + RCLCPP_INFO( + node_->get_logger(), "%d, %p", count++, + reinterpret_cast(img.get())); #endif - // Publish the image/info - camera_pub_->publish(std::move(img)); - cinfo_pub_->publish(std::move(cinfo)); - } - - // Release the buffer - gst_memory_unmap(memory, &info); - gst_memory_unref(memory); - gst_sample_unref(sample); + // Publish the image/info + camera_pub_->publish(std::move(img)); + cinfo_pub_->publish(std::move(cinfo)); } - void GSCamNode::impl::restart() - { - if (pipeline_) { - // Stop thread - stop_signal_ = true; - pipeline_thread_.join(); + // Release the buffer + gst_memory_unmap(memory, &info); + gst_memory_unref(memory); + gst_sample_unref(sample); +} - // Delete pipeline - delete_pipeline(); - } +void GSCamNode::impl::restart() +{ + if (pipeline_) { + // Stop thread + stop_signal_ = true; + pipeline_thread_.join(); - // If gscam_config is empty look for GSCAM_CONFIG in the environment - if (cxt_.gscam_config_.empty()) { - auto gsconfig_env = getenv("GSCAM_CONFIG"); - if (gsconfig_env) { - RCLCPP_INFO(node_->get_logger(), "Using GSCAM_CONFIG env var: %s", gsconfig_env); - cxt_.gscam_config_ = gsconfig_env; - } else { - RCLCPP_FATAL(node_->get_logger(), - "GSCAM_CONFIG env var and gscam_config param are both missing, can't start stream"); - return; - } - } + // Delete pipeline + delete_pipeline(); + } - if (cxt_.image_encoding_ != sensor_msgs::image_encodings::RGB8 && - cxt_.image_encoding_ != sensor_msgs::image_encodings::MONO8 && - cxt_.image_encoding_ != "jpeg") { - RCLCPP_FATAL(node_->get_logger(), "Unsupported image encoding: %s", cxt_.image_encoding_.c_str()); + // If gscam_config is empty look for GSCAM_CONFIG in the environment + if (cxt_.gscam_config_.empty()) { + auto gsconfig_env = getenv("GSCAM_CONFIG"); + if (gsconfig_env) { + RCLCPP_INFO(node_->get_logger(), "Using GSCAM_CONFIG env var: %s", gsconfig_env); + cxt_.gscam_config_ = gsconfig_env; + } else { + RCLCPP_FATAL( + node_->get_logger(), + "GSCAM_CONFIG env var and gscam_config param are both missing, can't start stream"); return; } + } - camera_info_manager_.setCameraName(cxt_.camera_name_); - - if (camera_info_manager_.validateURL(cxt_.camera_info_url_)) { - camera_info_manager_.loadCameraInfo(cxt_.camera_info_url_); - RCLCPP_INFO(node_->get_logger(), "Loaded camera calibration from %s", cxt_.camera_info_url_.c_str()); - } else { - RCLCPP_ERROR(node_->get_logger(), "Camera info url '%s' is not valid, missing 'file://' prefix?", - cxt_.camera_info_url_.c_str()); - } + if (cxt_.image_encoding_ != sensor_msgs::image_encodings::RGB8 && + cxt_.image_encoding_ != sensor_msgs::image_encodings::MONO8 && + cxt_.image_encoding_ != "jpeg") + { + RCLCPP_FATAL( + node_->get_logger(), "Unsupported image encoding: %s", + cxt_.image_encoding_.c_str()); + return; + } - // [Re-]start the pipeline in its own thread - if (create_pipeline()) { - pipeline_thread_ = std::thread( - [this]() - { - RCLCPP_INFO(node_->get_logger(), "Thread running"); // NOLINT + camera_info_manager_.setCameraName(cxt_.camera_name_); - while (!stop_signal_ && rclcpp::ok()) { - process_frame(); - } + if (camera_info_manager_.validateURL(cxt_.camera_info_url_)) { + camera_info_manager_.loadCameraInfo(cxt_.camera_info_url_); + RCLCPP_INFO( + node_->get_logger(), "Loaded camera calibration from %s", cxt_.camera_info_url_.c_str()); + } else { + RCLCPP_ERROR( + node_->get_logger(), "Camera info url '%s' is not valid, missing 'file://' prefix?", + cxt_.camera_info_url_.c_str()); + } - stop_signal_ = false; - RCLCPP_INFO(node_->get_logger(), "Thread stopped"); // NOLINT - }); - } else { - delete_pipeline(); - } + // [Re-]start the pipeline in its own thread + if (create_pipeline()) { + pipeline_thread_ = std::thread( + [this]() + { + RCLCPP_INFO(node_->get_logger(), "Thread running"); // NOLINT + + while (!stop_signal_ && rclcpp::ok()) { + process_frame(); + } + + stop_signal_ = false; + RCLCPP_INFO(node_->get_logger(), "Thread stopped"); // NOLINT + }); + } else { + delete_pipeline(); } +} - //============================================================================= - // GSCamNode - //============================================================================= +//============================================================================= +// GSCamNode +//============================================================================= - GSCamNode::GSCamNode(const rclcpp::NodeOptions &options) : - rclcpp::Node("gscam_publisher", options), - pImpl_(std::make_unique(this)) - { - RCLCPP_INFO(get_logger(), "use_intra_process_comms=%d", options.use_intra_process_comms()); +GSCamNode::GSCamNode(const rclcpp::NodeOptions & options) +: rclcpp::Node("gscam_publisher", options), + pImpl_(std::make_unique(this)) +{ + RCLCPP_INFO(get_logger(), "use_intra_process_comms=%d", options.use_intra_process_comms()); - // Declare and get parameters, this will call validate_parameters() + // Declare and get parameters, this will call validate_parameters() #undef CXT_MACRO_MEMBER #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOAD_PARAMETER((*this), pImpl_->cxt_, n, t, d) - CXT_MACRO_INIT_PARAMETERS(GSCAM_ALL_PARAMS, validate_parameters) + CXT_MACRO_INIT_PARAMETERS(GSCAM_ALL_PARAMS, validate_parameters) - // Register parameters, if they change validate_parameters() will be called again + // Register parameters, if they change validate_parameters() will be called again #undef CXT_MACRO_MEMBER #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_PARAMETER_CHANGED(n, t) - CXT_MACRO_REGISTER_PARAMETERS_CHANGED((*this), pImpl_->cxt_, GSCAM_ALL_PARAMS, validate_parameters) // NOLINT - } + CXT_MACRO_REGISTER_PARAMETERS_CHANGED( + (*this), pImpl_->cxt_, GSCAM_ALL_PARAMS, + validate_parameters) // NOLINT +} - GSCamNode::~GSCamNode() - { - pImpl_.reset(); - } +GSCamNode::~GSCamNode() +{ + pImpl_.reset(); +} - void GSCamNode::validate_parameters() - { +void GSCamNode::validate_parameters() +{ #undef CXT_MACRO_MEMBER -#define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOG_PARAMETER(RCLCPP_INFO, get_logger(), pImpl_->cxt_, n, t, d) - GSCAM_ALL_PARAMS +#define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOG_PARAMETER( \ + RCLCPP_INFO, \ + get_logger(), pImpl_->cxt_, n, t, d) + GSCAM_ALL_PARAMS - pImpl_->restart(); - } + pImpl_->restart(); +} } // namespace gscam2 diff --git a/src/ipc_test_main.cpp b/src/ipc_test_main.cpp index 74694ec..fbbe2d1 100644 --- a/src/ipc_test_main.cpp +++ b/src/ipc_test_main.cpp @@ -3,7 +3,7 @@ // Manually compose OpencvCamNode and ImageSubscriberNode with use_intra_process_comms=true -int main(int argc, char **argv) +int main(int argc, char ** argv) { // Force flush of the stdout buffer setvbuf(stdout, nullptr, _IONBF, BUFSIZ); diff --git a/src/subscriber_node.cpp b/src/subscriber_node.cpp index aaf4af2..c74433f 100644 --- a/src/subscriber_node.cpp +++ b/src/subscriber_node.cpp @@ -3,34 +3,34 @@ namespace gscam2 { - ImageSubscriberNode::ImageSubscriberNode(const rclcpp::NodeOptions &options) : - Node("image_subscriber", options) - { - (void) sub_; +ImageSubscriberNode::ImageSubscriberNode(const rclcpp::NodeOptions & options) +: Node("image_subscriber", options) +{ + (void) sub_; - RCLCPP_INFO(get_logger(), "use_intra_process_comms=%d", options.use_intra_process_comms()); + RCLCPP_INFO(get_logger(), "use_intra_process_comms=%d", options.use_intra_process_comms()); - sub_ = this->create_subscription( - "image_raw", 10, - [this](sensor_msgs::msg::Image::UniquePtr msg) - { - RCLCPP_INFO_ONCE(get_logger(), "receiving messages"); // NOLINT + sub_ = this->create_subscription( + "image_raw", 10, + [this](sensor_msgs::msg::Image::UniquePtr msg) + { + RCLCPP_INFO_ONCE(get_logger(), "receiving messages"); // NOLINT #undef SHOW_ADDRESS #ifdef SHOW_ADDRESS - static int count = 0; - RCLCPP_INFO(get_logger(), "%d, %p", count++, reinterpret_cast(msg.get())); + static int count = 0; + RCLCPP_INFO(get_logger(), "%d, %p", count++, reinterpret_cast(msg.get())); #else - (void) this; - (void) msg; + (void) this; + (void) msg; #endif - }); + }); - RCLCPP_INFO(get_logger(), "ready"); - } + RCLCPP_INFO(get_logger(), "ready"); +} } // namespace gscam2 #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(gscam2::ImageSubscriberNode) // NOLINT \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(gscam2::ImageSubscriberNode) // NOLINT diff --git a/test/smoke_test.cpp b/test/smoke_test.cpp new file mode 100644 index 0000000..80699b3 --- /dev/null +++ b/test/smoke_test.cpp @@ -0,0 +1,65 @@ +#include +#include + +#include "gscam2/gscam_node.hpp" +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/node.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "utils.hpp" + +using namespace std::chrono_literals; + +// Launch a test GStreamer pipeline and verify that at least 1 image was published +// See GSCAM_CONFIG in CMakeLists.txt +TEST(SmokeTest, smoke_test) // NOLINT +{ + const size_t max_loops = 200; + const std::chrono::milliseconds sleep_per_loop = std::chrono::milliseconds(10); + const char * topic = "image_raw"; + + rclcpp::NodeOptions options{}; + options.use_intra_process_comms(true); + auto cam_node = std::make_shared(options); + auto sub_node = std::make_shared("sub_node", options); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(cam_node); + executor.add_node(sub_node); + + int count = 0; + + auto sub = sub_node->create_subscription( + topic, 10, + [&](const sensor_msgs::msg::Image::ConstSharedPtr image) // NOLINT + { + // Match GSCAM_CONFIG and param defaults + EXPECT_EQ(image->header.frame_id, "camera_frame"); + EXPECT_EQ(image->width, 800u); + EXPECT_EQ(image->height, 600u); + EXPECT_EQ(image->encoding, sensor_msgs::image_encodings::RGB8); + EXPECT_EQ(image->data.size(), image->width * image->height * 3); + count++; + } + ); + + test_rclcpp::wait_for_subscriber(sub_node, topic); + + EXPECT_EQ(0, count); + + for (size_t loop = 0; count < 1 && loop < max_loops; ++loop) { + std::this_thread::sleep_for(sleep_per_loop); + executor.spin_some(); + } + + EXPECT_GT(count, 0); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/test/utils.hpp b/test/utils.hpp new file mode 100644 index 0000000..aee9d9c --- /dev/null +++ b/test/utils.hpp @@ -0,0 +1,71 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" + +namespace test_rclcpp +{ + +// Sleep for timeout ms or until a subscriber has registered for the topic +// if to_be_available is true, then it will wait for the number of +// subscribers to be > 0, if false it will wait for the number of +// subscribers to be 0 +void wait_for_subscriber( + std::shared_ptr node, + const std::string & topic_name, + bool to_be_available = true, + std::chrono::milliseconds timeout = std::chrono::milliseconds(1), + std::chrono::microseconds sleep_period = std::chrono::seconds(1)) +{ + using std::chrono::duration_cast; + using std::chrono::microseconds; + using std::chrono::steady_clock; + auto start = steady_clock::now(); + microseconds time_slept(0); + auto predicate = [&node, &topic_name, &to_be_available]() -> bool { + if (to_be_available) { + // the subscriber is available if the count is gt 0 + return node->count_subscribers(topic_name) > 0; + } else { + // the subscriber is no longer available when the count is 0 + return node->count_subscribers(topic_name) == 0; + } + }; + while (!predicate() && time_slept < duration_cast(timeout)) { + rclcpp::Event::SharedPtr graph_event = node->get_graph_event(); + node->wait_for_graph_change(graph_event, sleep_period); + time_slept = duration_cast(steady_clock::now() - start); + } + int64_t time_slept_count = + std::chrono::duration_cast(time_slept).count(); + printf( + "Waited %" PRId64 " microseconds for the subscriber to %s topic '%s'\n", + time_slept_count, + to_be_available ? "connect to" : "disconnect from", + topic_name.c_str()); +} + +} // namespace test_rclcpp + +#endif // UTILS_HPP_