diff --git a/Dockerfile.rov b/Dockerfile.rov index 56326a4..0383b0c 100644 --- a/Dockerfile.rov +++ b/Dockerfile.rov @@ -27,7 +27,6 @@ RUN add-apt-repository universe && apt-get update && \ libgtk-3-dev \ libgtk2.0-dev \ gstreamer1.0-x && \ -# install v4l2 apt-get install -y libv4l-dev v4l-utils && \ apt-get install -y libopenjp2-7 \ libopenjp2-7-dev \ diff --git a/libraries.txt b/libraries.txt index 32b5922..eefe79f 100644 --- a/libraries.txt +++ b/libraries.txt @@ -1,2 +1,3 @@ OpenCV4 >= 4.2 -Eigen3 >= 3.3.7 \ No newline at end of file +Eigen3 >= 3.3.7 +libi2c >= 4.3 \ No newline at end of file diff --git a/src/rov/.run_docker.sh.swo b/src/rov/.run_docker.sh.swo deleted file mode 100644 index e69de29..0000000 diff --git a/src/rov/rov_cameras/CMakeLists.txt b/src/rov/rov_cameras/CMakeLists.txt index a2111f2..04469aa 100644 --- a/src/rov/rov_cameras/CMakeLists.txt +++ b/src/rov/rov_cameras/CMakeLists.txt @@ -1,66 +1,52 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(rov_cameras) -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(DEBUG_OUTPUT) - add_compile_definitions(DEBUG_OUTPUT=true) -endif() - if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(rov_interfaces REQUIRED) find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(compressed_image_transport REQUIRED) -find_package(OpenCV REQUIRED) - -add_executable(rov_cameras src/main.cpp src/mjpeg_camera.cpp src/h264_camera.cpp src/camera_to_file.cpp) +find_package(PkgConfig REQUIRED) +pkg_search_module(GST REQUIRED gstreamer-1.0>=1.4 + gstreamer-sdp-1.0>=1.4 + gstreamer-video-1.0>=1.4 + gstreamer-app-1.0>=1.4) +pkg_search_module(GST_RTSP REQUIRED gstreamer-rtsp-server-1.0) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(rov_cameras src/cameras.cpp src/main.cpp) target_include_directories(rov_cameras PUBLIC $ - $ - ${OpenCV_INCLUDE_DIRS}) -ament_target_dependencies( - rov_cameras - "rclcpp" - "sensor_msgs" - "std_msgs" - "cv_bridge" - "compressed_image_transport" + $) +target_compile_features(rov_cameras PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(rov_cameras PRIVATE ${GST_INCLUDE_DIRS} ${GST_RTSP_INCLUDE_DIRS}) +target_link_libraries(rov_cameras ${GST_LIBRARIES} ${GST_RTSP_LIBRARIES} gstrtsp-1.0 gstsdp-1.0 gstreamer-1.0 gio-2.0 gobject-2.0 glib-2.0) + +ament_target_dependencies(rov_cameras + rclcpp + rov_interfaces ) -target_link_libraries(rov_cameras ${OpenCV_LIBS} udev pthread) - install(TARGETS rov_cameras DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY include/ - DESTINATION include/) - -install(DIRECTORY launch/ - DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() diff --git a/src/rov/rov_cameras/config/params.yaml b/src/rov/rov_cameras/config/params.yaml new file mode 100644 index 0000000..5d447c9 --- /dev/null +++ b/src/rov/rov_cameras/config/params.yaml @@ -0,0 +1,13 @@ +rov_cameras: + ros__parameters: + # cameras: ["test", "front", "transect"] + cameras: ["test"] + test: + port: "8809" + pipeline: videotestsrc is-live=true ! x264enc speed-preset=ultrafast tune=zerolatency ! rtph264pay name=pay0 pt=96 + front: + port: "8554" + pipeline: nvarguscamerasrc sensor-id=0 ! video/x-raw(memory:NVMM),width=1920,height=1080 ! nvvidconv ! nvv4l2h264enc ! h264parse ! rtph264pay name=pay0 pt=96 + transect: + port: "8555" + pipeline: nvarguscamerasrc sensor-id=1 ! video/x-raw(memory:NVMM),width=1920,height=1080 ! nvvidconv ! nvv4l2h264enc ! h264parse ! rtph264pay name=pay0 pt=96 \ No newline at end of file diff --git a/src/rov/rov_cameras/include/rov_cameras/camera_to_file.hpp b/src/rov/rov_cameras/include/rov_cameras/camera_to_file.hpp deleted file mode 100644 index 10602f4..0000000 --- a/src/rov/rov_cameras/include/rov_cameras/camera_to_file.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef CAMERA_TO_FILE_INCLUDED_HPP -#define CAMERA_TO_FILE_INCLUDED_HPP - -#include -#include -#include - -#define PIPELINE_H264_F_STREAM "gst-launch-1.0 v4l2src device=%s ! nvv4l2decoder ! nvvidconv ! mp4mux ! filesink location=%s" -#define PIPELINE_MJPEG_F_STREAM "gst-launch-1.0 v4l2src device=%s io-mode=2 ! image/jpeg,width=320,height=240,framerate=30/1 ! jpegparse ! jpegdec ! jpegenc ! avimux ! filesink location=%s" - -class File_Camera : public rclcpp::Node { -public: - File_Camera(const std::string& device, uint8_t id, const std::string& file_name, bool mjpeg = false); - ~File_Camera() override; -private: - std::thread thread; -}; - -#endif \ No newline at end of file diff --git a/src/rov/rov_cameras/include/rov_cameras/cameras.hpp b/src/rov/rov_cameras/include/rov_cameras/cameras.hpp new file mode 100644 index 0000000..7bf5984 --- /dev/null +++ b/src/rov/rov_cameras/include/rov_cameras/cameras.hpp @@ -0,0 +1,52 @@ +#ifndef CAMERAS_HPP +#define CAMERAS_HPP + +#include "gst/gst.h" + +#include +#include +#include + +#include + +struct CameraConfig { + std::string id; + std::string port; + std::string pipeline; +}; + +class Camera { +public: + Camera() = default; + Camera(int argc, char **argv, CameraConfig sensorConfig); + +private: + void run_rtsp_stream(int argc, char **argv, CameraConfig sensorConfig); + + std::thread gst_rtsp_server_thread; +}; + +class CameraManager { +public: + static CameraManager *getInstance(int argc, char **argv); + + bool addCamera(CameraConfig sensorConfig); + +private: + CameraManager(int argc, char **argv); + + int argc; + char **argv; + + std::unordered_map cameras; +}; + +class CameraNode : public rclcpp::Node { +public: + CameraNode(int argc, char **argv); + +private: + CameraManager *manager; +}; + +#endif \ No newline at end of file diff --git a/src/rov/rov_cameras/include/rov_cameras/enumerate_cameras.hpp b/src/rov/rov_cameras/include/rov_cameras/enumerate_cameras.hpp deleted file mode 100644 index e32474e..0000000 --- a/src/rov/rov_cameras/include/rov_cameras/enumerate_cameras.hpp +++ /dev/null @@ -1,100 +0,0 @@ -#ifndef ENUMERATE_CAMERAS_INCLUDED_HPP -#define ENUMERATE_CAMERAS_INCLUDED_HPP - -#include -#include - -extern "C" { -#include -#include -#include -#include -#include -#include -#include -} - -#define DELIM " -> " -#define REMOVE "../../" - -struct Camera_Metadata { - std::vector device_names; - std::unordered_map> formats; - v4l2_capability capability; -}; - -// enumerate udev devices that match subsystem video4linux and evaluate them for capture formats and v4l2 capabilities -void enumerateCameras(std::unordered_map& cameras) { - struct udev *udev; - struct udev_enumerate *enumerate; - struct udev_list_entry *devices, *dev_list_entry; - struct udev_device *dev; - - udev = udev_new(); - - enumerate = udev_enumerate_new(udev); - udev_enumerate_add_match_subsystem(enumerate, "video4linux"); - udev_enumerate_scan_devices(enumerate); - devices = udev_enumerate_get_list_entry(enumerate); - - udev_list_entry_foreach(dev_list_entry, devices) { - const char *path; - - path = udev_list_entry_get_name(dev_list_entry); - dev = udev_device_new_from_syspath(udev, path); - // printf("path: %s\n", path); - // printf("serial_id: %s\n", udev_device_get_property_value(dev, "ID_PATH")); - const char* camera_id_path = udev_device_get_property_value(dev, "ID_PATH"); - if(camera_id_path == nullptr) { - continue; - } - - auto it = cameras.find(std::string(camera_id_path)); - if(it != cameras.end()) { - char* dev_name = strdup(udev_device_get_property_value(dev, "DEVNAME")); - // printf("Device Name: %s\n", dev_name); - it->second.device_names.emplace_back(std::string(dev_name)); - free(dev_name); - } else { - cameras.emplace(std::make_pair(std::string(camera_id_path), Camera_Metadata())); - char* dev_name = strdup(udev_device_get_property_value(dev, "DEVNAME")); - // printf("Device Name: %s\n", dev_name); - cameras.at(std::string(camera_id_path)).device_names.emplace_back(std::string(dev_name)); - free(dev_name); - } - - int fd = open(cameras.at(std::string(camera_id_path)).device_names.back().c_str(), O_RDWR); - - v4l2_capability capability; - if(ioctl(fd, VIDIOC_QUERYCAP, &capability) != -1) { - cameras.at(std::string(camera_id_path)).capability = capability; - // printf("\tDriver: %s\n\tCard: %s\n\tBus info: %s\n\tCapabilities: %0X\n", capability.driver, capability.card, capability.bus_info, capability.capabilities); - } - - if(fd != -1) { - std::string format = ""; - v4l2_fmtdesc fmtdesc; - memset(&fmtdesc, 0, sizeof(fmtdesc)); - fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - while(ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0) { - format = format + (char*)fmtdesc.description; - if(format != "") { - cameras.at(std::string(camera_id_path)).formats[cameras.at(std::string(camera_id_path)).device_names.back().c_str()].emplace_back(format); - // printf("Format: %s\n", format.c_str()); - format = ""; - } - fmtdesc.index++; - } - } - close(fd); - - // printf("subsystem: %s\n", udev_device_get_sysattr_value(dev, "subsystem:")); - udev_device_unref(dev); - } - - udev_enumerate_unref(enumerate); - udev_unref(udev); - -} - -#endif \ No newline at end of file diff --git a/src/rov/rov_cameras/include/rov_cameras/h264_camera.hpp b/src/rov/rov_cameras/include/rov_cameras/h264_camera.hpp deleted file mode 100644 index 2941fae..0000000 --- a/src/rov/rov_cameras/include/rov_cameras/h264_camera.hpp +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef H264_CAMERA_INCLUDED_HPP -#define H264_CAMERA_INCLUDED_HPP - -#include -#include -#include -#include -#include - -#include -#include - -#define WIDTH 960 -#define HEIGHT 540 -#define PIPELINE_H264_F "v4l2src device=%s ! nvv4l2decoder ! nvvidconv ! video/x-raw,width=%d,height=%d,format=I420 ! appsink max-buffers=1 drop=1" - -class H264_Camera : public rclcpp::Node { -public: - H264_Camera(std::string device, uint8_t pub_id); -private: - void poll(); - - cv::Mat img_i420 = cv::Mat(HEIGHT*3/2, WIDTH, CV_8UC1); - cv::Mat img_bgr = cv::Mat(HEIGHT,WIDTH,CV_8UC3); - cv::VideoCapture cap; - - sensor_msgs::msg::CameraInfo camera_info_msg; - rclcpp::Publisher::SharedPtr camera_pub; - rclcpp::TimerBase::SharedPtr poll_func; - rclcpp::Publisher::SharedPtr camera_info_pub; - -}; - -#endif diff --git a/src/rov/rov_cameras/include/rov_cameras/mjpeg_camera.hpp b/src/rov/rov_cameras/include/rov_cameras/mjpeg_camera.hpp deleted file mode 100644 index 6cf6c24..0000000 --- a/src/rov/rov_cameras/include/rov_cameras/mjpeg_camera.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef MJPEG_CAMERA_INCLUDED_HPP -#define MJPEG_CAMERA_INCLUDED_HPP - -#include -#include -#include -#include - -#include -#include - -#define PIPELINE_MJPEG_F "v4l2src device=%s io-mode=2 ! image/jpeg,width=320,height=240,framerate=30/1 ! jpegparse ! jpegdec ! nvvidconv ! video/x-raw,format=I420 ! appsink max-buffers=1 drop=true" - -class MJPEG_Camera : public rclcpp::Node { -public: - MJPEG_Camera(std::string device, uint8_t pub_id); -private: - void poll(); - - cv::Mat img_i420 = cv::Mat(240*3/2, 320, CV_8UC1); - cv::Mat img_bgr = cv::Mat(240,320,CV_8UC3); - cv::VideoCapture cap; - rclcpp::TimerBase::SharedPtr poll_func; - rclcpp::Publisher::SharedPtr image_pub; -}; - -#endif diff --git a/src/rov/rov_cameras/launch/rov_cameras_launch.py b/src/rov/rov_cameras/launch/rov_cameras_launch.py deleted file mode 100644 index 5a5b6e7..0000000 --- a/src/rov/rov_cameras/launch/rov_cameras_launch.py +++ /dev/null @@ -1,8 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - -def generate_launch_description(): - return LaunchDescription([ - Node(package="rov_cameras", - executable="rov_cameras") - ]) diff --git a/src/rov/rov_cameras/package.xml b/src/rov/rov_cameras/package.xml index d6cf9dc..3b86076 100644 --- a/src/rov/rov_cameras/package.xml +++ b/src/rov/rov_cameras/package.xml @@ -2,20 +2,14 @@ rov_cameras - 0.0.0 - TODO: Package description + 1.0.0 + Configurable node which uses GStreamer and Nvidia Acceleration (targetting the Orin) to expose cameras over RTSP yameat TODO: License declaration ament_cmake - rclcpp - sensor_msgs - std_msgs - cv_bridge - compressed_image_transport - h264_msgs - opencv2 + rov_interfaces ament_lint_auto ament_lint_common diff --git a/src/rov/rov_cameras/src/camera_to_file.cpp b/src/rov/rov_cameras/src/camera_to_file.cpp deleted file mode 100644 index f06cf4b..0000000 --- a/src/rov/rov_cameras/src/camera_to_file.cpp +++ /dev/null @@ -1,21 +0,0 @@ -#include "rov_cameras/camera_to_file.hpp" - -File_Camera::File_Camera(const std::string& device, uint8_t id, const std::string& file_name, bool mjpeg) : rclcpp::Node(std::string("file_camera") + std::to_string(id)) { - char command[1025] = {0}; - - if(mjpeg) - snprintf(command, sizeof(command)-1, PIPELINE_MJPEG_F_STREAM, device.c_str(), file_name.c_str()); - else - snprintf(command, sizeof(command)-1, PIPELINE_H264_F_STREAM, device.c_str(), file_name.c_str()); - - - auto pipeline = [&](){ - system(command); - }; - - thread = std::thread(pipeline); -} - -File_Camera::~File_Camera() { - thread.~thread(); -} \ No newline at end of file diff --git a/src/rov/rov_cameras/src/cameras.cpp b/src/rov/rov_cameras/src/cameras.cpp new file mode 100644 index 0000000..3ea5fac --- /dev/null +++ b/src/rov/rov_cameras/src/cameras.cpp @@ -0,0 +1,104 @@ +#include "rov_cameras/cameras.hpp" + +#include "gst/rtsp-server/rtsp-server.h" +#include +#include + +Camera::Camera(int argc, char **argv, CameraConfig sensorConfig) { + gst_rtsp_server_thread = std::thread( + std::bind(&Camera::run_rtsp_stream, this, argc, argv, sensorConfig)); + gst_rtsp_server_thread.detach(); +} + +void Camera::run_rtsp_stream(int argc, char **argv, CameraConfig sensorConfig) { + GMainLoop *loop; + GstRTSPServer *server; + GstRTSPMountPoints *mounts; + GstRTSPMediaFactory *factory; + + gst_init(&argc, &argv); + + // create main loop to spin the server + loop = g_main_loop_new(NULL, false); + + // create new server + server = gst_rtsp_server_new(); + g_object_set(server, "service", sensorConfig.port.c_str(), NULL); + + // get mount points to map uri to media factory + mounts = gst_rtsp_server_get_mount_points(server); + + // media factory to create pipeline, enables rtcp + factory = gst_rtsp_media_factory_new(); + + // apply the pertinent pipeline + gst_rtsp_media_factory_set_launch(factory, sensorConfig.pipeline.c_str()); + + gst_rtsp_media_factory_set_shared(factory, true); + gst_rtsp_media_factory_set_enable_rtcp(factory, true); + + // attach this factory to the mount point + gst_rtsp_mount_points_add_factory(mounts, "/video", factory); + + // dont need the pointer anymore + g_object_unref(mounts); + + // attach server to default context + gst_rtsp_server_attach(server, NULL); + + // serve that server + std::string msg = + "Now serving RTSP video stream '" + sensorConfig.id + "' on 127.0.0.1:" + sensorConfig.port + + "/video"; + RCLCPP_INFO(rclcpp::get_logger("rtsp_streamer"), msg.c_str()); + g_main_loop_run(loop); +} + +CameraManager *CameraManager::getInstance(int argc, char **argv) { + static CameraManager instance{argc, argv}; + return &instance; +} + +CameraManager::CameraManager(int argc, char **argv) : argc(argc), argv(argv) {} + +bool CameraManager::addCamera(CameraConfig sensorConfig) { + if (cameras.count(sensorConfig.id) > 0) { + RCLCPP_WARN(rclcpp::get_logger("rtsp_streamer"), + "Attempted to overwrite sensor, ignoring"); + return false; + } + + cameras[sensorConfig.id] = Camera(argc, argv, sensorConfig); + return true; +} + +CameraNode::CameraNode(int argc, char **argv) : Node("rov_cameras") { + manager = CameraManager::getInstance(argc, argv); + this->declare_parameter("cameras", std::vector()); + + std::vector camera_ids = + get_parameter("cameras").as_string_array(); + if (camera_ids.size() == 0) { + RCLCPP_ERROR( + rclcpp::get_logger("rtsp_streamer"), + "No cameras found, perhaps you forgot to add the params.yaml file?"); + } + for (auto &name : camera_ids) { + this->declare_parameter(name + ".port", ""); + this->declare_parameter(name + ".pipeline", ""); + + CameraConfig config; + config.id = name; + config.port = this->get_parameter(name + ".port").value_to_string(); + config.pipeline = this->get_parameter(name + ".pipeline").as_string(); + if (config.port == "" || config.pipeline == "") { + RCLCPP_ERROR(rclcpp::get_logger("rtsp_streamer"), + "Missing or empty parameters for camera '%s'", name.c_str()); + continue; + } + if (!manager->addCamera(config)) { + RCLCPP_ERROR(rclcpp::get_logger("rtsp_streamer"), + "Failed to add camera %s", name.c_str()); + } + } +} \ No newline at end of file diff --git a/src/rov/rov_cameras/src/h264_camera.cpp b/src/rov/rov_cameras/src/h264_camera.cpp deleted file mode 100644 index defce06..0000000 --- a/src/rov/rov_cameras/src/h264_camera.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "rov_cameras/h264_camera.hpp" - -#include -#include - -namespace { -void declareParameters(rclcpp::Node* node) { - -} -} - -H264_Camera::H264_Camera(std::string device, uint8_t pub_id) : rclcpp::Node(std::string("camera_h264") + std::to_string(static_cast(pub_id))) { - declareParameters(this); - - camera_pub = this->create_publisher(std::string("camera_h264") + std::to_string(static_cast(pub_id)) + std::string("/image_raw/compressed"), 10); - poll_func = this->create_wall_timer(std::chrono::milliseconds(1000/30), std::bind(&H264_Camera::poll, this)); - - char pipeline[1025] = {0}; - snprintf(pipeline, sizeof(pipeline)-1, PIPELINE_H264_F, device.c_str(), WIDTH, HEIGHT); - cap = cv::VideoCapture(std::string(pipeline), cv::CAP_GSTREAMER); - RCLCPP_INFO(this->get_logger(), "Device %s initialized as H264 Camera", device.c_str()); -} - -void H264_Camera::poll() { - cap.grab(); - cap.retrieve(this->img_i420); - cv::cvtColor(this->img_i420, this->img_bgr, cv::COLOR_YUV2BGR_I420, 3); - sensor_msgs::msg::CompressedImage msg; - std_msgs::msg::Header header; - header.set__stamp(this->now()); - cv_bridge::CvImage img_bridge(header, sensor_msgs::image_encodings::BGR8, this->img_bgr); - img_bridge.toCompressedImageMsg(msg, cv_bridge::Format::PNG); - camera_pub->publish(msg); -} \ No newline at end of file diff --git a/src/rov/rov_cameras/src/main.cpp b/src/rov/rov_cameras/src/main.cpp index 0422e12..d353c62 100644 --- a/src/rov/rov_cameras/src/main.cpp +++ b/src/rov/rov_cameras/src/main.cpp @@ -1,136 +1,9 @@ -#if DEBUG_OUTPUT - #include -#endif - -#include -#include -#include - -#include - -#include "rov_cameras/enumerate_cameras.hpp" -#include "rov_cameras/h264_camera.hpp" -#include "rov_cameras/mjpeg_camera.hpp" -#include "rov_cameras/camera_to_file.hpp" +#include "rov_cameras/cameras.hpp" int main(int argc, char** argv) { - rclcpp::init(argc, argv); - std::unordered_map camera_metadata; - enumerateCameras(camera_metadata); - - // sets simplify my life when parsing - std::set mjpeg_cameras; - std::set mjpeg_devices; - std::set h264_cameras; - std::set h264_devices; - -#if DEBUG_OUTPUT - std::stringstream ss; -#endif - // for each camera enumerated - for(auto c : camera_metadata) { -#if DEBUG_OUTPUT - ss << "Devices for camera: " << c.first.c_str() << '\n'; - // printf("Devices for camera %s\n", c.first.c_str()); -#endif - // for each v4l2 device associated with each camera - for(auto d : c.second.device_names) { -#if DEBUG_OUTPUT - ss << '\t' << d.c_str() << '\n'; - // printf("\t%s\n", d.c_str()); -#endif - try { - // for each format associated with each v4l2 device - for(auto f : c.second.formats.at(d)) { -#if DEBUG_OUTPUT - ss << "\t\t" << f.c_str() << '\n'; - // printf("\t\t%s\n", f.c_str()); -#endif - if(f == "Motion-JPEG") { - mjpeg_devices.emplace(d); - mjpeg_cameras.emplace(c.first); - } - - if(f == "H.264") { - h264_devices.emplace(d); - h264_cameras.emplace(c.first); - } - } - } catch ([[maybe_unused]] std::out_of_range& e) {} - } - } -#if DEBUG_OUTPUT - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "%s\n", ss.str().c_str()); -#endif - - // calculate the intersection of cameras that support mjpeg and h264 - // yeah i do set theory (⌐□_□) - std::vector camera_intersection; - std::set_intersection(mjpeg_cameras.begin(), mjpeg_cameras.end(), h264_cameras.begin(), h264_cameras.end(), std::inserter(camera_intersection, camera_intersection.begin())); - - // prefer h264 - for(auto cam : camera_intersection) { - mjpeg_cameras.erase(cam); - for(auto dev : camera_metadata.at(cam).device_names) { - auto it = std::find(mjpeg_devices.begin(), mjpeg_devices.end(), dev); - if(it != mjpeg_devices.end()) { - mjpeg_devices.erase(*it); - } - } - } - -#if DEBUG_OUTPUT - printf("\n\nMJPEG Cameras\n"); - for(auto cam : mjpeg_cameras) { - printf("\t%s\n", cam.c_str()); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "MJPEG Camera: %s", cam.c_str()); - } - - printf("MJPEG Devices\n"); - for(auto dev : mjpeg_devices) { - printf("\t%s\n", dev.c_str()); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "MJPEG Device: %s", dev.c_str()); - } - - printf("H264 Cameras\n"); - for(auto cam : h264_cameras) { - printf("\t%s\n", cam.c_str()); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "H264 Camera: %s", cam.c_str()); - } - - printf("H264 Devices\n"); - for(auto dev : h264_devices) { - printf("\t%s\n", dev.c_str()); - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "H264 Device: %s", dev.c_str()); - } -#endif - - // add all cameras to multithreaded executor - rclcpp::executors::MultiThreadedExecutor exec; - std::vector mjpeg_nodes; - std::vector h264_nodes; - std::vector file_nodes; - - int camera_idx = 0; - for(auto device : mjpeg_devices) { - mjpeg_nodes.emplace_back(std::make_shared(device, camera_idx)); - file_nodes.emplace_back(std::make_shared(device, camera_idx, "bottom_cam.avi")); - exec.add_node(mjpeg_nodes.back()); - exec.add_node(file_nodes.back()); - camera_idx++; - } - for(auto device : h264_devices) { - h264_nodes.emplace_back(std::make_shared(device, camera_idx)); - file_nodes.emplace_back(std::make_shared(device, camera_idx, "front_cam.mp4")); - exec.add_node(h264_nodes.back()); - exec.add_node(file_nodes.back()); - camera_idx++; - } - - exec.spin(); + rclcpp::spin(std::make_shared(argc, argv)); rclcpp::shutdown(); - return 0; } \ No newline at end of file diff --git a/src/rov/rov_cameras/src/mjpeg_camera.cpp b/src/rov/rov_cameras/src/mjpeg_camera.cpp deleted file mode 100644 index 84e082e..0000000 --- a/src/rov/rov_cameras/src/mjpeg_camera.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include "rov_cameras/mjpeg_camera.hpp" - -#include -#include - -MJPEG_Camera::MJPEG_Camera(std::string device, uint8_t pub_id) : rclcpp::Node(std::string("camera_mjpeg") + std::to_string(static_cast(pub_id))) { - image_pub = this->create_publisher(std::string("camera_mjpeg") + std::to_string(static_cast(pub_id)) + std::string("/image_raw/compressed"), 10); - poll_func = this->create_wall_timer(std::chrono::milliseconds(1000/30), std::bind(&MJPEG_Camera::poll, this)); - - char pipeline[1025] = {0}; - snprintf(pipeline, sizeof(pipeline)-1, PIPELINE_MJPEG_F, device.c_str()); - cap = cv::VideoCapture(std::string(pipeline), cv::CAP_GSTREAMER); - RCLCPP_INFO(this->get_logger(), "Device %s initialized as MJPEG Camera", device.c_str()); -} - -void MJPEG_Camera::poll() { - cap.grab(); - cap.retrieve(this->img_i420); - cv::cvtColor(this->img_i420, this->img_bgr, cv::COLOR_YUV2BGR_I420, 3); - sensor_msgs::msg::CompressedImage msg; - std_msgs::msg::Header header; - header.set__stamp(this->now()); - cv_bridge::CvImage img_bridge(header, sensor_msgs::image_encodings::BGR8, this->img_bgr); - img_bridge.toCompressedImageMsg(msg, cv_bridge::Format::PNG); - image_pub->publish(msg); -} diff --git a/src/rov/rov_interfaces/CMakeLists.txt b/src/rov/rov_interfaces/CMakeLists.txt index 322eca6..f1a0de8 100644 --- a/src/rov/rov_interfaces/CMakeLists.txt +++ b/src/rov/rov_interfaces/CMakeLists.txt @@ -34,6 +34,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/VectorD.msg" "msg/BNO055Data.msg" "msg/TSYS01Data.msg" + "srv/CameraStreamCommand.srv" "srv/CreateContinuousServo.srv" "srv/CreateServo.srv" DEPENDENCIES diff --git a/src/rov/rov_interfaces/srv/CameraStreamCommand.srv b/src/rov/rov_interfaces/srv/CameraStreamCommand.srv new file mode 100644 index 0000000..595de11 --- /dev/null +++ b/src/rov/rov_interfaces/srv/CameraStreamCommand.srv @@ -0,0 +1,4 @@ +uint8 camera_id +uint8 command +--- +bool result \ No newline at end of file diff --git a/src/rov/rov_launch/launch/rov.launch.py b/src/rov/rov_launch/launch/rov.launch.py index 31a513d..324fa80 100644 --- a/src/rov/rov_launch/launch/rov.launch.py +++ b/src/rov/rov_launch/launch/rov.launch.py @@ -11,10 +11,10 @@ def generate_launch_description(): package="rov_gpio", executable="rov_gpio" ) - bar02 = Node( - package="bar02", - executable="bar02" - ) + # bar02 = Node( + # package="bar02", + # executable="bar02" + # ) bno055 = Node( package="rov_bno055", executable="bno055_node" @@ -31,7 +31,9 @@ def generate_launch_description(): ) cameras = Node( package="rov_cameras", - executable="rov_cameras" + executable="rov_cameras", + name="rov_cameras", + parameters=[os.path.join(get_package_share_directory("rov_cameras"), "config", "params.yaml")], ) control = Node( package="rov_control", @@ -44,6 +46,7 @@ def generate_launch_description(): executable="manipulator_controller", parameters=[os.path.join(get_package_share_directory("rov_manipulator_controller"), "config", "params.yaml")] ) + led_controller = Node( package="rov_led_controller", executable="led_controller", @@ -55,7 +58,7 @@ def generate_launch_description(): ld.add_action(pca9685) ld.add_action(cameras) ld.add_action(control) - ld.add_action(bar02) + # ld.add_action(bar02) ld.add_action(gpio) ld.add_action(manipulator_controller) ld.add_action(led_controller)