From 44780cb535588c9e4203d63973ce1207df2cfd75 Mon Sep 17 00:00:00 2001 From: BryanMqz Date: Wed, 20 Mar 2024 12:37:41 -0500 Subject: [PATCH 1/6] Migration to ROS 2 Foxy, 'colcon build' finished successfully. --- CMakeLists.txt | 30 ++++++---- package.xml | 11 ++-- src/rviz_tool_path_display.cpp | 101 ++++++++++++++++++++------------- src/rviz_tool_path_display.h | 47 +++++++-------- 4 files changed, 110 insertions(+), 79 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3412eed..75624cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(rviz_tool_path_display) set(CMAKE_CXX_STANDARD 14) @@ -6,22 +6,28 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) find_package(Qt5 REQUIRED COMPONENTS Widgets) -find_package(catkin REQUIRED COMPONENTS rviz pluginlib) +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_rendering REQUIRED) -catkin_package(CATKIN_DEPENDS rviz pluginlib) - -include_directories(${catkin_INCLUDE_DIRS}) +include_directories(include ${Boost_INCLUDE_DIRS}) qt5_wrap_cpp(MOC_FILES src/rviz_tool_path_display.h) -add_library(${PROJECT_NAME} src/rviz_tool_path_display.cpp ${MOC_FILES}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} Qt5::Widgets) + +add_library(${PROJECT_NAME} SHARED src/rviz_tool_path_display.cpp ${MOC_FILES}) +ament_target_dependencies(${PROJECT_NAME} geometry_msgs rclcpp rviz_common rviz_rendering) install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(DIRECTORY icons DESTINATION share/${PROJECT_NAME}/) -install(DIRECTORY icons DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(FILES plugin_description.xml DESTINATION share/${PROJECT_NAME}/) -install(FILES plugin_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +ament_package() \ No newline at end of file diff --git a/package.xml b/package.xml index 503c8c6..3ed116c 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,5 @@ - + rviz_tool_path_display 0.1.1 The rviz_tool_path_display package @@ -8,11 +8,14 @@ Michael Ripperger Apache 2.0 - catkin - rviz + ament_cmake + rclcpp + rviz_common + rviz_ogre_vendor pluginlib - qtbase5-dev + qtbase5-dev libqt5-widgets + geometry_msgs diff --git a/src/rviz_tool_path_display.cpp b/src/rviz_tool_path_display.cpp index 458e7d5..14461d9 100644 --- a/src/rviz_tool_path_display.cpp +++ b/src/rviz_tool_path_display.cpp @@ -28,31 +28,28 @@ */ #include "rviz_tool_path_display.h" +#include +#include +#include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include - -#include +#include +#include +#include +#include namespace { -Ogre::Vector3 fromMsg(geometry_msgs::Point const& point) +Ogre::Vector3 fromMsg(geometry_msgs::msg::Point const& point) { return Ogre::Vector3(point.x, point.y, point.z); } -Ogre::Quaternion fromMsg(geometry_msgs::Quaternion const& quaternion) +Ogre::Quaternion fromMsg(geometry_msgs::msg::Quaternion const& quaternion) { Ogre::Quaternion q; - rviz::normalizeQuaternion(quaternion, q); + q.normalise(); return q; } @@ -68,34 +65,33 @@ void updateMaterialColor(Ogre::MaterialPtr material, const QColor& color, const } } // namespace -namespace rviz +namespace rviz_common { ToolPathDisplay::ToolPathDisplay() { axes_visibility_property_ = new BoolProperty("Show Axes", true, "Toggles the visibility of the axes display", this, SLOT(updateAxesVisibility())); axes_length_property_ = - new FloatProperty("Axes Length", 0.3, "Length of each axis, in meters.", this, SLOT(updateAxesGeometry())); + new properties::FloatProperty("Axes Length", 0.3, "Length of each axis, in meters.", this, SLOT(updateAxesGeometry())); axes_radius_property_ = - new FloatProperty("Axes Radius", 0.01, "Radius of each axis, in meters.", this, SLOT(updateAxesGeometry())); + new properties::FloatProperty("Axes Radius", 0.01, "Radius of each axis, in meters.", this, SLOT(updateAxesGeometry())); pts_visibility_property_ = new BoolProperty("Show Points", true, "Toggles the visibility of the points display", this, SLOT(updatePtsVisibility())); - pts_color_property_ = new ColorProperty("Points Color", QColor(255, 255, 255), "The color of the points display", + pts_color_property_ = new properties::ColorProperty("Points Color", QColor(255, 255, 255), "The color of the points display", this, SLOT(updatePtsColor())); pts_size_property_ = - new FloatProperty("Points Size", 5.0, "The size of the points (pixels)", this, SLOT(updatePtsSize())); + new properties::FloatProperty("Points Size", 5.0, "The size of the points (pixels)", this, SLOT(updatePtsSize())); lines_visibility_property_ = new BoolProperty("Show Lines", true, "Toggles the visibility of the lines display", this, SLOT(updateLinesVisibility())); - lines_color_property_ = new ColorProperty("Lines Color", QColor(255, 255, 255), "The color of the lines display", + lines_color_property_ = new properties::ColorProperty("Lines Color", QColor(255, 255, 255), "The color of the lines display", this, SLOT(updateLinesColor())); text_visibility_property_ = new BoolProperty("Show Text", true, "Toggles the visibility of the text display", this, SLOT(updateTextVisibility())); - text_size_property_ = - new FloatProperty("Text Size", 0.1f, "Height of the text display (m)", this, SLOT(updateTextSize())); + text_size_property_ =new properties::FloatProperty("Text Size", 0.1f, "Height of the text display (m)", this, SLOT(updateTextSize())); } ToolPathDisplay::~ToolPathDisplay() @@ -144,13 +140,13 @@ void ToolPathDisplay::onInitialize() // Start/End text { start_text_node_ = scene_node_->createChildSceneNode(); - start_text_ = new MovableText("0", "Liberation Sans"); - start_text_->setTextAlignment(MovableText::H_CENTER, MovableText::V_BELOW); + start_text_ = new rviz_rendering::MovableText("0", "Liberation Sans"); + start_text_->setTextAlignment(rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_BELOW); start_text_node_->attachObject(start_text_); end_text_node_ = scene_node_->createChildSceneNode(); - end_text_ = new MovableText("0", "Liberation Sans"); - end_text_->setTextAlignment(MovableText::H_CENTER, MovableText::V_BELOW); + end_text_ = new rviz_rendering::MovableText("0", "Liberation Sans"); + end_text_->setTextAlignment(rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_BELOW); end_text_node_->attachObject(end_text_); } @@ -162,33 +158,57 @@ void ToolPathDisplay::onInitialize() updateTextSize(); } -bool validateFloats(const geometry_msgs::PoseArray& msg) +bool validateFloats(const geometry_msgs::msg::PoseArray& msg) { return validateFloats(msg.poses); } -void ToolPathDisplay::processMessage(const geometry_msgs::PoseArray::ConstPtr& msg) +bool validateQuaternions(const geometry_msgs::msg::PoseArray& pose) { + for (const auto& pose : pose.poses) { + // Extract quaternion components + double qw = pose.orientation.w; + double qx = pose.orientation.x; + double qy = pose.orientation.y; + double qz = pose.orientation.z; + + // Calculate the quaternion norm + double norm = std::sqrt(qw*qw + qx*qx + qy*qy + qz*qz); + + // Set a tolerance for normalization + const double tolerance = 1e-6; + + // Check if the quaternion is approximately normalized + if (std::abs(norm - 1.0) > tolerance) { + // Handle invalid quaternion (not normalized) + return false; + } + } + return true; +} + +void ToolPathDisplay::processMessage(const std::shared_ptr msg) { + auto node = std::make_shared("processMessage_node"); if (!validateFloats(*msg)) { - setStatus(StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)"); + setStatus(properties::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)"); return; } - if (!validateQuaternions(msg->poses)) + if (!validateQuaternions(*msg)) { - ROS_WARN_ONCE_NAMED("quaternions", - "PoseArray msg received on topic '%s' contains unnormalized quaternions. " - "This warning will only be output once but may be true for others; " + RCLCPP_WARN(node->get_logger(),"quaternions" + "PoseArray msg received on topic '%s' contains unnormalized quaternions." + "This warning will only be output once but may be true for others;" "enable DEBUG messages for ros.rviz.quaternions to see more details.", topic_property_->getTopicStd().c_str()); - ROS_DEBUG_NAMED("quaternions", "PoseArray msg received on topic '%s' contains unnormalized quaternions.", + RCLCPP_DEBUG(node->get_logger(), "quaternions", "PoseArray msg received on topic '%s' contains unnormalized quaternions.", topic_property_->getTopicStd().c_str()); } if (!setTransform(msg->header)) { - setStatus(StatusProperty::Error, "Topic", "Failed to look up transform"); + setStatus(properties::StatusProperty::Error, "Topic", "Failed to look up transform"); return; } @@ -203,13 +223,14 @@ void ToolPathDisplay::processMessage(const geometry_msgs::PoseArray::ConstPtr& m context_->queueRender(); } -bool ToolPathDisplay::setTransform(std_msgs::Header const& header) +bool ToolPathDisplay::setTransform(std_msgs::msg::Header const& header) { + auto node = std::make_shared("setTransform_node"); Ogre::Vector3 position; Ogre::Quaternion orientation; if (!context_->getFrameManager()->getTransform(header, position, orientation)) { - ROS_ERROR("Error transforming pose '%s' from frame '%s' to frame '%s'", qPrintable(getName()), + RCLCPP_ERROR(node->get_logger(), "Error transforming pose '%s' from frame '%s' to frame '%s'", qPrintable(getName()), header.frame_id.c_str(), qPrintable(fixed_frame_)); return false; } @@ -241,9 +262,9 @@ void ToolPathDisplay::updateAxes() axes_node_->setVisible(axes_visibility_property_->getBool()); } -Axes* ToolPathDisplay::makeAxes() +rviz_rendering::Axes* ToolPathDisplay::makeAxes() { - return new Axes(scene_manager_, axes_node_, axes_length_property_->getFloat(), axes_radius_property_->getFloat()); + return new rviz_rendering::Axes(scene_manager_, axes_node_, axes_length_property_->getFloat(), axes_radius_property_->getFloat()); } void ToolPathDisplay::updatePoints() @@ -374,7 +395,7 @@ void ToolPathDisplay::updateTextSize() end_text_->setCharacterHeight(height); } -} // namespace rviz +} // namespace rviz_common #include -PLUGINLIB_EXPORT_CLASS(rviz::ToolPathDisplay, rviz::Display) +PLUGINLIB_EXPORT_CLASS(rviz_common::ToolPathDisplay, rviz_common::MessageFilterDisplay) diff --git a/src/rviz_tool_path_display.h b/src/rviz_tool_path_display.h index d197ce1..d5cefd2 100644 --- a/src/rviz_tool_path_display.h +++ b/src/rviz_tool_path_display.h @@ -1,17 +1,18 @@ -#include -#include -#include - +#include +#include +#include +#include +#include +#include +#include +#include +#include #include -namespace rviz +namespace rviz_common { -class ColorProperty; -class FloatProperty; -class Axes; -class MovableText; -class ToolPathDisplay : public rviz::MessageFilterDisplay +class ToolPathDisplay : public rviz_common::MessageFilterDisplay { Q_OBJECT public: @@ -21,13 +22,13 @@ class ToolPathDisplay : public rviz::MessageFilterDisplay msg) override; private: - bool setTransform(std_msgs::Header const& header); + bool setTransform(std_msgs::msg::Header const& header); void updateAxes(); void updateDisplay(); - rviz::Axes* makeAxes(); + rviz_rendering::Axes* makeAxes(); void updatePoints(); void updateLines(); void updateText(); @@ -41,32 +42,32 @@ class ToolPathDisplay : public rviz::MessageFilterDisplay poses_; // Axes Display - boost::ptr_vector axes_; + boost::ptr_vector axes_; Ogre::SceneNode* axes_node_; - rviz::FloatProperty* axes_length_property_; - rviz::FloatProperty* axes_radius_property_; + properties::FloatProperty* axes_length_property_; + properties::FloatProperty* axes_radius_property_; BoolProperty* axes_visibility_property_; // Points Display Ogre::ManualObject* pts_object_; Ogre::MaterialPtr pts_material_; BoolProperty* pts_visibility_property_; - ColorProperty* pts_color_property_; - FloatProperty* pts_size_property_; + properties::ColorProperty* pts_color_property_; + properties::FloatProperty* pts_size_property_; // Lines Display Ogre::ManualObject* lines_object_; Ogre::MaterialPtr lines_material_; BoolProperty* lines_visibility_property_; - ColorProperty* lines_color_property_; + properties::ColorProperty* lines_color_property_; // Text Ogre::SceneNode* start_text_node_; - MovableText* start_text_; + rviz_rendering::MovableText* start_text_; Ogre::SceneNode* end_text_node_; - MovableText* end_text_; + rviz_rendering::MovableText* end_text_; BoolProperty* text_visibility_property_; - FloatProperty* text_size_property_; + properties::FloatProperty* text_size_property_; private Q_SLOTS: void updateAxesGeometry(); @@ -83,4 +84,4 @@ private Q_SLOTS: void updateTextSize(); }; -} // namespace rviz +} // namespace rviz_common From 2c09202c56daa182756717db23937da759aded3b Mon Sep 17 00:00:00 2001 From: BryanMqz Date: Thu, 21 Mar 2024 16:12:57 -0500 Subject: [PATCH 2/6] Plugin showes up in rviz2 --- CMakeLists.txt | 33 +++++++++++++++++---- package.xml | 14 +++++++-- plugin_description.xml | 8 +++-- src/rviz_tool_path_display.cpp | 53 ++++++++++++++++++++-------------- src/rviz_tool_path_display.h | 37 +++++++++++++++++------- 5 files changed, 102 insertions(+), 43 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 75624cf..8249ec2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -find_package(Qt5 REQUIRED COMPONENTS Widgets) +find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) @@ -16,18 +16,41 @@ find_package(rviz_rendering REQUIRED) include_directories(include ${Boost_INCLUDE_DIRS}) qt5_wrap_cpp(MOC_FILES src/rviz_tool_path_display.h) - add_library(${PROJECT_NAME} SHARED src/rviz_tool_path_display.cpp ${MOC_FILES}) -ament_target_dependencies(${PROJECT_NAME} geometry_msgs rclcpp rviz_common rviz_rendering) + +target_include_directories(${PROJECT_NAME} PUBLIC +${Qt5Widgets_INCLUDE_DIRS} +${OGRE_INCLUDE_DIRS} +) + +target_link_libraries(${PROJECT_NAME} +rviz_common::rviz_common +) + +pluginlib_export_plugin_description_file(rviz_common plugin_description.xml) + +ament_target_dependencies(${PROJECT_NAME} Qt5 geometry_msgs rclcpp rviz_common rviz_rendering pluginlib) +ament_export_include_directories(include) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + Qt5 + rviz_common + geometry_msgs + rclcpp +) install( TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) -install(DIRECTORY icons DESTINATION share/${PROJECT_NAME}/) +install(DIRECTORY icons DESTINATION include/${PROJECT_NAME}/) install(FILES plugin_description.xml DESTINATION share/${PROJECT_NAME}/) + ament_package() \ No newline at end of file diff --git a/package.xml b/package.xml index 3ed116c..08f15e7 100644 --- a/package.xml +++ b/package.xml @@ -1,23 +1,31 @@ rviz_tool_path_display - 0.1.1 + 0.2.1 The rviz_tool_path_display package Michael Ripperger Michael Ripperger Apache 2.0 + libqt5-core + libqt5-gui + libqt5-opengl + libqt5-widgets + ament_cmake + qtbase5-dev rclcpp + rviz2 rviz_common rviz_ogre_vendor + rviz_rendering + rviz_default_plugins pluginlib - qtbase5-dev - libqt5-widgets geometry_msgs + ament_cmake diff --git a/plugin_description.xml b/plugin_description.xml index 9ddd6e2..ae82969 100644 --- a/plugin_description.xml +++ b/plugin_description.xml @@ -1,8 +1,12 @@ - - + + Rviz display for visualizing tool paths + geometry_msgs/msg/PoseArray diff --git a/src/rviz_tool_path_display.cpp b/src/rviz_tool_path_display.cpp index 14461d9..448862f 100644 --- a/src/rviz_tool_path_display.cpp +++ b/src/rviz_tool_path_display.cpp @@ -32,12 +32,16 @@ #include #include #include +#include #include #include #include #include #include #include +#include +#include +#include namespace { @@ -69,29 +73,29 @@ namespace rviz_common { ToolPathDisplay::ToolPathDisplay() { - axes_visibility_property_ = new BoolProperty("Show Axes", true, "Toggles the visibility of the axes display", this, + axes_visibility_property_ = new rviz_common::properties::BoolProperty("Show Axes", true, "Toggles the visibility of the axes display", this, SLOT(updateAxesVisibility())); axes_length_property_ = - new properties::FloatProperty("Axes Length", 0.3, "Length of each axis, in meters.", this, SLOT(updateAxesGeometry())); + new rviz_common::properties::FloatProperty("Axes Length", 0.3, "Length of each axis, in meters.", this, SLOT(updateAxesGeometry())); axes_radius_property_ = - new properties::FloatProperty("Axes Radius", 0.01, "Radius of each axis, in meters.", this, SLOT(updateAxesGeometry())); + new rviz_common::properties::FloatProperty("Axes Radius", 0.01, "Radius of each axis, in meters.", this, SLOT(updateAxesGeometry())); - pts_visibility_property_ = new BoolProperty("Show Points", true, "Toggles the visibility of the points display", this, + pts_visibility_property_ = new rviz_common::properties::BoolProperty("Show Points", true, "Toggles the visibility of the points display", this, SLOT(updatePtsVisibility())); - pts_color_property_ = new properties::ColorProperty("Points Color", QColor(255, 255, 255), "The color of the points display", + pts_color_property_ = new rviz_common::properties::ColorProperty("Points Color", QColor(255, 255, 255), "The color of the points display", this, SLOT(updatePtsColor())); pts_size_property_ = - new properties::FloatProperty("Points Size", 5.0, "The size of the points (pixels)", this, SLOT(updatePtsSize())); + new rviz_common::properties::FloatProperty("Points Size", 5.0, "The size of the points (pixels)", this, SLOT(updatePtsSize())); - lines_visibility_property_ = new BoolProperty("Show Lines", true, "Toggles the visibility of the lines display", this, + lines_visibility_property_ = new rviz_common::properties::BoolProperty("Show Lines", true, "Toggles the visibility of the lines display", this, SLOT(updateLinesVisibility())); - lines_color_property_ = new properties::ColorProperty("Lines Color", QColor(255, 255, 255), "The color of the lines display", + lines_color_property_ = new rviz_common::properties::ColorProperty("Lines Color", QColor(255, 255, 255), "The color of the lines display", this, SLOT(updateLinesColor())); - text_visibility_property_ = new BoolProperty("Show Text", true, "Toggles the visibility of the text display", this, + text_visibility_property_ = new rviz_common::properties::BoolProperty("Show Text", true, "Toggles the visibility of the text display", this, SLOT(updateTextVisibility())); - text_size_property_ =new properties::FloatProperty("Text Size", 0.1f, "Height of the text display (m)", this, SLOT(updateTextSize())); + text_size_property_ =new rviz_common::properties::FloatProperty("Text Size", 0.1f, "Height of the text display (m)", this, SLOT(updateTextSize())); } ToolPathDisplay::~ToolPathDisplay() @@ -158,12 +162,12 @@ void ToolPathDisplay::onInitialize() updateTextSize(); } -bool validateFloats(const geometry_msgs::msg::PoseArray& msg) +bool ToolPathDisplay::validateFloats(const geometry_msgs::msg::PoseArray& msg) { - return validateFloats(msg.poses); + return rviz_common::validateFloats(msg.poses); } -bool validateQuaternions(const geometry_msgs::msg::PoseArray& pose) { +bool ToolPathDisplay::validateQuaternions(const geometry_msgs::msg::PoseArray& pose) { for (const auto& pose : pose.poses) { // Extract quaternion components double qw = pose.orientation.w; @@ -186,12 +190,12 @@ bool validateQuaternions(const geometry_msgs::msg::PoseArray& pose) { return true; } -void ToolPathDisplay::processMessage(const std::shared_ptr msg) +void ToolPathDisplay::processMessage(const geometry_msgs::msg::PoseArray::ConstSharedPtr msg) { auto node = std::make_shared("processMessage_node"); if (!validateFloats(*msg)) { - setStatus(properties::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)"); + setStatus(rviz_common::properties::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)"); return; } @@ -208,7 +212,7 @@ void ToolPathDisplay::processMessage(const std::shared_ptrheader)) { - setStatus(properties::StatusProperty::Error, "Topic", "Failed to look up transform"); + setStatus(rviz_common::properties::StatusProperty::Error, "Topic", "Failed to look up transform"); return; } @@ -255,16 +259,21 @@ void ToolPathDisplay::updateAxes() axes_.pop_back(); for (std::size_t i = 0; i < poses_.size(); ++i) { - axes_[i].setPosition(poses_[i].position); - axes_[i].setOrientation(poses_[i].orientation); + axes_[i]->setPosition(poses_[i].position); + axes_[i]->setOrientation(poses_[i].orientation); } axes_node_->setVisible(axes_visibility_property_->getBool()); } -rviz_rendering::Axes* ToolPathDisplay::makeAxes() +std::unique_ptr ToolPathDisplay::makeAxes() { - return new rviz_rendering::Axes(scene_manager_, axes_node_, axes_length_property_->getFloat(), axes_radius_property_->getFloat()); + return std::make_unique( + scene_manager_, + axes_node_, + axes_length_property_->getFloat(), + axes_radius_property_->getFloat() + ); } void ToolPathDisplay::updatePoints() @@ -331,7 +340,7 @@ void ToolPathDisplay::updateAxesGeometry() { for (std::size_t i = 0; i < poses_.size(); ++i) { - axes_[i].set(axes_length_property_->getFloat(), axes_radius_property_->getFloat()); + axes_[i]->set(axes_length_property_->getFloat(), axes_radius_property_->getFloat()); } context_->queueRender(); } @@ -398,4 +407,4 @@ void ToolPathDisplay::updateTextSize() } // namespace rviz_common #include -PLUGINLIB_EXPORT_CLASS(rviz_common::ToolPathDisplay, rviz_common::MessageFilterDisplay) +PLUGINLIB_EXPORT_CLASS(rviz_common::ToolPathDisplay, rviz_common::Display) diff --git a/src/rviz_tool_path_display.h b/src/rviz_tool_path_display.h index d5cefd2..cc90d61 100644 --- a/src/rviz_tool_path_display.h +++ b/src/rviz_tool_path_display.h @@ -1,16 +1,28 @@ -#include +#include #include #include #include +#include +#include +#include #include #include #include -#include -#include -#include + +namespace rviz_rendering +{ + class Axes; + class MovableText; +} // namespace rviz_rendering namespace rviz_common { + namespace properties + { + class ColorProperty; + class FloatProperty; + class BoolProperty; + } class ToolPathDisplay : public rviz_common::MessageFilterDisplay { @@ -22,13 +34,15 @@ class ToolPathDisplay : public rviz_common::MessageFilterDisplay msg) override; + void processMessage(const geometry_msgs::msg::PoseArray::ConstSharedPtr msg) override; private: + bool validateFloats(const geometry_msgs::msg::PoseArray& msg); + bool validateQuaternions(const geometry_msgs::msg::PoseArray& pose); bool setTransform(std_msgs::msg::Header const& header); void updateAxes(); void updateDisplay(); - rviz_rendering::Axes* makeAxes(); + std::unique_ptr makeAxes(); void updatePoints(); void updateLines(); void updateText(); @@ -42,23 +56,24 @@ class ToolPathDisplay : public rviz_common::MessageFilterDisplay poses_; // Axes Display - boost::ptr_vector axes_; + // boost::ptr_vector axes_; + std::vector> axes_; Ogre::SceneNode* axes_node_; properties::FloatProperty* axes_length_property_; properties::FloatProperty* axes_radius_property_; - BoolProperty* axes_visibility_property_; + properties::BoolProperty* axes_visibility_property_; // Points Display Ogre::ManualObject* pts_object_; Ogre::MaterialPtr pts_material_; - BoolProperty* pts_visibility_property_; + properties::BoolProperty* pts_visibility_property_; properties::ColorProperty* pts_color_property_; properties::FloatProperty* pts_size_property_; // Lines Display Ogre::ManualObject* lines_object_; Ogre::MaterialPtr lines_material_; - BoolProperty* lines_visibility_property_; + properties::BoolProperty* lines_visibility_property_; properties::ColorProperty* lines_color_property_; // Text @@ -66,7 +81,7 @@ class ToolPathDisplay : public rviz_common::MessageFilterDisplay Date: Fri, 22 Mar 2024 16:15:11 -0500 Subject: [PATCH 3/6] Commenting apparently unnecessary stuff --- CMakeLists.txt | 14 ++++++-------- package.xml | 20 ++++++++++---------- plugin_description.xml | 1 - 3 files changed, 16 insertions(+), 19 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8249ec2..946fa96 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,21 +5,21 @@ set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) find_package(ament_cmake REQUIRED) +# find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) find_package(geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz_common REQUIRED) -find_package(rviz_rendering REQUIRED) +# find_package(rviz_rendering REQUIRED) -include_directories(include ${Boost_INCLUDE_DIRS}) +# include_directories(include ${Boost_INCLUDE_DIRS}) qt5_wrap_cpp(MOC_FILES src/rviz_tool_path_display.h) add_library(${PROJECT_NAME} SHARED src/rviz_tool_path_display.cpp ${MOC_FILES}) target_include_directories(${PROJECT_NAME} PUBLIC -${Qt5Widgets_INCLUDE_DIRS} +# ${Qt5Widgets_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} ) @@ -29,14 +29,13 @@ rviz_common::rviz_common pluginlib_export_plugin_description_file(rviz_common plugin_description.xml) -ament_target_dependencies(${PROJECT_NAME} Qt5 geometry_msgs rclcpp rviz_common rviz_rendering pluginlib) -ament_export_include_directories(include) +ament_target_dependencies(${PROJECT_NAME} rviz_common pluginlib) ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies( - Qt5 rviz_common geometry_msgs rclcpp + pluginlib ) install( @@ -52,5 +51,4 @@ install(DIRECTORY icons DESTINATION include/${PROJECT_NAME}/) install(FILES plugin_description.xml DESTINATION share/${PROJECT_NAME}/) - ament_package() \ No newline at end of file diff --git a/package.xml b/package.xml index 08f15e7..832e6c8 100644 --- a/package.xml +++ b/package.xml @@ -8,24 +8,24 @@ Michael Ripperger Apache 2.0 - libqt5-core - libqt5-gui - libqt5-opengl - libqt5-widgets + + + + ament_cmake - qtbase5-dev + rclcpp - rviz2 + rviz_common - rviz_ogre_vendor - rviz_rendering - rviz_default_plugins + + + pluginlib geometry_msgs ament_cmake - + diff --git a/plugin_description.xml b/plugin_description.xml index ae82969..9928391 100644 --- a/plugin_description.xml +++ b/plugin_description.xml @@ -1,4 +1,3 @@ - Date: Mon, 25 Mar 2024 11:00:24 -0500 Subject: [PATCH 4/6] Erasing apparently unnecessary stuff --- CMakeLists.txt | 5 ----- package.xml | 11 ----------- src/rviz_tool_path_display.h | 1 - 3 files changed, 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 946fa96..54877d8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,20 +6,15 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) find_package(ament_cmake REQUIRED) -# find_package(Qt5 REQUIRED COMPONENTS Core Gui Widgets Test Concurrent) find_package(geometry_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz_common REQUIRED) -# find_package(rviz_rendering REQUIRED) - -# include_directories(include ${Boost_INCLUDE_DIRS}) qt5_wrap_cpp(MOC_FILES src/rviz_tool_path_display.h) add_library(${PROJECT_NAME} SHARED src/rviz_tool_path_display.cpp ${MOC_FILES}) target_include_directories(${PROJECT_NAME} PUBLIC -# ${Qt5Widgets_INCLUDE_DIRS} ${OGRE_INCLUDE_DIRS} ) diff --git a/package.xml b/package.xml index 832e6c8..47bb5be 100644 --- a/package.xml +++ b/package.xml @@ -8,24 +8,13 @@ Michael Ripperger Apache 2.0 - - - - - ament_cmake - rclcpp - rviz_common - - - pluginlib geometry_msgs ament_cmake - diff --git a/src/rviz_tool_path_display.h b/src/rviz_tool_path_display.h index cc90d61..04cd9ae 100644 --- a/src/rviz_tool_path_display.h +++ b/src/rviz_tool_path_display.h @@ -56,7 +56,6 @@ class ToolPathDisplay : public rviz_common::MessageFilterDisplay poses_; // Axes Display - // boost::ptr_vector axes_; std::vector> axes_; Ogre::SceneNode* axes_node_; properties::FloatProperty* axes_length_property_; From ff10316d89325abe7fc9388ebb167eb38d11a6f7 Mon Sep 17 00:00:00 2001 From: BryanMqz Date: Fri, 31 May 2024 13:58:52 -0500 Subject: [PATCH 5/6] Format updates --- CMakeLists.txt | 23 ++++-- src/rviz_tool_path_display.cpp | 141 ++++++++++++++++++--------------- 2 files changed, 95 insertions(+), 69 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 54877d8..ff8b8ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,9 +1,21 @@ cmake_minimum_required(VERSION 3.5) project(rviz_tool_path_display) -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CMAKE_AUTOMOC ON) find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) @@ -36,10 +48,7 @@ ament_export_dependencies( install( TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include + DESTINATION lib ) install(DIRECTORY icons DESTINATION include/${PROJECT_NAME}/) diff --git a/src/rviz_tool_path_display.cpp b/src/rviz_tool_path_display.cpp index 448862f..60757a5 100644 --- a/src/rviz_tool_path_display.cpp +++ b/src/rviz_tool_path_display.cpp @@ -52,7 +52,7 @@ Ogre::Vector3 fromMsg(geometry_msgs::msg::Point const& point) Ogre::Quaternion fromMsg(geometry_msgs::msg::Quaternion const& quaternion) { - Ogre::Quaternion q; + Ogre::Quaternion q(quaternion.w, quaternion.x, quaternion.y, quaternion.z); q.normalise(); return q; } @@ -67,35 +67,41 @@ void updateMaterialColor(Ogre::MaterialPtr material, const QColor& color, const if (override_self_illumination) material->setSelfIllumination(r, g, b); } -} // namespace +} // namespace namespace rviz_common { ToolPathDisplay::ToolPathDisplay() { - axes_visibility_property_ = new rviz_common::properties::BoolProperty("Show Axes", true, "Toggles the visibility of the axes display", this, - SLOT(updateAxesVisibility())); - axes_length_property_ = - new rviz_common::properties::FloatProperty("Axes Length", 0.3, "Length of each axis, in meters.", this, SLOT(updateAxesGeometry())); - axes_radius_property_ = - new rviz_common::properties::FloatProperty("Axes Radius", 0.01, "Radius of each axis, in meters.", this, SLOT(updateAxesGeometry())); - - pts_visibility_property_ = new rviz_common::properties::BoolProperty("Show Points", true, "Toggles the visibility of the points display", this, - SLOT(updatePtsVisibility())); - - pts_color_property_ = new rviz_common::properties::ColorProperty("Points Color", QColor(255, 255, 255), "The color of the points display", - this, SLOT(updatePtsColor())); - pts_size_property_ = - new rviz_common::properties::FloatProperty("Points Size", 5.0, "The size of the points (pixels)", this, SLOT(updatePtsSize())); - - lines_visibility_property_ = new rviz_common::properties::BoolProperty("Show Lines", true, "Toggles the visibility of the lines display", this, - SLOT(updateLinesVisibility())); - lines_color_property_ = new rviz_common::properties::ColorProperty("Lines Color", QColor(255, 255, 255), "The color of the lines display", - this, SLOT(updateLinesColor())); - - text_visibility_property_ = new rviz_common::properties::BoolProperty("Show Text", true, "Toggles the visibility of the text display", this, - SLOT(updateTextVisibility())); - text_size_property_ =new rviz_common::properties::FloatProperty("Text Size", 0.1f, "Height of the text display (m)", this, SLOT(updateTextSize())); + axes_visibility_property_ = new rviz_common::properties::BoolProperty( + "Show Axes", true, "Toggles the visibility of the axes display", this, SLOT(updateAxesVisibility())); + + axes_length_property_ = new rviz_common::properties::FloatProperty( + "Axes Length", 0.01, "Length of each axis, in meters.", this, SLOT(updateAxesGeometry())); + + axes_radius_property_ = new rviz_common::properties::FloatProperty( + "Axes Radius", 0.001, "Radius of each axis, in meters.", this, SLOT(updateAxesGeometry())); + + pts_visibility_property_ = new rviz_common::properties::BoolProperty( + "Show Points", true, "Toggles the visibility of the points display", this, SLOT(updatePtsVisibility())); + + pts_color_property_ = new rviz_common::properties::ColorProperty( + "Points Color", QColor(255, 255, 255), "The color of the points display", this, SLOT(updatePtsColor())); + + pts_size_property_ = new rviz_common::properties::FloatProperty( + "Points Size", 10.0, "The size of the points (pixels)", this, SLOT(updatePtsSize())); + + lines_visibility_property_ = new rviz_common::properties::BoolProperty( + "Show Lines", true, "Toggles the visibility of the lines display", this, SLOT(updateLinesVisibility())); + + lines_color_property_ = new rviz_common::properties::ColorProperty( + "Lines Color", QColor(255, 255, 255), "The color of the lines display", this, SLOT(updateLinesColor())); + + text_visibility_property_ = new rviz_common::properties::BoolProperty( + "Show Text", true, "Toggles the visibility of the text display", this, SLOT(updateTextVisibility())); + + text_size_property_ = new rviz_common::properties::FloatProperty( + "Text Size", 0.01f, "Height of the text display (m)", this, SLOT(updateTextSize())); } ToolPathDisplay::~ToolPathDisplay() @@ -168,56 +174,62 @@ bool ToolPathDisplay::validateFloats(const geometry_msgs::msg::PoseArray& msg) } bool ToolPathDisplay::validateQuaternions(const geometry_msgs::msg::PoseArray& pose) { - for (const auto& pose : pose.poses) { - // Extract quaternion components - double qw = pose.orientation.w; - double qx = pose.orientation.x; - double qy = pose.orientation.y; - double qz = pose.orientation.z; - - // Calculate the quaternion norm - double norm = std::sqrt(qw*qw + qx*qx + qy*qy + qz*qz); - - // Set a tolerance for normalization - const double tolerance = 1e-6; - - // Check if the quaternion is approximately normalized - if (std::abs(norm - 1.0) > tolerance) { - // Handle invalid quaternion (not normalized) - return false; - } + for (const auto& pose : pose.poses) + { + // Extract quaternion components + double qw = pose.orientation.w; + double qx = pose.orientation.x; + double qy = pose.orientation.y; + double qz = pose.orientation.z; + + // Calculate the quaternion norm + double norm = std::sqrt(qw*qw + qx*qx + qy*qy + qz*qz); + + // Set a tolerance for normalization + const double tolerance = 1e-6; + + // Check if the quaternion is approximately normalized + if (std::abs(norm - 1.0) > tolerance) + { + // Handle invalid quaternion (not normalized) + return false; } - return true; + } + + return true; } void ToolPathDisplay::processMessage(const geometry_msgs::msg::PoseArray::ConstSharedPtr msg) { auto node = std::make_shared("processMessage_node"); - if (!validateFloats(*msg)) + + if (!validateFloats(*msg)) { setStatus(rviz_common::properties::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)"); return; } - if (!validateQuaternions(*msg)) + if (!validateQuaternions(*msg)) { RCLCPP_WARN(node->get_logger(),"quaternions" - "PoseArray msg received on topic '%s' contains unnormalized quaternions." - "This warning will only be output once but may be true for others;" - "enable DEBUG messages for ros.rviz.quaternions to see more details.", - topic_property_->getTopicStd().c_str()); - RCLCPP_DEBUG(node->get_logger(), "quaternions", "PoseArray msg received on topic '%s' contains unnormalized quaternions.", - topic_property_->getTopicStd().c_str()); + "PoseArray msg received on topic '%s' contains unnormalized quaternions." + "This warning will only be output once but may be true for others;" + "enable DEBUG messages for ros.rviz.quaternions to see more details.", + topic_property_->getTopicStd().c_str()); + RCLCPP_DEBUG(node->get_logger(), "quaternions", + "PoseArray msg received on topic '%s' contains unnormalized quaternions.", + topic_property_->getTopicStd().c_str()); } - if (!setTransform(msg->header)) + if (!setTransform(msg->header)) { setStatus(rviz_common::properties::StatusProperty::Error, "Topic", "Failed to look up transform"); return; } poses_.resize(msg->poses.size()); - for (std::size_t i = 0; i < msg->poses.size(); ++i) + + for (std::size_t i = 0; i < msg->poses.size(); ++i) { poses_[i].position = fromMsg(msg->poses[i].position); poses_[i].orientation = fromMsg(msg->poses[i].orientation); @@ -232,14 +244,17 @@ bool ToolPathDisplay::setTransform(std_msgs::msg::Header const& header) auto node = std::make_shared("setTransform_node"); Ogre::Vector3 position; Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(header, position, orientation)) + + if (!context_->getFrameManager()->getTransform(header, position, orientation)) { RCLCPP_ERROR(node->get_logger(), "Error transforming pose '%s' from frame '%s' to frame '%s'", qPrintable(getName()), header.frame_id.c_str(), qPrintable(fixed_frame_)); return false; } + scene_node_->setPosition(position); scene_node_->setOrientation(orientation); + return true; } @@ -253,11 +268,11 @@ void ToolPathDisplay::updateDisplay() void ToolPathDisplay::updateAxes() { - while (axes_.size() < poses_.size()) + while (axes_.size() < poses_.size()) axes_.push_back(makeAxes()); - while (axes_.size() > poses_.size()) + while (axes_.size() > poses_.size()) axes_.pop_back(); - for (std::size_t i = 0; i < poses_.size(); ++i) + for (std::size_t i = 0; i < poses_.size(); ++i) { axes_[i]->setPosition(poses_[i].position); axes_[i]->setOrientation(poses_[i].orientation); @@ -284,7 +299,8 @@ void ToolPathDisplay::updatePoints() pts_object_->clear(); pts_object_->estimateVertexCount(poses_.size()); pts_object_->begin(pts_material_->getName(), Ogre::RenderOperation::OT_POINT_LIST); - for (const OgrePose& pose : poses_) + + for (const OgrePose& pose : poses_) { pts_object_->position(pose.position); } @@ -302,7 +318,7 @@ void ToolPathDisplay::updateLines() lines_object_->estimateIndexCount(poses_.size()); lines_object_->begin(lines_material_->getName(), Ogre::RenderOperation::OT_LINE_STRIP); - for (unsigned i = 0; i < poses_.size(); ++i) + for (unsigned i = 0; i < poses_.size(); ++i) { lines_object_->position(poses_[i].position); lines_object_->index(i); @@ -317,7 +333,8 @@ void ToolPathDisplay::updateText() const bool show = poses_.size() > 1; start_text_node_->setVisible(show); end_text_node_->setVisible(show); - if (show) + + if (show) { // Update the caption of the end text end_text_->setCaption(std::to_string(poses_.size() - 1)); @@ -338,7 +355,7 @@ void ToolPathDisplay::reset() void ToolPathDisplay::updateAxesGeometry() { - for (std::size_t i = 0; i < poses_.size(); ++i) + for (std::size_t i = 0; i < poses_.size(); ++i) { axes_[i]->set(axes_length_property_->getFloat(), axes_radius_property_->getFloat()); } From 5844cf147208dade526ae9ecbd8f632fdc416bae Mon Sep 17 00:00:00 2001 From: BryanMqz <85954509+BryanMqz@users.noreply.github.com> Date: Fri, 31 May 2024 14:04:50 -0500 Subject: [PATCH 6/6] Update package.xml version --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 47bb5be..20c61d4 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rviz_tool_path_display - 0.2.1 + 1.0.0 The rviz_tool_path_display package Michael Ripperger