Added Rviz tool for defining raster origin and direction

CMakeLists.txt

@@ -6,12 +6,17 @@ if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
 endif()
 
-# find dependencies
+set(CMAKE_AUTOMOC ON)
+
+# Find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(noether_tpp REQUIRED)
+find_package(noether_gui REQUIRED)
+find_package(pluginlib)
 find_package(rclcpp REQUIRED)
+find_package(rviz_common)
 find_package(tf2_eigen REQUIRED)
 find_package(yaml-cpp REQUIRED)
 
@@ -21,6 +26,7 @@ rosidl_generate_interfaces(
   ${PROJECT_NAME}
   "msg/ToolPath.msg"
   "msg/ToolPaths.msg"
+  "srv/GetLocatedVector.srv"
   "srv/PlanToolPath.srv"
   DEPENDENCIES
   builtin_interfaces
@@ -29,18 +35,42 @@ rosidl_generate_interfaces(
 rosidl_get_typesupport_target(interfaces_target "${PROJECT_NAME}" "rosidl_typesupport_cpp")
 
 # Library
-add_library(${PROJECT_NAME}_conversions SHARED src/conversions.cpp)
+add_library(
+  ${PROJECT_NAME}_lib SHARED src/conversions.cpp src/located_vector_generators.cpp
+                             src/located_vector_generator_widgets.cpp
+)
 target_include_directories(
-  ${PROJECT_NAME}_conversions PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-                                     "$<INSTALL_INTERFACE:include>"
+  ${PROJECT_NAME}_lib PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>" "$<INSTALL_INTERFACE:include>"
+)
+target_link_libraries(
+  ${PROJECT_NAME}_lib
+  PUBLIC noether::noether_gui
+         noether::noether_tpp
+         tf2_eigen::tf2_eigen
+         rclcpp::rclcpp
+         ${interfaces_target}
 )
-target_link_libraries(${PROJECT_NAME}_conversions noether::noether_tpp ${interfaces_target})
-ament_target_dependencies(${PROJECT_NAME}_conversions tf2_eigen)
 
 # Tool path planning server
 add_executable(${PROJECT_NAME}_tool_path_planning_server src/tool_path_planning_server.cpp)
-target_link_libraries(${PROJECT_NAME}_tool_path_planning_server ${PROJECT_NAME}_conversions yaml-cpp)
-ament_target_dependencies(${PROJECT_NAME}_tool_path_planning_server rclcpp)
+target_link_libraries(${PROJECT_NAME}_tool_path_planning_server PUBLIC ${PROJECT_NAME}_lib rclcpp::rclcpp yaml-cpp)
+
+# Located vector Rviz tool
+add_library(${PROJECT_NAME}_located_vector_tool SHARED src/located_vector_rviz_tool.h src/located_vector_rviz_tool.cpp)
+target_link_libraries(
+  ${PROJECT_NAME}_located_vector_tool
+  pluginlib::pluginlib
+  rviz_common::rviz_common
+  ${interfaces_target}
+)
+
+# Plugins
+add_library(${PROJECT_NAME}_plugins SHARED src/plugins.cpp)
+target_link_libraries(${PROJECT_NAME}_plugins ${PROJECT_NAME}_lib noether::noether_tpp)
+
+# GUI Plugins
+add_library(${PROJECT_NAME}_gui_plugins SHARED src/gui_plugins.cpp)
+target_link_libraries(${PROJECT_NAME}_gui_plugins ${PROJECT_NAME}_lib noether::noether_gui)
 
 if(${ENABLE_TESTING})
   enable_testing()
@@ -52,7 +82,10 @@ install(DIRECTORY include/ DESTINATION include/)
 
 # Install the library
 install(
-  TARGETS ${PROJECT_NAME}_conversions
+  TARGETS ${PROJECT_NAME}_lib
+          ${PROJECT_NAME}_located_vector_tool
+          ${PROJECT_NAME}_plugins
+          ${PROJECT_NAME}_gui_plugins
   EXPORT ${PROJECT_NAME}-targets
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -66,8 +99,13 @@ install(TARGETS ${PROJECT_NAME}_tool_path_planning_server DESTINATION lib/${PROJ
 ament_export_dependencies(
   builtin_interfaces
   geometry_msgs
+  noether_gui
   noether_tpp
+  pluginlib
   rosidl_default_runtime
+  rviz_common
   tf2_eigen
 )
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/noether_plugin_libs.dsv.in")
+pluginlib_export_plugin_description_file(rviz_common plugin_description.xml)
 ament_package()

hooks/noether_plugin_libs.dsv.in

@@ -0,0 +1,2 @@
+prepend-non-duplicate;NOETHER_PLUGIN_LIBS;lib/@PROJECT_NAME@_plugins
+prepend-non-duplicate;NOETHER_PLUGIN_LIBS;lib/@PROJECT_NAME@_gui_plugins

include/noether_ros/located_vector_generator_widgets.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <noether_gui/widgets.h>
+
+class QLineEdit;
+
+namespace noether_ros
+{
+class LocatedVectorDirectionGeneratorWidget : public noether::BaseWidget
+{
+public:
+  LocatedVectorDirectionGeneratorWidget(QWidget* parent = nullptr);
+
+  void configure(const YAML::Node&) override;
+  void save(YAML::Node&) const override;
+
+protected:
+  QLineEdit* line_edit_service_name_;
+};
+
+struct LocatedVectorOriginGeneratorWidget : public noether::BaseWidget
+{
+public:
+  LocatedVectorOriginGeneratorWidget(QWidget* parent = nullptr);
+
+  void configure(const YAML::Node&) override;
+  void save(YAML::Node&) const override;
+
+protected:
+  QLineEdit* line_edit_service_name_;
+};
+
+}  // namespace noether_ros

include/noether_ros/located_vector_generators.h

@@ -0,0 +1,55 @@
+#pragma once
+
+#include <noether_tpp/macros.h>
+#include <noether_tpp/tool_path_planners/raster/raster_planner.h>
+
+FWD_DECLARE_YAML_STRUCTS()
+
+namespace noether_ros
+{
+/**
+ * @brief Calls a `noether_ros/srv/GetLocatedVector` service to generate the direction vector
+ */
+class LocatedVectorDirectionGenerator : public noether::DirectionGenerator
+{
+public:
+  /**
+   * @brief Constructor
+   * @param service_name Name of the service providing `noether_ros/srv/GetLocatedVector`
+   */
+  LocatedVectorDirectionGenerator(const std::string& service_name);
+
+  Eigen::Vector3d generate(const pcl::PolygonMesh& mesh) const override;
+
+protected:
+  LocatedVectorDirectionGenerator() = default;
+  DECLARE_YAML_FRIEND_CLASSES(LocatedVectorDirectionGenerator)
+
+  std::string service_name_;
+};
+
+/**
+ * @brief Calls a `noether_ros/srv/GetLocatedVector` service to generate the origin location
+ */
+class LocatedVectorOriginGenerator : public noether::OriginGenerator
+{
+public:
+  /**
+   * @brief Constructor
+   * @param service_name Name of the service providing `noether_ros/srv/GetLocatedVector`
+   */
+  LocatedVectorOriginGenerator(const std::string& service_name);
+
+  Eigen::Vector3d generate(const pcl::PolygonMesh& mesh) const override;
+
+protected:
+  LocatedVectorOriginGenerator() = default;
+  DECLARE_YAML_FRIEND_CLASSES(LocatedVectorOriginGenerator)
+
+  std::string service_name_;
+};
+
+}  // namespace noether_ros
+
+FWD_DECLARE_YAML_CONVERT(noether_ros::LocatedVectorOriginGenerator)
+FWD_DECLARE_YAML_CONVERT(noether_ros::LocatedVectorDirectionGenerator)

package.xml

@@ -12,8 +12,11 @@
   <build_depend>rosidl_default_generators</build_depend>
   <depend>builtin_interfaces</depend>
   <depend>geometry_msgs</depend>
+  <depend>noether_gui</depend>
   <depend>noether_tpp</depend>
+  <depend>rviz_common</depend>
   <depend>rclcpp</depend>
+  <depend>pluginlib</depend>
   <depend>tf2_eigen</depend>
   <depend>yaml-cpp</depend>
   <exec_depend>rosidl_default_runtime</exec_depend>

plugin_description.xml

@@ -0,0 +1,7 @@
+<library path="noether_ros_located_vector_tool">
+  <class name="noether_ros/LocatedVector" type="noether_ros::LocatedVectorTool" base_class_type="rviz_common::Tool">
+    <description>
+      Draws a 2-point line on geometry in the 3D window and provides access to the resulting line (or "located vector") via a ROS 2 service
+    </description>
+  </class>
+</library>

src/gui_plugins.cpp

@@ -0,0 +1,11 @@
+#include <noether_ros/located_vector_generator_widgets.h>
+
+#include <noether_gui/plugin_interface.h>
+
+using namespace noether;
+
+namespace noether_ros
+{
+EXPORT_SIMPLE_DIRECTION_GENERATOR_WIDGET_PLUGIN(LocatedVectorDirectionGeneratorWidget, LocatedVectorDirection)
+EXPORT_SIMPLE_ORIGIN_GENERATOR_WIDGET_PLUGIN(LocatedVectorOriginGeneratorWidget, LocatedVectorOrigin)
+}  // namespace noether_ros

src/located_vector_generator_widgets.cpp

@@ -0,0 +1,59 @@
+#include <noether_ros/located_vector_generator_widgets.h>
+
+#include <noether_tpp/serialization.h>
+#include <QLineEdit>
+#include <QLabel>
+#include <QFormLayout>
+
+static const char* SERVICE_NAME_KEY = "service_name";
+static const char* DEFAULT_SERVICE_NAME = "get_located_vector";
+
+namespace noether_ros
+{
+LocatedVectorDirectionGeneratorWidget::LocatedVectorDirectionGeneratorWidget(QWidget* parent)
+  : BaseWidget(parent), line_edit_service_name_(new QLineEdit(this))
+{
+  line_edit_service_name_->setPlaceholderText(DEFAULT_SERVICE_NAME);
+  line_edit_service_name_->setText(DEFAULT_SERVICE_NAME);
+  line_edit_service_name_->setToolTip("Name of the service providing noether_ros/srv/GetLocatedVector");
+
+  auto layout = new QFormLayout(this);
+  layout->addRow("Service name", line_edit_service_name_);
+}
+
+void LocatedVectorDirectionGeneratorWidget::configure(const YAML::Node& config)
+{
+  if (config[SERVICE_NAME_KEY])
+    line_edit_service_name_->setText(QString::fromStdString(YAML::getMember<std::string>(config, SERVICE_NAME_KEY)));
+}
+
+void LocatedVectorDirectionGeneratorWidget::save(YAML::Node& config) const
+{
+  config["name"] = "LocatedVectorDirection";
+  config[SERVICE_NAME_KEY] = line_edit_service_name_->text().toStdString();
+}
+
+LocatedVectorOriginGeneratorWidget::LocatedVectorOriginGeneratorWidget(QWidget* parent)
+  : BaseWidget(parent), line_edit_service_name_(new QLineEdit(this))
+{
+  line_edit_service_name_->setPlaceholderText(DEFAULT_SERVICE_NAME);
+  line_edit_service_name_->setText(DEFAULT_SERVICE_NAME);
+  line_edit_service_name_->setToolTip("Name of the service providing noether_ros/srv/GetLocatedVector");
+
+  auto layout = new QFormLayout(this);
+  layout->addRow("Service name", line_edit_service_name_);
+}
+
+void LocatedVectorOriginGeneratorWidget::configure(const YAML::Node& config)
+{
+  if (config[SERVICE_NAME_KEY])
+    line_edit_service_name_->setText(QString::fromStdString(YAML::getMember<std::string>(config, SERVICE_NAME_KEY)));
+}
+
+void LocatedVectorOriginGeneratorWidget::save(YAML::Node& config) const
+{
+  config["name"] = "LocatedVectorOrigin";
+  config[SERVICE_NAME_KEY] = line_edit_service_name_->text().toStdString();
+}
+
+}  // namespace noether_ros