diff --git a/CMakeLists.txt b/CMakeLists.txt index 3412eed..ff8b8ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,27 +1,58 @@ -cmake_minimum_required(VERSION 2.8.3) +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() -find_package(Qt5 REQUIRED COMPONENTS Widgets) -find_package(catkin REQUIRED COMPONENTS rviz pluginlib) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -catkin_package(CATKIN_DEPENDS rviz pluginlib) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -include_directories(${catkin_INCLUDE_DIRS}) +set(CMAKE_AUTOMOC ON) + +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz_common REQUIRED) 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}) + +target_include_directories(${PROJECT_NAME} PUBLIC +${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} rviz_common pluginlib) +ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) +ament_export_dependencies( + rviz_common + geometry_msgs + rclcpp + pluginlib +) install( TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) + EXPORT ${PROJECT_NAME} + DESTINATION lib +) + +install(DIRECTORY icons DESTINATION include/${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..20c61d4 100644 --- a/package.xml +++ b/package.xml @@ -1,20 +1,20 @@ - + rviz_tool_path_display - 0.1.1 + 1.0.0 The rviz_tool_path_display package Michael Ripperger Michael Ripperger Apache 2.0 - catkin - rviz + ament_cmake + rclcpp + rviz_common pluginlib - qtbase5-dev - libqt5-widgets + geometry_msgs - + ament_cmake diff --git a/plugin_description.xml b/plugin_description.xml index 9ddd6e2..9928391 100644 --- a/plugin_description.xml +++ b/plugin_description.xml @@ -1,8 +1,11 @@ - - - + + 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 458e7d5..60757a5 100644 --- a/src/rviz_tool_path_display.cpp +++ b/src/rviz_tool_path_display.cpp @@ -28,31 +28,32 @@ */ #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 +#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); + Ogre::Quaternion q(quaternion.w, quaternion.x, quaternion.y, quaternion.z); + q.normalise(); return q; } @@ -66,36 +67,41 @@ void updateMaterialColor(Ogre::MaterialPtr material, const QColor& color, const if (override_self_illumination) material->setSelfIllumination(r, g, b); } -} // namespace +} // 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())); - axes_radius_property_ = - new 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", - this, SLOT(updatePtsColor())); - pts_size_property_ = - new 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", - 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())); + 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() @@ -144,13 +150,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,38 +168,68 @@ void ToolPathDisplay::onInitialize() updateTextSize(); } -bool validateFloats(const geometry_msgs::PoseArray& msg) +bool ToolPathDisplay::validateFloats(const geometry_msgs::msg::PoseArray& msg) { - return validateFloats(msg.poses); + return rviz_common::validateFloats(msg.poses); } -void ToolPathDisplay::processMessage(const geometry_msgs::PoseArray::ConstPtr& 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; + } + } + + return true; +} + +void ToolPathDisplay::processMessage(const geometry_msgs::msg::PoseArray::ConstSharedPtr msg) { - if (!validateFloats(*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(rviz_common::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; " - "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.", - topic_property_->getTopicStd().c_str()); + 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()); } - if (!setTransform(msg->header)) + if (!setTransform(msg->header)) { - setStatus(StatusProperty::Error, "Topic", "Failed to look up transform"); + 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); @@ -203,18 +239,22 @@ 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)) + + 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; } + scene_node_->setPosition(position); scene_node_->setOrientation(orientation); + return true; } @@ -228,22 +268,27 @@ 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); + axes_[i]->setPosition(poses_[i].position); + axes_[i]->setOrientation(poses_[i].orientation); } axes_node_->setVisible(axes_visibility_property_->getBool()); } -Axes* ToolPathDisplay::makeAxes() +std::unique_ptr ToolPathDisplay::makeAxes() { - return new 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() @@ -254,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); } @@ -272,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); @@ -287,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)); @@ -308,9 +355,9 @@ 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()); + axes_[i]->set(axes_length_property_->getFloat(), axes_radius_property_->getFloat()); } context_->queueRender(); } @@ -374,7 +421,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::Display) diff --git a/src/rviz_tool_path_display.h b/src/rviz_tool_path_display.h index d197ce1..04cd9ae 100644 --- a/src/rviz_tool_path_display.h +++ b/src/rviz_tool_path_display.h @@ -1,17 +1,30 @@ -#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 +namespace rviz_common { -class ColorProperty; -class FloatProperty; -class Axes; -class MovableText; + namespace properties + { + class ColorProperty; + class FloatProperty; + class BoolProperty; + } -class ToolPathDisplay : public rviz::MessageFilterDisplay +class ToolPathDisplay : public rviz_common::MessageFilterDisplay { Q_OBJECT public: @@ -21,13 +34,15 @@ class ToolPathDisplay : public rviz::MessageFilterDisplay makeAxes(); void updatePoints(); void updateLines(); void updateText(); @@ -41,32 +56,32 @@ class ToolPathDisplay : public rviz::MessageFilterDisplay poses_; // Axes Display - boost::ptr_vector axes_; + std::vector> axes_; Ogre::SceneNode* axes_node_; - rviz::FloatProperty* axes_length_property_; - rviz::FloatProperty* axes_radius_property_; - BoolProperty* axes_visibility_property_; + properties::FloatProperty* axes_length_property_; + properties::FloatProperty* axes_radius_property_; + properties::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::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_; - ColorProperty* lines_color_property_; + properties::BoolProperty* lines_visibility_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_; - BoolProperty* text_visibility_property_; - FloatProperty* text_size_property_; + rviz_rendering::MovableText* end_text_; + properties::BoolProperty* text_visibility_property_; + properties::FloatProperty* text_size_property_; private Q_SLOTS: void updateAxesGeometry(); @@ -83,4 +98,4 @@ private Q_SLOTS: void updateTextSize(); }; -} // namespace rviz +} // namespace rviz_common