From d5986a3da56ecce6f3a0034cbb07fafd9284f822 Mon Sep 17 00:00:00 2001 From: jginesclavero Date: Tue, 18 Aug 2020 10:40:42 +0200 Subject: [PATCH 1/3] Migrate tracker_persons to eloquent --- CMakeLists.txt | 197 ++++++++++++------ .../additional_topic_subscriber.hpp | 0 .../detected_persons_display.hpp | 52 ++--- .../human_attributes_display.hpp | 0 .../person_display_common.hpp | 113 +++++----- .../social_activities_display.hpp | 0 .../social_relations_display.hpp | 0 .../tracked_groups_display.hpp | 0 .../tracked_persons_cache.hpp | 0 .../tracked_persons_display.hpp | 82 ++++---- .../visuals/covariance_visual.hpp | 28 ++- .../visuals/mesh_node.hpp | 33 +-- .../visuals/person_visual.hpp | 64 +++--- .../visuals/text_node.hpp | 13 +- package.xml | 43 ++-- plugin_description.xml | 45 ---- plugins_description.xml | 69 ++++++ src/detected_persons_display.cpp | 55 +++-- src/person_display_common.cpp | 34 +-- src/tracked_persons_display.cpp | 139 ++++++------ src/visuals/person_visual.cpp | 50 ++--- 21 files changed, 569 insertions(+), 448 deletions(-) rename src/additional_topic_subscriber.h => include/spencer_tracking_rviz_plugin/additional_topic_subscriber.hpp (100%) rename src/detected_persons_display.h => include/spencer_tracking_rviz_plugin/detected_persons_display.hpp (70%) rename src/human_attributes_display.h => include/spencer_tracking_rviz_plugin/human_attributes_display.hpp (100%) rename src/person_display_common.h => include/spencer_tracking_rviz_plugin/person_display_common.hpp (80%) rename src/social_activities_display.h => include/spencer_tracking_rviz_plugin/social_activities_display.hpp (100%) rename src/social_relations_display.h => include/spencer_tracking_rviz_plugin/social_relations_display.hpp (100%) rename src/tracked_groups_display.h => include/spencer_tracking_rviz_plugin/tracked_groups_display.hpp (100%) rename src/tracked_persons_cache.h => include/spencer_tracking_rviz_plugin/tracked_persons_cache.hpp (100%) rename src/tracked_persons_display.h => include/spencer_tracking_rviz_plugin/tracked_persons_display.hpp (60%) rename src/visuals/covariance_visual.h => include/spencer_tracking_rviz_plugin/visuals/covariance_visual.hpp (87%) rename src/visuals/mesh_node.h => include/spencer_tracking_rviz_plugin/visuals/mesh_node.hpp (84%) rename src/visuals/person_visual.h => include/spencer_tracking_rviz_plugin/visuals/person_visual.hpp (77%) rename src/visuals/text_node.h => include/spencer_tracking_rviz_plugin/visuals/text_node.hpp (87%) delete mode 100644 plugin_description.xml create mode 100644 plugins_description.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b259ec..c5f7e02 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,90 +27,159 @@ # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +cmake_minimum_required(VERSION 3.5) -cmake_minimum_required(VERSION 2.8.3) project(spencer_tracking_rviz_plugin) -find_package(catkin REQUIRED COMPONENTS rviz spencer_tracking_msgs spencer_human_attribute_msgs spencer_social_relation_msgs) -catkin_package() -include_directories(${catkin_INCLUDE_DIRS}) -link_directories(${catkin_LIBRARY_DIRS}) - - - -## This plugin includes Qt widgets, so we must include Qt. -## We'll use the version that rviz used so they are compatible. -if(rviz_QT_VERSION VERSION_LESS "5") - message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) - ## pull in all required include dirs, define QT_LIBRARIES, etc. - include(${QT_USE_FILE}) - qt4_wrap_cpp(MOC_FILES - src/detected_persons_display.h - src/tracked_persons_display.h - src/tracked_groups_display.h - src/social_relations_display.h - src/social_activities_display.h - src/human_attributes_display.h - src/person_display_common.h - src/additional_topic_subscriber.h -) +# 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 -Wnon-virtual-dtor -Woverloaded-virtual) +endif() + +# Tests currently only run on OS X @ OSRF jenkins +# Enable on Linux by providing a display, enable on Windows via EnableDisplayTests=True +option(EnableDisplayTests "EnableDisplayTests") +set(DisplayTests "False" CACHE STRING "DisplayTestsVariable") + +if(DEFINED ENV{DISPLAY}) + set(DISPLAYPRESENT TRUE) +endif() + +if(APPLE OR DISPLAYPRESENT OR EnableDisplayTests STREQUAL "True") + message(STATUS "Enabling tests requiring a display") else() - message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") - find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) - ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies - set(QT_LIBRARIES Qt5::Widgets) - qt5_wrap_cpp(MOC_FILES - src/detected_persons_display.h - src/tracked_persons_display.h - src/tracked_groups_display.h - src/social_relations_display.h - src/social_activities_display.h - src/human_attributes_display.h - src/person_display_common.h - src/additional_topic_subscriber.h -) + set(SKIP_DISPLAY_TESTS "SKIP_TEST") endif() -add_definitions(-DQT_NO_KEYWORDS) +# options and directories for visual tests (see visual_testing_framework documentation) +option(EnableVisualTests "decides whether or not to enable the tests") +add_definitions(-D_BUILD_DIR_PATH="${CMAKE_CURRENT_BINARY_DIR}") +add_definitions(-D_SRC_DIR_PATH="${CMAKE_CURRENT_SOURCE_DIR}") +file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/test_images) +file(MAKE_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/test/reference_images) + +# Run visual tests only if "EnableVisualTests=True" +if(EnableVisualTests STREQUAL "True") + message(STATUS "Enabling visual tests") +else() + set(SKIP_VISUAL_TESTS "SKIP_TEST") +endif() + +if(MSVC) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /bigobj") +endif() -set(SOURCE_FILES +# We specifically don't turn on CMAKE_AUTOMOC, since it generates one huge +# mocs_compilation.cpp file that takes a lot of memory to compile. Instead +# we create individual moc files that can be compiled separately. + +find_package(ament_cmake REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_rendering REQUIRED) +find_package(rviz_default_plugins REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) +find_package(spencer_tracking_msgs REQUIRED) +find_package(spencer_human_attribute_msgs REQUIRED) +find_package(spencer_social_relation_msgs REQUIRED) +find_package(Qt5 REQUIRED COMPONENTS Core Widgets) + +set(spencer_tracking_rviz_plugin_headers_to_moc + include/spencer_tracking_rviz_plugin/detected_persons_display.hpp + include/spencer_tracking_rviz_plugin/tracked_persons_display.hpp + ##include/spencer_tracking_rviz_plugin/tracked_groups_display.hpp + ##include/spencer_tracking_rviz_plugin/social_relations_display.hpp + ##include/spencer_tracking_rviz_plugin/social_activities_display.hpp + ##include/spencer_tracking_rviz_plugin/human_attributes_display.hpp + include/spencer_tracking_rviz_plugin/person_display_common.hpp + ##include/spencer_tracking_rviz_plugin/additional_topic_subscriber.hpp +) + +foreach(header "${spencer_tracking_rviz_plugin_headers_to_moc}") + qt5_wrap_cpp(spencer_tracking_rviz_plugin_moc_files "${header}") +endforeach() + +set(spencer_tracking_rviz_plugin_source_files src/detected_persons_display.cpp src/tracked_persons_display.cpp - src/tracked_groups_display.cpp - src/social_relations_display.cpp - src/social_activities_display.cpp - src/human_attributes_display.cpp + ##src/tracked_groups_display.cpp + ##src/social_relations_display.cpp + ##src/social_activities_display.cpp + ##src/human_attributes_display.cpp src/person_display_common.cpp - src/tracked_persons_cache.cpp + ##src/tracked_persons_cache.cpp src/visuals/person_visual.cpp - ${MOC_FILES} ) +add_library(spencer_tracking_rviz_plugin SHARED + ${spencer_tracking_rviz_plugin_moc_files} + ${spencer_tracking_rviz_plugin_source_files} +) -add_library(${PROJECT_NAME} ${SOURCE_FILES}) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # for generation of message dependencies -target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +target_include_directories(spencer_tracking_rviz_plugin PUBLIC + $ + $ + ${Qt5Widgets_INCLUDE_DIRS} +) + +target_link_libraries(spencer_tracking_rviz_plugin PUBLIC + rviz_ogre_vendor::OgreMain + rviz_ogre_vendor::OgreOverlay +) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(spencer_tracking_rviz_plugin PRIVATE "SPENCER_TRACKING_RVIZ_PLUGIN_BUILDING_LIBRARY") -install(TARGETS - ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# prevent pluginlib from using boost +target_compile_definitions(spencer_tracking_rviz_plugin PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) + +ament_target_dependencies(spencer_tracking_rviz_plugin + PUBLIC + spencer_tracking_msgs + spencer_human_attribute_msgs + spencer_social_relation_msgs + rviz_common + rviz_rendering + rviz_default_plugins +) + +ament_export_include_directories(include) +ament_export_interfaces(spencer_tracking_rviz_plugin HAS_LIBRARY_TARGET) +ament_export_dependencies( + Qt5 + rviz_common + rviz_rendering + rviz_default_plugins + rviz_ogre_vendor + spencer_tracking_msgs + spencer_human_attribute_msgs + spencer_social_relation_msgs ) -install(FILES - plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install( + TARGETS spencer_tracking_rviz_plugin + EXPORT spencer_tracking_rviz_plugin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) -install(DIRECTORY media/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/media) +install(DIRECTORY include DESTINATION include) +install(DIRECTORY media DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY icons/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons) +install( + DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/icons" + DESTINATION "share/${PROJECT_NAME}" +) -install(PROGRAMS scripts/send_test_msgs.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +ament_package() diff --git a/src/additional_topic_subscriber.h b/include/spencer_tracking_rviz_plugin/additional_topic_subscriber.hpp similarity index 100% rename from src/additional_topic_subscriber.h rename to include/spencer_tracking_rviz_plugin/additional_topic_subscriber.hpp diff --git a/src/detected_persons_display.h b/include/spencer_tracking_rviz_plugin/detected_persons_display.hpp similarity index 70% rename from src/detected_persons_display.h rename to include/spencer_tracking_rviz_plugin/detected_persons_display.hpp index d210b59..fa2c780 100644 --- a/src/detected_persons_display.h +++ b/include/spencer_tracking_rviz_plugin/detected_persons_display.hpp @@ -33,9 +33,12 @@ #ifndef Q_MOC_RUN #include +#include +#include #include -#include -#include "person_display_common.h" +#include +#include "spencer_tracking_rviz_plugin/person_display_common.hpp" +#include "rviz_common/logging.hpp" #endif namespace spencer_tracking_rviz_plugin @@ -43,12 +46,12 @@ namespace spencer_tracking_rviz_plugin /// The visual of a tracked person. struct DetectedPersonVisual { - boost::shared_ptr sceneNode; + std::shared_ptr sceneNode; - boost::shared_ptr personVisual; - boost::shared_ptr detectionIdText, confidenceText, modalityText; - boost::shared_ptr orientationArrow; - boost::shared_ptr covarianceVisual; + std::shared_ptr personVisual; + std::shared_ptr detectionIdText, confidenceText, modalityText; + std::shared_ptr orientationArrow; + std::shared_ptr covarianceVisual; float confidence; bool hasValidOrientation; @@ -57,7 +60,7 @@ namespace spencer_tracking_rviz_plugin // The DetectedPersonsDisplay class itself just implements a circular buffer, // editable parameters, and Display subclass machinery. - class DetectedPersonsDisplay: public PersonDisplayCommon + class DetectedPersonsDisplay: public PersonDisplayCommon { Q_OBJECT public: @@ -73,16 +76,18 @@ namespace spencer_tracking_rviz_plugin // and broken. virtual void onInitialize(); + // Function to handle an incoming ROS message. + void processMessage(spencer_tracking_msgs::msg::DetectedPersons::ConstSharedPtr msg) override; protected: // A helper to clear this display back to the initial state. virtual void reset(); // Must be implemented by derived classes because MOC doesn't work in templates - virtual rviz::DisplayContext* getContext() { + virtual rviz_common::DisplayContext* getContext() { return context_; } - + private Q_SLOTS: void personVisualTypeChanged(); @@ -90,23 +95,22 @@ namespace spencer_tracking_rviz_plugin virtual void stylesChanged(); private: - // Function to handle an incoming ROS message. - void processMessage(const spencer_tracking_msgs::DetectedPersons::ConstPtr& msg); - + + // All currently active tracks, with unique track ID as map key - vector > m_previousDetections; + vector > m_previousDetections; // Properties - rviz::BoolProperty* m_render_covariances_property; - rviz::BoolProperty* m_render_detection_ids_property; - rviz::BoolProperty* m_render_confidences_property; - rviz::FloatProperty* m_low_confidence_threshold_property; - rviz::FloatProperty* m_low_confidence_alpha_property; - rviz::BoolProperty* m_render_orientations_property; - rviz::BoolProperty* m_render_modality_text_property; - - rviz::FloatProperty* m_text_spacing_property; - rviz::FloatProperty* m_covariance_line_width_property; + rviz_common::properties::BoolProperty* m_render_covariances_property; + rviz_common::properties::BoolProperty* m_render_detection_ids_property; + rviz_common::properties::BoolProperty* m_render_confidences_property; + rviz_common::properties::FloatProperty* m_low_confidence_threshold_property; + rviz_common::properties::FloatProperty* m_low_confidence_alpha_property; + rviz_common::properties::BoolProperty* m_render_orientations_property; + rviz_common::properties::BoolProperty* m_render_modality_text_property; + + rviz_common::properties::FloatProperty* m_text_spacing_property; + rviz_common::properties::FloatProperty* m_covariance_line_width_property; }; } // end namespace spencer_tracking_rviz_plugin diff --git a/src/human_attributes_display.h b/include/spencer_tracking_rviz_plugin/human_attributes_display.hpp similarity index 100% rename from src/human_attributes_display.h rename to include/spencer_tracking_rviz_plugin/human_attributes_display.hpp diff --git a/src/person_display_common.h b/include/spencer_tracking_rviz_plugin/person_display_common.hpp similarity index 80% rename from src/person_display_common.h rename to include/spencer_tracking_rviz_plugin/person_display_common.hpp index 9a49763..f9d32c2 100644 --- a/src/person_display_common.h +++ b/include/spencer_tracking_rviz_plugin/person_display_common.hpp @@ -35,32 +35,30 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include -#include "visuals/person_visual.h" -#include "visuals/text_node.h" -#include "visuals/covariance_visual.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "visuals/person_visual.hpp" +#include "visuals/text_node.hpp" +#include "visuals/covariance_visual.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include #endif - - - - - - using namespace std; using namespace boost; @@ -105,45 +103,45 @@ namespace spencer_tracking_rviz_plugin class PersonDisplayCommonProperties : public QObject { Q_OBJECT public: - PersonDisplayCommonProperties(rviz::Display* display, StylesChangedSubscriber* stylesChangedSubscriber); + PersonDisplayCommonProperties(rviz_common::Display* display, StylesChangedSubscriber* stylesChangedSubscriber); // User-editable property variables. - rviz::EnumProperty* style; - rviz::EnumProperty* color_transform; - rviz::IntProperty* color_map_offset; + rviz_common::properties::EnumProperty* style; + rviz_common::properties::EnumProperty* color_transform; + rviz_common::properties::IntProperty* color_map_offset; - rviz::ColorProperty* constant_color; - rviz::FloatProperty* alpha; + rviz_common::properties::ColorProperty* constant_color; + rviz_common::properties::FloatProperty* alpha; - rviz::FloatProperty* line_width; - rviz::FloatProperty* z_offset; - rviz::FloatProperty* scaling_factor; + rviz_common::properties::FloatProperty* line_width; + rviz_common::properties::FloatProperty* z_offset; + rviz_common::properties::FloatProperty* scaling_factor; - rviz::BoolProperty* use_actual_z_position; + rviz_common::properties::BoolProperty* use_actual_z_position; - rviz::EnumProperty* font_color_style; - rviz::ColorProperty* constant_font_color; - rviz::FloatProperty* font_scale; + rviz_common::properties::EnumProperty* font_color_style; + rviz_common::properties::ColorProperty* constant_font_color; + rviz_common::properties::FloatProperty* font_scale; - rviz::StringProperty* m_excluded_person_ids_property; - rviz::StringProperty* m_included_person_ids_property; + rviz_common::properties::StringProperty* m_excluded_person_ids_property; + rviz_common::properties::StringProperty* m_included_person_ids_property; /// These sets get updated automatically whenever the corresponding properties are updated. set m_excludedPersonIDs, m_includedPersonIDs; private: - rviz::Display* m_display; + rviz_common::Display* m_display; StylesChangedSubscriber* m_stylesChangedSubscriber; void hideIrrelevantProperties(); - + private Q_SLOTS: void stylesChanged(); }; /// A display with common properties that are shared by multiple specializations. template - class PersonDisplayCommon: public rviz::MessageFilterDisplay, public StylesChangedSubscriber + class PersonDisplayCommon: public rviz_common::MessageFilterDisplay, public StylesChangedSubscriber { public: /// Constructor. pluginlib::ClassLoader creates instances by calling @@ -154,9 +152,13 @@ namespace spencer_tracking_rviz_plugin /// Overrides base class method virtual void onInitialize() { - rviz::MessageFilterDisplay::onInitialize(); + rviz_common::MessageFilterDisplay::onInitialize(); m_commonProperties = new PersonDisplayCommonProperties(this, this); } + //void processMessage(typename MessageType::ConstSharedPtr msg) + //{ +// + //} protected: /// Common message processing. This method needs to be called by derived classes @@ -166,7 +168,9 @@ namespace spencer_tracking_rviz_plugin // fixed frame to the frame in the header of this Imu message. If // it fails, we can't do anything else so we return. if (!getContext()->getFrameManager()->getTransform(msg->header, m_framePosition, m_frameOrientation)) { - ROS_ERROR_THROTTLE(5.0, "Error transforming from frame '%s' into fixed frame!", msg->header.frame_id.c_str()); + rclcpp::Clock clock(RCL_ROS_TIME); + RCLCPP_ERROR_THROTTLE(rclcpp::get_logger("spencer_tracking_rviz_plugin.PersonDisplayCommon"), clock, 5, + "Error transforming from frame '%s' into fixed frame!", msg->header.frame_id.c_str()); return false; } @@ -175,7 +179,7 @@ namespace spencer_tracking_rviz_plugin } /// Create a visual representation of the person itself, if not set yet - void createPersonVisualIfRequired(Ogre::SceneNode* sceneNode, boost::shared_ptr &personVisual) + void createPersonVisualIfRequired(Ogre::SceneNode* sceneNode, std::shared_ptr &personVisual) { if (!personVisual) { PersonVisualDefaultArgs defaultArgs(getContext()->getSceneManager(), sceneNode); @@ -195,7 +199,7 @@ namespace spencer_tracking_rviz_plugin } /// Applies common styles which apply to person visuals, such as line width etc. - void applyCommonStyles(boost::shared_ptr &personVisual) { + void applyCommonStyles(std::shared_ptr &personVisual) { if(!personVisual) return; // Set line width of wireframe visualization @@ -209,14 +213,14 @@ namespace spencer_tracking_rviz_plugin } // Builds velocity vector for a person from a twist message - Ogre::Vector3 getVelocityVector(const geometry_msgs::TwistWithCovariance& twist) { + Ogre::Vector3 getVelocityVector(const geometry_msgs::msg::TwistWithCovariance& twist) { const double zVelocityVariance = twist.covariance[2 * 6 + 2]; const double zVelocity = (isnan(zVelocityVariance) || isinf(zVelocityVariance)) ? 0.0 : twist.twist.linear.z; return Ogre::Vector3(twist.twist.linear.x, twist.twist.linear.y, zVelocity); } /// Returns true if all xyz rotation variances are finite - bool hasValidOrientation(const geometry_msgs::PoseWithCovariance& pose) + bool hasValidOrientation(const geometry_msgs::msg::PoseWithCovariance& pose) { // Check if quaternion has not been initialized, then it's invalid (all-zero elements) if(pose.pose.orientation.x == 0 && pose.pose.orientation.y == 0 && pose.pose.orientation.z == 0 && pose.pose.orientation.w == 0) return false; @@ -235,25 +239,28 @@ namespace spencer_tracking_rviz_plugin /// Rotate the position (xyz) part of a pose covariance matrix into the fixed frame used for visualization /// The covariance matrix needs to be transformed from the source (e.g. sensor) into the target (e.g. odometry) frame /// This is mainly be a problem if the sensor is rotated vertically compared to the odometry frame, so that axes get swapped - Ogre::Matrix3 covarianceXYZIntoTargetFrame(const geometry_msgs::PoseWithCovariance& pose) { + Ogre::Matrix3 covarianceXYZIntoTargetFrame(const geometry_msgs::msg::PoseWithCovariance& pose) { Ogre::Matrix3 xyzcov; - for(int row = 0; row < 3; row++) for(int col = 0; col < 3; col++) xyzcov[row][col] = pose.covariance[row*6 + col]; // 6 = dimension of ROS covariance matrix - if(!isfinite(xyzcov.Determinant())) ROS_WARN_STREAM("Covariance matrix supplied to covarianceXYZIntoTargetFrame() contains non-finite elements: " << xyzcov); + for(int row = 0; row < 3; row++) + for(int col = 0; col < 3; col++) + xyzcov[row][col] = pose.covariance[row*6 + col]; // 6 = dimension of ROS covariance matrix + if(!isfinite(xyzcov.Determinant())) + RVIZ_COMMON_LOG_WARNING_STREAM("Covariance matrix supplied to covarianceXYZIntoTargetFrame() contains non-finite elements: "<< xyzcov); return m_frameRotationMatrix * xyzcov * m_frameRotationMatrix.Transpose(); // cov(AX + a) = A cov(X) A^T } /// Set pose and orientation of person visual - void setPoseOrientation(Ogre::SceneNode* sceneNode, const geometry_msgs::PoseWithCovariance& pose, const Ogre::Matrix3& covXYZinTargetFrame, double personVisualHeight) + void setPoseOrientation(Ogre::SceneNode* sceneNode, const geometry_msgs::msg::PoseWithCovariance& pose, const Ogre::Matrix3& covXYZinTargetFrame, double personVisualHeight) { - const geometry_msgs::Point& position = pose.pose.position; - const geometry_msgs::Quaternion& orientation = pose.pose.orientation; + const geometry_msgs::msg::Point& position = pose.pose.position; + const geometry_msgs::msg::Quaternion& orientation = pose.pose.orientation; Ogre::Matrix4 transform(m_frameOrientation); transform.setTrans(m_framePosition); Ogre::Vector3 originalPosition(position.x, position.y, position.z); if(!isfinite(originalPosition.x) || !isfinite(originalPosition.y) || !isfinite(originalPosition.z)) { - ROS_WARN("Detected or tracked person has non-finite position! Something is wrong!"); + RVIZ_COMMON_LOG_WARNING("Detected or tracked person has non-finite position! Something is wrong!"); return; } @@ -366,7 +373,7 @@ namespace spencer_tracking_rviz_plugin } /// Must be implemented by derived classes because MOC doesn't work in templates - virtual rviz::DisplayContext* getContext() = 0; + virtual rviz_common::DisplayContext* getContext() = 0; /// Common properties for the displays in this plugin PersonDisplayCommonProperties* m_commonProperties; diff --git a/src/social_activities_display.h b/include/spencer_tracking_rviz_plugin/social_activities_display.hpp similarity index 100% rename from src/social_activities_display.h rename to include/spencer_tracking_rviz_plugin/social_activities_display.hpp diff --git a/src/social_relations_display.h b/include/spencer_tracking_rviz_plugin/social_relations_display.hpp similarity index 100% rename from src/social_relations_display.h rename to include/spencer_tracking_rviz_plugin/social_relations_display.hpp diff --git a/src/tracked_groups_display.h b/include/spencer_tracking_rviz_plugin/tracked_groups_display.hpp similarity index 100% rename from src/tracked_groups_display.h rename to include/spencer_tracking_rviz_plugin/tracked_groups_display.hpp diff --git a/src/tracked_persons_cache.h b/include/spencer_tracking_rviz_plugin/tracked_persons_cache.hpp similarity index 100% rename from src/tracked_persons_cache.h rename to include/spencer_tracking_rviz_plugin/tracked_persons_cache.hpp diff --git a/src/tracked_persons_display.h b/include/spencer_tracking_rviz_plugin/tracked_persons_display.hpp similarity index 60% rename from src/tracked_persons_display.h rename to include/spencer_tracking_rviz_plugin/tracked_persons_display.hpp index 0c70469..553895b 100644 --- a/src/tracked_persons_display.h +++ b/include/spencer_tracking_rviz_plugin/tracked_persons_display.hpp @@ -32,10 +32,16 @@ #define TRACKED_PERSONS_DISPLAY_H #ifndef Q_MOC_RUN -#include #include -#include -#include "person_display_common.h" +#include +#include "rclcpp/rclcpp.hpp" +#include +#include +#include + +#include + +#include #endif namespace spencer_tracking_rviz_plugin @@ -46,27 +52,27 @@ namespace spencer_tracking_rviz_plugin struct TrackedPersonHistoryEntry { Ogre::Vector3 position; - boost::shared_ptr shape; + std::shared_ptr shape; bool wasOccluded; track_id trackId; }; /// History of a tracked person. - typedef circular_buffer > TrackedPersonHistory; + typedef circular_buffer > TrackedPersonHistory; /// The visual of a tracked person. struct TrackedPersonVisual { TrackedPersonHistory history; - boost::shared_ptr historyLine; + std::shared_ptr historyLine; Ogre::Vector3 positionOfLastHistoryEntry; - boost::shared_ptr sceneNode, historySceneNode, historyLineSceneNode; + std::shared_ptr sceneNode, historySceneNode, historyLineSceneNode; - boost::shared_ptr personVisual; - boost::shared_ptr idText, detectionIdText, stateText; - boost::shared_ptr velocityArrow; - boost::shared_ptr covarianceVisual; + std::shared_ptr personVisual; + std::shared_ptr idText, detectionIdText, stateText; + std::shared_ptr velocityArrow; + std::shared_ptr covarianceVisual; Ogre::Matrix4 lastObservedPose; @@ -76,7 +82,7 @@ namespace spencer_tracking_rviz_plugin // The TrackedPersonsDisplay class itself just implements a circular buffer, // editable parameters, and Display subclass machinery. - class TrackedPersonsDisplay: public PersonDisplayCommon + class TrackedPersonsDisplay: public PersonDisplayCommon { Q_OBJECT public: @@ -102,7 +108,7 @@ namespace spencer_tracking_rviz_plugin virtual void reset(); // Must be implemented by derived classes because MOC doesn't work in templates - virtual rviz::DisplayContext* getContext() { + virtual rviz_common::DisplayContext* getContext() { return context_; } @@ -114,40 +120,40 @@ namespace spencer_tracking_rviz_plugin private: // Function to handle an incoming ROS message. - void processMessage(const spencer_tracking_msgs::TrackedPersons::ConstPtr& msg); + void processMessage(spencer_tracking_msgs::msg::TrackedPersons::ConstSharedPtr msg) override; // All currently active tracks, with unique track ID as map key - typedef map > track_map; + typedef map > track_map; track_map m_cachedTracks; // Scene node for track history visualization - boost::shared_ptr m_trackHistorySceneNode; + std::shared_ptr m_trackHistorySceneNode; std::string m_realFixedFrame; // User-editable property variables. - rviz::FloatProperty* m_occlusion_alpha_property; - rviz::FloatProperty* m_missed_alpha_property; - rviz::IntProperty* m_history_length_property; - rviz::IntProperty* m_delete_after_ncycles_property; - - rviz::BoolProperty* m_show_deleted_property; - rviz::BoolProperty* m_show_occluded_property; - rviz::BoolProperty* m_show_missed_property; - rviz::BoolProperty* m_show_matched_property; + rviz_common::properties::FloatProperty* m_occlusion_alpha_property; + rviz_common::properties::FloatProperty* m_missed_alpha_property; + rviz_common::properties::IntProperty* m_history_length_property; + rviz_common::properties::IntProperty* m_delete_after_ncycles_property; + + rviz_common::properties::BoolProperty* m_show_deleted_property; + rviz_common::properties::BoolProperty* m_show_occluded_property; + rviz_common::properties::BoolProperty* m_show_missed_property; + rviz_common::properties::BoolProperty* m_show_matched_property; - rviz::BoolProperty* m_render_person_property; - rviz::BoolProperty* m_render_history_property; - rviz::BoolProperty* m_render_history_as_line_property; - rviz::BoolProperty* m_render_covariances_property; - rviz::BoolProperty* m_render_state_prediction_property; - rviz::BoolProperty* m_render_velocities_property; - rviz::BoolProperty* m_render_ids_property; - rviz::BoolProperty* m_render_detection_ids_property; - rviz::BoolProperty* m_render_track_state_property; - - rviz::FloatProperty* m_history_line_width_property; - rviz::FloatProperty* m_history_min_point_distance_property; - rviz::FloatProperty* m_covariance_line_width_property; + rviz_common::properties::BoolProperty* m_render_person_property; + rviz_common::properties::BoolProperty* m_render_history_property; + rviz_common::properties::BoolProperty* m_render_history_as_line_property; + rviz_common::properties::BoolProperty* m_render_covariances_property; + rviz_common::properties::BoolProperty* m_render_state_prediction_property; + rviz_common::properties::BoolProperty* m_render_velocities_property; + rviz_common::properties::BoolProperty* m_render_ids_property; + rviz_common::properties::BoolProperty* m_render_detection_ids_property; + rviz_common::properties::BoolProperty* m_render_track_state_property; + + rviz_common::properties::FloatProperty* m_history_line_width_property; + rviz_common::properties::FloatProperty* m_history_min_point_distance_property; + rviz_common::properties::FloatProperty* m_covariance_line_width_property; }; } // end namespace spencer_tracking_rviz_plugin diff --git a/src/visuals/covariance_visual.h b/include/spencer_tracking_rviz_plugin/visuals/covariance_visual.hpp similarity index 87% rename from src/visuals/covariance_visual.h rename to include/spencer_tracking_rviz_plugin/visuals/covariance_visual.hpp index b0a7c43..ac1de16 100644 --- a/src/visuals/covariance_visual.h +++ b/include/spencer_tracking_rviz_plugin/visuals/covariance_visual.hpp @@ -32,8 +32,9 @@ #ifndef COVARIANCE_VISUAL_H #define COVARIANCE_VISUAL_H -#include -#include +#include "rclcpp/rclcpp.hpp" +#include +#include #include @@ -47,7 +48,8 @@ namespace spencer_tracking_rviz_plugin { } virtual ~CovarianceVisual() { - m_sceneManager->destroySceneNode(m_sceneNode->getName()); + if (m_sceneNode->getName() != "") + m_sceneManager->destroySceneNode(m_sceneNode->getName()); }; void setPosition(const Ogre::Vector3& position) { @@ -68,7 +70,8 @@ namespace spencer_tracking_rviz_plugin { /// NOTE: It is assumed that the covariance matrix is already rotated into the target frame of the sceneNode! virtual void setMeanCovariance(const Ogre::Vector3& mean, const Ogre::Matrix3& cov) = 0; - + + protected: Ogre::SceneManager* m_sceneManager; Ogre::SceneNode* m_sceneNode; @@ -80,7 +83,7 @@ namespace spencer_tracking_rviz_plugin { public: ProbabilityEllipseCovarianceVisual(Ogre::SceneManager* sceneManager, Ogre::SceneNode* parentNode) : CovarianceVisual(sceneManager, parentNode) { - m_line = new rviz::BillboardLine(m_sceneManager, m_sceneNode); + m_line = new rviz_rendering::BillboardLine(m_sceneManager, m_sceneNode); } virtual ~ProbabilityEllipseCovarianceVisual() { @@ -99,17 +102,20 @@ namespace spencer_tracking_rviz_plugin { int numberOfPoints; double *xs, *ys; double determinant = cov[0][0]*cov[1][1] - cov[1][0]*cov[0][1]; - + rclcpp::Logger logger = rclcpp::get_logger("spencer_tracking_rviz_plugin.CovarianceVisual"); + rclcpp::Clock clock(RCL_ROS_TIME); m_line->clear(); if(!std::isfinite(determinant)) { - ROS_WARN_STREAM_THROTTLE(5.0, "Covariance matrix has non-finite values in ProbabilityEllipseCovarianceVisual::setMeanCovariance(): " << cov); + RCLCPP_WARN_STREAM_THROTTLE(logger, clock, 5, + "Covariance matrix has non-finite values in ProbabilityEllipseCovarianceVisual::setMeanCovariance(): " << cov); return; } if(std::abs(cov[0][1] - cov[1][0]) > 0.00001) { - ROS_WARN_STREAM_THROTTLE(5.0, "Covariance matrix is not symmetric in ProbabilityEllipseCovarianceVisual::setMeanCovariance(): " << cov); + RCLCPP_WARN_STREAM_THROTTLE(logger, clock, 5, + "Covariance matrix is not symmetric in ProbabilityEllipseCovarianceVisual::setMeanCovariance(): " << cov); return; } @@ -125,13 +131,15 @@ namespace spencer_tracking_rviz_plugin { } } else { - ROS_WARN_STREAM_THROTTLE(5.0, "Covariance matrix is not positive (semi-)definite in ProbabilityEllipseCovarianceVisual::setMeanCovariance(): " << cov); + RCLCPP_WARN_STREAM_THROTTLE(logger, clock, 5000, + "Covariance matrix is not positive (semi-)definite in ProbabilityEllipseCovarianceVisual::setMeanCovariance(): " << + cov); } } private: - rviz::BillboardLine* m_line; + rviz_rendering::BillboardLine* m_line; // Puts angle alpha into the interval [min..min+2*pi[ double set_angle_to_range(double alpha, double min) diff --git a/src/visuals/mesh_node.h b/include/spencer_tracking_rviz_plugin/visuals/mesh_node.hpp similarity index 84% rename from src/visuals/mesh_node.h rename to include/spencer_tracking_rviz_plugin/visuals/mesh_node.hpp index 87e5a55..7ab08db 100644 --- a/src/visuals/mesh_node.h +++ b/include/spencer_tracking_rviz_plugin/visuals/mesh_node.hpp @@ -31,14 +31,16 @@ #ifndef MESH_NODE_H #define MESH_NODE_H -#include +#include #ifndef Q_MOC_RUN #include #endif -#include -#include // hack to get camera position - +// #include +#include // hack to get camera position +#include #include +#include +#include #include #include #include @@ -50,7 +52,7 @@ namespace spencer_tracking_rviz_plugin { class MeshNode : public Ogre::FrameListener { public: - MeshNode(rviz::DisplayContext* displayContext, Ogre::SceneNode* parentNode, const std::string& meshResource, Ogre::Vector3 position = Ogre::Vector3::ZERO) + MeshNode(rviz_common::DisplayContext* displayContext, Ogre::SceneNode* parentNode, const std::string& meshResource, Ogre::Vector3 position = Ogre::Vector3::ZERO) : m_sceneManager(displayContext->getSceneManager()), m_displayContext(displayContext), m_meshResource(meshResource) { m_cameraFacing = false; @@ -58,7 +60,7 @@ namespace spencer_tracking_rviz_plugin { m_sceneNode->setVisible(false); // Load mesh - assert(!rviz::loadMeshFromResource(meshResource).isNull()); + assert(!rviz_rendering::loadMeshFromResource(meshResource).isNull()); // Create scene entity std::stringstream ss; @@ -101,7 +103,8 @@ namespace spencer_tracking_rviz_plugin { } } m_materials.clear(); - m_sceneManager->destroySceneNode(m_sceneNode->getName()); + if (m_sceneNode->getName() != "") + m_sceneManager->destroySceneNode(m_sceneNode->getName()); } void setOrientation(const Ogre::Quaternion& orientation) { @@ -162,13 +165,13 @@ namespace spencer_tracking_rviz_plugin { virtual bool frameStarted(const Ogre::FrameEvent &evt) { Ogre::Quaternion cameraQuat; - if(m_cameraFacing) { - // Align with camera view direction - // FIXME: The following way of retrieving the camera and its position is a bit hacky, don't try this at home! - rviz::VisualizationManager* visualizationManager = dynamic_cast(m_displayContext); - assert(visualizationManager != NULL); - cameraQuat = visualizationManager->getRenderPanel()->getCamera()->getOrientation(); - } + //if(m_cameraFacing) { + // // Align with camera view direction + // // FIXME: The following way of retrieving the camera and its position is a bit hacky, don't try this at home! + // rviz_common::VisualizationManager* visualizationManager = dynamic_cast(m_displayContext); + // assert(visualizationManager != NULL); + // cameraQuat = visualizationManager->getRenderPanel()->getCamera()->getOrientation(); + //} m_sceneNode->setOrientation(cameraQuat * m_orientation); return true; } @@ -176,7 +179,7 @@ namespace spencer_tracking_rviz_plugin { private: Ogre::SceneManager* m_sceneManager; Ogre::SceneNode* m_sceneNode; - rviz::DisplayContext* m_displayContext; + rviz_common::DisplayContext* m_displayContext; Ogre::Quaternion m_orientation; Ogre::Entity* m_entity; diff --git a/src/visuals/person_visual.h b/include/spencer_tracking_rviz_plugin/visuals/person_visual.hpp similarity index 77% rename from src/visuals/person_visual.h rename to include/spencer_tracking_rviz_plugin/visuals/person_visual.hpp index f57fe67..67c101b 100644 --- a/src/visuals/person_visual.h +++ b/include/spencer_tracking_rviz_plugin/visuals/person_visual.hpp @@ -31,14 +31,23 @@ #ifndef PERSON_VISUAL_H #define PERSON_VISUAL_H -#include -#include - -#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include #include +#include #include #include +#include "rviz_common/logging.hpp" +#include "rviz_common/uniform_string_stream.hpp" namespace spencer_tracking_rviz_plugin { // Abstract class for visuals which have got an adjustable line width @@ -69,7 +78,8 @@ namespace spencer_tracking_rviz_plugin { } virtual ~PersonVisual() { - m_sceneManager->destroySceneNode(m_sceneNode->getName()); + if (m_sceneNode->getName() != "") + m_sceneManager->destroySceneNode(m_sceneNode->getName()); }; void setPosition(const Ogre::Vector3& position) { @@ -116,8 +126,8 @@ namespace spencer_tracking_rviz_plugin { public: CylinderPersonVisual(const PersonVisualDefaultArgs& args) : PersonVisual(args) { - m_bodyShape = new rviz::Shape(rviz::Shape::Cylinder, args.sceneManager, m_sceneNode); - m_headShape = new rviz::Shape(rviz::Shape::Sphere, args.sceneManager, m_sceneNode); + m_bodyShape = new rviz_rendering::Shape(rviz_rendering::Shape::Cylinder, args.sceneManager, m_sceneNode); + m_headShape = new rviz_rendering::Shape(rviz_rendering::Shape::Sphere, args.sceneManager, m_sceneNode); const float headDiameter = 0.4; const float totalHeight = getHeight(); @@ -145,7 +155,7 @@ namespace spencer_tracking_rviz_plugin { } private: - rviz::Shape *m_bodyShape, *m_headShape; + rviz_rendering::Shape *m_bodyShape, *m_headShape; }; @@ -187,7 +197,7 @@ namespace spencer_tracking_rviz_plugin { protected: virtual void generateWireframe() { delete m_wireframe; - m_wireframe = new rviz::BillboardLine(m_sceneManager, m_sceneNode); + m_wireframe = new rviz_rendering::BillboardLine(m_sceneManager, m_sceneNode); m_wireframe->setLineWidth(m_lineWidth); m_wireframe->setMaxPointsPerLine(2); @@ -199,27 +209,27 @@ namespace spencer_tracking_rviz_plugin { // Front quad m_wireframe->addPoint(bottomLeft); m_wireframe->addPoint(bottomRight); - m_wireframe->newLine(); m_wireframe->addPoint(bottomRight); m_wireframe->addPoint(topRight); - m_wireframe->newLine(); m_wireframe->addPoint(topRight); m_wireframe->addPoint(topLeft); - m_wireframe->newLine(); m_wireframe->addPoint(topLeft); m_wireframe->addPoint(bottomLeft); + m_wireframe->finishLine(); m_wireframe->addPoint(bottomRight); m_wireframe->addPoint(topRight); + m_wireframe->finishLine(); m_wireframe->addPoint(topRight); m_wireframe->addPoint(topLeft); + m_wireframe->finishLine(); m_wireframe->addPoint(topLeft); m_wireframe->addPoint(bottomLeft); // Rear quad - m_wireframe->newLine(); m_wireframe->addPoint(bottomLeft + rear); m_wireframe->addPoint(bottomRight + rear); - m_wireframe->newLine(); m_wireframe->addPoint(bottomRight + rear); m_wireframe->addPoint(topRight + rear); - m_wireframe->newLine(); m_wireframe->addPoint(topRight + rear); m_wireframe->addPoint(topLeft + rear); - m_wireframe->newLine(); m_wireframe->addPoint(topLeft + rear); m_wireframe->addPoint(bottomLeft + rear); + m_wireframe->finishLine(); m_wireframe->addPoint(bottomLeft + rear); m_wireframe->addPoint(bottomRight + rear); + m_wireframe->finishLine(); m_wireframe->addPoint(bottomRight + rear); m_wireframe->addPoint(topRight + rear); + m_wireframe->finishLine(); m_wireframe->addPoint(topRight + rear); m_wireframe->addPoint(topLeft + rear); + m_wireframe->finishLine(); m_wireframe->addPoint(topLeft + rear); m_wireframe->addPoint(bottomLeft + rear); // Four connecting lines between front and rear - m_wireframe->newLine(); m_wireframe->addPoint(bottomLeft); m_wireframe->addPoint(bottomLeft + rear); - m_wireframe->newLine(); m_wireframe->addPoint(bottomRight); m_wireframe->addPoint(bottomRight + rear); - m_wireframe->newLine(); m_wireframe->addPoint(topRight); m_wireframe->addPoint(topRight + rear); - m_wireframe->newLine(); m_wireframe->addPoint(topLeft); m_wireframe->addPoint(topLeft + rear); + m_wireframe->finishLine(); m_wireframe->addPoint(bottomLeft); m_wireframe->addPoint(bottomLeft + rear); + m_wireframe->finishLine(); m_wireframe->addPoint(bottomRight); m_wireframe->addPoint(bottomRight + rear); + m_wireframe->finishLine(); m_wireframe->addPoint(topRight); m_wireframe->addPoint(topRight + rear); + m_wireframe->finishLine(); m_wireframe->addPoint(topLeft); m_wireframe->addPoint(topLeft + rear); m_wireframe->setPosition(Ogre::Vector3(-w/2, w/2, -h/2)); } private: - rviz::BillboardLine *m_wireframe; + rviz_rendering::BillboardLine *m_wireframe; double m_width, m_height, m_scalingFactor, m_lineWidth; }; @@ -254,7 +264,7 @@ namespace spencer_tracking_rviz_plugin { protected: virtual void generateCrosshair() { delete m_crosshair; - m_crosshair = new rviz::BillboardLine(m_sceneManager, m_sceneNode); + m_crosshair = new rviz_rendering::BillboardLine(m_sceneManager, m_sceneNode); m_crosshair->setLineWidth(m_lineWidth); m_crosshair->setMaxPointsPerLine(2); @@ -268,17 +278,17 @@ namespace spencer_tracking_rviz_plugin { Ogre::Vector3 arrow_a(0.7*w, -0.2*w, 0), arrow_m(w, 0, 0), arrow_b(0.7*w, +0.2*w, 0); m_crosshair->addPoint(p1a); m_crosshair->addPoint(p1b); - m_crosshair->newLine(); m_crosshair->addPoint(p2a); m_crosshair->addPoint(p2b); - m_crosshair->newLine(); m_crosshair->addPoint(p3a); m_crosshair->addPoint(p3b); + m_crosshair->finishLine(); m_crosshair->addPoint(p2a); m_crosshair->addPoint(p2b); + m_crosshair->finishLine(); m_crosshair->addPoint(p3a); m_crosshair->addPoint(p3b); - m_crosshair->newLine(); m_crosshair->addPoint(arrow_m); m_crosshair->addPoint(arrow_a); - m_crosshair->newLine(); m_crosshair->addPoint(arrow_m); m_crosshair->addPoint(arrow_b); + m_crosshair->finishLine(); m_crosshair->addPoint(arrow_m); m_crosshair->addPoint(arrow_a); + m_crosshair->finishLine(); m_crosshair->addPoint(arrow_m); m_crosshair->addPoint(arrow_b); m_crosshair->setPosition(Ogre::Vector3(0, 0, 0)); } private: - rviz::BillboardLine *m_crosshair; + rviz_rendering::BillboardLine *m_crosshair; double m_width, m_height, m_lineWidth; }; diff --git a/src/visuals/text_node.h b/include/spencer_tracking_rviz_plugin/visuals/text_node.hpp similarity index 87% rename from src/visuals/text_node.h rename to include/spencer_tracking_rviz_plugin/visuals/text_node.hpp index 1389a33..9d1b805 100644 --- a/src/visuals/text_node.h +++ b/include/spencer_tracking_rviz_plugin/visuals/text_node.hpp @@ -31,8 +31,8 @@ #ifndef TEXT_NODE_H #define TEXT_NODE_H -#include -#include +#include +#include namespace spencer_tracking_rviz_plugin { class TextNode { @@ -41,8 +41,8 @@ namespace spencer_tracking_rviz_plugin { { m_sceneNode = parentNode->createChildSceneNode(); - m_text = new rviz::MovableText("text"); - m_text->setTextAlignment(rviz::MovableText::H_CENTER, rviz::MovableText::V_BELOW); + m_text = new rviz_rendering::MovableText("text"); + m_text->setTextAlignment(rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_BELOW); m_sceneNode->attachObject(m_text); setCharacterHeight(1.0); @@ -51,7 +51,8 @@ namespace spencer_tracking_rviz_plugin { } virtual ~TextNode() { - m_sceneManager->destroySceneNode(m_sceneNode->getName()); + if (m_sceneNode->getName() != "") + m_sceneManager->destroySceneNode(m_sceneNode->getName()); delete m_text; }; @@ -87,7 +88,7 @@ namespace spencer_tracking_rviz_plugin { private: Ogre::SceneManager* m_sceneManager; Ogre::SceneNode* m_sceneNode; - rviz::MovableText* m_text; + rviz_rendering::MovableText* m_text; }; } diff --git a/package.xml b/package.xml index 1810f29..d04e661 100644 --- a/package.xml +++ b/package.xml @@ -1,30 +1,37 @@ - + spencer_tracking_rviz_plugin - 1.0.8 - + 8.2.0 RViz plugin for visualizing tracked and detected persons and groups - + jginesclavero Timm Linder Timm Linder + BSD + + ament_cmake + + qtbase5-dev + rviz_ogre_vendor - BSD; CC BY 3.0 (Fugue Icons by Yusuke Kamiyamane) - - catkin + rviz_ogre_vendor - rviz - spencer_tracking_msgs - spencer_social_relation_msgs - spencer_human_attribute_msgs - - rviz - spencer_tracking_msgs - spencer_social_relation_msgs - spencer_human_attribute_msgs + libqt5-core + libqt5-gui + libqt5-opengl + libqt5-widgets + rviz_ogre_vendor + pluginlib + rclcpp + resource_retriever + rviz_common + rviz_rendering + spencer_tracking_msgs + spencer_social_relation_msgs + spencer_human_attribute_msgs - + ament_cmake -roslibroslib + diff --git a/plugin_description.xml b/plugin_description.xml deleted file mode 100644 index 78b9c56..0000000 --- a/plugin_description.xml +++ /dev/null @@ -1,45 +0,0 @@ - - - - - Displays detected persons from spencer_tracking_msgs/DetectedPersons messages. - - spencer_tracking_msgs/DetectedPersons - - - - - Displays tracked persons from spencer_tracking_msgs/TrackedPersons messages. - - spencer_tracking_msgs/TrackedPersons - - - - - Displays tracked groups from spencer_tracking_msgs/TrackedGroups messages. - - spencer_tracking_msgs/TrackedGroups - - - - - Displays social relations from spencer_social_relation_msgs/SocialRelations messages. - - spencer_social_relation_msgs/SocialRelations - - - - - Displays social activities from spencer_social_relation_msgs/SocialActivities messages. - - spencer_social_relation_msgs/SocialActivities - - - - - Displays human attributes from spencer_human_attribute_msgs/HumanAttributes messages. - - spencer_human_attribute_msgs/HumanAttributes - - - diff --git a/plugins_description.xml b/plugins_description.xml new file mode 100644 index 0000000..33ab751 --- /dev/null +++ b/plugins_description.xml @@ -0,0 +1,69 @@ + + + + + Displays detected persons from spencer_tracking_msgs/DetectedPersons messages. + + spencer_tracking_msgs/msg/DetectedPersons + + + + + Displays tracked persons from spencer_tracking_msgs/TrackedPersons messages. + + spencer_tracking_msgs/msg/TrackedPersons + + + + + Displays tracked groups from spencer_tracking_msgs/TrackedGroups messages. + + spencer_tracking_msgs/msg/TrackedGroups + + + + + Displays social relations from spencer_social_relation_msgs/SocialRelations messages. + + spencer_social_relation_msgs/msg/SocialRelations + + + + + Displays social activities from spencer_social_relation_msgs/SocialActivities messages. + + spencer_social_relation_msgs/msg/SocialActivities + + + + + Displays human attributes from spencer_human_attribute_msgs/HumanAttributes messages. + + spencer_human_attribute_msgs/msg/HumanAttributes + + + diff --git a/src/detected_persons_display.cpp b/src/detected_persons_display.cpp index 766f4fa..488df63 100644 --- a/src/detected_persons_display.cpp +++ b/src/detected_persons_display.cpp @@ -28,15 +28,10 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -#include -#include -#include "rviz/selection/selection_manager.h" +//#include +//#include "rviz_common/selection/selection_manager.hpp" +#include "spencer_tracking_rviz_plugin/detected_persons_display.hpp" -#include "detected_persons_display.h" -#ifndef Q_MOC_RUN -#include -#endif -#define foreach BOOST_FOREACH namespace spencer_tracking_rviz_plugin { @@ -49,18 +44,18 @@ void DetectedPersonsDisplay::onInitialize() QObject::connect(m_commonProperties->style, SIGNAL(changed()), this, SLOT(personVisualTypeChanged()) ); - m_render_covariances_property = new rviz::BoolProperty( "Render covariances", true, "Render track covariance ellipses", this, SLOT(stylesChanged()) ); - m_render_detection_ids_property = new rviz::BoolProperty( "Render detection IDs", true, "Render IDs of the detection that a track was matched against, if any", this, SLOT(stylesChanged())); - m_render_confidences_property = new rviz::BoolProperty( "Render confidences", false, "Render detection confidences", this, SLOT(stylesChanged())); - m_render_orientations_property = new rviz::BoolProperty( "Render orientation arrows", true, "Render orientation arrows (only if orientation covariances are finite!)", this, SLOT(stylesChanged())); - m_render_modality_text_property = new rviz::BoolProperty( "Render modality text", false, "Render detection modality as text below detected person", this, SLOT(stylesChanged())); + m_render_covariances_property = new rviz_common::properties::BoolProperty( "Render covariances", true, "Render track covariance ellipses", this, SLOT(stylesChanged()) ); + m_render_detection_ids_property = new rviz_common::properties::BoolProperty( "Render detection IDs", true, "Render IDs of the detection that a track was matched against, if any", this, SLOT(stylesChanged())); + m_render_confidences_property = new rviz_common::properties::BoolProperty( "Render confidences", false, "Render detection confidences", this, SLOT(stylesChanged())); + m_render_orientations_property = new rviz_common::properties::BoolProperty( "Render orientation arrows", true, "Render orientation arrows (only if orientation covariances are finite!)", this, SLOT(stylesChanged())); + m_render_modality_text_property = new rviz_common::properties::BoolProperty( "Render modality text", false, "Render detection modality as text below detected person", this, SLOT(stylesChanged())); - m_text_spacing_property = new rviz::FloatProperty( "Text spacing", 1.0, "Factor for vertical spacing betweent texts", this, SLOT(stylesChanged()), this ); + m_text_spacing_property = new rviz_common::properties::FloatProperty( "Text spacing", 1.0, "Factor for vertical spacing betweent texts", this, SLOT(stylesChanged()), this ); - m_low_confidence_threshold_property = new rviz::FloatProperty( "Low-confidence threshold", 0.5, "Detection confidence below which alpha will be reduced", this, SLOT(stylesChanged())); - m_low_confidence_alpha_property = new rviz::FloatProperty( "Low-confidence alpha", 0.5, "Alpha multiplier for detections with confidence below low-confidence threshold", this, SLOT(stylesChanged())); + m_low_confidence_threshold_property = new rviz_common::properties::FloatProperty( "Low-confidence threshold", 0.5, "Detection confidence below which alpha will be reduced", this, SLOT(stylesChanged())); + m_low_confidence_alpha_property = new rviz_common::properties::FloatProperty( "Low-confidence alpha", 0.5, "Alpha multiplier for detections with confidence below low-confidence threshold", this, SLOT(stylesChanged())); - m_covariance_line_width_property = new rviz::FloatProperty( "Line width", 0.1, "Line width of covariance ellipses", m_render_covariances_property, SLOT(stylesChanged()), this ); + m_covariance_line_width_property = new rviz_common::properties::FloatProperty( "Line width", 0.1, "Line width of covariance ellipses", m_render_covariances_property, SLOT(stylesChanged()), this ); } DetectedPersonsDisplay::~DetectedPersonsDisplay() @@ -78,7 +73,7 @@ void DetectedPersonsDisplay::reset() // Set the rendering style (cylinders, meshes, ...) of detected persons void DetectedPersonsDisplay::personVisualTypeChanged() { - foreach(boost::shared_ptr& detectedPersonVisual, m_previousDetections) + for(auto detectedPersonVisual : m_previousDetections) { detectedPersonVisual->personVisual.reset(); createPersonVisualIfRequired(detectedPersonVisual->sceneNode.get(), detectedPersonVisual->personVisual); @@ -89,7 +84,7 @@ void DetectedPersonsDisplay::personVisualTypeChanged() // Update dynamically adjustable properties of all existing detections void DetectedPersonsDisplay::stylesChanged() { - foreach(boost::shared_ptr detectedPersonVisual, m_previousDetections) + foreach(std::shared_ptr detectedPersonVisual, m_previousDetections) { bool personHidden = isPersonHidden(detectedPersonVisual->detectionId); @@ -145,7 +140,7 @@ void DetectedPersonsDisplay::stylesChanged() } // This is our callback to handle an incoming message. -void DetectedPersonsDisplay::processMessage(const spencer_tracking_msgs::DetectedPersons::ConstPtr& msg) +void DetectedPersonsDisplay::processMessage(spencer_tracking_msgs::msg::DetectedPersons::ConstSharedPtr msg) { // Get transforms into fixed frame etc. if(!preprocessMessage(msg)) return; @@ -159,16 +154,16 @@ void DetectedPersonsDisplay::processMessage(const spencer_tracking_msgs::Detecte // // Iterate over all detections in this message and create a visual representation // - for (vector::const_iterator detectedPersonIt = msg->detections.begin(); detectedPersonIt != msg->detections.end(); ++detectedPersonIt) + for (vector::const_iterator detectedPersonIt = msg->detections.begin(); detectedPersonIt != msg->detections.end(); ++detectedPersonIt) { - boost::shared_ptr detectedPersonVisual; + std::shared_ptr detectedPersonVisual; // Create a new visual representation of the detected person - detectedPersonVisual = boost::shared_ptr(new DetectedPersonVisual); + detectedPersonVisual = std::shared_ptr(new DetectedPersonVisual); m_previousDetections.push_back(detectedPersonVisual); // This scene node is the parent of all visualization elements for the detected person - detectedPersonVisual->sceneNode = boost::shared_ptr(scene_node_->createChildSceneNode()); + detectedPersonVisual->sceneNode = std::shared_ptr(scene_node_->createChildSceneNode()); detectedPersonVisual->detectionId = detectedPersonIt->detection_id; detectedPersonVisual->confidence = detectedPersonIt->confidence; Ogre::SceneNode* currentSceneNode = detectedPersonVisual->sceneNode.get(); @@ -179,7 +174,7 @@ void DetectedPersonsDisplay::processMessage(const spencer_tracking_msgs::Detecte // // Create new visual for the person itself, if needed - boost::shared_ptr &personVisual = detectedPersonVisual->personVisual; + std::shared_ptr &personVisual = detectedPersonVisual->personVisual; createPersonVisualIfRequired(currentSceneNode, personVisual); const double personHeight = personVisual ? personVisual->getHeight() : 0; @@ -211,7 +206,7 @@ void DetectedPersonsDisplay::processMessage(const spencer_tracking_msgs::Detecte detectedPersonVisual->confidenceText.reset(new TextNode(context_->getSceneManager(), currentSceneNode)); } - ss.str(""); ss << fixed << setprecision(2) << detectedPersonIt->confidence; + ss.str(""); ss << fixed << std::setprecision(2) << detectedPersonIt->confidence; detectedPersonVisual->confidenceText->setCaption(ss.str()); detectedPersonVisual->confidenceText->showOnTop(); @@ -244,7 +239,7 @@ void DetectedPersonsDisplay::processMessage(const spencer_tracking_msgs::Detecte // Orientation arrows // if (!detectedPersonVisual->orientationArrow) { - detectedPersonVisual->orientationArrow.reset(new rviz::Arrow(context_->getSceneManager(), currentSceneNode)); + detectedPersonVisual->orientationArrow.reset(new rviz_rendering::Arrow(context_->getSceneManager(), currentSceneNode)); } // Update orientation arrow @@ -268,12 +263,12 @@ void DetectedPersonsDisplay::processMessage(const spencer_tracking_msgs::Detecte // ss.str(""); ss << msg->detections.size() << " detections received"; - setStatusStd(rviz::StatusProperty::Ok, "Tracks", ss.str()); + setStatusStd(rviz_common::properties::StatusProperty::Ok, "Tracks", ss.str()); } } // end namespace spencer_tracking_rviz_plugin // Tell pluginlib about this class. It is important to do this in // global scope, outside our package's namespace. -#include -PLUGINLIB_EXPORT_CLASS(spencer_tracking_rviz_plugin::DetectedPersonsDisplay, rviz::Display) +#include +PLUGINLIB_EXPORT_CLASS(spencer_tracking_rviz_plugin::DetectedPersonsDisplay, rviz_common::Display) diff --git a/src/person_display_common.cpp b/src/person_display_common.cpp index 0333dea..2584949 100644 --- a/src/person_display_common.cpp +++ b/src/person_display_common.cpp @@ -29,29 +29,29 @@ */ #ifndef Q_MOC_RUN -#include "person_display_common.h" +#include "spencer_tracking_rviz_plugin/person_display_common.hpp" #include #include #include #endif -#define foreach BOOST_FOREACH +//#define foreach BOOST_FOREACH namespace spencer_tracking_rviz_plugin { // The constructor must have no arguments, so we can't give the // constructor the parameters it needs to fully initialize. -PersonDisplayCommonProperties::PersonDisplayCommonProperties(rviz::Display* display, StylesChangedSubscriber* stylesChangedSubscriber) +PersonDisplayCommonProperties::PersonDisplayCommonProperties(rviz_common::Display* display, StylesChangedSubscriber* stylesChangedSubscriber) : m_display(display), m_stylesChangedSubscriber(stylesChangedSubscriber) { - style = new rviz::EnumProperty( "Style", "Cylinders", "Rendering mode to use, in order of computational complexity.", m_display, SLOT(stylesChanged()), this ); + style = new rviz_common::properties::EnumProperty( "Style", "Cylinders", "Rendering mode to use, in order of computational complexity.", m_display, SLOT(stylesChanged()), this ); style->addOption( "Simple", STYLE_SIMPLE ); style->addOption( "Cylinders", STYLE_CYLINDER ); style->addOption( "Person meshes", STYLE_PERSON_MESHES ); style->addOption( "Bounding boxes", STYLE_BOUNDING_BOXES ); style->addOption( "Crosshairs", STYLE_CROSSHAIRS ); - color_transform = new rviz::EnumProperty( "Color transform", "Rainbow", "How to color the tracked persons", m_display, SLOT(stylesChanged()), this ); + color_transform = new rviz_common::properties::EnumProperty( "Color transform", "Rainbow", "How to color the tracked persons", m_display, SLOT(stylesChanged()), this ); color_transform->addOption( "SRL Tracking Colors", COLORS_SRL ); color_transform->addOption( "Alternative SRL colors", COLORS_SRL_ALTERNATIVE ); color_transform->addOption( "Rainbow", COLORS_RAINBOW ); @@ -60,38 +60,38 @@ PersonDisplayCommonProperties::PersonDisplayCommonProperties(rviz::Display* disp color_transform->addOption( "Vintage", COLORS_VINTAGE ); color_transform->addOption( "Constant color", COLORS_CONSTANT ); - constant_color = new rviz::ColorProperty("Color", QColor( 130, 130, 130 ), "Color for tracked persons if using constant color transform.", m_display, SLOT(stylesChanged()), this ); + constant_color = new rviz_common::properties::ColorProperty("Color", QColor( 130, 130, 130 ), "Color for tracked persons if using constant color transform.", m_display, SLOT(stylesChanged()), this ); - color_map_offset = new rviz::IntProperty( "Color map offset", 0, "By how many indices to shift the offset in the color map (useful if not happy with the current colors)", m_display, SLOT(stylesChanged()), this); + color_map_offset = new rviz_common::properties::IntProperty( "Color map offset", 0, "By how many indices to shift the offset in the color map (useful if not happy with the current colors)", m_display, SLOT(stylesChanged()), this); color_map_offset->setMin( 0 ); - alpha = new rviz::FloatProperty( "Alpha", 1.0, "0 is fully transparent, 1.0 is fully opaque.", m_display, SLOT(stylesChanged()), this); + alpha = new rviz_common::properties::FloatProperty( "Alpha", 1.0, "0 is fully transparent, 1.0 is fully opaque.", m_display, SLOT(stylesChanged()), this); alpha->setMin( 0.0 ); alpha->setMax( 1.0 ); - line_width = new rviz::FloatProperty( "Line width", 0.05, "Line width for person visual", style, SLOT(stylesChanged()), this); + line_width = new rviz_common::properties::FloatProperty( "Line width", 0.05, "Line width for person visual", style, SLOT(stylesChanged()), this); line_width->setMin( 0.0 ); line_width->setMax( 1.0 ); - scaling_factor = new rviz::FloatProperty( "Scaling factor", 1.0, "Scaling factor for person visual", style); + scaling_factor = new rviz_common::properties::FloatProperty( "Scaling factor", 1.0, "Scaling factor for person visual", style); scaling_factor->setMin( 0.0 ); scaling_factor->setMax( 100.0 ); - font_color_style = new rviz::EnumProperty( "Font color style", "Same color", "Which type of font coloring to use", m_display, SLOT(stylesChanged()), this ); + font_color_style = new rviz_common::properties::EnumProperty( "Font color style", "Same color", "Which type of font coloring to use", m_display, SLOT(stylesChanged()), this ); font_color_style->addOption( "Same color", FONT_COLOR_FROM_PERSON ); font_color_style->addOption( "Constant color", FONT_COLOR_CONSTANT ); - constant_font_color = new rviz::ColorProperty("Font color", QColor( 255, 255, 255 ), "Font color if using a constant color", m_display, SLOT(stylesChanged()), this ); + constant_font_color = new rviz_common::properties::ColorProperty("Font color", QColor( 255, 255, 255 ), "Font color if using a constant color", m_display, SLOT(stylesChanged()), this ); - font_scale = new rviz::FloatProperty( "Font scale", 2.0, "Larger values mean bigger font", m_display); + font_scale = new rviz_common::properties::FloatProperty( "Font scale", 2.0, "Larger values mean bigger font", m_display); font_scale->setMin( 0.0 ); - z_offset = new rviz::FloatProperty( "Z offset", 0.0, "Offset of all visualizations on the z (height) axis", m_display, SLOT(stylesChanged()), this); + z_offset = new rviz_common::properties::FloatProperty( "Z offset", 0.0, "Offset of all visualizations on the z (height) axis", m_display, SLOT(stylesChanged()), this); - use_actual_z_position = new rviz::BoolProperty( "Use Z position from message", false, "Use Z position from message (otherwise place above ground plane)", z_offset, SLOT(stylesChanged()), this); + use_actual_z_position = new rviz_common::properties::BoolProperty( "Use Z position from message", false, "Use Z position from message (otherwise place above ground plane)", z_offset, SLOT(stylesChanged()), this); - m_excluded_person_ids_property = new rviz::StringProperty( "Excluded person IDs", "", "Comma-separated list of person IDs whose visualization should be hidden", m_display, SLOT(stylesChanged()), this ); - m_included_person_ids_property = new rviz::StringProperty( "Included person IDs", "", "Comma-separated list of person IDs whose visualization should be visible. Overrides excluded IDs.", m_display, SLOT(stylesChanged()), this ); + m_excluded_person_ids_property = new rviz_common::properties::StringProperty( "Excluded person IDs", "", "Comma-separated list of person IDs whose visualization should be hidden", m_display, SLOT(stylesChanged()), this ); + m_included_person_ids_property = new rviz_common::properties::StringProperty( "Included person IDs", "", "Comma-separated list of person IDs whose visualization should be visible. Overrides excluded IDs.", m_display, SLOT(stylesChanged()), this ); hideIrrelevantProperties(); } diff --git a/src/tracked_persons_display.cpp b/src/tracked_persons_display.cpp index 10a04a9..813982b 100644 --- a/src/tracked_persons_display.cpp +++ b/src/tracked_persons_display.cpp @@ -30,15 +30,15 @@ #ifndef Q_MOC_RUN -#include -#include -#include "rviz/selection/selection_manager.h" -#include "tracked_persons_display.h" -#include -#include -#include +//#include +//#include +//#include "rviz/selection/selection_manager.h" +//#include +//#include +//#include #endif -#define foreach BOOST_FOREACH +#include "spencer_tracking_rviz_plugin/tracked_persons_display.hpp" +//#define foreach BOOST_FOREACH namespace spencer_tracking_rviz_plugin @@ -53,45 +53,45 @@ void TrackedPersonsDisplay::onInitialize() m_realFixedFrame = "odom"; QObject::connect(m_commonProperties->style, SIGNAL(changed()), this, SLOT(personVisualTypeChanged()) ); - m_occlusion_alpha_property = new rviz::FloatProperty( "Occlusion alpha", 0.3, "Alpha multiplier for occluded tracks", this, SLOT(stylesChanged()) ); + m_occlusion_alpha_property = new rviz_common::properties::FloatProperty( "Occlusion alpha", 0.3, "Alpha multiplier for occluded tracks", this, SLOT(stylesChanged()) ); m_occlusion_alpha_property->setMin( 0.0 ); - m_missed_alpha_property = new rviz::FloatProperty( "Missed alpha", 0.5, "Alpha multiplier for missed tracks", this, SLOT(stylesChanged()) ); + m_missed_alpha_property = new rviz_common::properties::FloatProperty( "Missed alpha", 0.5, "Alpha multiplier for missed tracks", this, SLOT(stylesChanged()) ); m_missed_alpha_property->setMin( 0.0 ); - m_history_length_property = new rviz::IntProperty( "History size", 100, "Number of prior track positions to display.", this, SLOT(stylesChanged())); + m_history_length_property = new rviz_common::properties::IntProperty( "History size", 100, "Number of prior track positions to display.", this, SLOT(stylesChanged())); m_history_length_property->setMin( 1 ); m_history_length_property->setMax( 10000000 ); - m_delete_after_ncycles_property = new rviz::IntProperty( "Delete after no. cycles", 100, "After how many time steps to delete an old track that has not been seen again, including its history", this, SLOT(stylesChanged())); + m_delete_after_ncycles_property = new rviz_common::properties::IntProperty( "Delete after no. cycles", 100, "After how many time steps to delete an old track that has not been seen again, including its history", this, SLOT(stylesChanged())); m_delete_after_ncycles_property->setMin( 0 ); m_delete_after_ncycles_property->setMax( 10000000 ); - m_show_deleted_property = new rviz::BoolProperty( "Show DELETED tracks", false, "Show tracks which have been marked as deleted", this, SLOT(stylesChanged())); - m_show_occluded_property = new rviz::BoolProperty( "Show OCCLUDED tracks", true, "Show tracks which could not be matched to an detection due to sensor occlusion", this, SLOT(stylesChanged())); - m_show_missed_property = new rviz::BoolProperty( "Show MISSED tracks", true, "Show tracks which could not be matched to an detection but should be observable by the sensor", this, SLOT(stylesChanged())); - m_show_matched_property = new rviz::BoolProperty( "Show MATCHED tracks", true, "Show tracks which could be matched to an detection", this, SLOT(stylesChanged())); + m_show_deleted_property = new rviz_common::properties::BoolProperty( "Show DELETED tracks", false, "Show tracks which have been marked as deleted", this, SLOT(stylesChanged())); + m_show_occluded_property = new rviz_common::properties::BoolProperty( "Show OCCLUDED tracks", true, "Show tracks which could not be matched to an detection due to sensor occlusion", this, SLOT(stylesChanged())); + m_show_missed_property = new rviz_common::properties::BoolProperty( "Show MISSED tracks", true, "Show tracks which could not be matched to an detection but should be observable by the sensor", this, SLOT(stylesChanged())); + m_show_matched_property = new rviz_common::properties::BoolProperty( "Show MATCHED tracks", true, "Show tracks which could be matched to an detection", this, SLOT(stylesChanged())); - m_render_history_property = new rviz::BoolProperty( "Render history", true, "Render prior track positions", this, SLOT(stylesChanged())); - m_render_history_as_line_property = new rviz::BoolProperty( "History as line", true, "Display history as line instead of dots", this, SLOT(stylesChanged())); - m_render_person_property = new rviz::BoolProperty( "Render person visual", true, "Render person visualization", this, SLOT(stylesChanged())); - m_render_covariances_property = new rviz::BoolProperty( "Render covariances", true, "Render track covariance ellipses", this, SLOT(stylesChanged())); - m_render_velocities_property = new rviz::BoolProperty( "Render velocities", true, "Render track velocity arrows", this, SLOT(stylesChanged())); - m_render_ids_property = new rviz::BoolProperty( "Render track IDs", true, "Render track IDs as text", this, SLOT(stylesChanged())); - m_render_detection_ids_property = new rviz::BoolProperty( "Render detection IDs", true, "Render IDs of the detection that a track was matched against, if any", this, SLOT(stylesChanged())); - m_render_track_state_property = new rviz::BoolProperty( "Render track state", true, "Render track state text", this, SLOT(stylesChanged())); + m_render_history_property = new rviz_common::properties::BoolProperty( "Render history", true, "Render prior track positions", this, SLOT(stylesChanged())); + m_render_history_as_line_property = new rviz_common::properties::BoolProperty( "History as line", true, "Display history as line instead of dots", this, SLOT(stylesChanged())); + m_render_person_property = new rviz_common::properties::BoolProperty( "Render person visual", true, "Render person visualization", this, SLOT(stylesChanged())); + m_render_covariances_property = new rviz_common::properties::BoolProperty( "Render covariances", true, "Render track covariance ellipses", this, SLOT(stylesChanged())); + m_render_velocities_property = new rviz_common::properties::BoolProperty( "Render velocities", true, "Render track velocity arrows", this, SLOT(stylesChanged())); + m_render_ids_property = new rviz_common::properties::BoolProperty( "Render track IDs", true, "Render track IDs as text", this, SLOT(stylesChanged())); + m_render_detection_ids_property = new rviz_common::properties::BoolProperty( "Render detection IDs", true, "Render IDs of the detection that a track was matched against, if any", this, SLOT(stylesChanged())); + m_render_track_state_property = new rviz_common::properties::BoolProperty( "Render track state", true, "Render track state text", this, SLOT(stylesChanged())); - m_history_min_point_distance_property = new rviz::FloatProperty( "Min. history point distance", 0.4, "Minimum distance between history points before a new one is placed", this, SLOT(stylesChanged()) ); - m_history_line_width_property = new rviz::FloatProperty( "Line width", 0.05, "Line width of history", m_render_history_as_line_property, SLOT(stylesChanged()), this ); - m_covariance_line_width_property = new rviz::FloatProperty( "Line width", 0.1, "Line width of covariance ellipses", m_render_covariances_property, SLOT(stylesChanged()), this ); + m_history_min_point_distance_property = new rviz_common::properties::FloatProperty( "Min. history point distance", 0.4, "Minimum distance between history points before a new one is placed", this, SLOT(stylesChanged()) ); + m_history_line_width_property = new rviz_common::properties::FloatProperty( "Line width", 0.05, "Line width of history", m_render_history_as_line_property, SLOT(stylesChanged()), this ); + m_covariance_line_width_property = new rviz_common::properties::FloatProperty( "Line width", 0.1, "Line width of covariance ellipses", m_render_covariances_property, SLOT(stylesChanged()), this ); // TODO: Implement functionality //m_render_state_prediction_property = new rviz::BoolProperty( "Render state prediction", true, "Render state prediction from Kalman filter", this, SLOT( updateRenderFlags() )); // Create a scene node for visualizing track history - m_trackHistorySceneNode = boost::shared_ptr(scene_node_->createChildSceneNode()); + m_trackHistorySceneNode = std::shared_ptr(scene_node_->createChildSceneNode()); } TrackedPersonsDisplay::~TrackedPersonsDisplay() @@ -110,15 +110,16 @@ void TrackedPersonsDisplay::update(float wall_dt, float ros_dt) { // Move map scene node Ogre::Vector3 mapFramePosition; Ogre::Quaternion mapFrameOrientation; - getContext()->getFrameManager()->getTransform(m_realFixedFrame, ros::Time(0), mapFramePosition, mapFrameOrientation); + rclcpp::Clock clock(RCL_ROS_TIME); + getContext()->getFrameManager()->getTransform(m_realFixedFrame, clock.now(), mapFramePosition, mapFrameOrientation); Ogre::Matrix4 mapFrameTransform(mapFrameOrientation); mapFrameTransform.setTrans(mapFramePosition); m_trackHistorySceneNode->setPosition(mapFramePosition); m_trackHistorySceneNode->setOrientation(mapFrameOrientation); // Update position of deleted tracks (because they are not being updated by ROS messages any more) - foreach(const track_map::value_type& entry, m_cachedTracks) + for(auto entry : m_cachedTracks) { - const boost::shared_ptr& trackedPersonVisual = entry.second; + const std::shared_ptr& trackedPersonVisual = entry.second; if(trackedPersonVisual->isDeleted) { Ogre::Matrix4 poseInCurrentFrame = mapFrameTransform * trackedPersonVisual->lastObservedPose; Ogre::Vector3 position = poseInCurrentFrame.getTrans(); Ogre::Quaternion orientation = poseInCurrentFrame.extractQuaternion(); @@ -140,10 +141,10 @@ void TrackedPersonsDisplay::stylesChanged() const Ogre::Quaternion shapeQuaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ); // Update each track - foreach(const track_map::value_type& entry, m_cachedTracks) + for(auto entry : m_cachedTracks) { const track_id trackId = entry.first; - const boost::shared_ptr& trackedPersonVisual = entry.second; + const std::shared_ptr& trackedPersonVisual = entry.second; // Update common styles to person visual, such as line width applyCommonStyles(trackedPersonVisual->personVisual); @@ -179,7 +180,7 @@ void TrackedPersonsDisplay::stylesChanged() trackedPersonVisual->history.rset_capacity(m_history_length_property->getInt()); // Update history color - foreach(boost::shared_ptr historyEntry, trackedPersonVisual->history) { + for(auto historyEntry : trackedPersonVisual->history) { const double historyShapeDiameter = 0.1; Ogre::ColourValue historyColor = trackColorWithFullAlpha; historyColor.a *= m_commonProperties->alpha->getFloat(); // general alpha @@ -242,7 +243,7 @@ void TrackedPersonsDisplay::personVisualTypeChanged() { foreach(const track_map::value_type& entry, m_cachedTracks) { - const boost::shared_ptr& trackedPersonVisual = entry.second; + const std::shared_ptr& trackedPersonVisual = entry.second; trackedPersonVisual->personVisual.reset(); createPersonVisualIfRequired(trackedPersonVisual->sceneNode.get(), trackedPersonVisual->personVisual); } @@ -250,7 +251,7 @@ void TrackedPersonsDisplay::personVisualTypeChanged() } // This is our callback to handle an incoming message. -void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedPersons::ConstPtr& msg) +void TrackedPersonsDisplay::processMessage(spencer_tracking_msgs::msg::TrackedPersons::ConstSharedPtr msg) { // Get transforms into fixed frame etc. if(!preprocessMessage(msg)) return; @@ -268,13 +269,16 @@ void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedP // Iterate over all tracks in this message, see if we have a cached visual (then update it) or create a new one. // set encounteredTrackIds; - for (vector::const_iterator trackedPersonIt = msg->tracks.begin(); trackedPersonIt != msg->tracks.end(); ++trackedPersonIt) + for (vector::const_iterator trackedPersonIt = msg->tracks.begin(); trackedPersonIt != msg->tracks.end(); ++trackedPersonIt) { - boost::shared_ptr trackedPersonVisual; + std::shared_ptr trackedPersonVisual; // See if we encountered this track ID before in this loop (means duplicate track ID) if (encounteredTrackIds.find(trackedPersonIt->track_id) != encounteredTrackIds.end()) { - ROS_ERROR_STREAM("spencer_tracking_msgs::TrackedPersons contains duplicate track ID " << trackedPersonIt->track_id << "! Skipping duplicate track."); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("tracked_persons_logger"), + "spencer_tracking_msgs::TrackedPersons contains duplicate track ID " << + trackedPersonIt->track_id << + "! Skipping duplicate track."); continue; } else { @@ -287,15 +291,14 @@ void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedP } else { // Create a new visual representation of the tracked person - trackedPersonVisual = boost::shared_ptr(new TrackedPersonVisual); + trackedPersonVisual = std::shared_ptr(new TrackedPersonVisual); m_cachedTracks[trackedPersonIt->track_id] = trackedPersonVisual; // This scene node is the parent of all visualization elements for the tracked person - trackedPersonVisual->sceneNode = boost::shared_ptr(scene_node_->createChildSceneNode()); - trackedPersonVisual->historySceneNode = boost::shared_ptr(m_trackHistorySceneNode->createChildSceneNode()); - trackedPersonVisual->historyLineSceneNode = boost::shared_ptr(m_trackHistorySceneNode->createChildSceneNode()); + trackedPersonVisual->sceneNode = std::shared_ptr(scene_node_->createChildSceneNode()); + trackedPersonVisual->historySceneNode = std::shared_ptr(m_trackHistorySceneNode->createChildSceneNode()); + trackedPersonVisual->historyLineSceneNode = std::shared_ptr(m_trackHistorySceneNode->createChildSceneNode()); } - // These values need to be remembered for later use in stylesChanged() if(trackedPersonIt->is_occluded && !trackedPersonIt->is_matched){ trackedPersonVisual->isOccluded = true; @@ -315,13 +318,12 @@ void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedP Ogre::SceneNode* currentSceneNode = trackedPersonVisual->sceneNode.get(); - // // Person visualization // // Create new visual for the person itself, if needed - boost::shared_ptr &personVisual = trackedPersonVisual->personVisual; + std::shared_ptr personVisual = trackedPersonVisual->personVisual; createPersonVisualIfRequired(currentSceneNode, personVisual); const double personHeight = personVisual ? personVisual->getHeight() : 0; @@ -341,19 +343,18 @@ void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedP // Ogre::Vector3 newHistoryEntryPosition = mapFrameTransform.inverse() * currentSceneNode->getPosition(); - const float MIN_HISTORY_ENTRY_DISTANCE = m_history_min_point_distance_property->getFloat(); // in meters if((trackedPersonVisual->positionOfLastHistoryEntry - newHistoryEntryPosition).length() > MIN_HISTORY_ENTRY_DISTANCE) { // General history - boost::shared_ptr newHistoryEntry(new TrackedPersonHistoryEntry); + std::shared_ptr newHistoryEntry(new TrackedPersonHistoryEntry); newHistoryEntry->trackId = trackedPersonIt->track_id; newHistoryEntry->position = newHistoryEntryPosition; // used by history lines (below) even if no shape is set newHistoryEntry->wasOccluded = trackedPersonIt->is_occluded; trackedPersonVisual->history.push_back(newHistoryEntry); // Always need to reset history line since history is like a queue, oldest element has to be removed but BillboardLine doesn't offer that functionality - trackedPersonVisual->historyLine.reset(new rviz::BillboardLine(context_->getSceneManager(), trackedPersonVisual->historyLineSceneNode.get()) ); + trackedPersonVisual->historyLine.reset(new rviz_rendering::BillboardLine(context_->getSceneManager(), trackedPersonVisual->historyLineSceneNode.get()) ); if(m_render_history_as_line_property->getBool()) { // History lines @@ -361,7 +362,7 @@ void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedP trackedPersonVisual->historyLine->setLineWidth(m_history_line_width_property->getFloat()); trackedPersonVisual->historyLine->setMaxPointsPerLine(trackedPersonVisual->history.size()); - foreach(const boost::shared_ptr& historyEntry, trackedPersonVisual->history) { + for(auto historyEntry : trackedPersonVisual->history) { historyEntry->shape.reset(); // remove existing dot shapes, if any, for better performance trackedPersonVisual->historyLine->addPoint(historyEntry->position); } @@ -369,7 +370,12 @@ void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedP } else { // History dots - newHistoryEntry->shape = boost::shared_ptr(new rviz::Shape(rviz::Shape::Cylinder, context_->getSceneManager(), trackedPersonVisual->historySceneNode.get())); + newHistoryEntry->shape = std::shared_ptr( + new rviz_rendering::Shape( + rviz_rendering::Shape::Cylinder, context_->getSceneManager(), + trackedPersonVisual->historySceneNode.get() + ) + ); newHistoryEntry->shape->setPosition(newHistoryEntryPosition); newHistoryEntry->shape->setOrientation(shapeQuaternion); } @@ -377,7 +383,6 @@ void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedP trackedPersonVisual->positionOfLastHistoryEntry = newHistoryEntryPosition; } - // // Texts // @@ -413,7 +418,8 @@ void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedP // Velocity arrows // if (!trackedPersonVisual->velocityArrow) { - trackedPersonVisual->velocityArrow.reset(new rviz::Arrow(context_->getSceneManager(), currentSceneNode)); + trackedPersonVisual->velocityArrow.reset( + new rviz_rendering::Arrow(context_->getSceneManager(), currentSceneNode)); } // Update velocity arrow @@ -422,7 +428,9 @@ void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedP if(velocityVector.isZeroLength() || velocityVector.length() > 100 || velocityVector.isNaN()) { if(!velocityVector.isZeroLength()) { // do not show warning for zero velocity - ROS_WARN("Track %lu has suspicious velocity (%.1f m/s), not showing velocity vector!", trackedPersonIt->track_id, velocityVector.length()); + RCLCPP_WARN(rclcpp::get_logger("tracked_persons_logger"), + "Track %lu has suspicious velocity (%.1f m/s), not showing velocity vector!", + trackedPersonIt->track_id, velocityVector.length()); } } else { @@ -436,29 +444,29 @@ void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedP trackedPersonVisual->hasZeroVelocity = velocityVector.length() < 0.05; } - boost::shared_ptr meshPersonVisual = boost::dynamic_pointer_cast(personVisual); + std::shared_ptr meshPersonVisual = + std::dynamic_pointer_cast(personVisual); if(meshPersonVisual) { meshPersonVisual->setWalkingSpeed(velocityVector.length()); } } - // // Covariance visualization // if(!trackedPersonVisual->covarianceVisual) { - trackedPersonVisual->covarianceVisual.reset(new ProbabilityEllipseCovarianceVisual(context_->getSceneManager(), currentSceneNode)); + trackedPersonVisual->covarianceVisual.reset( + new ProbabilityEllipseCovarianceVisual(context_->getSceneManager(), currentSceneNode)); } // Update covariance ellipse { - Ogre::Vector3 covarianceMean(0,0,0); // zero mean because parent node is already centered at pose mean + Ogre::Vector3 covarianceMean(0 ,0, 0); // zero mean because parent node is already centered at pose mean trackedPersonVisual->covarianceVisual->setOrientation(currentSceneNode->getOrientation().Inverse()); trackedPersonVisual->covarianceVisual->setMeanCovariance(covarianceMean, covXYZinTargetFrame); } - + } // end for loop over all tracked persons - // Set all properties which can be dynamically in the GUI. This iterates over all tracks. stylesChanged(); @@ -466,9 +474,9 @@ void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedP // First hide, then delete old cached tracks which have not been seen for a while // set trackIdsToDelete; - for (map >::const_iterator cachedTrackIt = m_cachedTracks.begin(); cachedTrackIt != m_cachedTracks.end(); ++cachedTrackIt) { + for (auto cachedTrackIt = m_cachedTracks.begin(); cachedTrackIt != m_cachedTracks.end(); ++cachedTrackIt) { if (encounteredTrackIds.end() == encounteredTrackIds.find(cachedTrackIt->first)) { - const boost::shared_ptr& trackedPersonVisual = cachedTrackIt->second; + const std::shared_ptr& trackedPersonVisual = cachedTrackIt->second; // Update state and visibility if(!trackedPersonVisual->isDeleted) { @@ -497,12 +505,13 @@ void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedP // ss.str(""); ss << msg->tracks.size() << " tracks received"; - setStatusStd(rviz::StatusProperty::Ok, "Tracks", ss.str()); + + setStatusStd(rviz_common::properties::StatusProperty::Ok, "Tracks", ss.str()); } } // end namespace spencer_tracking_rviz_plugin // Tell pluginlib about this class. It is important to do this in // global scope, outside our package's namespace. -#include -PLUGINLIB_EXPORT_CLASS(spencer_tracking_rviz_plugin::TrackedPersonsDisplay, rviz::Display) +#include +PLUGINLIB_EXPORT_CLASS(spencer_tracking_rviz_plugin::TrackedPersonsDisplay, rviz_common::Display) \ No newline at end of file diff --git a/src/visuals/person_visual.cpp b/src/visuals/person_visual.cpp index db3bdc3..e1d69b0 100644 --- a/src/visuals/person_visual.cpp +++ b/src/visuals/person_visual.cpp @@ -28,27 +28,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include "spencer_tracking_rviz_plugin/visuals/person_visual.hpp" -#include -#ifndef Q_MOC_RUN -#include -#include -#include - -#include -#include -#endif -#include -#include -#include -#include -#include -#include - -#include "person_visual.h" - - -namespace fs = boost::filesystem; namespace spencer_tracking_rviz_plugin { @@ -56,23 +37,22 @@ namespace spencer_tracking_rviz_plugin { class RosPackagePathResourceLoadingListener : public Ogre::ResourceLoadingListener { public: - RosPackagePathResourceLoadingListener(const fs::path& parentPath) : _parentPath(parentPath) { + RosPackagePathResourceLoadingListener(std::string parentPath) : _parentPath(parentPath) { } /** This event is called when a resource beings loading. */ virtual Ogre::DataStreamPtr resourceLoading(const Ogre::String &name, const Ogre::String &group, Ogre::Resource *resource) { - fs::path absolutePath = _parentPath / name; - ROS_INFO_STREAM("RosPackagePathResourceLoadingListener loading resource: " << absolutePath.string()); - + std::string absolutePath = _parentPath + "/" + name; + RVIZ_COMMON_LOG_INFO_STREAM("RosPackagePathResourceLoadingListener loading resource: " << absolutePath); try { resource_retriever::Retriever retriever; - _lastResource = retriever.get(absolutePath.string()); // not thread-safe! + _lastResource = retriever.get(absolutePath); // not thread-safe! return Ogre::DataStreamPtr(new Ogre::MemoryDataStream(_lastResource.data.get(), _lastResource.size)); } catch (resource_retriever::Exception& e) { - ROS_ERROR("In RosPackagePathResourceLoadingListener: %s", e.what()); + RVIZ_COMMON_LOG_ERROR_STREAM("In RosPackagePathResourceLoadingListener: " << e.what()); return Ogre::DataStreamPtr(); } } @@ -85,7 +65,7 @@ class RosPackagePathResourceLoadingListener : public Ogre::ResourceLoadingListen } private: - const fs::path& _parentPath; + const std::string _parentPath; resource_retriever::MemoryResource _lastResource; }; @@ -96,17 +76,15 @@ MeshPersonVisual::MeshPersonVisual(const PersonVisualDefaultArgs& args) : Person m_childSceneNode = m_sceneNode->createChildSceneNode(); m_childSceneNode->setVisible(false); - std::string meshResource = "package://" ROS_PACKAGE_NAME "/media/animated_walking_man.mesh"; - + std::string meshResource = "package://spencer_tracking_rviz_plugin/media/animated_walking_man.mesh"; + std::string parent_path = "package://spencer_tracking_rviz_plugin/media"; /// This is required to load referenced skeletons from package:// path - fs::path model_path(meshResource); - fs::path parent_path(model_path.parent_path()); - + Ogre::ResourceLoadingListener *newListener = new RosPackagePathResourceLoadingListener(parent_path), *oldListener = Ogre::ResourceGroupManager::getSingleton().getLoadingListener(); Ogre::ResourceGroupManager::getSingleton().setLoadingListener(newListener); - bool loadFailed = rviz::loadMeshFromResource(meshResource).isNull(); + bool loadFailed = rviz_rendering::loadMeshFromResource(meshResource).isNull(); Ogre::ResourceGroupManager::getSingleton().setLoadingListener(oldListener); delete newListener; @@ -160,8 +138,8 @@ MeshPersonVisual::~MeshPersonVisual() { } } materials_.clear(); - - m_sceneManager->destroySceneNode(m_childSceneNode->getName()); + if (m_childSceneNode->getName() != "") + m_sceneManager->destroySceneNode(m_childSceneNode->getName()); } void MeshPersonVisual::setColor(const Ogre::ColourValue& c) { @@ -209,7 +187,7 @@ void MeshPersonVisual::setAnimationState(const std::string& nameOfAnimationState } // Not found. Set first animation state then. - ROS_WARN_STREAM_ONCE("Person mesh animation state " << nameOfAnimationState << " does not exist in mesh!"); + RVIZ_COMMON_LOG_WARNING_STREAM("Person mesh animation state " << nameOfAnimationState << " does not exist in mesh!"); setAnimationState(""); } } From 737eb1dc792be38939039a953366789029d75ea8 Mon Sep 17 00:00:00 2001 From: Jonatan Gines Clavero Date: Tue, 18 Aug 2020 11:03:38 +0200 Subject: [PATCH 2/3] Update README.md --- README.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/README.md b/README.md index 7ef689b..db2148e 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,8 @@ RViz plugins for visualization of detected and tracked persons, groups, social relations, activities and human attributes ------------------------------------------------------------------------------------------------------------------------- + +## **_WARNING_**: _The migration to ROS2 eloquent is in progress. Please, [check the migrated nodes and the ones still to be migrated](#migration-progress)._ + Author: ©2013-2015 Timm Linder, Social Robotics Laboratory, Albert-Ludwigs-University Freiburg, Germany E-mail: linder@cs.uni-freiburg.de @@ -36,3 +39,12 @@ Known issues Rviz may crash with a segfault when exiting. This usually can be ignored. Person visuals in the tracked groups and human attributes display do not support walking animations. However, these displays can be combined with the tracked persons display (which does support animations) by disabling their person visuals. + + +## Migration progress +- [x] Detected persons +- [x] Tracked persons. _(There is a warning log with the covariance and the Person Meshes style causes rviz2 crashed.)_ +- [ ] Tracked groups +- [ ] Social relations +- [ ] Social activities +- [ ] Human attributes From 30263cc99fe328270bf8ad01a95e41174e6a4254 Mon Sep 17 00:00:00 2001 From: Jonatan Gines Clavero Date: Tue, 18 Aug 2020 11:08:17 +0200 Subject: [PATCH 3/3] Update README.md --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index db2148e..349f7e8 100644 --- a/README.md +++ b/README.md @@ -48,3 +48,4 @@ However, these displays can be combined with the tracked persons display (which - [ ] Social relations - [ ] Social activities - [ ] Human attributes +- [ ] Python send msgs test