From c47d4e2903b099a89958f06e27e1326c64bdbf7e Mon Sep 17 00:00:00 2001 From: Corwin Perren Date: Sun, 4 Mar 2018 19:59:07 -0800 Subject: [PATCH] Started rewrite of rover_camera so we can view rtsp streams and control which streams are enabled. Changed senders / receivers for new camera. --- .gitignore | 3 + .../ros_packages/rover_camera/CMakeLists.txt | 18 +- .../rover_camera/msg/CameraControlMessage.msg | 3 + .../rover_camera/src/rover_camera.cpp | 170 ++++++++++++------ .../rover_camera/src/rover_camera_old.cpp | 88 +++++++++ .../topic_transport_receivers.launch | 15 +- .../rover_main/launch/rover/cameras.launch | 6 + .../rover/topic_transport_senders.launch | 22 ++- 8 files changed, 261 insertions(+), 64 deletions(-) create mode 100644 software/ros_packages/rover_camera/msg/CameraControlMessage.msg create mode 100644 software/ros_packages/rover_camera/src/rover_camera_old.cpp diff --git a/.gitignore b/.gitignore index 015af31..85bc658 100644 --- a/.gitignore +++ b/.gitignore @@ -101,3 +101,6 @@ ground_station/resources/core/key # Don't commit key file key + +# Remove build folder +cmake-build-debug/ diff --git a/software/ros_packages/rover_camera/CMakeLists.txt b/software/ros_packages/rover_camera/CMakeLists.txt index e3358a1..bd6a446 100644 --- a/software/ros_packages/rover_camera/CMakeLists.txt +++ b/software/ros_packages/rover_camera/CMakeLists.txt @@ -3,9 +3,25 @@ project(rover_camera) add_compile_options(-std=c++11) -find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport message_generation sensor_msgs) +find_package(catkin REQUIRED COMPONENTS + cv_bridge + image_transport + message_generation + sensor_msgs + std_msgs + message_generation +) # add the resized image message +add_message_files( + FILES + CameraControlMessage.msg +) + +generate_messages( + DEPENDENCIES + std_msgs +) catkin_package(CATKIN_DEPENDS cv_bridge image_transport message_runtime sensor_msgs) diff --git a/software/ros_packages/rover_camera/msg/CameraControlMessage.msg b/software/ros_packages/rover_camera/msg/CameraControlMessage.msg new file mode 100644 index 0000000..2e05d9a --- /dev/null +++ b/software/ros_packages/rover_camera/msg/CameraControlMessage.msg @@ -0,0 +1,3 @@ +bool enable_small_broadcast +bool enable_medium_broadcast +bool enable_large_broadcast \ No newline at end of file diff --git a/software/ros_packages/rover_camera/src/rover_camera.cpp b/software/ros_packages/rover_camera/src/rover_camera.cpp index 669d9dd..87e2e62 100644 --- a/software/ros_packages/rover_camera/src/rover_camera.cpp +++ b/software/ros_packages/rover_camera/src/rover_camera.cpp @@ -9,11 +9,105 @@ #include -int main(int argc, char** argv) -{ +class RoverCamera{ +public: + RoverCamera(int argc, char** argv){ + ros::init(argc, argv, "camera"); + + node_handle = new ros::NodeHandle("~"); + + node_handle->param("is_rtsp_camera", is_rtsp_camera, false); + node_handle->param("device_path", capture_device_path, std::string("/dev/video0")); + node_handle->param("fps", fps, 30); + + node_handle->param("large_image_width", large_image_width, 1280); + node_handle->param("large_image_height", large_image_height, 720); + node_handle->param("medium_image_width", medium_image_width, 640); + node_handle->param("medium_image_height", medium_image_height, 360); + node_handle->param("small_image_width", small_image_width, 256); + node_handle->param("small_image_height", small_image_height, 144); + + broadcast_large_image = true; + broadcast_medium_image = false; + broadcast_small_image = false; + + if(is_rtsp_camera){ + cap = new cv::VideoCapture(capture_device_path); + + }else{ + cap = new cv::VideoCapture(capture_device_path); + + cap->set(CV_CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G')); + cap->set(CV_CAP_PROP_FRAME_WIDTH, large_image_width); + cap->set(CV_CAP_PROP_FRAME_HEIGHT, large_image_height); + cap->set(CV_CAP_PROP_FPS, fps); + } + + large_image_node_name = "image_" + std::to_string(large_image_width) + "x" + std::to_string(large_image_height); + medium_image_node_name = "image_" + std::to_string(medium_image_width) + "x" + std::to_string(medium_image_height); + small_image_node_name = "image_" + std::to_string(small_image_width) + "x" + std::to_string(small_image_height); + + large_image_transport = new image_transport::ImageTransport(*node_handle); + medium_image_transport = new image_transport::ImageTransport(*node_handle); + small_image_transport = new image_transport::ImageTransport(*node_handle); + + large_image_publisher = large_image_transport->advertise(large_image_node_name, 1); + medium_image_publisher = medium_image_transport->advertise(medium_image_node_name, 1); + small_image_publisher = small_image_transport->advertise(small_image_node_name, 1); + + if(is_rtsp_camera){ + loop_rate = new ros::Rate(fps + 10); + }else{ + loop_rate = new ros::Rate(fps + 2); + } + } + + void run(){ + if(!cap->isOpened()){ + std::cout << "Could not open device: " << capture_device_path << std::endl; + return; + } + + while (ros::ok()) { + + cap->read(image_large); + + if(!image_large.empty()){ + cv::resize(image_large, image_medium, cv::Size(medium_image_width, medium_image_height)); + cv::resize(image_medium, image_small, cv::Size(small_image_width, small_image_height)); + + sensor_msgs::ImagePtr large_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_large).toImageMsg(); + sensor_msgs::ImagePtr medium_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_medium).toImageMsg(); + sensor_msgs::ImagePtr small_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_small).toImageMsg(); + + large_image_publisher.publish(large_image_message); + medium_image_publisher.publish(medium_image_message); + small_image_publisher.publish(small_image_message); + } + + ros::spinOnce(); + loop_rate->sleep(); + } + } + + ~RoverCamera(){ + if(cap->isOpened()){ + cap->release(); + } + } + +private: + ros::NodeHandle *node_handle; + + cv::VideoCapture *cap; + + bool is_rtsp_camera; + std::string capture_device_path; int fps; + ros::Rate *loop_rate; + int large_image_width; int large_image_height; int medium_image_width; @@ -21,68 +115,30 @@ int main(int argc, char** argv) int small_image_width; int small_image_height; - ros::init(argc, argv, "camera"); - ros::NodeHandle node_handle("~"); + bool broadcast_large_image; + bool broadcast_medium_image; + bool broadcast_small_image; - node_handle.param("device_path", capture_device_path, std::string("/dev/video0")); - node_handle.param("fps", fps, 30); + std::string large_image_node_name; + std::string medium_image_node_name; + std::string small_image_node_name; - node_handle.param("large_image_width", large_image_width, 1280); - node_handle.param("large_image_height", large_image_height, 720); - node_handle.param("medium_image_width", medium_image_width, 640); - node_handle.param("medium_image_height", medium_image_height, 360); - node_handle.param("small_image_width", small_image_width, 256); - node_handle.param("small_image_height", small_image_height, 144); + image_transport::ImageTransport *large_image_transport; + image_transport::ImageTransport *medium_image_transport; + image_transport::ImageTransport *small_image_transport; - cv::VideoCapture cap(capture_device_path); - - cap.set(CV_CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G')); - cap.set(CV_CAP_PROP_FRAME_WIDTH, large_image_width); - cap.set(CV_CAP_PROP_FRAME_HEIGHT, large_image_height); - cap.set(CV_CAP_PROP_FPS, fps); - - if(cap.isOpened() == false){ - return -1; - } - - std::string large_image_node_name = "image_" + std::to_string(large_image_width) + "x" + std::to_string(large_image_height); - std::string medium_image_node_name = "image_" + std::to_string(medium_image_width) + "x" + std::to_string(medium_image_height); - std::string small_image_node_name = "image_" + std::to_string(small_image_width) + "x" + std::to_string(small_image_height); - - image_transport::ImageTransport large_image_transport(node_handle); - image_transport::ImageTransport medium_image_transport(node_handle); - image_transport::ImageTransport small_image_transport(node_handle); - - image_transport::Publisher large_image_publisher = large_image_transport.advertise(large_image_node_name, 1); - image_transport::Publisher medium_image_publisher = medium_image_transport.advertise(medium_image_node_name, 1); - image_transport::Publisher small_image_publisher = small_image_transport.advertise(small_image_node_name, 1); + image_transport::Publisher large_image_publisher; + image_transport::Publisher medium_image_publisher; + image_transport::Publisher small_image_publisher; cv::Mat image_large; cv::Mat image_medium; cv::Mat image_small; - ros::Rate loop_rate(fps + 2); +}; - while (ros::ok()) { - - cap.read(image_large); - - if(!image_large.empty()){ - cv::resize(image_large, image_medium, cv::Size(medium_image_width, medium_image_height)); - cv::resize(image_medium, image_small, cv::Size(small_image_width, small_image_height)); - - sensor_msgs::ImagePtr large_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_large).toImageMsg(); - sensor_msgs::ImagePtr medium_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_medium).toImageMsg(); - sensor_msgs::ImagePtr small_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_small).toImageMsg(); - - large_image_publisher.publish(large_image_message); - medium_image_publisher.publish(medium_image_message); - small_image_publisher.publish(small_image_message); - } - - ros::spinOnce(); - loop_rate.sleep(); - } - - cap.release(); +int main(int argc, char** argv) +{ + RoverCamera camera(argc, argv); + camera.run(); } diff --git a/software/ros_packages/rover_camera/src/rover_camera_old.cpp b/software/ros_packages/rover_camera/src/rover_camera_old.cpp new file mode 100644 index 0000000..032a9f7 --- /dev/null +++ b/software/ros_packages/rover_camera/src/rover_camera_old.cpp @@ -0,0 +1,88 @@ +#include +#include + +#include +#include +#include +#include +#include + +#include + +int main(int argc, char** argv) +{ + std::string capture_device_path; + int fps; + + int large_image_width; + int large_image_height; + int medium_image_width; + int medium_image_height; + int small_image_width; + int small_image_height; + + ros::init(argc, argv, "camera"); + ros::NodeHandle node_handle("~"); + + node_handle.param("device_path", capture_device_path, std::string("/dev/video0")); + node_handle.param("fps", fps, 30); + + node_handle.param("large_image_width", large_image_width, 1280); + node_handle.param("large_image_height", large_image_height, 720); + node_handle.param("medium_image_width", medium_image_width, 640); + node_handle.param("medium_image_height", medium_image_height, 360); + node_handle.param("small_image_width", small_image_width, 256); + node_handle.param("small_image_height", small_image_height, 144); + + cv::VideoCapture cap(capture_device_path); + + cap.set(CV_CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G')); + cap.set(CV_CAP_PROP_FRAME_WIDTH, large_image_width); + cap.set(CV_CAP_PROP_FRAME_HEIGHT, large_image_height); + cap.set(CV_CAP_PROP_FPS, fps); + + if(!cap.isOpened()){ + return -1; + } + + std::string large_image_node_name = "image_" + std::to_string(large_image_width) + "x" + std::to_string(large_image_height); + std::string medium_image_node_name = "image_" + std::to_string(medium_image_width) + "x" + std::to_string(medium_image_height); + std::string small_image_node_name = "image_" + std::to_string(small_image_width) + "x" + std::to_string(small_image_height); + + image_transport::ImageTransport large_image_transport(node_handle); + image_transport::ImageTransport medium_image_transport(node_handle); + image_transport::ImageTransport small_image_transport(node_handle); + + image_transport::Publisher large_image_publisher = large_image_transport.advertise(large_image_node_name, 1); + image_transport::Publisher medium_image_publisher = medium_image_transport.advertise(medium_image_node_name, 1); + image_transport::Publisher small_image_publisher = small_image_transport.advertise(small_image_node_name, 1); + + cv::Mat image_large; + cv::Mat image_medium; + cv::Mat image_small; + + ros::Rate loop_rate(fps + 2); + + while (ros::ok()) { + + cap.read(image_large); + + if(!image_large.empty()){ + cv::resize(image_large, image_medium, cv::Size(medium_image_width, medium_image_height)); + cv::resize(image_medium, image_small, cv::Size(small_image_width, small_image_height)); + + sensor_msgs::ImagePtr large_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_large).toImageMsg(); + sensor_msgs::ImagePtr medium_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_medium).toImageMsg(); + sensor_msgs::ImagePtr small_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_small).toImageMsg(); + + large_image_publisher.publish(large_image_message); + medium_image_publisher.publish(medium_image_message); + small_image_publisher.publish(small_image_message); + } + + ros::spinOnce(); + loop_rate.sleep(); + } + + cap.release(); +} diff --git a/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch b/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch index 5cc8a5b..12af19c 100644 --- a/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch +++ b/software/ros_packages/rover_main/launch/ground_station/topic_transport_receivers.launch @@ -12,16 +12,25 @@ - + - + - + + + + + + + + + + diff --git a/software/ros_packages/rover_main/launch/rover/cameras.launch b/software/ros_packages/rover_main/launch/rover/cameras.launch index 7afa57f..8ac5550 100644 --- a/software/ros_packages/rover_main/launch/rover/cameras.launch +++ b/software/ros_packages/rover_main/launch/rover/cameras.launch @@ -14,5 +14,11 @@ + + + + + + diff --git a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch index db549b5..e38efc7 100644 --- a/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch +++ b/software/ros_packages/rover_main/launch/rover/topic_transport_senders.launch @@ -26,9 +26,17 @@ - + + + [{name: "/cameras/end_effector/image_640x360/compressed", compress: false, rate: 30.0}] + + + + + + [{name: "/cameras/chassis/image_256x144/compressed", compress: false, rate: 30.0}] @@ -36,7 +44,7 @@ - + [{name: "/cameras/undercarriage/image_256x144/compressed", compress: false, rate: 30.0}] @@ -44,10 +52,18 @@ - + [{name: "/cameras/main_navigation/image_256x144/compressed", compress: false, rate: 30.0}] + + + + + + [{name: "/cameras/end_effector/image_256x144/compressed", compress: false, rate: 30.0}] + +