diff --git a/CMakeLists.txt b/CMakeLists.txt index c854980..f2ec4ff 100644 --- a/CMakeLists.txt +++ b/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 "$" - "$" + ${PROJECT_NAME}_lib PUBLIC "$" "$" +) +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() diff --git a/hooks/noether_plugin_libs.dsv.in b/hooks/noether_plugin_libs.dsv.in new file mode 100644 index 0000000..adaca3f --- /dev/null +++ b/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 diff --git a/include/noether_ros/located_vector_generator_widgets.h b/include/noether_ros/located_vector_generator_widgets.h new file mode 100644 index 0000000..c03b794 --- /dev/null +++ b/include/noether_ros/located_vector_generator_widgets.h @@ -0,0 +1,33 @@ +#pragma once + +#include + +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 diff --git a/include/noether_ros/located_vector_generators.h b/include/noether_ros/located_vector_generators.h new file mode 100644 index 0000000..066d90f --- /dev/null +++ b/include/noether_ros/located_vector_generators.h @@ -0,0 +1,55 @@ +#pragma once + +#include +#include + +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) diff --git a/package.xml b/package.xml index bde0d9c..402596c 100644 --- a/package.xml +++ b/package.xml @@ -12,8 +12,11 @@ rosidl_default_generators builtin_interfaces geometry_msgs + noether_gui noether_tpp + rviz_common rclcpp + pluginlib tf2_eigen yaml-cpp rosidl_default_runtime diff --git a/plugin_description.xml b/plugin_description.xml new file mode 100644 index 0000000..f64ca1f --- /dev/null +++ b/plugin_description.xml @@ -0,0 +1,7 @@ + + + + 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 + + + diff --git a/src/gui_plugins.cpp b/src/gui_plugins.cpp new file mode 100644 index 0000000..1205077 --- /dev/null +++ b/src/gui_plugins.cpp @@ -0,0 +1,11 @@ +#include + +#include + +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 diff --git a/src/located_vector_generator_widgets.cpp b/src/located_vector_generator_widgets.cpp new file mode 100644 index 0000000..0b5b8a2 --- /dev/null +++ b/src/located_vector_generator_widgets.cpp @@ -0,0 +1,59 @@ +#include + +#include +#include +#include +#include + +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(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(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 diff --git a/src/located_vector_generators.cpp b/src/located_vector_generators.cpp new file mode 100644 index 0000000..a670439 --- /dev/null +++ b/src/located_vector_generators.cpp @@ -0,0 +1,178 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +static const char* SERVICE_NAME_KEY = "service_name"; +static const char* DEFAULT_SERVICE_NAME = "get_located_vector"; + +namespace noether_ros +{ +using LocatedVector = std::pair; + +class LocatedVectorClient +{ +public: + LocatedVectorClient(const std::string& service_name) + { + // Initialize RCLPP if not already + if (!rclcpp::ok()) + rclcpp::init(0, nullptr); + + node_ = std::make_shared("located_vector_client_node"); + + // Create the client + client_ = node_->create_client(service_name); + + // Create the transform listener + buffer_ = std::make_shared( + node_->get_clock(), tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), node_); + listener_ = std::make_shared(*buffer_, node_); + } + + virtual ~LocatedVectorClient() = default; + + Eigen::Isometry3d lookupTransform(const std::string& source, const std::string& target) const + { + rclcpp::spin_some(node_); + return tf2::transformToEigen( + buffer_->lookupTransform(source, target, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0))); + } + + LocatedVector getLocatedVector() const + { + // Check if the service is ready + rclcpp::spin_some(node_); + using namespace std::chrono_literals; + if (!client_->wait_for_service(1s)) + { + std::stringstream ss; + ss << "Service '" << client_->get_service_name() << "' is not available"; + throw std::runtime_error(ss.str()); + } + + // Call the ROI selection service + auto request = std::make_shared(); + auto future = client_->async_send_request(request); + + switch (rclcpp::spin_until_future_complete(node_, future, std::chrono::seconds(3))) + { + case rclcpp::FutureReturnCode::SUCCESS: + break; + case rclcpp::FutureReturnCode::TIMEOUT: + throw std::runtime_error("Service call to '" + std::string(client_->get_service_name()) + "' timed out"); + default: + throw std::runtime_error("Service call to '" + std::string(client_->get_service_name()) + "' failed"); + } + + srv::GetLocatedVector::Response::ConstSharedPtr response = future.get(); + if (response->success) + return std::make_pair(response->source, response->target); + + throw std::runtime_error(response->message); + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Client::SharedPtr client_; + tf2_ros::Buffer::SharedPtr buffer_; + std::shared_ptr listener_; +}; + +LocatedVectorDirectionGenerator::LocatedVectorDirectionGenerator(const std::string& service_name) + : service_name_(service_name) +{ +} + +Eigen::Vector3d LocatedVectorDirectionGenerator::generate(const pcl::PolygonMesh& mesh) const +{ + LocatedVectorClient client(service_name_); + const LocatedVector lv = client.getLocatedVector(); + + // Transform the source point into the mesh frame + Eigen::Vector3d source; + { + tf2::fromMsg(lv.first.point, source); + const Eigen::Isometry3d mesh_to_source = client.lookupTransform(mesh.header.frame_id, lv.first.header.frame_id); + source = mesh_to_source * source; + } + + // Transform the target point into the mesh frame + Eigen::Vector3d target; + { + tf2::fromMsg(lv.second.point, target); + const Eigen::Isometry3d mesh_to_target = client.lookupTransform(mesh.header.frame_id, lv.second.header.frame_id); + target = mesh_to_target * target; + } + + return (target - source).normalized(); +} + +LocatedVectorOriginGenerator::LocatedVectorOriginGenerator(const std::string& service_name) + : service_name_(service_name) +{ +} + +Eigen::Vector3d LocatedVectorOriginGenerator::generate(const pcl::PolygonMesh& mesh) const +{ + LocatedVectorClient client(service_name_); + LocatedVector lv = client.getLocatedVector(); + + // Convert to Eigen + Eigen::Vector3d source; + tf2::fromMsg(lv.first.point, source); + + // Look up the transform from the mesh frame to the source frame + Eigen::Isometry3d mesh_to_source = client.lookupTransform(mesh.header.frame_id, lv.first.header.frame_id); + + // Transform the source point into the mesh frame + return mesh_to_source * source; +} + +} // namespace noether_ros + +namespace YAML +{ +/** @cond */ +Node convert::encode( + const noether_ros::LocatedVectorDirectionGenerator& val) +{ + Node node; + node[SERVICE_NAME_KEY] = val.service_name_; + return node; +} + +bool convert::decode(const Node& node, + noether_ros::LocatedVectorDirectionGenerator& val) +{ + if (node[SERVICE_NAME_KEY]) + val.service_name_ = getMember(node, SERVICE_NAME_KEY); + else + val.service_name_ = DEFAULT_SERVICE_NAME; + return true; +} + +Node convert::encode(const noether_ros::LocatedVectorOriginGenerator& val) +{ + Node node; + node[SERVICE_NAME_KEY] = val.service_name_; + return node; +} + +bool convert::decode(const Node& node, + noether_ros::LocatedVectorOriginGenerator& val) +{ + if (node[SERVICE_NAME_KEY]) + val.service_name_ = getMember(node, SERVICE_NAME_KEY); + else + val.service_name_ = DEFAULT_SERVICE_NAME; + return true; +} +/** @endcond */ +} // namespace YAML diff --git a/src/located_vector_rviz_tool.cpp b/src/located_vector_rviz_tool.cpp new file mode 100644 index 0000000..7ecf20e --- /dev/null +++ b/src/located_vector_rviz_tool.cpp @@ -0,0 +1,216 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// Copyright (c) 2018, Bosch Software Innovations GmbH. +// All rights reserved. + +#include "located_vector_rviz_tool.h" + +#include // NOLINT: cpplint is unable to handle the include order here +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if __has_include() +#include + +#if (RCLCPP_VERSION_MAJOR >= 28) +#define QOS_REQUIRED_IN_SERVICE +#endif + +#endif + +namespace noether_ros +{ +Ogre::ManualObject* Line::getManualObject() const { return manual_object_; } + +Ogre::MaterialPtr Line::getManualObjectMaterial() const { return manual_object_material_; } + +LocatedVectorTool::LocatedVectorTool() : rviz_common::Tool(), start_(nullptr), end_(nullptr) +{ + shortcut_key_ = 'l'; + + color_property_ = new rviz_common::properties::ColorProperty("Line color", + Qt::darkYellow, + "The topic on which to publish points.", + getPropertyContainer(), + SLOT(updateLineColor()), + this); + + render_as_overlay_property_ = new rviz_common::properties::BoolProperty("Render as overlay", + false, + "Render the drawn lines on top of all other " + "geometry", + getPropertyContainer(), + SLOT(updateRenderAsOverlay()), + this); + + service_name_property_ = new rviz_common::properties::StringProperty( + "Service name", "get_located_vector", "Desc", getPropertyContainer(), SLOT(updateServiceName()), this); +} + +LocatedVectorTool::~LocatedVectorTool() +{ + executor_.cancel(); + executor_thread_.join(); +} + +void LocatedVectorTool::onInitialize() +{ + // Create the line display + line_ = std::make_shared(context_->getSceneManager()); + updateLineColor(); + updateRenderAsOverlay(); + + std_cursor_ = rviz_common::getDefaultCursor(); + hit_cursor_ = rviz_common::makeIconCursor("package://rviz_common/icons/crosshair.svg"); + + updateServiceName(); +} + +void LocatedVectorTool::updateServiceName() +{ + const std::string service_name = service_name_property_->getStdString(); + + rclcpp::Node::SharedPtr node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + + executor_.cancel(); + if (executor_thread_.joinable()) + executor_thread_.join(); + + executor_callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + executor_.add_callback_group(executor_callback_group_, node->get_node_base_interface()); + +#ifdef QOS_REQUIRED_IN_SERVICE + rclcpp::QoS qos(rclcpp::QoSInitialization(RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT, 1), + rmw_qos_profile_services_default); + server_ = node->create_service( + service_name, + std::bind(&LocatedVectorTool::callback, this, std::placeholders::_1, std::placeholders::_2), + qos, + executor_callback_group_); +#else + server_ = node->create_service( + service_name, + std::bind(&LocatedVectorTool::callback, this, std::placeholders::_1, std::placeholders::_2), + rmw_qos_profile_services_default, + executor_callback_group_); +#endif + + executor_thread_ = std::thread([&]() { executor_.spin(); }); +} + +void LocatedVectorTool::activate() {} + +void LocatedVectorTool::deactivate() {} + +int LocatedVectorTool::processMouseEvent(rviz_common::ViewportMouseEvent& event) +{ + Ogre::Vector3 pos; + bool success = context_->getViewPicker()->get3DPoint(event.panel, event.x, event.y, pos); + setCursor(success ? hit_cursor_ : std_cursor_); + + // Update the line display + if (success && start_ && !end_) + line_->setPoints(*start_, pos); + + if (event.leftUp() && success) + { + processLeftButton(pos); + return Render; + } + if (event.rightUp()) + { + processRightButton(); + } + + return 0; +} + +void LocatedVectorTool::updateLineColor() +{ + Ogre::ColourValue color = rviz_common::properties::qtToOgre(color_property_->getColor()); + line_->setColor(color); +} + +void LocatedVectorTool::updateRenderAsOverlay() +{ + const bool render_as_overlay = render_as_overlay_property_->getBool(); + const Ogre::RenderQueueGroupID render_id = + render_as_overlay ? Ogre::RenderQueueGroupID::RENDER_QUEUE_OVERLAY : Ogre::RenderQueueGroupID::RENDER_QUEUE_MAIN; + + line_->getManualObjectMaterial()->setDepthCheckEnabled(!render_as_overlay); + line_->getManualObjectMaterial()->setDepthWriteEnabled(!render_as_overlay); + line_->getManualObject()->setRenderQueueGroup(render_id); +} + +void LocatedVectorTool::processLeftButton(const Ogre::Vector3& pos) +{ + if (start_) + { + if (end_) + { + // Both the start and end points are defined -> reset the start to the current position + start_ = std::make_shared(pos); + end_.reset(); + } + else + { + // Only the start point is defined -> set the current position as the end + end_ = std::make_shared(pos); + } + } + else + { + start_ = std::make_shared(pos); + } +} + +void LocatedVectorTool::processRightButton() +{ + start_.reset(); + end_.reset(); + line_->setVisible(false); +} + +void LocatedVectorTool::callback(srv::GetLocatedVector::Request::ConstSharedPtr req, + srv::GetLocatedVector::Response::SharedPtr res) const +{ + if (!start_) + { + res->success = false; + res->message = "Start point has not been set"; + return; + } + + if (!end_) + { + res->success = false; + res->message = "End point has not been set"; + return; + } + + res->source.header.frame_id = context_->getFixedFrame().toStdString(); + res->source.point.x = start_->x; + res->source.point.y = start_->y; + res->source.point.z = start_->z; + + res->target.header.frame_id = context_->getFixedFrame().toStdString(); + res->target.point.x = end_->x; + res->target.point.y = end_->y; + res->target.point.z = end_->z; + + res->success = true; +} + +} // namespace noether_ros + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(noether_ros::LocatedVectorTool, rviz_common::Tool) diff --git a/src/located_vector_rviz_tool.h b/src/located_vector_rviz_tool.h new file mode 100644 index 0000000..c17bb05 --- /dev/null +++ b/src/located_vector_rviz_tool.h @@ -0,0 +1,80 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// Copyright (c) 2018, Bosch Software Innovations GmbH. +// All rights reserved. + +#pragma once + +#include + +#include +#include // NOLINT cpplint cannot handle include order +#include +#include +#include +#include +#include +#include + +namespace rviz_common +{ +namespace properties +{ +class BoolProperty; +class ColorProperty; +class StringProperty; +} // namespace properties +} // namespace rviz_common + +namespace noether_ros +{ +class Line : public rviz_rendering::Line +{ +public: + using rviz_rendering::Line::Line; + + Ogre::ManualObject* getManualObject() const; + Ogre::MaterialPtr getManualObjectMaterial() const; +}; + +class LocatedVectorTool : public rviz_common::Tool +{ + Q_OBJECT + +public: + LocatedVectorTool(); + virtual ~LocatedVectorTool(); + + void onInitialize() override; + void activate() override; + void deactivate() override; + int processMouseEvent(rviz_common::ViewportMouseEvent& event) override; + +public Q_SLOTS: + void updateLineColor(); + void updateRenderAsOverlay(); + void updateServiceName(); + +private: + void processLeftButton(const Ogre::Vector3& pos); + void processRightButton(); + void callback(srv::GetLocatedVector::Request::ConstSharedPtr req, + srv::GetLocatedVector::Response::SharedPtr res) const; + + rviz_common::properties::ColorProperty* color_property_; + rviz_common::properties::BoolProperty* render_as_overlay_property_; + rviz_common::properties::StringProperty* service_name_property_; + + rclcpp::Service::SharedPtr server_; + std::thread executor_thread_; + rclcpp::executors::SingleThreadedExecutor executor_; + rclcpp::CallbackGroup::SharedPtr executor_callback_group_; + + std::shared_ptr line_; + std::shared_ptr start_; + std::shared_ptr end_; + + QCursor std_cursor_; + QCursor hit_cursor_; +}; + +} // namespace noether_ros diff --git a/src/plugins.cpp b/src/plugins.cpp new file mode 100644 index 0000000..6deb06d --- /dev/null +++ b/src/plugins.cpp @@ -0,0 +1,11 @@ +#include + +#include + +using namespace noether; + +namespace noether_ros +{ +EXPORT_SIMPLE_DIRECTION_GENERATOR_PLUGIN(LocatedVectorDirectionGenerator, LocatedVectorDirection) +EXPORT_SIMPLE_ORIGIN_GENERATOR_PLUGIN(LocatedVectorOriginGenerator, LocatedVectorOrigin) +} // namespace noether_ros diff --git a/srv/GetLocatedVector.srv b/srv/GetLocatedVector.srv new file mode 100644 index 0000000..a26518f --- /dev/null +++ b/srv/GetLocatedVector.srv @@ -0,0 +1,5 @@ +--- +bool success +string message +geometry_msgs/PointStamped source +geometry_msgs/PointStamped target