Rviz plugin (#5)

* Updated NodeInfo.msg

* Publish type

* Coresense instrumentation panel

* Add tab panel & control

* Reformated

coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationConsumer.hpp

@@ -82,6 +82,7 @@ class InstrumentationConsumer : public rclcpp_lifecycle::LifecycleNode
 
   std::string topic_;
   std::string topic_type_;
+  std::string type_;
 };
 
 } // namespace coresense_instrumentation_driver

coresense_instrumentation_driver/include/coresense_instrumentation_driver/InstrumentationProducer.hpp

@@ -80,6 +80,7 @@ class InstrumentationProducer : public rclcpp_lifecycle::LifecycleNode
 
   std::string topic_;
   std::string topic_type_;
+  std::string type_;
 };
 
 template<>
@@ -131,6 +132,7 @@ class InstrumentationProducer<sensor_msgs::msg::Image>: public rclcpp_lifecycle:
   image_transport::Subscriber sub_;
   std::string topic_;
   std::string topic_type_;
+  std::string type_;
 };
 
 } // namespace coresense_instrumentation_driver

coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationConsumerGeneric.cpp

@@ -29,9 +29,11 @@ InstrumentationConsumer<TopicT>::InstrumentationConsumer(
 {
   declare_parameter("topic", std::string(""));
   declare_parameter("topic_type", std::string(""));
+  declare_parameter("type", std::string(""));
 
   get_parameter("topic", topic_);
   get_parameter("topic_type", topic_type_);
+  get_parameter("type", type_);
 
   status_pub_ = this->create_publisher<coresense_instrumentation_interfaces::msg::NodeInfo>(
     "/status", 10);
@@ -98,7 +100,8 @@ void InstrumentationConsumer<TopicT>::publish_status()
 
   std::free(demangled_name);
 
-  status_msg->type = result;
+  status_msg->type_msg = result;
+  status_msg->type = type_;
 
   status_pub_->publish(std::move(status_msg));
 }

coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerGeneric.cpp

@@ -29,9 +29,11 @@ InstrumentationProducer<TopicT>::InstrumentationProducer(
 {
   declare_parameter("topic", std::string(""));
   declare_parameter("topic_type", std::string(""));
+  declare_parameter("type", std::string(""));
 
   get_parameter("topic", topic_);
   get_parameter("topic_type", topic_type_);
+  get_parameter("type", type_);
 
   status_pub_ = this->create_publisher<coresense_instrumentation_interfaces::msg::NodeInfo>(
     "/status", 10);
@@ -98,7 +100,8 @@ void InstrumentationProducer<TopicT>::publish_status()
 
   std::free(demangled_name);
 
-  status_msg->type = result;
+  status_msg->type_msg = result;
+  status_msg->type = type_;
 
   status_pub_->publish(std::move(status_msg));
 }

coresense_instrumentation_driver/src/coresense_instrumentation_driver/InstrumentationProducerImage.cpp

@@ -25,9 +25,11 @@ InstrumentationProducer<sensor_msgs::msg::Image>::InstrumentationProducer(
 {
   declare_parameter("topic", std::string(""));
   declare_parameter("topic_type", std::string(""));
+  declare_parameter("type", std::string(""));
 
   get_parameter("topic", topic_);
   get_parameter("topic_type", topic_type_);
+  get_parameter("type", type_);
 
   node_ = rclcpp::Node::make_shared("subnode");
 
@@ -62,6 +64,29 @@ void InstrumentationProducer<sensor_msgs::msg::Image>::publish_status()
   status_msg->state = lifecycle_state.id();
   status_msg->stamp = this->now();
 
+  for (const auto & entry : publishers_) {
+    status_msg->topics.push_back(entry.first);
+  }
+
+  int status;
+  char * demangled_name = abi::__cxa_demangle(typeid(sensor_msgs::msg::Image).name(), nullptr, nullptr, &status);
+  std::string result(demangled_name);
+
+  size_t pos = result.find('<');
+  if (pos != std::string::npos) {
+    result = result.substr(0, pos);
+  }
+
+  size_t last_underscore = result.rfind('_');
+  if (last_underscore != std::string::npos) {
+    result = result.substr(0, last_underscore);
+  }
+
+  std::free(demangled_name);
+
+  status_msg->type_msg = result;
+  status_msg->type = type_;
+
   status_pub_->publish(std::move(status_msg));
 }
 

coresense_instrumentation_interfaces/msg/NodeInfo.msg

@@ -1,5 +1,6 @@
 string node_name
 uint8 state
+string type_msg
 string type
 builtin_interfaces/Time stamp
 string[] topics

coresense_instrumentation_rviz/CMakeLists.txt

@@ -0,0 +1,117 @@
+cmake_minimum_required(VERSION 3.5)
+project(coresense_instrumentation_rviz)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+# Qt5 boilerplate options from http://doc.qt.io/qt-5/cmake-manual.html
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_AUTOMOC ON)
+
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rviz_common REQUIRED)
+find_package(rviz_default_plugins REQUIRED)
+find_package(rviz_ogre_vendor REQUIRED)
+find_package(rviz_rendering REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(coresense_instrumentation_interfaces REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+
+set(coresense_instrumentation_rviz_headers_to_moc
+  include/coresense_instrumentation_rviz/CoresenseInstrumentationPanel.hpp
+)
+
+include_directories(
+  include
+)
+
+set(library_name ${PROJECT_NAME})
+
+add_library(${library_name} SHARED
+  src/CoresenseInstrumentationPanel.cpp
+  ${coresense_instrumentation_rviz_headers_to_moc}
+)
+
+set(dependencies
+  geometry_msgs
+  pluginlib
+  Qt5
+  rclcpp
+  rclcpp_lifecycle
+  rviz_common
+  rviz_default_plugins
+  rviz_ogre_vendor
+  rviz_rendering
+  std_msgs
+  tf2_geometry_msgs
+  coresense_instrumentation_interfaces
+  lifecycle_msgs
+)
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+target_include_directories(${library_name} PUBLIC
+  ${Qt5Widgets_INCLUDE_DIRS}
+  ${OGRE_INCLUDE_DIRS}
+)
+
+target_link_libraries(${library_name}
+  rviz_common::rviz_common
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+# TODO: Make this specific to this project (not rviz default plugins)
+target_compile_definitions(${library_name} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY")
+
+pluginlib_export_plugin_description_file(rviz_common plugins_description.xml)
+
+install(
+  TARGETS ${library_name}
+  EXPORT ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/
+)
+
+install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+ament_export_targets(${library_name} HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+  Qt5
+  rviz_common
+  geometry_msgs
+  map_msgs
+  rclcpp
+  coresense_instrumentation_interfaces
+  lifecycle_msgs
+)
+
+ament_package()