Allow showimage and cam2image to subscribe/publish to alternate topic…

…s. (#117)

* Allow showimage and cam2image to subscribe/publish to alternate topics.

This allows them to be a little more generic and useful for
debugging.  The command line option is "-t", and we print
out which topic we are publishing/subscribing to on program startup.

image_tools/include/image_tools/options.hpp

@@ -54,6 +54,7 @@ bool parse_command_options(
   int argc, char ** argv, size_t * depth,
   rmw_qos_reliability_policy_t * reliability_policy,
   rmw_qos_history_policy_t * history_policy, bool * show_camera = nullptr, double * freq = nullptr,
-  size_t * width = nullptr, size_t * height = nullptr, bool * burger_mode = nullptr);
+  size_t * width = nullptr, size_t * height = nullptr, bool * burger_mode = nullptr,
+  std::string * topic = nullptr);
 
 #endif  // IMAGE_TOOLS__OPTIONS_HPP_

image_tools/src/cam2image.cpp

@@ -83,11 +83,12 @@ int main(int argc, char * argv[])
   size_t width = 320;
   size_t height = 240;
   bool burger_mode = false;
+  std::string topic("image");
 
   // Configure demo parameters with command line options.
   bool success = parse_command_options(
     argc, argv, &depth, &reliability_policy, &history_policy, &show_camera, &freq, &width, &height,
-    &burger_mode);
+    &burger_mode, &topic);
   if (!success) {
     return 0;
   }
@@ -113,9 +114,10 @@ int main(int argc, char * argv[])
   // parameter.
   custom_camera_qos_profile.history = history_policy;
 
+  printf("Publishing data on topic '%s'\n", topic.c_str());
   // Create the image publisher with our custom QoS profile.
   auto pub = node->create_publisher<sensor_msgs::msg::Image>(
-    "image", custom_camera_qos_profile);
+    topic, custom_camera_qos_profile);
 
   // is_flipped will cause the incoming camera image message to flip about the y-axis.
   bool is_flipped = false;
@@ -193,7 +195,7 @@ int main(int argc, char * argv[])
         cv::waitKey(1);
       }
       // Publish the image message and increment the frame_id.
-      std::cout << "Publishing image #" << i << std::endl;
+      printf("Publishing image #%zd\n", i);
       pub->publish(msg);
       ++i;
     }

image_tools/src/options.cpp

@@ -47,7 +47,8 @@ bool parse_command_options(
   int argc, char ** argv, size_t * depth,
   rmw_qos_reliability_policy_t * reliability_policy,
   rmw_qos_history_policy_t * history_policy, bool * show_camera,
-  double * freq, size_t * width, size_t * height, bool * burger_mode)
+  double * freq, size_t * width, size_t * height, bool * burger_mode,
+  std::string * topic)
 {
   std::vector<std::string> args(argv, argv + argc);
 
@@ -76,6 +77,9 @@ bool parse_command_options(
     if (burger_mode != nullptr) {
       ss << " -b: produce images of burgers rather than connecting to a camera" << std::endl;
     }
+    if (topic != nullptr) {
+      ss << " -t TOPIC: use topic TOPIC instead of the default" << std::endl;
+    }
     std::cout << ss.str();
     return false;
   }
@@ -123,5 +127,12 @@ bool parse_command_options(
     *burger_mode = get_flag_option(args, "-b");
   }
 
+  if (topic != nullptr) {
+    std::string tmptopic = get_command_option(args, "-t");
+    if (!tmptopic.empty()) {
+      *topic = tmptopic;
+    }
+  }
+
   return true;
 }

image_tools/src/showimage.cpp

@@ -90,10 +90,12 @@ int main(int argc, char * argv[])
   rmw_qos_reliability_policy_t reliability_policy = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
   rmw_qos_history_policy_t history_policy = RMW_QOS_POLICY_HISTORY_KEEP_ALL;
   bool show_camera = true;
+  std::string topic("image");
 
   // Configure demo parameters with command line options.
   if (!parse_command_options(
-      argc, argv, &depth, &reliability_policy, &history_policy, &show_camera))
+      argc, argv, &depth, &reliability_policy, &history_policy, &show_camera, nullptr, nullptr,
+      nullptr, nullptr, &topic))
   {
     return 0;
   }
@@ -128,9 +130,10 @@ int main(int argc, char * argv[])
       show_image(msg, show_camera);
     };
 
+  printf("Subscribing to topic '%s'\n", topic.c_str());
   // Initialize a subscriber that will receive the ROS Image message to be displayed.
   auto sub = node->create_subscription<sensor_msgs::msg::Image>(
-    "image", callback, custom_qos_profile);
+    topic, callback, custom_qos_profile);
 
   rclcpp::spin(node);