From 9a1cb2c1aae9dfc220b71cded2711b15127ec4db Mon Sep 17 00:00:00 2001 From: Joe Schornak Date: Mon, 1 Mar 2021 13:51:52 -0500 Subject: [PATCH 1/8] WIP: port to ROS2 --- CMakeLists.txt | 76 ++++++++--------- .../rviz_tool_cursor/get_point_on_plane.hpp | 16 ++++ include/rviz_tool_cursor/rviz_tool_cursor.h | 37 +++++---- package.xml | 7 +- plugin_description.xml | 6 +- src/circle_tool_cursor.cpp | 9 +- src/circle_tool_cursor.h | 12 +-- src/mesh_tool_cursor.cpp | 19 +++-- src/mesh_tool_cursor.h | 9 +- src/rviz_tool_cursor/get_point_on_plane.cpp | 34 ++++++++ src/rviz_tool_cursor/rviz_tool_cursor.cpp | 83 +++++++++++-------- 11 files changed, 181 insertions(+), 127 deletions(-) create mode 100644 include/rviz_tool_cursor/get_point_on_plane.hpp create mode 100644 src/rviz_tool_cursor/get_point_on_plane.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 558def9..c6d174a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,33 +5,17 @@ set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -find_package(catkin REQUIRED COMPONENTS - rviz - pluginlib - pcl_ros -) - -find_package(Eigen3 REQUIRED) - -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - tool_cursor - CATKIN_DEPENDS - rviz - pluginlib - pcl_ros -) +cmake_policy(SET CMP0057 NEW) -########### -## Build ## -########### +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(rviz_common REQUIRED) +find_package(pluginlib REQUIRED) +find_package(PCL REQUIRED COMPONENTS common io geometry) +#find_package(pcl_ros REQUIRED) -include_directories( - include - ${catkin_INCLUDE_DIRS} -) +find_package(Eigen3 REQUIRED) # QT find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) @@ -44,40 +28,48 @@ qt5_wrap_cpp(MOC_FILES add_library(tool_cursor src/${PROJECT_NAME}/rviz_tool_cursor.cpp + src/${PROJECT_NAME}/get_point_on_plane.cpp src/circle_tool_cursor.cpp src/mesh_tool_cursor.cpp ${MOC_FILES} ) target_link_libraries(tool_cursor - ${catkin_LIBRARIES} Qt5::Widgets -) - -############# -## Install ## -############# + ${rclcpp_LIBRARIES} + ${rviz_common_LIBRARIES} + ${rviz_rendering_LIBRARIES} + ${pluginlib_LIBRARIES} + Eigen3::Eigen + ${PCL_LIBRARIES}) +target_include_directories(tool_cursor + PUBLIC $ + PUBLIC $) +target_include_directories(tool_cursor SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS}) +ament_target_dependencies(tool_cursor + rclcpp + rviz_common + rviz_rendering + pluginlib) # Mark executables and/or libraries for installation install(TARGETS tool_cursor - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) # Mark cpp header files for installation -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) # Mark other files for installation (e.g. launch and bag files, etc.) install(FILES plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) install(DIRECTORY resources - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) -############# -## Testing ## -############# - +ament_package() diff --git a/include/rviz_tool_cursor/get_point_on_plane.hpp b/include/rviz_tool_cursor/get_point_on_plane.hpp new file mode 100644 index 0000000..08d4c3b --- /dev/null +++ b/include/rviz_tool_cursor/get_point_on_plane.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include + +namespace rviz_rendering +{ +/** @brief Given a viewport and an x,y position in window-pixel coordinates, + * find the point on a plane directly behind it, if any. + * @return true if the intersection exists, false if it does not. */ +bool getPointOnPlaneFromWindowXY(Ogre::Viewport* viewport, + Ogre::Plane& plane, + int window_x, + int window_y, + Ogre::Vector3& intersection_out); + +} diff --git a/include/rviz_tool_cursor/rviz_tool_cursor.h b/include/rviz_tool_cursor/rviz_tool_cursor.h index 642c163..b34ea89 100644 --- a/include/rviz_tool_cursor/rviz_tool_cursor.h +++ b/include/rviz_tool_cursor/rviz_tool_cursor.h @@ -1,9 +1,8 @@ -#ifndef RVIZ_TOOL_CURSOR_RVIZ_TOOL_CURSOR_H -#define RVIZ_TOOL_CURSOR_RVIZ_TOOL_CURSOR_H +#pragma once -#include -#include -#include +#include +#include +#include namespace Ogre { @@ -11,17 +10,26 @@ namespace Ogre class MovableObject; } -namespace rviz +namespace rviz_common +{ + class RenderPanel; +} + +namespace rviz_common +{ +namespace properties { class StringProperty; class IntProperty; class ColorProperty; + class FloatProperty; +} } namespace rviz_tool_cursor { -class ToolCursor : public rviz::Tool +class ToolCursor : public rviz_common::Tool { Q_OBJECT public: @@ -36,7 +44,7 @@ Q_OBJECT virtual void deactivate() override; - virtual int processMouseEvent(rviz::ViewportMouseEvent& event) override; + virtual int processMouseEvent(rviz_common::ViewportMouseEvent& event) override; public Q_SLOTS: @@ -58,17 +66,16 @@ public Q_SLOTS: Ogre::MovableObject* movable_obj_; - ros::NodeHandle nh_; +// ros::NodeHandle nh_; +// rclcpp::Node::SharedPtr node_; - ros::Publisher pub_; +// rclcpp::Publisher::SharedPtr pub_; - rviz::StringProperty* topic_property_; + rviz_common::properties::StringProperty* topic_property_; - rviz::IntProperty* patch_size_property_; + rviz_common::properties::IntProperty* patch_size_property_; - rviz::ColorProperty* color_property_; + rviz_common::properties::ColorProperty* color_property_; }; } // namespace rviz_tool_cursor - -#endif // RVIZ_TOOL_CURSOR_RVIZ_TOOL_CURSOR_H diff --git a/package.xml b/package.xml index 2485ece..e0a97cf 100644 --- a/package.xml +++ b/package.xml @@ -9,10 +9,11 @@ Apache 2.0 - catkin - rviz + ament_cmake + rviz_common + rviz_rendering pluginlib - pcl_ros + diff --git a/plugin_description.xml b/plugin_description.xml index c1f2fa1..3363dd6 100644 --- a/plugin_description.xml +++ b/plugin_description.xml @@ -1,11 +1,11 @@ - + + A cursor representing an input mesh that follows the geometry of mesh surfaces diff --git a/src/circle_tool_cursor.cpp b/src/circle_tool_cursor.cpp index 7e8d20e..419918d 100644 --- a/src/circle_tool_cursor.cpp +++ b/src/circle_tool_cursor.cpp @@ -1,11 +1,12 @@ #include #include #include +#include #include -#include -#include +#include +#include #include "circle_tool_cursor.h" @@ -17,7 +18,7 @@ namespace rviz_tool_cursor CircleToolCursor::CircleToolCursor() : ToolCursor() { - radius_property_ = new rviz::FloatProperty("Tool Radius", 0.210f, + radius_property_ = new rviz_common::properties::FloatProperty("Tool Radius", 0.210f, "The radius of the tool circle display", getPropertyContainer(), SLOT(updateToolVisualization()), this); } @@ -77,4 +78,4 @@ void CircleToolCursor::updateToolVisualization() } // namespace rviz_tool_cursor -PLUGINLIB_EXPORT_CLASS(rviz_tool_cursor::CircleToolCursor, rviz::Tool) +PLUGINLIB_EXPORT_CLASS(rviz_tool_cursor::CircleToolCursor, rviz_common::Tool) diff --git a/src/circle_tool_cursor.h b/src/circle_tool_cursor.h index f75761b..c583e3c 100644 --- a/src/circle_tool_cursor.h +++ b/src/circle_tool_cursor.h @@ -1,13 +1,7 @@ -#ifndef RVIZ_TOOL_CURSOR_CIRCLE_TOOL_CURSOR_H -#define RVIZ_TOOL_CURSOR_CIRCLE_TOOL_CURSOR_H +#pragma once #include -namespace rviz -{ - class FloatProperty; -} - namespace Ogre { class ManualObject; @@ -33,11 +27,9 @@ public Q_SLOTS: virtual Ogre::MovableObject* createToolVisualization() override; - rviz::FloatProperty* radius_property_; + rviz_common::properties::FloatProperty* radius_property_; const std::string object_name_ = "circle_tool_cursor"; }; } // namespace rviz_tool_cursor - -#endif // RVIZ_TOOL_CURSOR_CIRCLE_TOOL_CURSOR_H diff --git a/src/mesh_tool_cursor.cpp b/src/mesh_tool_cursor.cpp index f918c1c..9c3d10e 100644 --- a/src/mesh_tool_cursor.cpp +++ b/src/mesh_tool_cursor.cpp @@ -1,16 +1,19 @@ #include #include +#include #include #include #include +#include +#include #include #include "mesh_tool_cursor.h" -#include -#include +#include +#include -#include +#include const static std::string COLOR_NAME = "mesh_cursor_tool_color"; const static std::string DEFAULT_MESH_RESOURCE = "package://rviz_tool_cursor/resources/default.stl"; @@ -20,7 +23,7 @@ namespace rviz_tool_cursor MeshToolCursor::MeshToolCursor() { - mesh_file_ = new rviz::StringProperty("Mesh Filename", QString(DEFAULT_MESH_RESOURCE.c_str()), + mesh_file_ = new rviz_common::properties::StringProperty("Mesh Filename", QString(DEFAULT_MESH_RESOURCE.c_str()), "The mesh resource to display as a cursor", getPropertyContainer(), SLOT(updateToolVisualization()), this); @@ -45,13 +48,13 @@ MeshToolCursor::~MeshToolCursor() Ogre::MovableObject* MeshToolCursor::createToolVisualization() { // Attempt to load the mesh - Ogre::MeshPtr mesh = rviz::loadMeshFromResource(mesh_file_->getStdString()); + Ogre::MeshPtr mesh = rviz_rendering::loadMeshFromResource(mesh_file_->getStdString()); if(mesh.isNull()) { - ROS_WARN("Loading default mesh..."); +// ROS_WARN("Loading default mesh..."); // Load a default mesh - mesh = rviz::loadMeshFromResource(DEFAULT_MESH_RESOURCE); + mesh = rviz_rendering::loadMeshFromResource(DEFAULT_MESH_RESOURCE); } Ogre::Entity* entity = scene_manager_->createEntity(object_name_, mesh); @@ -83,4 +86,4 @@ void MeshToolCursor::updateToolVisualization() } // namespace rviz_tool_cursor -PLUGINLIB_EXPORT_CLASS(rviz_tool_cursor::MeshToolCursor, rviz::Tool) +PLUGINLIB_EXPORT_CLASS(rviz_tool_cursor::MeshToolCursor, rviz_common::Tool) diff --git a/src/mesh_tool_cursor.h b/src/mesh_tool_cursor.h index be9ec6e..cd3358f 100644 --- a/src/mesh_tool_cursor.h +++ b/src/mesh_tool_cursor.h @@ -1,12 +1,11 @@ -#ifndef MESH_TOOL_CURSOR_H -#define MESH_TOOL_CURSOR_H - +#pragma once #include #include namespace Ogre { class Entity; + class MovableObject; } namespace rviz_tool_cursor @@ -29,7 +28,7 @@ public Q_SLOTS: virtual Ogre::MovableObject* createToolVisualization() override; - rviz::StringProperty* mesh_file_; + rviz_common::properties::StringProperty* mesh_file_; const std::string object_name_ = "mesh_tool_cursor"; @@ -37,5 +36,3 @@ public Q_SLOTS: }; } // namespace rviz_tool_cursor - -#endif // MESH_TOOL_CURSOR_H diff --git a/src/rviz_tool_cursor/get_point_on_plane.cpp b/src/rviz_tool_cursor/get_point_on_plane.cpp new file mode 100644 index 0000000..5c873c9 --- /dev/null +++ b/src/rviz_tool_cursor/get_point_on_plane.cpp @@ -0,0 +1,34 @@ +#include +#include +#include +#include +#include + +#include "rviz_tool_cursor/get_point_on_plane.hpp" + +namespace rviz_rendering +{ +/** Given a viewport and an x,y position in window-pixel coordinates, + * find the point on a plane directly behind it, if any. + * @return true if the intersection exists, false if it does not. */ +bool getPointOnPlaneFromWindowXY(Ogre::Viewport* viewport, + Ogre::Plane& plane, + int window_x, + int window_y, + Ogre::Vector3& intersection_out) +{ + int width = viewport->getActualWidth(); + int height = viewport->getActualHeight(); + + Ogre::Ray mouse_ray = viewport->getCamera()->getCameraToViewportRay((float)window_x / (float)width, + (float)window_y / (float)height); + std::pair intersection = mouse_ray.intersects(plane); + if (!intersection.first) + { + return false; + } + intersection_out = mouse_ray.getPoint(intersection.second); + + return true; +} +} // namespace rviz_rendering diff --git a/src/rviz_tool_cursor/rviz_tool_cursor.cpp b/src/rviz_tool_cursor/rviz_tool_cursor.cpp index c191e90..9f87c0c 100644 --- a/src/rviz_tool_cursor/rviz_tool_cursor.cpp +++ b/src/rviz_tool_cursor/rviz_tool_cursor.cpp @@ -1,19 +1,27 @@ -#include - +#include #include #include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include -#include -#include -#include -#include -#include +#include +#include -#include -#include -#include #include +#include #include @@ -74,7 +82,6 @@ Ogre::Quaternion estimateNormal(const std::vector& points, Eigen::Vector3f norm = evecs.col(2); norm.normalize(); - // The Eigen::Vector3f camera_normal; camera_normal << camera_norm.x, camera_norm.y, camera_norm.z; camera_normal.normalize(); @@ -103,20 +110,20 @@ namespace rviz_tool_cursor { ToolCursor::ToolCursor() - : rviz::Tool() + : rviz_common::Tool() { shortcut_key_ = 'c'; - topic_property_ = new rviz::StringProperty("Topic", "/selection_point", + topic_property_ = new rviz_common::properties::StringProperty("Topic", "/selection_point", "The topic on which to publish points", getPropertyContainer(), SLOT(updateTopic()), this); - patch_size_property_ = new rviz::IntProperty("Patch Size", 10, + patch_size_property_ = new rviz_common::properties::IntProperty("Patch Size", 10, "The number of pixels with which to estimate the surface normal", getPropertyContainer()); - color_property_ = new rviz::ColorProperty("Color", QColor(255, 255, 255), + color_property_ = new rviz_common::properties::ColorProperty("Color", QColor(255, 255, 255), "The color of the tool visualization", getPropertyContainer(), SLOT(updateToolVisualization()), this); @@ -142,7 +149,7 @@ void ToolCursor::onInitialize() // Set the cursors hit_cursor_ = cursor_; - std_cursor_ = rviz::getDefaultCursor(); + std_cursor_ = rviz_common::getDefaultCursor(); } void ToolCursor::activate() @@ -157,10 +164,10 @@ void ToolCursor::deactivate() void ToolCursor::updateTopic() { - pub_ = nh_.advertise(topic_property_->getStdString(), 1, true); +// pub_ = nh_.advertise(topic_property_->getStdString(), 1, true); } -int ToolCursor::processMouseEvent(rviz::ViewportMouseEvent& event) +int ToolCursor::processMouseEvent(rviz_common::ViewportMouseEvent& event) { // Get the 3D point in space indicated by the mouse and a patch of points around it // with which to estimate the surface normal @@ -172,8 +179,12 @@ int ToolCursor::processMouseEvent(rviz::ViewportMouseEvent& event) // Set the visibility of this node off so the selection manager won't choose a point on our cursor mesh in the point and patch cursor_node_->setVisible(false); - bool got_point = context_->getSelectionManager()->get3DPoint(event.viewport, event.x, event.y, position); - bool got_patch = context_->getSelectionManager()->get3DPatch(event.viewport, event.x, event.y, patch_size, patch_size, true, points); + bool got_point = context_->getViewPicker()->get3DPoint(event.panel, event.x, event.y, position); + + // TODO: move to public API (private member function) +// bool got_patch = context_->getViewPicker()->get3DPatch(event.panel, event.x, event.y, patch_size, patch_size, true, points); + + bool got_patch = false; // Revisualize the cursor node cursor_node_->setVisible(true); @@ -181,45 +192,45 @@ int ToolCursor::processMouseEvent(rviz::ViewportMouseEvent& event) if(got_point && got_patch && points.size() > 3) { // Set the cursor - rviz::Tool::setCursor(hit_cursor_); + rviz_common::Tool::setCursor(hit_cursor_); // Estimate the surface normal from the patch of points - Ogre::Quaternion q = estimateNormal(points, event.viewport->getCamera()->getDirection()); + Ogre::Quaternion q = estimateNormal(points, rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(event.panel->getRenderWindow())->getCamera()->getDirection()); cursor_node_->setOrientation(q); cursor_node_->setPosition(position); if(event.leftUp()) { // Publish a point message upon release of the left mouse button - geometry_msgs::PoseStamped msg; - msg.header.frame_id = context_->getFixedFrame().toStdString(); - msg.header.stamp = ros::Time::now(); + auto msg = std::make_unique(); + msg->header.frame_id = context_->getFixedFrame().toStdString(); +// msg->header.stamp = ros::Time::now(); - msg.pose.position.x = static_cast(position.x); - msg.pose.position.y = static_cast(position.y); - msg.pose.position.z = static_cast(position.z); + msg->pose.position.x = static_cast(position.x); + msg->pose.position.y = static_cast(position.y); + msg->pose.position.z = static_cast(position.z); - msg.pose.orientation.w = static_cast(q.w); - msg.pose.orientation.x = static_cast(q.x); - msg.pose.orientation.y = static_cast(q.y); - msg.pose.orientation.z = static_cast(q.z); + msg->pose.orientation.w = static_cast(q.w); + msg->pose.orientation.x = static_cast(q.x); + msg->pose.orientation.y = static_cast(q.y); + msg->pose.orientation.z = static_cast(q.z); - pub_.publish(msg); +// pub_.publish(std::move(msg)); } } else { // Set the standard cursor - rviz::Tool::setCursor(std_cursor_); + rviz_common::Tool::setCursor(std_cursor_); // Project the tool visualization onto the ground Ogre::Plane plane (Ogre::Vector3::UNIT_Z, 0.0f); - rviz::getPointOnPlaneFromWindowXY(event.viewport, plane, event.x, event.y, position); + rviz_rendering::getPointOnPlaneFromWindowXY(rviz_rendering::RenderWindowOgreAdapter::getOgreViewport(event.panel->getRenderWindow()), plane, event.x, event.y, position); cursor_node_->setOrientation(1.0f, 0.0f, 0.0f, 0.0f); cursor_node_->setPosition(position); } - return rviz::Tool::Render; + return rviz_common::Tool::Render; } } // namespace rviz_tool_cursor From 83b6b6a43d40141da9938e1d5459f211cdaf837e Mon Sep 17 00:00:00 2001 From: Joe Schornak Date: Mon, 1 Mar 2021 15:49:20 -0500 Subject: [PATCH 2/8] WIP: more fixes to build successfully --- CMakeLists.txt | 24 +++++++++++++++------ include/rviz_tool_cursor/rviz_tool_cursor.h | 8 +++---- plugin_description.xml | 6 +++--- src/rviz_tool_cursor/rviz_tool_cursor.cpp | 12 ++++++----- 4 files changed, 32 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c6d174a..dcff31e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,14 +26,14 @@ qt5_wrap_cpp(MOC_FILES src/mesh_tool_cursor.h ) -add_library(tool_cursor +add_library(${PROJECT_NAME} src/${PROJECT_NAME}/rviz_tool_cursor.cpp src/${PROJECT_NAME}/get_point_on_plane.cpp src/circle_tool_cursor.cpp src/mesh_tool_cursor.cpp ${MOC_FILES} ) -target_link_libraries(tool_cursor +target_link_libraries(${PROJECT_NAME} Qt5::Widgets ${rclcpp_LIBRARIES} ${rviz_common_LIBRARIES} @@ -41,22 +41,34 @@ target_link_libraries(tool_cursor ${pluginlib_LIBRARIES} Eigen3::Eigen ${PCL_LIBRARIES}) -target_include_directories(tool_cursor +target_include_directories(${PROJECT_NAME} PUBLIC $ PUBLIC $) -target_include_directories(tool_cursor SYSTEM PUBLIC +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PCL_INCLUDE_DIRS}) -ament_target_dependencies(tool_cursor +ament_target_dependencies(${PROJECT_NAME} rclcpp rviz_common rviz_rendering pluginlib) +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${PROJECT_NAME} + rclcpp + rviz_common + rviz_rendering + pluginlib) + +pluginlib_export_plugin_description_file(rviz_common plugin_description.xml) + # Mark executables and/or libraries for installation -install(TARGETS tool_cursor +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) # Mark cpp header files for installation diff --git a/include/rviz_tool_cursor/rviz_tool_cursor.h b/include/rviz_tool_cursor/rviz_tool_cursor.h index b34ea89..6be2c6a 100644 --- a/include/rviz_tool_cursor/rviz_tool_cursor.h +++ b/include/rviz_tool_cursor/rviz_tool_cursor.h @@ -1,7 +1,8 @@ #pragma once #include -#include +#include +#include #include namespace Ogre @@ -66,10 +67,9 @@ public Q_SLOTS: Ogre::MovableObject* movable_obj_; -// ros::NodeHandle nh_; -// rclcpp::Node::SharedPtr node_; + rclcpp::Clock::SharedPtr clock_; -// rclcpp::Publisher::SharedPtr pub_; + rclcpp::Publisher::SharedPtr pub_; rviz_common::properties::StringProperty* topic_property_; diff --git a/plugin_description.xml b/plugin_description.xml index 3363dd6..4922587 100644 --- a/plugin_description.xml +++ b/plugin_description.xml @@ -1,10 +1,10 @@ - - + A cursor representing an input mesh that follows the geometry of mesh surfaces diff --git a/src/rviz_tool_cursor/rviz_tool_cursor.cpp b/src/rviz_tool_cursor/rviz_tool_cursor.cpp index 9f87c0c..4eb3fb7 100644 --- a/src/rviz_tool_cursor/rviz_tool_cursor.cpp +++ b/src/rviz_tool_cursor/rviz_tool_cursor.cpp @@ -164,7 +164,9 @@ void ToolCursor::deactivate() void ToolCursor::updateTopic() { -// pub_ = nh_.advertise(topic_property_->getStdString(), 1, true); + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + pub_ = raw_node->template create_publisher(topic_property_->getStdString(), rclcpp::QoS(1)); + clock_ = raw_node->get_clock(); } int ToolCursor::processMouseEvent(rviz_common::ViewportMouseEvent& event) @@ -182,9 +184,9 @@ int ToolCursor::processMouseEvent(rviz_common::ViewportMouseEvent& event) bool got_point = context_->getViewPicker()->get3DPoint(event.panel, event.x, event.y, position); // TODO: move to public API (private member function) -// bool got_patch = context_->getViewPicker()->get3DPatch(event.panel, event.x, event.y, patch_size, patch_size, true, points); + bool got_patch = context_->getViewPicker()->get3DPatch(event.panel, event.x, event.y, patch_size, patch_size, true, points); - bool got_patch = false; +// bool got_patch = false; // Revisualize the cursor node cursor_node_->setVisible(true); @@ -204,7 +206,7 @@ int ToolCursor::processMouseEvent(rviz_common::ViewportMouseEvent& event) // Publish a point message upon release of the left mouse button auto msg = std::make_unique(); msg->header.frame_id = context_->getFixedFrame().toStdString(); -// msg->header.stamp = ros::Time::now(); + msg->header.stamp = clock_->now(); msg->pose.position.x = static_cast(position.x); msg->pose.position.y = static_cast(position.y); @@ -215,7 +217,7 @@ int ToolCursor::processMouseEvent(rviz_common::ViewportMouseEvent& event) msg->pose.orientation.y = static_cast(q.y); msg->pose.orientation.z = static_cast(q.z); -// pub_.publish(std::move(msg)); + pub_->publish(std::move(msg)); } } else From 480e70b471bd9a86f1194bbfb1bed1db49fd39c5 Mon Sep 17 00:00:00 2001 From: Joe Schornak Date: Mon, 1 Mar 2021 16:23:08 -0500 Subject: [PATCH 3/8] WIP: it builds! --- CMakeLists.txt | 29 ++++++++++++++++------------- package.xml | 3 +-- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dcff31e..bafd060 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,8 +9,8 @@ cmake_policy(SET CMP0057 NEW) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(rviz_rendering REQUIRED) find_package(rviz_common REQUIRED) +find_package(rviz_rendering REQUIRED) find_package(pluginlib REQUIRED) find_package(PCL REQUIRED COMPONENTS common io geometry) #find_package(pcl_ros REQUIRED) @@ -46,19 +46,14 @@ target_include_directories(${PROJECT_NAME} PUBLIC $) target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PCL_INCLUDE_DIRS}) -ament_target_dependencies(${PROJECT_NAME} - rclcpp - rviz_common - rviz_rendering - pluginlib) +target_include_directories(${PROJECT_NAME} PUBLIC + ${pluginlib_INCLUDE_DIRS} + ${rviz_common_INCLUDE_DIRS} + ${rviz_rendering_INCLUDE_DIRS} -ament_export_include_directories(include) -ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(${PROJECT_NAME} - rclcpp - rviz_common - rviz_rendering - pluginlib) + ) +target_compile_definitions(${PROJECT_NAME} PRIVATE "RVIZ_DEFAULT_PLUGINS_BUILDING_LIBRARY") +target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") pluginlib_export_plugin_description_file(rviz_common plugin_description.xml) @@ -84,4 +79,12 @@ install(DIRECTORY resources DESTINATION share/${PROJECT_NAME} ) +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies(${PROJECT_NAME} + rclcpp + rviz_common + rviz_rendering + pluginlib) + ament_package() diff --git a/package.xml b/package.xml index e0a97cf..82480a1 100644 --- a/package.xml +++ b/package.xml @@ -13,9 +13,8 @@ rviz_common rviz_rendering pluginlib - - + ament_cmake From 23111024651fad980defa223b9d1a54004eb0ef5 Mon Sep 17 00:00:00 2001 From: Joe Schornak Date: Mon, 1 Mar 2021 20:38:32 -0500 Subject: [PATCH 4/8] remaining fixes for functional plugin --- CMakeLists.txt | 2 +- src/mesh_tool_cursor.cpp | 4 ++-- src/rviz_tool_cursor/rviz_tool_cursor.cpp | 6 ++---- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bafd060..823ff66 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,7 +26,7 @@ qt5_wrap_cpp(MOC_FILES src/mesh_tool_cursor.h ) -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}/rviz_tool_cursor.cpp src/${PROJECT_NAME}/get_point_on_plane.cpp src/circle_tool_cursor.cpp diff --git a/src/mesh_tool_cursor.cpp b/src/mesh_tool_cursor.cpp index 9c3d10e..c318741 100644 --- a/src/mesh_tool_cursor.cpp +++ b/src/mesh_tool_cursor.cpp @@ -27,7 +27,7 @@ MeshToolCursor::MeshToolCursor() "The mesh resource to display as a cursor", getPropertyContainer(), SLOT(updateToolVisualization()), this); - material_ = Ogre::MaterialManager::getSingletonPtr()->create(COLOR_NAME, "rviz"); + material_ = Ogre::MaterialManager::getSingletonPtr()->create(COLOR_NAME, "rviz_rendering"); const Ogre::ColourValue& color = color_property_->getOgreColor(); material_->getTechnique(0)->getPass(0)->setDiffuse(color.r, color.g, color.b, 1.0); @@ -42,7 +42,7 @@ MeshToolCursor::~MeshToolCursor() scene_manager_->destroyEntity(object_name_); scene_manager_->destroySceneNode(cursor_node_); } - Ogre::MaterialManager::getSingletonPtr()->remove(COLOR_NAME); + Ogre::MaterialManager::getSingletonPtr()->remove(COLOR_NAME, "rviz_rendering"); } Ogre::MovableObject* MeshToolCursor::createToolVisualization() diff --git a/src/rviz_tool_cursor/rviz_tool_cursor.cpp b/src/rviz_tool_cursor/rviz_tool_cursor.cpp index 4eb3fb7..b9deb35 100644 --- a/src/rviz_tool_cursor/rviz_tool_cursor.cpp +++ b/src/rviz_tool_cursor/rviz_tool_cursor.cpp @@ -126,8 +126,6 @@ ToolCursor::ToolCursor() color_property_ = new rviz_common::properties::ColorProperty("Color", QColor(255, 255, 255), "The color of the tool visualization", getPropertyContainer(), SLOT(updateToolVisualization()), this); - - updateTopic(); } ToolCursor::~ToolCursor() @@ -150,6 +148,8 @@ void ToolCursor::onInitialize() // Set the cursors hit_cursor_ = cursor_; std_cursor_ = rviz_common::getDefaultCursor(); + + updateTopic(); } void ToolCursor::activate() @@ -186,8 +186,6 @@ int ToolCursor::processMouseEvent(rviz_common::ViewportMouseEvent& event) // TODO: move to public API (private member function) bool got_patch = context_->getViewPicker()->get3DPatch(event.panel, event.x, event.y, patch_size, patch_size, true, points); -// bool got_patch = false; - // Revisualize the cursor node cursor_node_->setVisible(true); From eb4052b8034c5ccbf3213049c2df54f1b8f39041 Mon Sep 17 00:00:00 2001 From: Joe Schornak Date: Fri, 5 Mar 2021 22:30:51 -0500 Subject: [PATCH 5/8] add ROS2 CI and PCL dependency --- .github/workflows/main.yml | 20 ++++---------------- CMakeLists.txt | 1 - package.xml | 3 ++- 3 files changed, 6 insertions(+), 18 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 0a64444..623a9f9 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -2,8 +2,8 @@ name: CI on: push: - branches: - - master +# branches: +# - master pull_request: branches: - master @@ -14,24 +14,12 @@ jobs: fail-fast: false matrix: env: - - {CI_NAME: xenial, - OS_NAME: ubuntu, - OS_CODE_NAME: xenial, - ROS_DISTRO: kinetic, - ROS_REPO: main, - DOCKER_IMAGE: "ros:kinetic"} - - {CI_NAME: bionic, - OS_NAME: ubuntu, - OS_CODE_NAME: bionic, - ROS_DISTRO: melodic, - ROS_REPO: main, - DOCKER_IMAGE: "ros:melodic"} - {CI_NAME: focal, OS_NAME: ubuntu, OS_CODE_NAME: focal, - ROS_DISTRO: noetic, + ROS_DISTRO: foxy, ROS_REPO: main, - DOCKER_IMAGE: "ros:noetic"} + DOCKER_IMAGE: "ros:foxy"} runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 diff --git a/CMakeLists.txt b/CMakeLists.txt index 823ff66..57dcb36 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,7 +13,6 @@ find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) find_package(pluginlib REQUIRED) find_package(PCL REQUIRED COMPONENTS common io geometry) -#find_package(pcl_ros REQUIRED) find_package(Eigen3 REQUIRED) diff --git a/package.xml b/package.xml index 82480a1..1938dac 100644 --- a/package.xml +++ b/package.xml @@ -10,9 +10,10 @@ Apache 2.0 ament_cmake + libpcl-all-dev + pluginlib rviz_common rviz_rendering - pluginlib ament_cmake From 4e5f7b000edb417a7ccb9722b83b211392d9ef1f Mon Sep 17 00:00:00 2001 From: Joe Schornak Date: Fri, 5 Mar 2021 22:32:29 -0500 Subject: [PATCH 6/8] alphabetize find_packages --- CMakeLists.txt | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 57dcb36..af4ec54 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,13 +8,12 @@ set(CMAKE_CXX_EXTENSIONS OFF) cmake_policy(SET CMP0057 NEW) find_package(ament_cmake REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED COMPONENTS common io geometry) +find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) -find_package(pluginlib REQUIRED) -find_package(PCL REQUIRED COMPONENTS common io geometry) - -find_package(Eigen3 REQUIRED) # QT find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) From b367e770a20fdaed48f8b314a802e73178d843d7 Mon Sep 17 00:00:00 2001 From: Joe Schornak Date: Sat, 6 Mar 2021 15:44:36 -0500 Subject: [PATCH 7/8] add a smaller arrow mesh --- resources/default_10cm.stl | Bin 0 -> 15084 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 resources/default_10cm.stl diff --git a/resources/default_10cm.stl b/resources/default_10cm.stl new file mode 100644 index 0000000000000000000000000000000000000000..acf282d0f3afcda712437ff1e908d1f7094994ba GIT binary patch literal 15084 zcmbW6dyEy;9mfyWM-Ws*Ar)%DSXv)sm%Ud|cE>wN3Rc+_lf{uh~oD5JNNfH^PPL=?A>(# zfCK0InKN^K&vUkYPV4xq7hS*TM_1mw==xo)y99zBQIW+E1)lc6l)7{oh*x zX+P0)RYTB!$qSZ1+E3g&;c&~=Z7*5^X+QCN=ff>uoA_r-AnhkQxAZm7UDRs{qz5E= zlt9`~&~wZBRzx`3Ptd!U^~;g=6SO~(?k>`Pf@~0}1(Eg>Wamh|1Zh7(z7(k^BJC&0 zUnBK)r2PcNM5Gagw4b23i!^GH_7fD_k!A*@{RHKYNV5;peu8pTq&W&{OLSJanhY1+ zKew$c8P3uEHTX@A@7ZAqUPEa=!C$*!`{+nAoTL2&@8~bvXIKJhKOt7L1k!#&Ji`)5 z`w8(rOCaqh#KSFtw4abDvINq8LL$`?Nc#!N29`kDPe{hG1k#S^G$!j;lGQAMv?FMc zCFr@Fca`9bYR7(p-ko#1zLlf>1nm#k4@)5JC&&g|qbz~6pCCJP9kc|}eu8|7Ypo@a z_7mi<})v#SVHi6;v2k0W-L9)ECT_>C$8RpRYJ>|6az>CpDChHMwK2dX5Zgg9gI z9i?#D88vT(Dv4SlR)y!3uDrc5thNWLB(Df@_cPC*|G^)NVYSUsCAmuoe)o-q$u$J3 zB*Ud^Y|dK)Rgy*1?}+oh16A}*uAei8_A1WXD^$_Gxqi+VvPYa}4^)wDTt8xqi<2Zj$G_s3Jdh{hW=DBriTtMX}=gIU9pXUJRm&;?ebU zHqMj0I7b!5yzA#|E=lro397hT(!65xRg#ylP=)CaW5N}Va-wIXY`9erUCDpr&Dv2&3 zBx-Bk3RM!pLQsa|%#c*u167hGgrE$^*(a&C2dX3!2_YG-hCmgKp(B$lnnySltp^>M zWa^slKoxzH>*s8S<7!Z|SE!jqB%ZhU2PMV{=rI{art2GaOgW znzurgR8iSo@?ENbwZ4lg@?+P}RXTvY_&^oKitFcW42F3zh$@Ol*UwctnY=hh6~(;k z=WH$s^KuEQD6hDFuF{d^;$s;M|B8P9k_TD zofC!!1Ahmq7Tj|L`R=K82vn_D_D0K?_b1jNP}OwRGcEm>Oszwp>fQ;rQk+k(L!j#U z&QmCtTvUfZRp*vA%0#VMqO;mdP(|aO8luLpP(|yN2lZA?Mc+g{+#tT(sc7HIgW6rE zBHNS)WkIJR`%@1$2rqFe@-eESgYZPBB0r`cZV=w?R1_=Gpd!ktC?2ChMXgg&%twRD z3{FLPB^p%raVpAP(WURbb=tsDiz->8XJ7(V@*U-Y2~^2m)ge$N_NYUkN^CB~w;wD& z*L=#5$<;B4D)B4M_mtZY+;Vxbv3yOjnn0EKt`HCYBq;9by1j-#mBfb--|QGuy!Dn< zH3X_828Gx;;)BA6$FHs-P$h9L#O`H}71mt%>ly-8l1qg6aXh{-|G39$2vkWX5`rt> z=a)WSL!gSroDg`mCa%G)bcNo5d%?9rWP4^&C@5rS;q zoo91YNre-F{Hi<8uTUjbR0#6jbMkx_RZ^+z#0RRR8>mB|N;-}@1gfN~sY9SjI;S-I zP}St0-2Td{usf@0D}U---xF z`w4pYvVJ+zeuDO=thJJNoFVj|Lr zLfTJI+(jC-Nc#zj?MO2N(td)wCCxra`w8wrG)EzAiOvdFli{LGbNhPOcaiq5!EZ9I zXI7aBUPEa=!C%|8{Q*lL?I(Cg?{9zE5=i?Aw%RLOKe7bUeu6z?NMU%y-+{EBVDB5- zcd8|j_7m*kDjrV<7eU9 zZ-%9n8;hr}qq-pS(K!Q~&FwzhC%p7tSETHrJX+cgRG%M8q{G|1LwIipS~g$2$|| zhWpPR8m^tQs|#n4U{BOaZOq$5{F{h7_iV`!sN!*YCv*9<8R2;ir-%JZ_jch75*#1e zKNvHQh$%!Y9nqg5P{rf)rl_OT7iwq!5L0FKx4i~ z#BYeW>CmtYfhr!ScYS~PX;j*E)b-(SUpl!PXOQ5$t9_|4|0LpXL{JQ72vqSny@9-G z{qoYupLT>Dn=a_a86>zGXt!*P5>GFlo*_`h&O0_fVvdiVNevkSRXk2(kfJuxIL8?z zDCXS>$;P=QIL?tk6_3+gLfI$Le1$VeQ0{VPDx0q?!TAaaRPi{?iIjH}&b#Lw{GtnI zkf0pwPGL6hCY*OiHSEg}sN!*&+o^seTtBw%-NKpL%^*Rw#GTu0{Ybcew6A)_fSJd`0&#pATrQgu##LG^qT)pMLdf@-@v z8`^rFa0kHk90^qMIPDpz%SgB{8Mpsv?f~2j64ax()1>W767EZGI5aFnpo+(7??e4n z!u{3GH>PgH86>hNNG7ftg3s$|#Y&hiGb`HFMb#wRSFU%(k8*s|JR zJDbOE?OvK8P$j+~YfwkVo*2J6`{@GCAi;jDUBC0)_$vq3We8MB6v!IXk#R&NPtMs@ zz!@Ys9`!8ZVlbf?%n+!OXqGjoBje1FOg{KU0cVilyrL&0mrD}r;W7lOBrC~XIOoK! zHO)z3$B9LpL4q@%p4%!pk%=PFak5U7%z zFKbXo7C&{_70J8P8;dxD1lKD)<5p@E6E93^$PlQKYA0(@N5+*iS@p@(BF-SebyrW` zuGS_mos=a|B~@G2xRH8>k)L)XH*dP2h%-oVXQ1~1#wf93&$JAID(NL;4e5Q78Q0A$ z;tUep;pi=e>wS`;tyuz9(i6!Vm2+xm(%*k!5of9hcN3!L+FZh^h=$*JNoQ%l17~=w z?@r(3?oM1UYQF;sRPmU4xYQmcwFk~1LAG(XE!G~ESig8W@2-0ms(4I2oU9?8zV$1d zL4thD-OsqQLGrIBn%U;=S*YSM^>DHVpI9_La0UsA6?dakIpZ*KbXya9yL%R@cuYN< ztU*!BXEht=ID-Vmyt@Okac&8Yb0kp3W9s2#4L(6?zQP$KD0jJAqsp06^A!@P;xYAb zvc`8SC*h<>c^7Appd9S(nT%1QgYqsCsNym8aB2PE6TPh;ID-V$5_dD@?f{YxqSraM zyJw+_$JE26b&zi+Y#qcIB&a63yDWE?5z}1;*C_WaRPmU4I9Y@0dC2uVn$p3Q)6F13 zwcXu*xjUJddbkXMDjri0Cu_(}Q9OSC(cA&J86>Djarb5JZYic7E<>P-$JE0~@56Uw f+%4^TV`>x5AVIwr{GL+@>ftg3s(7roZN~f$ Date: Mon, 12 Apr 2021 13:42:45 -0600 Subject: [PATCH 8/8] fix calculation of the patch normal vector relative to the camera --- src/rviz_tool_cursor/rviz_tool_cursor.cpp | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/src/rviz_tool_cursor/rviz_tool_cursor.cpp b/src/rviz_tool_cursor/rviz_tool_cursor.cpp index b9deb35..97c2613 100644 --- a/src/rviz_tool_cursor/rviz_tool_cursor.cpp +++ b/src/rviz_tool_cursor/rviz_tool_cursor.cpp @@ -60,6 +60,8 @@ Eigen::Matrix3f createMatrix(const Eigen::Vector3f& norm) Ogre::Quaternion estimateNormal(const std::vector& points, const Ogre::Vector3& camera_norm) { + (void) camera_norm; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud ()); for(std::size_t i = 0; i < points.size(); ++i) @@ -76,17 +78,26 @@ Ogre::Quaternion estimateNormal(const std::vector& points, pcl::PCA pca; pca.setInputCloud(cloud); Eigen::Matrix3f& evecs = pca.getEigenVectors(); + Eigen::Vector3f& evalues = pca.getEigenValues(); // Get the eigenvector associated with the smallest eigenvalue // (should be Z-axis, assuming points are relatively planar) Eigen::Vector3f norm = evecs.col(2); norm.normalize(); - Eigen::Vector3f camera_normal; - camera_normal << camera_norm.x, camera_norm.y, camera_norm.z; - camera_normal.normalize(); + // Alternate camera-relative normal vector check that doesn't require the camera vector: + // The patch points begin at the top left corner of the patch and end at the bottom right corner. + // The vector that goes between these points will point down and to the right relative to the RViz camera frame. + pcl::PointXYZ first_pt = cloud->front(); + pcl::PointXYZ last_pt = cloud->back(); + Eigen::Vector3f patch_span(last_pt.x - first_pt.x, last_pt.y - first_pt.y, last_pt.z - first_pt.z); + + // This vector (most of the time) points at least a little bit to the right, so its cross product with UnitZ + // will have a component pointing out of the screen. + Eigen::Vector3f out_vec = patch_span.cross(Eigen::Vector3f::UnitZ()); - if(norm.dot(camera_normal) < 0) + // Determine if the norm is pointing into the surface relative to the camera. If not, reverse it. + if(norm.dot(out_vec) > 0) { norm *= -1; }