From 2c4d9b85a3d66bce3c07da87f8cff3fff5c3e28e Mon Sep 17 00:00:00 2001
From: Lucas Walter <wsacul@gmail.com>
Date: Mon, 23 Nov 2020 12:03:20 -0800
Subject: [PATCH] Subscribe to an image and print out msg fields, but not
 manipulating the data yet.

---
 ros/rsc_test/CMakeLists.txt                   | 12 ++---
 ros/rsc_test/launch/images.launch             | 15 ++++++
 ros/rsc_test/rust_src/Cargo.toml              |  9 +++-
 .../rust_src/src/image_sub_pub/main.rs        | 49 +++++++++++++++++++
 ros/rsc_test/src/image_sub_pub.cpp            |  2 +
 5 files changed, 79 insertions(+), 8 deletions(-)
 create mode 100644 ros/rsc_test/launch/images.launch
 create mode 100644 ros/rsc_test/rust_src/src/image_sub_pub/main.rs
 create mode 100644 ros/rsc_test/src/image_sub_pub.cpp

diff --git a/ros/rsc_test/CMakeLists.txt b/ros/rsc_test/CMakeLists.txt
index 3352329..3e420c0 100644
--- a/ros/rsc_test/CMakeLists.txt
+++ b/ros/rsc_test/CMakeLists.txt
@@ -63,14 +63,13 @@ include_directories(
 # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 add_executable(hello_world2 src/hello_world2.cpp)
-target_link_libraries(hello_world2
-  ${catkin_LIBRARIES}
-)
+target_link_libraries(hello_world2 ${catkin_LIBRARIES})
 
 add_executable(test_pub_sub src/test_pub_sub.cpp)
-target_link_libraries(test_pub_sub
-  ${catkin_LIBRARIES}
-)
+target_link_libraries(test_pub_sub ${catkin_LIBRARIES})
+
+# add_executable(image_sub_pub src/image_sub_pub.cpp)
+# target_link_libraries(image_sub_pub ${catkin_LIBRARIES})
 
 # all install targets should use catkin DESTINATION variables
 # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
@@ -93,6 +92,7 @@ endfunction(rust_target)
 
 rust_target(hello_world1)
 rust_target(log_test)
+rust_target(image_sub_pub)
 
 # target is imported and does not build here
 # add_custom_command(TARGET hello_world1 POST_BUILD
diff --git a/ros/rsc_test/launch/images.launch b/ros/rsc_test/launch/images.launch
new file mode 100644
index 0000000..60b1d7c
--- /dev/null
+++ b/ros/rsc_test/launch/images.launch
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+
+  <node pkg="image_publisher" type="image_publisher"
+    name="image_pub" output="screen"
+    args="$(find image_manip)/data/mosaic.png" >
+    <param name="publish_rate" value="1.0"/>
+  </node>
+
+  <node pkg="rsc_test" type="image_sub_pub"
+    name="image_sub_pub" output="screen" >
+    <remap from="image_in" to="image_pub/image_raw" />
+  </node>
+
+</launch>
diff --git a/ros/rsc_test/rust_src/Cargo.toml b/ros/rsc_test/rust_src/Cargo.toml
index b2a41e9..1637c29 100644
--- a/ros/rsc_test/rust_src/Cargo.toml
+++ b/ros/rsc_test/rust_src/Cargo.toml
@@ -14,8 +14,13 @@ path = "src/hello_world/main.rs"
 name = "log_test"
 path = "src/loginfo/main.rs"
 
+[[bin]]
+name = "image_sub_pub"
+path = "src/image_sub_pub/main.rs"
+
 [dependencies]
-rosrust = "0.9.5"
-rosrust_msg = "0.1.1"
+rosrust = { git = "https://github.com/lucasw/rosrust", branch = "noetic_aggregated" }
+rosrust_msg = { git = "https://github.com/lucasw/rosrust", branch = "noetic_aggregated" }
+# rosrust_msg = "0.1.1"
 # rosrust = { path = "../../../../rosrust/rosrust" }
 # rosrust_msg = { path = "../../../../rosrust/rosrust_msg" }
diff --git a/ros/rsc_test/rust_src/src/image_sub_pub/main.rs b/ros/rsc_test/rust_src/src/image_sub_pub/main.rs
new file mode 100644
index 0000000..7d97554
--- /dev/null
+++ b/ros/rsc_test/rust_src/src/image_sub_pub/main.rs
@@ -0,0 +1,49 @@
+// Subscribe to an image topic and publish a modified version of the image in response.
+// For now modify the image directly in the callback and publish that output,
+// but later maybe store the image and then modify it in a separate thread.
+//
+
+use rosrust_msg::sensor_msgs::Image;
+use rosrust::api::raii as ros;
+// use std::sync::Mutex;
+
+struct ImageSubPub {
+    _image_sub: ros::Subscriber,  // <Image>
+    // image_pub: ros::Publisher<Image>,
+    // TODO(lucasw) add a variable float to scale the incoming image with
+    // fr : f32,
+}
+
+
+impl ImageSubPub {
+    fn new() -> Self{
+        rosrust::ros_info!("new ImageSubPub");
+        let image_pub = rosrust::publish("image_out", 4).unwrap();
+        let fr = 2.5;
+
+        let image_callback = {
+            move |msg: Image| {
+                rosrust::ros_info!("preferred callback {} {} {} {} {}",
+                                   msg.width, msg.height, msg.data.len(), msg.encoding,
+                                   fr);
+                image_pub.send(msg).unwrap();
+            }
+        };
+
+        let _image_sub = rosrust::subscribe("image_in", 4, image_callback).unwrap();
+
+        let image_sub_pub = Self {
+            _image_sub,
+            // image_pub,
+            // fr,
+        };
+
+        image_sub_pub
+    }
+}
+
+fn main() {
+    rosrust::init("image_sub_pub_rs");
+    let _image_sub_pub = ImageSubPub::new();
+    rosrust::spin();
+}
diff --git a/ros/rsc_test/src/image_sub_pub.cpp b/ros/rsc_test/src/image_sub_pub.cpp
new file mode 100644
index 0000000..d35c159
--- /dev/null
+++ b/ros/rsc_test/src/image_sub_pub.cpp
@@ -0,0 +1,2 @@
+// Subscribe to an image topic and publish a modified version of the image in response.
+// For now modify the image directly in the callback and publish that output.