[jsk_topic_tools] Show input/output topics with --inout opt

jsk_topic_tools/CMakeLists.txt

@@ -12,6 +12,7 @@ catkin_python_setup()
 add_message_files(
   FILES TopicInfo.msg
 )
+find_package(Boost REQUIRED COMPONENTS program_options)
 
 add_service_files(
   FILES List.srv Update.srv ChangeTopic.srv PassthroughDuration.srv
@@ -26,7 +27,7 @@ catkin_package(
     CFG_EXTRAS nodelet.cmake
 )
 
-include_directories(include ${catkin_INCLUDE_DIRS})
+include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
 #include_directories(${Boost_INCLUDE_DIRS})
 add_executable(standalone_complexed_nodelet src/standalone_complexed_nodelet.cpp)
 add_executable(topic_buffer_server src/topic_buffer_server.cpp)

jsk_topic_tools/cmake/nodelet.cmake

@@ -24,7 +24,7 @@ macro(jsk_nodelet _nodelet_cpp _nodelet_class
     rosbuild_add_executable(${_single_nodelet_exec_name} build/${_single_nodelet_exec_name}.cpp)
   else(${USE_ROSBUILD})
     add_executable(${_single_nodelet_exec_name} ${_single_nodelet_exec_name}.cpp)
-    target_link_libraries(${_single_nodelet_exec_name} ${catkin_LIBRARIES})
+    target_link_libraries(${_single_nodelet_exec_name} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
   endif(${USE_ROSBUILD})
   if (${ARGC} GREATER 4)
     list(APPEND ${ARGV4} ${_single_nodelet_exec_name})

jsk_topic_tools/cmake/single_nodelet_exec.cpp.in

@@ -34,6 +34,19 @@
 
 #include <ros/ros.h>
 #include <nodelet/loader.h>
+#include <boost/program_options.hpp>
+
+namespace po = boost::program_options;
+
+std::string unresolve_name(const std::string node_name,
+                           const std::string resolved_name)
+{
+  std::string suffix = node_name + "/";
+  if (resolved_name.find(suffix) == 0) {
+    return "~" + resolved_name.substr(suffix.length());
+  }
+  return resolved_name;
+}
 
 int main(int argc, char **argv)
 {
@@ -46,6 +59,50 @@ int main(int argc, char **argv)
 
   manager.load(ros::this_node::getName(), "@NODELET@", remappings, my_argv);
 
+  // Parse commane line arguments
+  po::options_description desc("option");
+  desc.add_options() ("inout", "show input/output topics");
+  po::variables_map vm;
+  try {
+    po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
+  } catch (boost::program_options::invalid_command_line_syntax& e) {
+    throw ros::Exception(e.what());
+  }  catch (boost::program_options::unknown_option& e) {
+    throw ros::Exception(e.what());
+  }
+  // Show input/output topics with --inout option
+  if (vm.count("inout")) {
+    std::string node_name = ros::this_node::getName();
+    bool original_value;
+    ros::param::get("~always_subscribe", original_value);
+    ros::param::set("~always_subscribe", true);
+    ros::V_string topics;
+    ros::this_node::getAdvertisedTopics(topics);
+    std::cout << "Publications:" << std::endl;
+    for (size_t i = 0; i < topics.size(); i++) {
+      if (topics[i] != "/rosout") {
+        std::cout << " * " << unresolve_name(node_name, topics[i]) << std::endl;
+      }
+    }
+    topics.clear();
+    std::cout << "Subscriptions:" << std::endl;
+    ros::this_node::getSubscribedTopics(topics);
+    bool at_least_a_topic = false;
+    for (size_t i = 0; i < topics.size(); i++) {
+      if (topics[i] != "/rosout") {
+        std::cout << " * " << unresolve_name(node_name, topics[i]) << std::endl;
+        at_least_a_topic = true;
+      }
+    }
+    if (!at_least_a_topic) {
+      // FIXME: https://github.com/jsk-ros-pkg/jsk_common/issues/1145
+      std::cout << "[WARNING] No Subscriptions. This may be caused by unsupported '~always_subscribe' rosparam." << std::endl;
+      std::cout << "[WARNING] See: https://github.com/jsk-ros-pkg/jsk_common/issues/1145" << std::endl;
+    }
+    ros::param::set("~always_subscribe", original_value);
+    return 0;
+  }
+
   ros::spin();
   return 0;
 }