From c49b90b038ad2b31b2b1643f9f55f86c5cdfa658 Mon Sep 17 00:00:00 2001
From: Corwin Perren <caperren@gmail.com>
Date: Sat, 20 Jan 2018 17:16:01 -0800
Subject: [PATCH] Added thrid resolution for cameras

---
 rover/rover_camera/src/rover_camera.cpp | 44 +++++++++++++++----------
 1 file changed, 27 insertions(+), 17 deletions(-)

diff --git a/rover/rover_camera/src/rover_camera.cpp b/rover/rover_camera/src/rover_camera.cpp
index a42588a..669d9dd 100644
--- a/rover/rover_camera/src/rover_camera.cpp
+++ b/rover/rover_camera/src/rover_camera.cpp
@@ -16,6 +16,8 @@ int main(int argc, char** argv)
 
     int large_image_width;
     int large_image_height;
+    int medium_image_width;
+    int medium_image_height;
     int small_image_width;
     int small_image_height;
 
@@ -27,8 +29,10 @@ int main(int argc, char** argv)
 
     node_handle.param("large_image_width", large_image_width, 1280);
     node_handle.param("large_image_height", large_image_height, 720);
-    node_handle.param("small_image_width", small_image_width, 640);
-    node_handle.param("small_image_height", small_image_height, 360);
+    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);
 
@@ -42,32 +46,38 @@ int main(int argc, char** argv)
     }
 
     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 full_res_image_transport(node_handle);
-    image_transport::ImageTransport lower_res_image_transport(node_handle);
+    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 full_size_publisher = full_res_image_transport.advertise(large_image_node_name, 1);
-    image_transport::Publisher lower_size_publisher = lower_res_image_transport.advertise(small_image_node_name, 1);
+    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;
-    cv::Mat image_smaller;
-
-    ros::Rate loop_rate(fps + 5);
+    cv::Mat image_large;
+    cv::Mat image_medium;
+    cv::Mat image_small;
 
+    ros::Rate loop_rate(fps + 2);
 
     while (ros::ok()) {
 
-        cap.read(image);
+        cap.read(image_large);
 
-        if(!image.empty()){
-            cv::resize(image, image_smaller, cv::Size(small_image_width, small_image_height));
+        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 full_res_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
-            sensor_msgs::ImagePtr lower_res_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_smaller).toImageMsg();
+            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();
 
-            full_size_publisher.publish(full_res_message);
-            lower_size_publisher.publish(lower_res_message);
+            large_image_publisher.publish(large_image_message);
+            medium_image_publisher.publish(medium_image_message);
+            small_image_publisher.publish(small_image_message);
         }
 
         ros::spinOnce();