diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt index 5065bc9f..a1d692d2 100644 --- a/visualization/CMakeLists.txt +++ b/visualization/CMakeLists.txt @@ -1,69 +1,56 @@ -cmake_minimum_required(VERSION 3.1.3) +cmake_minimum_required(VERSION 3.5) project(moveit_task_constructor_visualization) -find_package(catkin REQUIRED COMPONENTS - moveit_core - moveit_ros_visualization - moveit_task_constructor_core - moveit_task_constructor_msgs - roscpp - rviz -) - -# rviz transitively includes OGRE headers which break with `-Wall -Werror` -# so isolate these include dirs and add them as SYSTEM include where needed. -set(rviz_OGRE_INCLUDE_DIRS) -foreach(header IN ITEMS OgreRoot.h OgreOverlay.h) - find_path(include_dir ${header} - HINTS ${catkin_INCLUDE_DIRS} - NO_DEFAULT_PATH) - list(REMOVE_ITEM catkin_INCLUDE_DIRS "${include_dir}") - list(APPEND rviz_OGRE_INCLUDE_DIRS "${include_dir}") -endforeach() +find_package(ament_cmake REQUIRED) +find_package(Boost REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_ros_visualization REQUIRED) +find_package(moveit_task_constructor_core REQUIRED) +find_package(moveit_task_constructor_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rviz_common REQUIRED) +find_package(rviz_default_plugins REQUIRED) +find_package(rviz_ogre_vendor REQUIRED) # definition needed for boost/math/constants/constants.hpp included by Ogre to compile add_definitions(-DBOOST_MATH_DISABLE_FLOAT128) # Qt Stuff -if("${rviz_QT_VERSION}" VERSION_LESS "5") - find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui) - include(${QT_USE_FILE}) - macro(qt_wrap_ui) - qt4_wrap_ui(${ARGN}) - endmacro() -else() - find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) - set(QT_LIBRARIES Qt5::Widgets) - macro(qt_wrap_ui) - qt5_wrap_ui(${ARGN}) - endmacro() -endif() +find_package(Qt5 REQUIRED COMPONENTS Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +macro(qt_wrap_ui) + qt5_wrap_ui(${ARGN}) +endmacro() set(CMAKE_INCLUDE_CURRENT_DIR ON) set(CMAKE_AUTOMOC ON) set(CMAKE_AUTORCC ON) -add_definitions(-DQT_NO_KEYWORDS) -catkin_package( - LIBRARIES - moveit_task_visualization_tools - motion_planning_tasks_utils - INCLUDE_DIRS - visualization_tools/include - CATKIN_DEPENDS - moveit_core - moveit_task_constructor_msgs - roscpp - rviz -) - -set(CMAKE_CXX_STANDARD 14) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() add_subdirectory(visualization_tools) add_subdirectory(motion_planning_tasks) -install(FILES - motion_planning_tasks_rviz_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY icons DESTINATION share) -install(DIRECTORY icons DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +pluginlib_export_plugin_description_file(rviz_common motion_planning_tasks_rviz_plugin_description.xml) + +ament_export_include_directories(include) +ament_export_libraries(motion_planning_tasks_utils + motion_planning_tasks_properties + motion_planning_tasks_rviz_plugin + moveit_task_visualization_tools +) +ament_export_dependencies(ament_cmake) +ament_export_dependencies(Boost) +ament_export_dependencies(moveit_core) +ament_export_dependencies(moveit_ros_visualization) +ament_export_dependencies(moveit_task_constructor_core) +ament_export_dependencies(moveit_task_constructor_msgs) +ament_export_dependencies(rclcpp) +ament_export_dependencies(rviz_common) +ament_export_dependencies(rviz_default_plugins) +ament_export_dependencies(rviz_ogre_vendor) +ament_package() diff --git a/visualization/motion_planning_tasks/properties/CMakeLists.txt b/visualization/motion_planning_tasks/properties/CMakeLists.txt index b0fabdaf..d1351ab2 100644 --- a/visualization/motion_planning_tasks/properties/CMakeLists.txt +++ b/visualization/motion_planning_tasks/properties/CMakeLists.txt @@ -20,9 +20,14 @@ target_link_libraries(${MOVEIT_LIB_NAME} ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $ - PRIVATE ${catkin_INCLUDE_DIRS} ${YAML_INCLUDE_DIRS} + PRIVATE ${YAML_INCLUDE_DIRS} +) +ament_target_dependencies(${MOVEIT_LIB_NAME} + moveit_task_constructor_core + rviz_common ) install(TARGETS ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + EXPORT export_${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) diff --git a/visualization/motion_planning_tasks/properties/property_factory.cpp b/visualization/motion_planning_tasks/properties/property_factory.cpp index 95542a74..5d607944 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.cpp +++ b/visualization/motion_planning_tasks/properties/property_factory.cpp @@ -40,34 +40,34 @@ #include #include -#include -#include -#include +#include +#include +#include namespace mtc = ::moveit::task_constructor; namespace moveit_rviz_plugin { -static rviz::StringProperty* stringFactory(const QString& name, mtc::Property& mtc_prop, - const planning_scene::PlanningScene* /*unused*/, - rviz::DisplayContext* /*unused*/) { +static rviz_common::properties::StringProperty* stringFactory(const QString& name, mtc::Property& mtc_prop, + const planning_scene::PlanningScene* /*unused*/, + rviz_common::DisplayContext* /*unused*/) { std::string value; if (!mtc_prop.value().empty()) value = boost::any_cast(mtc_prop.value()); - rviz::StringProperty* rviz_prop = - new rviz::StringProperty(name, QString::fromStdString(value), QString::fromStdString(mtc_prop.description())); - QObject::connect(rviz_prop, &rviz::StringProperty::changed, + rviz_common::properties::StringProperty* rviz_prop = new rviz_common::properties::StringProperty( + name, QString::fromStdString(value), QString::fromStdString(mtc_prop.description())); + QObject::connect(rviz_prop, &rviz_common::properties::StringProperty::changed, [rviz_prop, &mtc_prop]() { mtc_prop.setValue(rviz_prop->getStdString()); }); return rviz_prop; } template -static rviz::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc_prop, - const planning_scene::PlanningScene* /*unused*/, - rviz::DisplayContext* /*unused*/) { +static rviz_common::properties::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc_prop, + const planning_scene::PlanningScene* /*unused*/, + rviz_common::DisplayContext* /*unused*/) { T value = !mtc_prop.value().empty() ? boost::any_cast(mtc_prop.value()) : T(); - rviz::FloatProperty* rviz_prop = - new rviz::FloatProperty(name, value, QString::fromStdString(mtc_prop.description())); - QObject::connect(rviz_prop, &rviz::FloatProperty::changed, + rviz_common::properties::FloatProperty* rviz_prop = + new rviz_common::properties::FloatProperty(name, value, QString::fromStdString(mtc_prop.description())); + QObject::connect(rviz_prop, &rviz_common::properties::FloatProperty::changed, [rviz_prop, &mtc_prop]() { mtc_prop.setValue(T(rviz_prop->getFloat())); }); return rviz_prop; } @@ -94,33 +94,34 @@ void PropertyFactory::registerStage(const std::type_index& type_index, const Pro stage_registry_.insert(std::make_pair(type_index, f)); } -rviz::Property* PropertyFactory::create(const std::string& prop_name, mtc::Property& prop, - const planning_scene::PlanningScene* scene, - rviz::DisplayContext* display_context) const { +rviz_common::properties::Property* PropertyFactory::create(const std::string& prop_name, mtc::Property& prop, + const planning_scene::PlanningScene* scene, + rviz_common::DisplayContext* display_context) const { auto it = property_registry_.find(prop.typeName()); if (it == property_registry_.end()) return createDefault(prop_name, prop.typeName(), prop.description(), prop.serialize()); return it->second(QString::fromStdString(prop_name), prop, scene, display_context); } -rviz::PropertyTreeModel* PropertyFactory::createPropertyTreeModel(moveit::task_constructor::Stage& stage, - const planning_scene::PlanningScene* scene, - rviz::DisplayContext* display_context) { +rviz_common::properties::PropertyTreeModel* +PropertyFactory::createPropertyTreeModel(moveit::task_constructor::Stage& stage, + const planning_scene::PlanningScene* scene, + rviz_common::DisplayContext* display_context) { auto it = stage_registry_.find(typeid(stage)); if (it == stage_registry_.end()) return defaultPropertyTreeModel(stage.properties(), scene, display_context); return it->second(stage.properties(), scene, display_context); } -rviz::PropertyTreeModel* PropertyFactory::defaultPropertyTreeModel(mtc::PropertyMap& properties, - const planning_scene::PlanningScene* scene, - rviz::DisplayContext* display_context) { - auto root = new rviz::Property(); +rviz_common::properties::PropertyTreeModel* +PropertyFactory::defaultPropertyTreeModel(mtc::PropertyMap& properties, const planning_scene::PlanningScene* scene, + rviz_common::DisplayContext* display_context) { + auto root = new rviz_common::properties::Property(); addRemainingProperties(root, properties, scene, display_context); - return new rviz::PropertyTreeModel(root, nullptr); + return new rviz_common::properties::PropertyTreeModel(root, nullptr); } -static bool hasChild(rviz::Property* root, const QString& name) { +static bool hasChild(rviz_common::properties::Property* root, const QString& name) { for (int i = 0, end = root->numChildren(); i != end; ++i) { if (root->childAt(i)->getName() == name) return true; @@ -128,37 +129,38 @@ static bool hasChild(rviz::Property* root, const QString& name) { return false; } -void PropertyFactory::addRemainingProperties(rviz::Property* root, mtc::PropertyMap& properties, +void PropertyFactory::addRemainingProperties(rviz_common::properties::Property* root, mtc::PropertyMap& properties, const planning_scene::PlanningScene* scene, - rviz::DisplayContext* display_context) { + rviz_common::DisplayContext* display_context) { for (auto& prop : properties) { const QString& name = QString::fromStdString(prop.first); if (hasChild(root, name)) continue; - rviz::Property* rviz_prop = create(prop.first, prop.second, scene, display_context); + rviz_common::properties::Property* rviz_prop = create(prop.first, prop.second, scene, display_context); if (!rviz_prop) - rviz_prop = new rviz::Property(name); + rviz_prop = new rviz_common::properties::Property(name); root->addChild(rviz_prop); } // just to see something, when no properties are defined if (root->numChildren() == 0) - new rviz::Property("no properties", QVariant(), QString(), root); + new rviz_common::properties::Property("no properties", QVariant(), QString(), root); } #ifndef HAVE_YAML -rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, - const std::string& description, const std::string& value, - rviz::Property* old) { +rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, + const std::string& description, + const std::string& value, + rviz_common::properties::Property* old) { if (old) { // reuse existing Property? assert(old->getNameStd() == name); old->setDescription(QString::fromStdString(description)); old->setValue(QString::fromStdString(value)); return old; } else { // create new Property? - rviz::Property* result = new rviz::StringProperty(QString::fromStdString(name), QString::fromStdString(value), - QString::fromStdString(description)); + rviz_common::properties::Property* result = new rviz::StringProperty( + QString::fromStdString(name), QString::fromStdString(value), QString::fromStdString(description)); result->setReadOnly(true); return result; } diff --git a/visualization/motion_planning_tasks/properties/property_factory.h b/visualization/motion_planning_tasks/properties/property_factory.h index 84c7c916..112d108a 100644 --- a/visualization/motion_planning_tasks/properties/property_factory.h +++ b/visualization/motion_planning_tasks/properties/property_factory.h @@ -44,11 +44,14 @@ #include -namespace rviz { +namespace rviz_common { +class DisplayContext; +namespace properties { class Property; class PropertyTreeModel; -class DisplayContext; -} // namespace rviz +} // namespace properties +} // namespace rviz_common + namespace planning_scene { class PlanningScene; } @@ -60,10 +63,10 @@ class Stage; namespace moveit_rviz_plugin { -/** Registry for rviz::Property and rviz::PropertyTreeModel creator functions. +/** Registry for rviz_common::properties::Property and rviz_common::properties::PropertyTreeModel creator functions. * * To inspect (and edit) properties of stages, our MTC properties are converted to rviz properties, - * which are finally shown in an rviz::PropertyTree. + * which are finally shown in an rviz_common::properties::PropertyTree. * To allow customization of property display, one can register creator functions for individual * properties as well as creator functions for a complete stage. The latter allows to fully customize * the display of stage properties, e.g. hiding specific properties, or returning a subclassed @@ -75,11 +78,11 @@ class PropertyFactory public: static PropertyFactory& instance(); - using PropertyFactoryFunction = - std::function; - using TreeFactoryFunction = std::function; + using PropertyFactoryFunction = std::function; + using TreeFactoryFunction = std::function; /// register a factory function for type T template @@ -94,27 +97,30 @@ public: registerStage(typeid(T), f); } - /// create rviz::Property for given MTC Property - rviz::Property* create(const std::string& prop_name, moveit::task_constructor::Property& prop, - const planning_scene::PlanningScene* scene, rviz::DisplayContext* display_context) const; - /// create rviz::Property for property of given name, type, description, and value - static rviz::Property* createDefault(const std::string& name, const std::string& type, - const std::string& description, const std::string& value, - rviz::Property* old = nullptr); + /// create rviz_common::properties::Property for given MTC Property + rviz_common::properties::Property* create(const std::string& prop_name, moveit::task_constructor::Property& prop, + const planning_scene::PlanningScene* scene, + rviz_common::DisplayContext* display_context) const; + /// create rviz_common::properties::Property for property of given name, type, description, and value + static rviz_common::properties::Property* createDefault(const std::string& name, const std::string& type, + const std::string& description, const std::string& value, + rviz_common::properties::Property* old = nullptr); /// create PropertyTreeModel for given Stage - rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage& stage, - const planning_scene::PlanningScene* scene, - rviz::DisplayContext* display_context); + rviz_common::properties::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage& stage, + const planning_scene::PlanningScene* scene, + rviz_common::DisplayContext* display_context); - /// turn a PropertyMap into an rviz::PropertyTreeModel - rviz::PropertyTreeModel* defaultPropertyTreeModel(moveit::task_constructor::PropertyMap& properties, - const planning_scene::PlanningScene* scene, - rviz::DisplayContext* display_context); + /// turn a PropertyMap into an rviz_common::properties::PropertyTreeModel + rviz_common::properties::PropertyTreeModel* + defaultPropertyTreeModel(moveit::task_constructor::PropertyMap& properties, + const planning_scene::PlanningScene* scene, rviz_common::DisplayContext* display_context); /// add all properties from map that are not yet in root - void addRemainingProperties(rviz::Property* root, moveit::task_constructor::PropertyMap& properties, - const planning_scene::PlanningScene* scene, rviz::DisplayContext* display_context); + void addRemainingProperties(rviz_common::properties::Property* root, + moveit::task_constructor::PropertyMap& properties, + const planning_scene::PlanningScene* scene, + rviz_common::DisplayContext* display_context); private: std::map property_registry_; diff --git a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp index a46d18bf..9d1ad862 100644 --- a/visualization/motion_planning_tasks/properties/property_from_yaml.cpp +++ b/visualization/motion_planning_tasks/properties/property_from_yaml.cpp @@ -36,12 +36,12 @@ #include "property_factory.h" #include -#include -#include +#include +#include namespace mtc = ::moveit::task_constructor; -/** Implement PropertyFactory::createDefault(), creating an rviz::Property (tree) +/** Implement PropertyFactory::createDefault(), creating an rviz_common::properties::Property (tree) * from a YAML-serialized string. * As we cannot know the required data type for a field from YAML parsing, * we only distinguish numbers (FloatProperty) and all other YAML scalars (StringProperty). @@ -60,7 +60,7 @@ private: yaml_event_t event_; }; -// Event-based YAML parser, creating an rviz::Property tree +// Event-based YAML parser, creating an rviz_common::properties::Property tree // https://www.wpsoftware.net/andrew/pages/libyaml.html class Parser { @@ -70,30 +70,35 @@ public: Parser(const std::string& value); ~Parser(); - rviz::Property* process(const QString& name, const QString& description, rviz::Property* old) const; + rviz_common::properties::Property* process(const QString& name, const QString& description, + rviz_common::properties::Property* old) const; private: - static rviz::Property* createScalar(const QString& name, const QString& description, const QByteArray& value, - rviz::Property* old); + static rviz_common::properties::Property* createScalar(const QString& name, const QString& description, + const QByteArray& value, + rviz_common::properties::Property* old); // return true if there was no error so far bool noError() const { return parser_.error == YAML_NO_ERROR; } // parse a single event and return it's type, YAML_ERROR_EVENT on parsing error int parse(yaml_event_t& event) const; // process events: scalar, start mapping, start sequence - rviz::Property* process(const yaml_event_t& event, const QString& name, const QString& description, - rviz::Property* old) const; + rviz_common::properties::Property* process(const yaml_event_t& event, const QString& name, + const QString& description, rviz_common::properties::Property* old) const; inline static QByteArray byteArray(const yaml_event_t& event) { assert(event.type == YAML_SCALAR_EVENT); return QByteArray::fromRawData(reinterpret_cast(event.data.scalar.value), event.data.scalar.length); } - // Try to set value of existing rviz::Property (expecting matching types). Return false on error. - static bool setValue(rviz::Property* old, const QByteArray& value); + // Try to set value of existing rviz_common::properties::Property (expecting matching types). Return false on error. + static bool setValue(rviz_common::properties::Property* old, const QByteArray& value); - static rviz::Property* createParent(const QString& name, const QString& description, rviz::Property* old); - rviz::Property* processMapping(const QString& name, const QString& description, rviz::Property* old) const; - rviz::Property* processSequence(const QString& name, const QString& description, rviz::Property* old) const; + static rviz_common::properties::Property* createParent(const QString& name, const QString& description, + rviz_common::properties::Property* old); + rviz_common::properties::Property* processMapping(const QString& name, const QString& description, + rviz_common::properties::Property* old) const; + rviz_common::properties::Property* processSequence(const QString& name, const QString& description, + rviz_common::properties::Property* old) const; private: mutable yaml_parser_t parser_; @@ -116,7 +121,8 @@ int Parser::parse(yaml_event_t& event) const { } // main processing function -rviz::Property* Parser::process(const QString& name, const QString& description, rviz::Property* old) const { +rviz_common::properties::Property* Parser::process(const QString& name, const QString& description, + rviz_common::properties::Property* old) const { bool stop = false; while (!stop) { ScopedYamlEvent event; @@ -141,8 +147,9 @@ rviz::Property* Parser::process(const QString& name, const QString& description, } // default processing for scalar, start mapping, start sequence events -rviz::Property* Parser::process(const yaml_event_t& event, const QString& name, const QString& description, - rviz::Property* old) const { +rviz_common::properties::Property* Parser::process(const yaml_event_t& event, const QString& name, + const QString& description, + rviz_common::properties::Property* old) const { switch (event.type) { case YAML_SEQUENCE_START_EVENT: return processSequence(name, description, old); @@ -153,18 +160,20 @@ rviz::Property* Parser::process(const yaml_event_t& event, const QString& name, default: throw std::runtime_error("Unhandled YAML event"); } + assert(false); // should not be reached + return nullptr; } // Try to set numeric or arbitrary scalar value from YAML node. Needs to match old's type. -bool Parser::setValue(rviz::Property* old, const QByteArray& value) { - if (rviz::FloatProperty* p = dynamic_cast(old)) { +bool Parser::setValue(rviz_common::properties::Property* old, const QByteArray& value) { + if (rviz_common::properties::FloatProperty* p = dynamic_cast(old)) { bool ok = true; double v = value.toDouble(&ok); if (ok) p->setValue(v); return ok; } - if (rviz::StringProperty* p = dynamic_cast(old)) { + if (rviz_common::properties::StringProperty* p = dynamic_cast(old)) { // value should be an arbitrary string. If not throws YAML::BadConversion p->setValue(value); return true; @@ -173,9 +182,10 @@ bool Parser::setValue(rviz::Property* old, const QByteArray& value) { } // Update existing old rviz:Property or create a new one from scalar YAML node -rviz::Property* Parser::createScalar(const QString& name, const QString& description, const QByteArray& value, - rviz::Property* old) { - // try to update value, expecting matching rviz::Property +rviz_common::properties::Property* Parser::createScalar(const QString& name, const QString& description, + const QByteArray& value, + rviz_common::properties::Property* old) { + // try to update value, expecting matching rviz_common::properties::Property if (old && setValue(old, value)) { // only if setValue succeeded, also update the rest old->setName(name); @@ -186,21 +196,23 @@ rviz::Property* Parser::createScalar(const QString& name, const QString& descrip bool ok = true; double v = value.toDouble(&ok); if (ok) // if value is a number, create a FloatProperty - old = new rviz::FloatProperty(name, v, description); + old = new rviz_common::properties::FloatProperty(name, v, description); else // otherwise create a StringProperty - old = new rviz::StringProperty(name, value, description); + old = new rviz_common::properties::StringProperty(name, value, description); old->setReadOnly(true); return old; } // Reuse old property (or create new one) as parent for a sequence or map -rviz::Property* Parser::createParent(const QString& name, const QString& description, rviz::Property* old) { +rviz_common::properties::Property* Parser::createParent(const QString& name, const QString& description, + rviz_common::properties::Property* old) { // don't reuse float or string properties (they are for scalars) - if (dynamic_cast(old) || dynamic_cast(old)) + if (dynamic_cast(old) || + dynamic_cast(old)) old = nullptr; if (!old) { - old = new rviz::Property(name, QVariant(), description); + old = new rviz_common::properties::Property(name, QVariant(), description); old->setReadOnly(true); } else { old->setName(name); @@ -210,7 +222,8 @@ rviz::Property* Parser::createParent(const QString& name, const QString& descrip } // Hierarchically create property from YAML map node -rviz::Property* Parser::processMapping(const QString& name, const QString& description, rviz::Property* root) const { +rviz_common::properties::Property* Parser::processMapping(const QString& name, const QString& description, + rviz_common::properties::Property* root) const { root = createParent(name, description, root); int index = 0; // current child index in root bool stop = false; @@ -233,11 +246,11 @@ rviz::Property* Parser::processMapping(const QString& name, const QString& descr num = root->numChildren(); // if names differ, insert a new child, otherwise reuse existing - rviz::Property* old_child = index < num ? root->childAt(index) : nullptr; + rviz_common::properties::Property* old_child = index < num ? root->childAt(index) : nullptr; if (old_child && old_child->getName() != key) old_child = nullptr; - rviz::Property* new_child = nullptr; + rviz_common::properties::Property* new_child = nullptr; switch (parse(event)) { // parse value case YAML_MAPPING_START_EVENT: case YAML_SEQUENCE_START_EVENT: @@ -268,7 +281,8 @@ rviz::Property* Parser::processMapping(const QString& name, const QString& descr } // Hierarchically create property from YAML sequence node. Items are named [#]. -rviz::Property* Parser::processSequence(const QString& name, const QString& description, rviz::Property* root) const { +rviz_common::properties::Property* Parser::processSequence(const QString& name, const QString& description, + rviz_common::properties::Property* root) const { root = createParent(name, description, root); int index = 0; // current child index in root bool stop = false; @@ -282,8 +296,8 @@ rviz::Property* Parser::processSequence(const QString& name, const QString& desc case YAML_MAPPING_START_EVENT: case YAML_SEQUENCE_START_EVENT: case YAML_SCALAR_EVENT: { - rviz::Property* old_child = root->childAt(index); // nullptr for invalid index - rviz::Property* new_child = process(event, QString("[%1]").arg(index), "", old_child); + rviz_common::properties::Property* old_child = root->childAt(index); // nullptr for invalid index + rviz_common::properties::Property* new_child = process(event, QString("[%1]").arg(index), "", old_child); if (new_child != old_child) root->addChild(new_child, index); if (++index >= 10) @@ -305,9 +319,10 @@ rviz::Property* Parser::processSequence(const QString& name, const QString& desc namespace moveit_rviz_plugin { -rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/, - const std::string& description, const std::string& value, - rviz::Property* old) { +rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/, + const std::string& description, + const std::string& value, + rviz_common::properties::Property* old) { QString qname = QString::fromStdString(name); QString qdesc = QString::fromStdString(description); Parser parser(value); diff --git a/visualization/motion_planning_tasks/src/CMakeLists.txt b/visualization/motion_planning_tasks/src/CMakeLists.txt index 832134c7..a46edd0b 100644 --- a/visualization/motion_planning_tasks/src/CMakeLists.txt +++ b/visualization/motion_planning_tasks/src/CMakeLists.txt @@ -6,7 +6,7 @@ qt_wrap_ui(UIC_FILES global_settings.ui ) -add_library(${MOVEIT_LIB_NAME} +add_library(${MOVEIT_LIB_NAME} SHARED factory_model.cpp icons.cpp job_queue.cpp @@ -28,7 +28,7 @@ add_library(${MOVEIT_LIB_NAME} set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(${MOVEIT_LIB_NAME} motion_planning_tasks_utils motion_planning_tasks_properties moveit_task_visualization_tools - ${catkin_LIBRARIES} ${QT_LIBRARIES} + ${QT_LIBRARIES} ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $ @@ -37,12 +37,9 @@ target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $ PUBLIC $ PUBLIC $ - PUBLIC ${catkin_INCLUDE_DIRS} -) -target_include_directories(${MOVEIT_LIB_NAME} SYSTEM - PUBLIC ${rviz_OGRE_INCLUDE_DIRS} ) install(TARGETS ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + EXPORT export_${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) diff --git a/visualization/motion_planning_tasks/src/factory_model.cpp b/visualization/motion_planning_tasks/src/factory_model.cpp index 18054a45..445ed903 100644 --- a/visualization/motion_planning_tasks/src/factory_model.cpp +++ b/visualization/motion_planning_tasks/src/factory_model.cpp @@ -35,29 +35,29 @@ /* Author: Robert Haschke */ #include "factory_model.h" -#include +#include #include #include namespace moveit_rviz_plugin { -FactoryModel::FactoryModel(rviz::Factory& factory, const QString& mime_type, QObject* parent) +FactoryModel::FactoryModel(rviz_common::Factory& factory, const QString& mime_type, QObject* parent) : QStandardItemModel(parent), mime_type_(mime_type) { setHorizontalHeaderLabels({ tr("Name") }); fillTree(factory); } -void FactoryModel::fillTree(rviz::Factory& factory) { - QIcon default_package_icon = rviz::loadPixmap("package://rviz/icons/default_package_icon.png"); +void FactoryModel::fillTree(rviz_common::Factory& factory) { + QIcon default_package_icon = rviz_common::loadPixmap("package://rviz/icons/default_package_icon.png"); - QStringList classes = factory.getDeclaredClassIds(); - classes.sort(); + auto plugins = factory.getDeclaredPlugins(); + std::sort(plugins.begin(), plugins.end()); // Map from package names to the corresponding top-level tree widget items. std::map package_items; - for (const QString& lookup_name : classes) { - QString package = factory.getClassPackage(lookup_name); + for (const auto& plugin : plugins) { + const QString& package = plugin.package; QStandardItem* package_item; auto mi = package_items.find(package); @@ -68,9 +68,9 @@ void FactoryModel::fillTree(rviz::Factory& factory) { } else { package_item = mi->second; } - QStandardItem* class_item = new QStandardItem(factory.getIcon(lookup_name), factory.getClassName(lookup_name)); - class_item->setWhatsThis(factory.getClassDescription(lookup_name)); - class_item->setData(lookup_name, Qt::UserRole); + QStandardItem* class_item = new QStandardItem(plugin.icon, plugin.name); + class_item->setWhatsThis(plugin.description); + class_item->setData(plugin.id, Qt::UserRole); class_item->setDragEnabled(true); package_item->appendRow(class_item); } diff --git a/visualization/motion_planning_tasks/src/factory_model.h b/visualization/motion_planning_tasks/src/factory_model.h index cbdf991d..634425ab 100644 --- a/visualization/motion_planning_tasks/src/factory_model.h +++ b/visualization/motion_planning_tasks/src/factory_model.h @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace moveit_rviz_plugin { @@ -47,10 +47,10 @@ namespace moveit_rviz_plugin { class FactoryModel : public QStandardItemModel { QString mime_type_; - void fillTree(rviz::Factory& factory); + void fillTree(rviz_common::Factory& factory); public: - FactoryModel(rviz::Factory& factory, const QString& mime_type, QObject* parent = nullptr); + FactoryModel(rviz_common::Factory& factory, const QString& mime_type, QObject* parent = nullptr); QStringList mimeTypes() const override; QMimeData* mimeData(const QModelIndexList& indexes) const override; diff --git a/visualization/motion_planning_tasks/src/global_settings.ui b/visualization/motion_planning_tasks/src/global_settings.ui index 4efb8c45..a61bc45c 100644 --- a/visualization/motion_planning_tasks/src/global_settings.ui +++ b/visualization/motion_planning_tasks/src/global_settings.ui @@ -51,7 +51,7 @@ - + @@ -59,9 +59,9 @@ - rviz::PropertyTreeWidget + rviz_common::properties::PropertyTreeWidget QTreeView -
rviz/properties/property_tree_widget.h
+
rviz_common/properties/property_tree_widget.hpp
diff --git a/visualization/motion_planning_tasks/src/job_queue.cpp b/visualization/motion_planning_tasks/src/job_queue.cpp index 929bf454..d49f2e4c 100644 --- a/visualization/motion_planning_tasks/src/job_queue.cpp +++ b/visualization/motion_planning_tasks/src/job_queue.cpp @@ -35,7 +35,9 @@ /* Author: Robert Haschke */ #include "job_queue.h" -#include +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.job_queue"); namespace moveit { namespace tools { @@ -71,7 +73,7 @@ void JobQueue::executeJobs() { try { fn(); } catch (std::exception& ex) { - ROS_ERROR("Exception caught executing main loop job: %s", ex.what()); + RCLCPP_ERROR(LOGGER, "Exception caught executing main loop job: %s", ex.what()); } ulock.lock(); } diff --git a/visualization/motion_planning_tasks/src/local_task_model.cpp b/visualization/motion_planning_tasks/src/local_task_model.cpp index 8fcb6246..bfe4be29 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.cpp +++ b/visualization/motion_planning_tasks/src/local_task_model.cpp @@ -37,9 +37,7 @@ #include "local_task_model.h" #include "factory_model.h" #include "properties/property_factory.h" -#include - -#include +#include #include @@ -78,7 +76,7 @@ QModelIndex LocalTaskModel::index(Node* n) const { } LocalTaskModel::LocalTaskModel(ContainerBase::pointer&& container, const planning_scene::PlanningSceneConstPtr& scene, - rviz::DisplayContext* display_context, QObject* parent) + rviz_common::DisplayContext* display_context, QObject* parent) : BaseTaskModel(scene, display_context, parent), Task("", true, std::move(container)) { root_ = this; flags_ |= LOCAL_MODEL; @@ -235,7 +233,7 @@ DisplaySolutionPtr LocalTaskModel::getSolution(const QModelIndex& /*index*/) { return DisplaySolutionPtr(); } -rviz::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex& index) { +rviz_common::properties::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex& index) { Node* n = node(index); if (!n) return nullptr; diff --git a/visualization/motion_planning_tasks/src/local_task_model.h b/visualization/motion_planning_tasks/src/local_task_model.h index dd34bd22..c01832d5 100644 --- a/visualization/motion_planning_tasks/src/local_task_model.h +++ b/visualization/motion_planning_tasks/src/local_task_model.h @@ -47,14 +47,14 @@ class LocalTaskModel : public BaseTaskModel, public moveit::task_constructor::Ta using Node = moveit::task_constructor::Stage; Node* root_; StageFactoryPtr stage_factory_; - std::map properties_; + std::map properties_; inline Node* node(const QModelIndex& index) const; QModelIndex index(Node* n) const; public: LocalTaskModel(ContainerBase::pointer&& container, const planning_scene::PlanningSceneConstPtr& scene, - rviz::DisplayContext* display_context, QObject* parent = nullptr); + rviz_common::DisplayContext* display_context, QObject* parent = nullptr); int rowCount(const QModelIndex& parent = QModelIndex()) const override; QModelIndex index(int row, int column, const QModelIndex& parent = QModelIndex()) const override; @@ -76,6 +76,6 @@ public: QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; DisplaySolutionPtr getSolution(const QModelIndex& index) override; - rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; + rviz_common::properties::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; }; } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/src/plugin_init.cpp b/visualization/motion_planning_tasks/src/plugin_init.cpp index 1bf29708..660bee24 100644 --- a/visualization/motion_planning_tasks/src/plugin_init.cpp +++ b/visualization/motion_planning_tasks/src/plugin_init.cpp @@ -34,9 +34,9 @@ /* Author: Robert Haschke */ -#include +#include #include "task_display.h" #include "task_panel.h" -PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskDisplay, rviz::Display) -PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskPanel, rviz::Panel) +PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskPanel, rviz_common::Panel) diff --git a/visualization/motion_planning_tasks/src/pluginlib_factory.h b/visualization/motion_planning_tasks/src/pluginlib_factory.h index 77b91206..9c325edb 100644 --- a/visualization/motion_planning_tasks/src/pluginlib_factory.h +++ b/visualization/motion_planning_tasks/src/pluginlib_factory.h @@ -45,12 +45,13 @@ #include #ifndef Q_MOC_RUN -#include -#include +#include +#include #include #endif -#include +#include +#include namespace moveit_rviz_plugin { @@ -58,7 +59,7 @@ namespace moveit_rviz_plugin { * This is a slightly modified version of rviz::PluginlibFactory, providing a custom mime type. */ template -class PluginlibFactory : public rviz::Factory +class PluginlibFactory : public rviz_common::Factory { private: struct BuiltInClassRecord @@ -80,41 +81,39 @@ public: /// retrieve mime type used for given factory QString mimeType() const { return mime_type_; } - QStringList getDeclaredClassIds() override { - QStringList ids; - for (const auto& record : built_ins_) - ids.push_back(record.class_id_); + std::vector getDeclaredPlugins() override { + std::vector plugins; + for (auto iter = built_ins_.cbegin(); iter != built_ins_.cend(); ++iter) + plugins.emplace_back(getPluginInfo(iter.key())); for (const auto& id : class_loader_->getDeclaredClasses()) { - QString sid = QString::fromStdString(id); - if (ids.contains(sid)) + auto sid = QString::fromStdString(id); + if (std::find_if(plugins.cbegin(), plugins.cend(), [&sid](const rviz_common::PluginInfo& plugin_info) { + return plugin_info.id == sid; + }) != plugins.cend()) continue; // built_in take precedence - ids.push_back(sid); + plugins.emplace_back(getPluginInfo(QString::fromStdString(id))); } - return ids; + return plugins; } - QString getClassDescription(const QString& class_id) const override { - auto it = built_ins_.find(class_id); - if (it != built_ins_.end()) { - return it->description_; + rviz_common::PluginInfo getPluginInfo(const QString& class_id) const override { + rviz_common::PluginInfo info; + const auto iter = built_ins_.find(class_id); + if (iter != built_ins_.end()) { + info.id = iter->class_id_; + info.name = iter->name_; + info.package = iter->package_; + info.description = iter->description_; + info.icon = getIcon(info); + return info; } - return QString::fromStdString(class_loader_->getClassDescription(class_id.toStdString())); - } - - QString getClassName(const QString& class_id) const override { - auto it = built_ins_.find(class_id); - if (it != built_ins_.end()) { - return it->name_; - } - return QString::fromStdString(class_loader_->getName(class_id.toStdString())); - } - - QString getClassPackage(const QString& class_id) const override { - auto it = built_ins_.find(class_id); - if (it != built_ins_.end()) { - return it->package_; - } - return QString::fromStdString(class_loader_->getClassPackage(class_id.toStdString())); + auto class_id_std = class_id.toStdString(); + info.id = class_id; + info.name = QString::fromStdString(class_loader_->getName(class_id_std)); + info.package = QString::fromStdString(class_loader_->getClassPackage(class_id_std)); + info.description = QString::fromStdString(class_loader_->getClassDescription(class_id_std)); + info.icon = getIcon(info); + return info; } virtual QString getPluginManifestPath(const QString& class_id) const { @@ -125,14 +124,12 @@ public: return QString::fromStdString(class_loader_->getPluginManifestPath(class_id.toStdString())); } - QIcon getIcon(const QString& class_id) const override { - QString package = getClassPackage(class_id); - QString class_name = getClassName(class_id); - QIcon icon = rviz::loadPixmap("package://" + package + "/icons/classes/" + class_name + ".svg"); + QIcon getIcon(const rviz_common::PluginInfo& info) const { + QIcon icon = rviz_common::loadPixmap("package://" + info.package + "/icons/classes/" + info.name + ".svg"); if (icon.isNull()) { - icon = rviz::loadPixmap("package://" + package + "/icons/classes/" + class_name + ".png"); + icon = rviz_common::loadPixmap("package://" + info.package + "/icons/classes/" + info.name + ".png"); if (icon.isNull()) { - icon = rviz::loadPixmap("package://rviz/icons/default_class_icon.png"); + icon = rviz_common::loadPixmap("package://rviz/icons/default_class_icon.png"); } } return icon; @@ -175,8 +172,9 @@ public: try { return class_loader_->createUnmanagedInstance(class_id.toStdString()); } catch (pluginlib::PluginlibException& ex) { - ROS_ERROR("PluginlibFactory: The plugin for class '%s' failed to load. Error: %s", qPrintable(class_id), - ex.what()); + RCLCPP_ERROR(rclcpp::get_logger("moveit_task_constructor_visualization.pluginlib_factory"), + "PluginlibFactory: The plugin for class '%s' failed to load. Error: %s", qPrintable(class_id), + ex.what()); if (error_return) { *error_return = QString::fromStdString(ex.what()); } diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index 79347694..de4057b4 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -41,15 +41,16 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include #include #include #include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.task_list_model"); + using namespace moveit::task_constructor; namespace moveit_rviz_plugin { @@ -69,12 +70,12 @@ struct RemoteTaskModel::Node InterfaceFlags interface_flags_; NodeFlags node_flags_; std::unique_ptr solutions_; - std::unique_ptr property_tree_; + std::unique_ptr property_tree_; std::map properties_; inline Node(Node* parent) : parent_(parent) { solutions_.reset(new RemoteSolutionModel()); - property_tree_.reset(new rviz::PropertyTreeModel(new rviz::Property())); + property_tree_.reset(new rviz_common::properties::PropertyTreeModel(new rviz_common::properties::Property())); } bool setName(const QString& name) { @@ -84,18 +85,20 @@ struct RemoteTaskModel::Node return true; } - void setProperties(const std::vector& props, - const planning_scene::PlanningSceneConstPtr& scene_, rviz::DisplayContext* display_context_); - rviz::Property* createProperty(const moveit_task_constructor_msgs::Property& prop, rviz::Property* old, - const planning_scene::PlanningSceneConstPtr& scene_, - rviz::DisplayContext* display_context_); + void setProperties(const std::vector& props, + const planning_scene::PlanningSceneConstPtr& scene_, + rviz_common::DisplayContext* display_context_); + rviz_common::properties::Property* createProperty(const moveit_task_constructor_msgs::msg::Property& prop, + rviz_common::properties::Property* old, + const planning_scene::PlanningSceneConstPtr& scene_, + rviz_common::DisplayContext* display_context_); }; -void RemoteTaskModel::Node::setProperties(const std::vector& props, +void RemoteTaskModel::Node::setProperties(const std::vector& props, const planning_scene::PlanningSceneConstPtr& scene_, - rviz::DisplayContext* display_context_) { + rviz_common::DisplayContext* display_context_) { // insert properties in same order as reported in description - rviz::Property* root = property_tree_->getRoot(); + rviz_common::properties::Property* root = property_tree_->getRoot(); int index = 0; // current child index in root for (const auto& prop : props) { int num = root->numChildren(); @@ -108,11 +111,11 @@ void RemoteTaskModel::Node::setProperties(const std::vectornumChildren(); // if names differ, insert a new child, otherwise reuse existing - rviz::Property* old_child = index < num ? root->childAt(index) : nullptr; + rviz_common::properties::Property* old_child = index < num ? root->childAt(index) : nullptr; if (old_child && old_child->getName().toStdString() != prop.name) old_child = nullptr; - rviz::Property* new_child = createProperty(prop, old_child, scene_, display_context_); + rviz_common::properties::Property* new_child = createProperty(prop, old_child, scene_, display_context_); if (new_child != old_child) root->addChild(new_child, index); ++index; @@ -121,25 +124,26 @@ void RemoteTaskModel::Node::setProperties(const std::vectorremoveChildren(index, root->numChildren() - index); } -rviz::Property* RemoteTaskModel::Node::createProperty(const moveit_task_constructor_msgs::Property& prop, - rviz::Property* old, - const planning_scene::PlanningSceneConstPtr& scene_, - rviz::DisplayContext* display_context_) { +rviz_common::properties::Property* RemoteTaskModel::Node::createProperty( + const moveit_task_constructor_msgs::msg::Property& prop, rviz_common::properties::Property* old, + const planning_scene::PlanningSceneConstPtr& scene_, rviz_common::DisplayContext* display_context_) { auto& factory = PropertyFactory::instance(); // try to deserialize from msg (using registered functions) boost::any value = Property::deserialize(prop.type, prop.value); - if (!value.empty()) { // if successful, create rviz::Property from mtc::Property using factory methods + if (!value.empty()) { // if successful, create rviz_common::properties::Property from mtc::Property using factory + // methods auto it = properties_.insert(std::make_pair(prop.name, Property())).first; it->second.setDescription(prop.description); it->second.setValue(value); - if (rviz::Property* rviz_prop = factory.create(prop.name, it->second, scene_.get(), display_context_)) { + if (rviz_common::properties::Property* rviz_prop = + factory.create(prop.name, it->second, scene_.get(), display_context_)) { rviz_prop->setReadOnly(true); return rviz_prop; } else properties_.erase(it); } - // otherwise create default, read-only rviz::Property by parsing serialized YAML + // otherwise create default, read-only rviz_common::properties::Property by parsing serialized YAML return factory.createDefault(prop.name, prop.type, prop.description, prop.value, old); } @@ -149,7 +153,7 @@ RemoteTaskModel::Node* RemoteTaskModel::node(const QModelIndex& index) const { return root_; if (index.model() != this) { - ROS_ERROR_NAMED("TaskModel", "invalid model in QModelIndex"); + RCLCPP_ERROR(LOGGER, "invalid model in QModelIndex"); return nullptr; } @@ -180,13 +184,18 @@ QModelIndex RemoteTaskModel::index(const Node* n) const { return QModelIndex(); } -RemoteTaskModel::RemoteTaskModel(ros::NodeHandle& nh, const std::string& service_name, - const planning_scene::PlanningSceneConstPtr& scene, - rviz::DisplayContext* display_context, QObject* parent) +RemoteTaskModel::RemoteTaskModel(const std::string& service_name, const planning_scene::PlanningSceneConstPtr& scene, + rviz_common::DisplayContext* display_context, QObject* parent) : BaseTaskModel(scene, display_context, parent), root_(new Node(nullptr)) { id_to_stage_[0] = root_; // root node has ID 0 + // Add random ID to prevent warnings about multiple publishers within the same node + rclcpp::NodeOptions options; + options.arguments({ "--ros-args", "-r", + "__node:=get_solution_node_" + std::to_string(reinterpret_cast(this)), "-r", + "__ns:=/moveit_task_constructor/remote_task_model" }); + node_ = rclcpp::Node::make_shared("_", options); // service to request solutions - get_solution_client_ = nh.serviceClient(service_name); + get_solution_client_ = node_->create_client(service_name); } RemoteTaskModel::~RemoteTaskModel() { @@ -277,13 +286,14 @@ QModelIndex RemoteTaskModel::indexFromStageId(size_t id) const { return n ? index(n) : QModelIndex(); } -void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type& msg) { +void RemoteTaskModel::processStageDescriptions( + const moveit_task_constructor_msgs::msg::TaskDescription::_stages_type& msg) { // iterate over descriptions and create new / update existing nodes where needed for (const auto& s : msg) { // find parent node for stage s, this should always exist auto parent_it = id_to_stage_.find(s.parent_id); if (parent_it == id_to_stage_.end()) { - ROS_ERROR_NAMED("TaskListModel", "No parent found for stage %d (%s)", s.id, s.name.c_str()); + RCLCPP_ERROR(LOGGER, "No parent found for stage %d (%s)", s.id, s.name.c_str()); continue; } Node* parent = parent_it->second; @@ -336,13 +346,14 @@ void RemoteTaskModel::processStageDescriptions(const moveit_task_constructor_msg } } -void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type& msg) { +void RemoteTaskModel::processStageStatistics( + const moveit_task_constructor_msgs::msg::TaskStatistics::_stages_type& msg) { // iterate over statistics and update node's solutions where needed for (const auto& s : msg) { // find node for stage s, this should always exist auto it = id_to_stage_.find(s.id); if (it == id_to_stage_.end()) { - ROS_ERROR_NAMED("TaskListModel", "No stage %d", s.id); + RCLCPP_ERROR(LOGGER, "No stage %d", s.id); continue; } Node* n = it->second; @@ -356,14 +367,14 @@ void RemoteTaskModel::processStageStatistics(const moveit_task_constructor_msgs: } } -void RemoteTaskModel::setSolutionData(const moveit_task_constructor_msgs::SolutionInfo& info) { +void RemoteTaskModel::setSolutionData(const moveit_task_constructor_msgs::msg::SolutionInfo& info) { if (info.id == 0) return; if (RemoteSolutionModel* m = getSolutionModel(info.stage_id)) m->setSolutionData(info.id, info.cost, QString::fromStdString(info.comment)); } -DisplaySolutionPtr RemoteTaskModel::processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg) { +DisplaySolutionPtr RemoteTaskModel::processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg) { DisplaySolutionPtr s(new DisplaySolution); s->setFromMessage(scene_->diff(), msg); @@ -416,18 +427,21 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex& index) { if (it == id_to_solution_.cend()) { // TODO: try to assemble (and cache) the solution from known leaves // to avoid some communication overhead - DisplaySolutionPtr result; if (!(flags_ & IS_DESTROYED)) { - // request solution via service - moveit_task_constructor_msgs::GetSolution srv; - srv.request.solution_id = id; - if (get_solution_client_.call(srv)) { - id_to_solution_[id] = result = processSolutionMessage(srv.response.solution); - return result; + if (get_solution_client_->service_is_ready()) { + // request solution via service + auto request = std::make_shared(); + request->solution_id = id; + auto result_future = get_solution_client_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, result_future) == rclcpp::FutureReturnCode::SUCCESS) { + id_to_solution_[id] = result = processSolutionMessage(result_future.get()->solution); + return result; + } } // on failure mark remote task as destroyed: don't retrieve more solutions - get_solution_client_.shutdown(); + get_solution_client_.reset(); + node_.reset(); flags_ |= IS_DESTROYED; } return result; @@ -435,7 +449,7 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex& index) { return it->second; } -rviz::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex& index) { +rviz_common::properties::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex& index) { Node* n = node(index); if (!n) return nullptr; diff --git a/visualization/motion_planning_tasks/src/remote_task_model.h b/visualization/motion_planning_tasks/src/remote_task_model.h index 8db812be..6343ecd6 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.h +++ b/visualization/motion_planning_tasks/src/remote_task_model.h @@ -38,7 +38,8 @@ #include "task_list_model.h" #include -#include +#include +#include #include #include @@ -54,7 +55,11 @@ class RemoteTaskModel : public BaseTaskModel Q_OBJECT struct Node; Node* const root_; - ros::ServiceClient get_solution_client_; + rclcpp::Client::SharedPtr get_solution_client_; + // TODO(JafarAbdi): We shouldn't need this, replace with callback groups (should be fully available in Galactic) + // RViz have a single threaded executor which is causing the get_solution_client_ to timeout without + // getting the result + rclcpp::Node::SharedPtr node_; std::map id_to_stage_; std::map id_to_solution_; @@ -64,12 +69,11 @@ class RemoteTaskModel : public BaseTaskModel Node* node(uint32_t stage_id) const; inline RemoteSolutionModel* getSolutionModel(uint32_t stage_id) const; - void setSolutionData(const moveit_task_constructor_msgs::SolutionInfo& info); + void setSolutionData(const moveit_task_constructor_msgs::msg::SolutionInfo& info); public: - RemoteTaskModel(ros::NodeHandle& nh, const std::string& service_name, - const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context, - QObject* parent = nullptr); + RemoteTaskModel(const std::string& service_name, const planning_scene::PlanningSceneConstPtr& scene, + rviz_common::DisplayContext* display_context, QObject* parent = nullptr); ~RemoteTaskModel() override; int rowCount(const QModelIndex& parent = QModelIndex()) const override; @@ -81,14 +85,14 @@ public: bool setData(const QModelIndex& index, const QVariant& value, int role = Qt::EditRole) override; QModelIndex indexFromStageId(size_t id) const override; - void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type& msg); - void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type& msg); - DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg); + void processStageDescriptions(const moveit_task_constructor_msgs::msg::TaskDescription::_stages_type& msg); + void processStageStatistics(const moveit_task_constructor_msgs::msg::TaskStatistics::_stages_type& msg); + DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg); QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; DisplaySolutionPtr getSolution(const QModelIndex& index) override; - rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; + rviz_common::properties::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override; }; /** Model representing solutions of a remote task */ diff --git a/visualization/motion_planning_tasks/src/task_display.cpp b/visualization/motion_planning_tasks/src/task_display.cpp index 8a3e875a..53d716aa 100644 --- a/visualization/motion_planning_tasks/src/task_display.cpp +++ b/visualization/motion_planning_tasks/src/task_display.cpp @@ -44,19 +44,22 @@ #include #include #include -#include +#include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.task_display"); + namespace moveit_rviz_plugin { TaskDisplay::TaskDisplay() : Display(), panel_requested_(false), received_task_description_(false) { @@ -71,12 +74,12 @@ TaskDisplay::TaskDisplay() : Display(), panel_requested_(false), received_task_d connect(task_list_model_.get(), SIGNAL(dataChanged(QModelIndex, QModelIndex)), this, SLOT(onTaskDataChanged(QModelIndex, QModelIndex))); - robot_description_property_ = new rviz::StringProperty( + robot_description_property_ = new rviz_common::properties::StringProperty( "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded", this, SLOT(changedRobotDescription()), this); - task_solution_topic_property_ = new rviz::RosTopicProperty( - "Task Solution Topic", "", ros::message_traits::datatype(), + task_solution_topic_property_ = new rviz_common::properties::RosTopicProperty( + "Task Solution Topic", "", rosidl_generator_traits::data_type(), "The topic on which task solutions (moveit_msgs::Solution messages) are received", this, SLOT(changedTaskSolutionTopic()), this); @@ -84,7 +87,8 @@ TaskDisplay::TaskDisplay() : Display(), panel_requested_(false), received_task_d connect(trajectory_visual_.get(), SIGNAL(activeStageChanged(size_t)), task_list_model_.get(), SLOT(highlightStage(size_t))); - tasks_property_ = new rviz::Property("Tasks", QVariant(), "Tasks received on monitored topic", this); + tasks_property_ = + new rviz_common::properties::Property("Tasks", QVariant(), "Tasks received on monitored topic", this); } TaskDisplay::~TaskDisplay() { @@ -94,6 +98,8 @@ TaskDisplay::~TaskDisplay() { void TaskDisplay::onInitialize() { Display::onInitialize(); + rviz_ros_node_ = context_->getRosNodeAbstraction(); + task_solution_topic_property_->initialize(rviz_ros_node_); trajectory_visual_->onInitialize(scene_node_, context_); task_list_model_->setDisplayContext(context_); } @@ -109,18 +115,19 @@ inline void TaskDisplay::requestPanel() { } void TaskDisplay::loadRobotModel() { - rdf_loader_.reset(new rdf_loader::RDFLoader(robot_description_property_->getStdString())); + rdf_loader_.reset( + new rdf_loader::RDFLoader(rviz_ros_node_.lock()->get_raw_node(), robot_description_property_->getStdString())); if (!rdf_loader_->getURDF()) { - this->setStatus(rviz::StatusProperty::Error, "Robot Model", + this->setStatus(rviz_common::properties::StatusProperty::Error, "Robot Model", "Failed to load from parameter " + robot_description_property_->getString()); return; } - this->setStatus(rviz::StatusProperty::Ok, "Robot Model", "Successfully loaded"); + this->setStatus(rviz_common::properties::StatusProperty::Ok, "Robot Model", "Successfully loaded"); const srdf::ModelSharedPtr& srdf = rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); - robot_model_.reset(new robot_model::RobotModel(rdf_loader_->getURDF(), srdf)); + robot_model_.reset(new moveit::core::RobotModel(rdf_loader_->getURDF(), srdf)); // Send to child class trajectory_visual_->onRobotModelLoaded(robot_model_); @@ -139,11 +146,11 @@ void TaskDisplay::reset() { trajectory_visual_->reset(); } -void TaskDisplay::save(rviz::Config config) const { +void TaskDisplay::save(rviz_common::Config config) const { Display::save(config); } -void TaskDisplay::load(const rviz::Config& config) { +void TaskDisplay::load(const rviz_common::Config& config) { Display::load(config); } @@ -170,7 +177,8 @@ void TaskDisplay::calculateOffsetPosition() { Ogre::Vector3 position; Ogre::Quaternion orientation; - context_->getFrameManager()->getTransform(robot_model_->getModelFrame(), ros::Time(0), position, orientation); + context_->getFrameManager()->getTransform(robot_model_->getModelFrame(), rclcpp::Time(0, 0, RCL_ROS_TIME), position, + orientation); scene_node_->setPosition(position); scene_node_->setOrientation(orientation); @@ -183,11 +191,6 @@ void TaskDisplay::update(float wall_dt, float ros_dt) { trajectory_visual_->update(wall_dt, ros_dt); } -void TaskDisplay::setName(const QString& name) { - BoolProperty::setName(name); - trajectory_visual_->setName(name); -} - void TaskDisplay::changedRobotDescription() { if (isEnabled()) reset(); @@ -195,28 +198,37 @@ void TaskDisplay::changedRobotDescription() { loadRobotModel(); } -void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg) { - setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); +void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr msg) { + setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK"); requestPanel(); - task_list_model_->processTaskDescriptionMessage(*msg, update_nh_, - base_ns_ + GET_SOLUTION_SERVICE "_" + msg->task_id); + task_list_model_->processTaskDescriptionMessage(*msg, base_ns_ + GET_SOLUTION_SERVICE "_" + msg->task_id); // Start listening to other topics if this is the first description // Waiting for the description ensures we do not receive data that cannot be interpreted yet if (!received_task_description_ && !msg->stages.empty()) { + auto ros_node_abstraction = context_->getRosNodeAbstraction().lock(); + if (!ros_node_abstraction) { + RCLCPP_INFO(LOGGER, "Unable to lock weak_ptr from DisplayContext in taskDescriptionCB"); + return; + } + auto node = ros_node_abstraction->get_raw_node(); received_task_description_ = true; - task_statistics_sub = update_nh_.subscribe(base_ns_ + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this); - task_solution_sub = update_nh_.subscribe(base_ns_ + SOLUTION_TOPIC, 2, &TaskDisplay::taskSolutionCB, this); + task_statistics_sub = node->create_subscription( + base_ns_ + STATISTICS_TOPIC, rclcpp::QoS(2).transient_local(), + std::bind(&TaskDisplay::taskStatisticsCB, this, std::placeholders::_1)); + task_solution_sub = node->create_subscription( + base_ns_ + SOLUTION_TOPIC, rclcpp::QoS(2).transient_local(), + std::bind(&TaskDisplay::taskSolutionCB, this, std::placeholders::_1)); } } -void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg) { - setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); +void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr msg) { + setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK"); task_list_model_->processTaskStatisticsMessage(*msg); } -void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg) { - setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); +void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr msg) { + setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK"); try { const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(*msg); if (s) @@ -224,7 +236,7 @@ void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionCon else setSolutionStatus(false); } catch (const std::invalid_argument& e) { - ROS_ERROR_STREAM(e.what()); + RCLCPP_ERROR_STREAM(LOGGER, e.what()); setSolutionStatus(false, e.what()); } } @@ -234,33 +246,41 @@ void TaskDisplay::changedTaskSolutionTopic() { if (!trajectory_visual_->getScene()) return; - task_description_sub.shutdown(); - task_statistics_sub.shutdown(); - task_solution_sub.shutdown(); + task_description_sub.reset(); + task_statistics_sub.reset(); + task_solution_sub.reset(); received_task_description_ = false; // generate task monitoring topics from solution topic const QString& solution_topic = task_solution_topic_property_->getString(); if (!solution_topic.endsWith(QString("/").append(SOLUTION_TOPIC))) { - setStatus(rviz::StatusProperty::Error, "Task Monitor", + setStatus(rviz_common::properties::StatusProperty::Error, "Task Monitor", QString("Invalid topic. Expecting a name ending on \"/%1\"").arg(SOLUTION_TOPIC)); return; } base_ns_ = solution_topic.toStdString().substr(0, solution_topic.length() - strlen(SOLUTION_TOPIC)); + auto ros_node_abstraction = context_->getRosNodeAbstraction().lock(); + if (!ros_node_abstraction) { + RCLCPP_INFO(LOGGER, "Unable to lock weak_ptr from DisplayContext in changedTaskSolutionTopic"); + return; + } // listen to task descriptions updates - task_description_sub = update_nh_.subscribe(base_ns_ + DESCRIPTION_TOPIC, 10, &TaskDisplay::taskDescriptionCB, this); + task_description_sub = + ros_node_abstraction->get_raw_node()->create_subscription( + base_ns_ + DESCRIPTION_TOPIC, rclcpp::QoS(10).transient_local(), + std::bind(&TaskDisplay::taskDescriptionCB, this, std::placeholders::_1)); - setStatus(rviz::StatusProperty::Warn, "Task Monitor", "No messages received"); + setStatus(rviz_common::properties::StatusProperty::Warn, "Task Monitor", "No messages received"); } void TaskDisplay::setSolutionStatus(bool ok, const char* msg) { if (ok) - setStatus(rviz::StatusProperty::Ok, "Solution", "Ok"); + setStatus(rviz_common::properties::StatusProperty::Ok, "Solution", "Ok"); else - setStatus(rviz::StatusProperty::Warn, "Solution", msg ? msg : "Retrieval failed"); + setStatus(rviz_common::properties::StatusProperty::Warn, "Solution", msg ? msg : "Retrieval failed"); } void TaskDisplay::onTasksInserted(const QModelIndex& parent, int first, int last) { @@ -270,7 +290,8 @@ void TaskDisplay::onTasksInserted(const QModelIndex& parent, int first, int last TaskListModel* m = static_cast(sender()); for (; first <= last; ++first) { QModelIndex idx = m->index(first, 0, parent); - tasks_property_->addChild(new rviz::Property(idx.data().toString(), idx.sibling(idx.row(), 1).data()), first); + tasks_property_->addChild( + new rviz_common::properties::Property(idx.data().toString(), idx.sibling(idx.row(), 1).data()), first); } } @@ -289,7 +310,7 @@ void TaskDisplay::onTaskDataChanged(const QModelIndex& topLeft, const QModelInde return; // only handle top-level items for (int row = topLeft.row(); row <= bottomRight.row(); ++row) { - rviz::Property* child = tasks_property_->childAt(row); + rviz_common::properties::Property* child = tasks_property_->childAt(row); assert(child); if (topLeft.column() <= 0 && 0 <= bottomRight.column()) // name changed diff --git a/visualization/motion_planning_tasks/src/task_display.h b/visualization/motion_planning_tasks/src/task_display.h index ec38dd8c..f91587c2 100644 --- a/visualization/motion_planning_tasks/src/task_display.h +++ b/visualization/motion_planning_tasks/src/task_display.h @@ -38,22 +38,26 @@ #pragma once -#include +#include +#include #include #ifndef Q_MOC_RUN #include "job_queue.h" +#include "local_task_model.h" #include -#include -#include -#include -#include +#include +#include +#include +#include #endif -namespace rviz { +namespace rviz_common { +namespace properties { class StringProperty; class RosTopicProperty; -} // namespace rviz +} // namespace properties +} // namespace rviz_common namespace moveit { namespace core { @@ -69,7 +73,7 @@ namespace moveit_rviz_plugin { MOVEIT_CLASS_FORWARD(DisplaySolution); class TaskListModel; -class TaskDisplay : public rviz::Display +class TaskDisplay : public rviz_common::Display { Q_OBJECT @@ -81,10 +85,9 @@ public: void update(float wall_dt, float ros_dt) override; void reset() override; - void save(rviz::Config config) const override; - void load(const rviz::Config& config) override; + void save(rviz_common::Config config) const override; + void load(const rviz_common::Config& config) override; - void setName(const QString& name) override; void setSolutionStatus(bool ok, const char* msg = ""); TaskListModel& getTaskListModel() { return *task_list_model_; } @@ -113,14 +116,19 @@ private Q_SLOTS: void onTasksRemoved(const QModelIndex& parent, int first, int last); void onTaskDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight); - void taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg); - void taskStatisticsCB(const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg); - void taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg); + void taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr msg); + void taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr msg); + void taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr msg); protected: - ros::Subscriber task_solution_sub; - ros::Subscriber task_description_sub; - ros::Subscriber task_statistics_sub; + /** @brief A Node which is registered with the main executor (used in the "update" thread). + * + * This is configured after the constructor within the initialize() method of Display. */ + rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node_; + + rclcpp::Subscription::SharedPtr task_solution_sub; + rclcpp::Subscription::SharedPtr task_description_sub; + rclcpp::Subscription::SharedPtr task_statistics_sub; // The trajectory playback component std::unique_ptr trajectory_visual_; @@ -138,9 +146,9 @@ protected: bool received_task_description_; // Properties - rviz::StringProperty* robot_description_property_; - rviz::RosTopicProperty* task_solution_topic_property_; - rviz::Property* tasks_property_; + rviz_common::properties::StringProperty* robot_description_property_; + rviz_common::properties::RosTopicProperty* task_solution_topic_property_; + rviz_common::properties::Property* tasks_property_; }; } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/src/task_list_model.cpp b/visualization/motion_planning_tasks/src/task_list_model.cpp index 485318ef..21a54261 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.cpp +++ b/visualization/motion_planning_tasks/src/task_list_model.cpp @@ -42,8 +42,6 @@ #include "factory_model.h" #include "icons.h" -#include - #include #include #include @@ -54,7 +52,7 @@ using namespace moveit::task_constructor; namespace moveit_rviz_plugin { -static const std::string LOGNAME("TaskListModel"); +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.task_list_model"); QVariant TaskListModel::horizontalHeader(int column, int role) { switch (role) { @@ -145,7 +143,7 @@ StageFactoryPtr getStageFactory() { factory = result; // remember for future uses return result; } catch (const std::exception& e) { - ROS_ERROR("Failed to initialize StageFactory"); + RCLCPP_ERROR(LOGGER, "Failed to initialize StageFactory"); return StageFactoryPtr(); } } @@ -164,12 +162,12 @@ void TaskListModel::onRemoveModel(QAbstractItemModel* model) { TaskListModel::TaskListModel(QObject* parent) : FlatMergeProxyModel(parent), old_task_handling_(TaskView::OLD_TASK_REPLACE) { - ROS_DEBUG_NAMED(LOGNAME, "created TaskListModel: %p", this); + RCLCPP_DEBUG(LOGGER, "created TaskListModel: %p", this); setStageFactory(getStageFactory()); } TaskListModel::~TaskListModel() { - ROS_DEBUG_NAMED(LOGNAME, "destroying TaskListModel: %p", this); + RCLCPP_DEBUG(LOGGER, "destroying TaskListModel: %p", this); // inform MetaTaskListModel that we will remove our stuff removeRows(0, rowCount()); // free RemoteTaskModels @@ -181,7 +179,7 @@ void TaskListModel::setScene(const planning_scene::PlanningSceneConstPtr& scene) scene_ = scene; } -void TaskListModel::setDisplayContext(rviz::DisplayContext* display_context) { +void TaskListModel::setDisplayContext(rviz_common::DisplayContext* display_context) { display_context_ = display_context; } @@ -241,8 +239,8 @@ QVariant TaskListModel::data(const QModelIndex& index, int role) const { // process a task description message: // update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one -void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg, - ros::NodeHandle& nh, const std::string& service_name) { +void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_msgs::msg::TaskDescription& msg, + const std::string& service_name) { // retrieve existing or insert new remote task for given task id auto it_inserted = remote_tasks_.insert(std::make_pair(msg.task_id, nullptr)); const auto& task_it = it_inserted.first; @@ -267,9 +265,9 @@ void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_ remote_task->processStageDescriptions(msg.stages); } else if (!remote_task) { // create new task model, if ID was not known before // the model is managed by this instance via Qt's parent-child mechanism - remote_task = new RemoteTaskModel(nh, service_name, scene_, display_context_, this); + remote_task = new RemoteTaskModel(service_name, scene_, display_context_, this); remote_task->processStageDescriptions(msg.stages); - ROS_DEBUG_NAMED(LOGNAME, "received new task: %s (%s)", msg.stages[0].name.c_str(), msg.task_id.c_str()); + RCLCPP_DEBUG(LOGGER, "received new task: %s (%s)", msg.stages[0].name.c_str(), msg.task_id.c_str()); // insert newly created model into this' model instance insertModel(remote_task, -1); @@ -280,10 +278,10 @@ void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_ } // process a task statistics message -void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg) { +void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_msgs::msg::TaskStatistics& msg) { auto it = remote_tasks_.find(msg.task_id); if (it == remote_tasks_.cend()) { - ROS_WARN("unknown task: %s", msg.task_id.c_str()); + RCLCPP_WARN(LOGGER, "unknown task: %s", msg.task_id.c_str()); return; } @@ -294,7 +292,7 @@ void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_m remote_task->processStageStatistics(msg.stages); } -DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg) { +DisplaySolutionPtr TaskListModel::processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg) { auto it = remote_tasks_.find(msg.task_id); if (it == remote_tasks_.cend()) return DisplaySolutionPtr(); // unkown task diff --git a/visualization/motion_planning_tasks/src/task_list_model.h b/visualization/motion_planning_tasks/src/task_list_model.h index 0386543d..b6e6d6c1 100644 --- a/visualization/motion_planning_tasks/src/task_list_model.h +++ b/visualization/motion_planning_tasks/src/task_list_model.h @@ -42,10 +42,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -53,10 +53,12 @@ #include #include -namespace rviz { +namespace rviz_common { +namespace properties { class PropertyTreeModel; +} class DisplayContext; -} // namespace rviz +} // namespace rviz_common namespace moveit_rviz_plugin { @@ -74,7 +76,7 @@ class BaseTaskModel : public QAbstractItemModel protected: unsigned int flags_ = 0; planning_scene::PlanningSceneConstPtr scene_; - rviz::DisplayContext* display_context_; + rviz_common::DisplayContext* display_context_; public: enum TaskModelFlag @@ -85,7 +87,7 @@ public: IS_RUNNING = 0x08, }; - BaseTaskModel(const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context, + BaseTaskModel(const planning_scene::PlanningSceneConstPtr& scene, rviz_common::DisplayContext* display_context, QObject* parent = nullptr) : QAbstractItemModel(parent), scene_(scene), display_context_(display_context) {} @@ -106,7 +108,7 @@ public: virtual DisplaySolutionPtr getSolution(const QModelIndex& index) = 0; /// get property model for given stage index - virtual rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) = 0; + virtual rviz_common::properties::PropertyTreeModel* getPropertyModel(const QModelIndex& index) = 0; }; /** The TaskListModel maintains a list of multiple BaseTaskModels, local and/or remote. @@ -124,7 +126,7 @@ class TaskListModel : public utils::FlatMergeProxyModel // planning scene / robot model used by all tasks in this model planning_scene::PlanningSceneConstPtr scene_; // rviz::DisplayContext used to show (interactive) markers by the property models - rviz::DisplayContext* display_context_ = nullptr; + rviz_common::DisplayContext* display_context_ = nullptr; // map from remote task IDs to (active) tasks // if task is destroyed remotely, it is marked with flag IS_DESTROYED @@ -146,7 +148,7 @@ public: ~TaskListModel() override; void setScene(const planning_scene::PlanningSceneConstPtr& scene); - void setDisplayContext(rviz::DisplayContext* display_context); + void setDisplayContext(rviz_common::DisplayContext* display_context); void setActiveTaskModel(BaseTaskModel* model) { active_task_model_ = model; } int columnCount(const QModelIndex& /*parent*/ = QModelIndex()) const override { return 4; } @@ -155,12 +157,12 @@ public: QVariant data(const QModelIndex& index, int role) const override; /// process an incoming task description message - only call in Qt's main loop - void processTaskDescriptionMessage(const moveit_task_constructor_msgs::TaskDescription& msg, ros::NodeHandle& nh, + void processTaskDescriptionMessage(const moveit_task_constructor_msgs::msg::TaskDescription& msg, const std::string& service_name); /// process an incoming task description message - only call in Qt's main loop - void processTaskStatisticsMessage(const moveit_task_constructor_msgs::TaskStatistics& msg); + void processTaskStatisticsMessage(const moveit_task_constructor_msgs::msg::TaskStatistics& msg); /// process an incoming solution message - only call in Qt's main loop - DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg); + DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg); /// insert a TaskModel, pos is relative to modelCount() bool insertModel(BaseTaskModel* model, int pos = -1); diff --git a/visualization/motion_planning_tasks/src/task_panel.cpp b/visualization/motion_planning_tasks/src/task_panel.cpp index b73e2e15..3c829f4f 100644 --- a/visualization/motion_planning_tasks/src/task_panel.cpp +++ b/visualization/motion_planning_tasks/src/task_panel.cpp @@ -48,21 +48,25 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.task_view"); + namespace moveit_rviz_plugin { -rviz::PanelDockWidget* getStageDockWidget(rviz::WindowManagerInterface* mgr) { - static QPointer widget = nullptr; +rviz_common::PanelDockWidget* getStageDockWidget(rviz_common::WindowManagerInterface* mgr) { + static QPointer widget = nullptr; if (!widget && mgr) { // create widget StageFactoryPtr factory = getStageFactory(); if (!factory) @@ -83,7 +87,7 @@ static QPointer SINGLETON; // count active TaskDisplays static uint DISPLAY_COUNT = 0; -TaskPanel::TaskPanel(QWidget* parent) : rviz::Panel(parent), d_ptr(new TaskPanelPrivate(this)) { +TaskPanel::TaskPanel(QWidget* parent) : rviz_common::Panel(parent), d_ptr(new TaskPanelPrivate(this)) { Q_D(TaskPanel); // sync checked tool button with displayed widget @@ -148,10 +152,10 @@ void TaskPanel::addSubPanel(SubPanel* w, const QString& title, const QIcon& icon * will never be called if the display is disabled... */ -void TaskPanel::request(rviz::WindowManagerInterface* window_manager) { +void TaskPanel::request(rviz_common::WindowManagerInterface* window_manager) { ++DISPLAY_COUNT; - rviz::VisualizationFrame* vis_frame = dynamic_cast(window_manager); + rviz_common::VisualizationFrame* vis_frame = dynamic_cast(window_manager); if (SINGLETON || !vis_frame) return; // already defined, nothing to do @@ -173,23 +177,23 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel* panel) : q_ptr(panel) { tool_buttons_group->setExclusive(true); button_show_stage_dock_widget->setEnabled(bool(getStageFactory())); button_show_stage_dock_widget->setToolTip(QStringLiteral("Show available stages")); - property_root = new rviz::Property("Global Settings"); + property_root = new rviz_common::properties::Property("Global Settings"); } void TaskPanel::onInitialize() { - d_ptr->window_manager_ = vis_manager_->getWindowManager(); + d_ptr->window_manager_ = getDisplayContext()->getWindowManager(); } -void TaskPanel::save(rviz::Config config) const { - rviz::Panel::save(config); +void TaskPanel::save(rviz_common::Config config) const { + rviz_common::Panel::save(config); for (int i = 0; i < d_ptr->stackedWidget->count(); ++i) { SubPanel* w = static_cast(d_ptr->stackedWidget->widget(i)); w->save(config.mapMakeChild(w->windowTitle())); } } -void TaskPanel::load(const rviz::Config& config) { - rviz::Panel::load(config); +void TaskPanel::load(const rviz_common::Config& config) { + rviz_common::Panel::load(config); for (int i = 0; i < d_ptr->stackedWidget->count(); ++i) { SubPanel* w = static_cast(d_ptr->stackedWidget->widget(i)); w->load(config.mapGetChild(w->windowTitle())); @@ -197,7 +201,7 @@ void TaskPanel::load(const rviz::Config& config) { } void TaskPanel::showStageDockWidget() { - rviz::PanelDockWidget* dock = getStageDockWidget(d_ptr->window_manager_); + rviz_common::PanelDockWidget* dock = getStageDockWidget(d_ptr->window_manager_); if (dock) dock->show(); } @@ -216,9 +220,15 @@ void setExpanded(QTreeView* view, const QModelIndex& index, bool expand, int dep view->setExpanded(index, expand); } -TaskViewPrivate::TaskViewPrivate(TaskView* view) : q_ptr(view), exec_action_client_("execute_task_solution") { +TaskViewPrivate::TaskViewPrivate(TaskView* view) : q_ptr(view) { setupUi(view); + rclcpp::NodeOptions options; + options.arguments({ "--ros-args", "-r", "__node:=task_view_private" }); + node_ = rclcpp::Node::make_shared("_", "", options); + exec_action_client_ = rclcpp_action::create_client( + node_, "execute_task_solution"); + MetaTaskListModel* meta_model = &MetaTaskListModel::instance(); StageFactoryPtr factory = getStageFactory(); if (factory) @@ -292,7 +302,7 @@ void TaskViewPrivate::lock(TaskDisplay* display) { locked_display_ = display; } -TaskView::TaskView(moveit_rviz_plugin::TaskPanel* parent, rviz::Property* root) +TaskView::TaskView(moveit_rviz_plugin::TaskPanel* parent, rviz_common::properties::Property* root) : SubPanel(parent), d_ptr(new TaskViewPrivate(this)) { Q_D(TaskView); @@ -318,23 +328,24 @@ TaskView::TaskView(moveit_rviz_plugin::TaskPanel* parent, rviz::Property* root) SIGNAL(configChanged())); // configuration settings - auto configs = new rviz::Property("Task View Settings", QVariant(), QString(), root); - initial_task_expand = - new rviz::EnumProperty("Task Expansion", "All Expanded", "Configure how to initially expand new tasks", configs); + auto configs = new rviz_common::properties::Property("Task View Settings", QVariant(), QString(), root); + initial_task_expand = new rviz_common::properties::EnumProperty( + "Task Expansion", "All Expanded", "Configure how to initially expand new tasks", configs); initial_task_expand->addOption("Top-level Expanded", EXPAND_TOP); initial_task_expand->addOption("All Expanded", EXPAND_ALL); initial_task_expand->addOption("All Closed", EXPAND_NONE); - old_task_handling = - new rviz::EnumProperty("Old task handling", "Keep", - "Configure what to do with old tasks whose solutions cannot be queried anymore", configs); + old_task_handling = new rviz_common::properties::EnumProperty( + "Old task handling", "Keep", "Configure what to do with old tasks whose solutions cannot be queried anymore", + configs); old_task_handling->addOption("Keep", OLD_TASK_KEEP); old_task_handling->addOption("Replace", OLD_TASK_REPLACE); old_task_handling->addOption("Remove", OLD_TASK_REMOVE); - connect(old_task_handling, &rviz::Property::changed, this, &TaskView::onOldTaskHandlingChanged); + connect(old_task_handling, &rviz_common::properties::Property::changed, this, &TaskView::onOldTaskHandlingChanged); - show_time_column = new rviz::BoolProperty("Show Computation Times", true, "Show the 'time' column", configs); - connect(show_time_column, &rviz::Property::changed, this, &TaskView::onShowTimeChanged); + show_time_column = + new rviz_common::properties::BoolProperty("Show Computation Times", true, "Show the 'time' column", configs); + connect(show_time_column, &rviz_common::properties::Property::changed, this, &TaskView::onShowTimeChanged); d_ptr->configureExistingModels(); } @@ -343,11 +354,11 @@ TaskView::~TaskView() { delete d_ptr; } -void TaskView::save(rviz::Config config) { +void TaskView::save(rviz_common::Config config) { auto write_splitter_sizes = [&config](QSplitter* splitter, const QString& key) { - rviz::Config group = config.mapMakeChild(key); + rviz_common::Config group = config.mapMakeChild(key); for (int s : splitter->sizes()) { - rviz::Config item = group.listAppendNew(); + rviz_common::Config item = group.listAppendNew(); item.setValue(s); } }; @@ -355,9 +366,9 @@ void TaskView::save(rviz::Config config) { write_splitter_sizes(d_ptr->tasks_solutions_splitter, "solutions_splitter"); auto write_column_sizes = [&config](QHeaderView* view, const QString& key) { - rviz::Config group = config.mapMakeChild(key); + rviz_common::Config group = config.mapMakeChild(key); for (int c = 0, end = view->count(); c != end; ++c) { - rviz::Config item = group.listAppendNew(); + rviz_common::Config item = group.listAppendNew(); item.setValue(view->sectionSize(c)); } }; @@ -365,21 +376,21 @@ void TaskView::save(rviz::Config config) { write_column_sizes(d_ptr->solutions_view->header(), "solutions_view_columns"); const QHeaderView* view = d_ptr->solutions_view->header(); - rviz::Config group = config.mapMakeChild("solution_sorting"); + rviz_common::Config group = config.mapMakeChild("solution_sorting"); group.mapSetValue("column", view->sortIndicatorSection()); group.mapSetValue("order", view->sortIndicatorOrder()); } -void TaskView::load(const rviz::Config& config) { +void TaskView::load(const rviz_common::Config& config) { if (!config.isValid()) return; auto read_sizes = [&config](const QString& key) { - rviz::Config group = config.mapGetChild(key); + rviz_common::Config group = config.mapGetChild(key); QList sizes, empty; for (int i = 0; i < group.listLength(); ++i) { - rviz::Config item = group.listChildAt(i); - if (item.getType() != rviz::Config::Value) + rviz_common::Config item = group.listChildAt(i); + if (item.getType() != rviz_common::Config::Value) return empty; QVariant value = item.getValue(); bool ok = false; @@ -401,7 +412,7 @@ void TaskView::load(const rviz::Config& config) { d_ptr->tasks_view->setColumnWidth(++column, w); QTreeView* view = d_ptr->solutions_view; - rviz::Config group = config.mapGetChild("solution_sorting"); + rviz_common::Config group = config.mapGetChild("solution_sorting"); int order = 0; if (group.mapGetInt("column", &column) && group.mapGetInt("order", &order)) view->sortByColumn(column, static_cast(order)); @@ -488,7 +499,7 @@ void TaskView::onCurrentSolutionChanged(const QModelIndex& current, const QModel solution = task->getSolution(current); display->setSolutionStatus(bool(solution)); } catch (const std::invalid_argument& e) { - ROS_ERROR_STREAM(e.what()); + RCLCPP_ERROR_STREAM(LOGGER, e.what()); display->setSolutionStatus(false, e.what()); } vis->interruptCurrentDisplay(); @@ -511,7 +522,7 @@ void TaskView::onSolutionSelectionChanged(const QItemSelection& /*selected*/, co solution = task->getSolution(index); display->setSolutionStatus(bool(solution)); } catch (const std::invalid_argument& e) { - ROS_ERROR_STREAM(e.what()); + RCLCPP_ERROR_STREAM(LOGGER, e.what()); display->setSolutionStatus(false, e.what()); } display->addMarkers(solution); @@ -528,14 +539,21 @@ void TaskView::onExecCurrentSolution() const { const DisplaySolutionPtr& solution = task->getSolution(current); - if (!d_ptr->exec_action_client_.waitForServer(ros::Duration(0.1))) { - ROS_ERROR("Failed to connect to task execution action"); + if (!d_ptr->exec_action_client_->wait_for_action_server(std::chrono::milliseconds(100))) { + RCLCPP_ERROR(LOGGER, "Failed to connect to task execution action"); return; } - moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal; + moveit_task_constructor_msgs::action::ExecuteTaskSolution::Goal goal; solution->fillMessage(goal.solution); - d_ptr->exec_action_client_.sendGoal(goal); + auto goal_handle_future = d_ptr->exec_action_client_->async_send_goal(goal); + if (rclcpp::spin_until_future_complete(d_ptr->node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(LOGGER, "send goal call failed"); + return; + } + auto goal_handle = goal_handle_future.get(); + if (!goal_handle) + RCLCPP_ERROR(LOGGER, "Goal was rejected by server"); } void TaskView::onShowTimeChanged() { @@ -550,30 +568,33 @@ void TaskView::onOldTaskHandlingChanged() { Q_EMIT oldTaskHandlingChanged(old_task_handling->getOptionInt()); } -GlobalSettingsWidgetPrivate::GlobalSettingsWidgetPrivate(GlobalSettingsWidget* widget, rviz::Property* root) +GlobalSettingsWidgetPrivate::GlobalSettingsWidgetPrivate(GlobalSettingsWidget* widget, + rviz_common::properties::Property* root) : q_ptr(widget) { setupUi(widget); - properties = new rviz::PropertyTreeModel(root, widget); + properties = new rviz_common::properties::PropertyTreeModel(root, widget); view->setModel(properties); } -GlobalSettingsWidget::GlobalSettingsWidget(moveit_rviz_plugin::TaskPanel* parent, rviz::Property* root) +GlobalSettingsWidget::GlobalSettingsWidget(moveit_rviz_plugin::TaskPanel* parent, + rviz_common::properties::Property* root) : SubPanel(parent), d_ptr(new GlobalSettingsWidgetPrivate(this, root)) { Q_D(GlobalSettingsWidget); d->view->expandAll(); - connect(d->properties, &rviz::PropertyTreeModel::configChanged, this, &GlobalSettingsWidget::configChanged); + connect(d->properties, &rviz_common::properties::PropertyTreeModel::configChanged, this, + &GlobalSettingsWidget::configChanged); } GlobalSettingsWidget::~GlobalSettingsWidget() { delete d_ptr; } -void GlobalSettingsWidget::save(rviz::Config config) { +void GlobalSettingsWidget::save(rviz_common::Config config) { d_ptr->properties->getRoot()->save(config); } -void GlobalSettingsWidget::load(const rviz::Config& config) { +void GlobalSettingsWidget::load(const rviz_common::Config& config) { d_ptr->properties->getRoot()->load(config); } } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/src/task_panel.h b/visualization/motion_planning_tasks/src/task_panel.h index f87d2d05..efd95d26 100644 --- a/visualization/motion_planning_tasks/src/task_panel.h +++ b/visualization/motion_planning_tasks/src/task_panel.h @@ -38,18 +38,21 @@ #pragma once -#include +#include #include #include class QItemSelection; class QIcon; -namespace rviz { +namespace rviz_common { class WindowManagerInterface; +class VisualizationManager; +namespace properties { class Property; class BoolProperty; class EnumProperty; -} // namespace rviz +} // namespace properties +} // namespace rviz_common namespace moveit_rviz_plugin { @@ -64,16 +67,15 @@ class SubPanel : public QWidget public: SubPanel(QWidget* parent = nullptr) : QWidget(parent) {} - virtual void save(rviz::Config /*config*/) {} // NOLINT(performance-unnecessary-value-param) - virtual void load(const rviz::Config& /*config*/) {} - + virtual void save(rviz_common::Config /*config*/) {} // NOLINT(performance-unnecessary-value-param) + virtual void load(const rviz_common::Config& /*config*/) {} Q_SIGNALS: void configChanged(); }; /** The TaskPanel is the central panel of this plugin, collecting various sub panels. */ class TaskPanelPrivate; -class TaskPanel : public rviz::Panel +class TaskPanel : public rviz_common::Panel { Q_OBJECT Q_DECLARE_PRIVATE(TaskPanel) @@ -91,12 +93,12 @@ public: * If not yet done, an instance is created. If use count drops to zero, * the global instance is destroyed. */ - static void request(rviz::WindowManagerInterface* window_manager); + static void request(rviz_common::WindowManagerInterface* window_manager); static void release(); void onInitialize() override; - void load(const rviz::Config& config) override; - void save(rviz::Config config) const override; + void load(const rviz_common::Config& config) override; + void save(rviz_common::Config config) const override; protected Q_SLOTS: void showStageDockWidget(); @@ -125,9 +127,9 @@ protected: EXPAND_NONE }; - rviz::EnumProperty* initial_task_expand; - rviz::EnumProperty* old_task_handling; - rviz::BoolProperty* show_time_column; + rviz_common::properties::EnumProperty* initial_task_expand; + rviz_common::properties::EnumProperty* old_task_handling; + rviz_common::properties::BoolProperty* show_time_column; public: enum OldTaskHandling @@ -137,11 +139,11 @@ public: OLD_TASK_REMOVE }; - TaskView(TaskPanel* parent, rviz::Property* root); + TaskView(TaskPanel* parent, rviz_common::properties::Property* root); ~TaskView() override; - void save(rviz::Config config) override; - void load(const rviz::Config& config) override; + void save(rviz_common::Config config) override; + void load(const rviz_common::Config& config) override; public Q_SLOTS: void addTask(); @@ -170,10 +172,10 @@ class GlobalSettingsWidget : public SubPanel GlobalSettingsWidgetPrivate* d_ptr; public: - GlobalSettingsWidget(TaskPanel* parent, rviz::Property* root); + GlobalSettingsWidget(TaskPanel* parent, rviz_common::properties::Property* root); ~GlobalSettingsWidget() override; - void save(rviz::Config config) override; - void load(const rviz::Config& config) override; + void save(rviz_common::Config config) override; + void load(const rviz_common::Config& config) override; }; } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/src/task_panel_p.h b/visualization/motion_planning_tasks/src/task_panel_p.h index abd9bf5e..2d9885a7 100644 --- a/visualization/motion_planning_tasks/src/task_panel_p.h +++ b/visualization/motion_planning_tasks/src/task_panel_p.h @@ -42,11 +42,11 @@ #include "ui_task_panel.h" #include "ui_task_view.h" #include "ui_global_settings.h" -#include -#include +#include +#include -#include -#include +#include +#include #include namespace moveit_rviz_plugin { @@ -62,9 +62,9 @@ public: TaskPanel* q_ptr; QButtonGroup* tool_buttons_group; - rviz::Property* property_root; + rviz_common::properties::Property* property_root; - rviz::WindowManagerInterface* window_manager_; + rviz_common::WindowManagerInterface* window_manager_; }; class TaskViewPrivate : public Ui_TaskView @@ -91,15 +91,16 @@ public: TaskView* q_ptr; QPointer locked_display_; - actionlib::SimpleActionClient exec_action_client_; + rclcpp::Node::SharedPtr node_; + rclcpp_action::Client::SharedPtr exec_action_client_; }; class GlobalSettingsWidgetPrivate : public Ui_GlobalSettingsWidget { public: - GlobalSettingsWidgetPrivate(GlobalSettingsWidget* q_ptr, rviz::Property* root); + GlobalSettingsWidgetPrivate(GlobalSettingsWidget* q_ptr, rviz_common::properties::Property* root); GlobalSettingsWidget* q_ptr; - rviz::PropertyTreeModel* properties; + rviz_common::properties::PropertyTreeModel* properties; }; } // namespace moveit_rviz_plugin diff --git a/visualization/motion_planning_tasks/src/task_view.ui b/visualization/motion_planning_tasks/src/task_view.ui index 1ce79f03..00783d4a 100644 --- a/visualization/motion_planning_tasks/src/task_view.ui +++ b/visualization/motion_planning_tasks/src/task_view.ui @@ -129,7 +129,7 @@ - + @@ -174,9 +174,9 @@
task_list_model.h
- rviz::PropertyTreeWidget + rviz_common::properties::PropertyTreeWidget QTreeView -
rviz/properties/property_tree_widget.h
+
rviz_common/properties/property_tree_widget.hpp
moveit_rviz_plugin::SolutionListView diff --git a/visualization/motion_planning_tasks/test/CMakeLists.txt b/visualization/motion_planning_tasks/test/CMakeLists.txt index a35c0373..97667690 100644 --- a/visualization/motion_planning_tasks/test/CMakeLists.txt +++ b/visualization/motion_planning_tasks/test/CMakeLists.txt @@ -3,18 +3,22 @@ ############# ## Add gtest based cpp test target and link libraries -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) +if (BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) - catkin_add_gtest(${PROJECT_NAME}-test-merge-models test_merge_models.cpp) + ament_add_gtest(${PROJECT_NAME}-test-merge-models test_merge_models.cpp) target_link_libraries(${PROJECT_NAME}-test-merge-models - motion_planning_tasks_utils gtest_main) + motion_planning_tasks_utils) - catkin_add_gmock(${PROJECT_NAME}-test-solution-models test_solution_models.cpp) + ament_add_gmock(${PROJECT_NAME}-test-solution-models test_solution_models.cpp) target_link_libraries(${PROJECT_NAME}-test-solution-models - motion_planning_tasks_rviz_plugin gtest_main) + motion_planning_tasks_rviz_plugin) - add_rostest_gtest(${PROJECT_NAME}-test-task_model test_task_model.launch test_task_model.cpp) + ament_add_gtest_executable(${PROJECT_NAME}-test-task_model test_task_model.cpp) target_link_libraries(${PROJECT_NAME}-test-task_model - motion_planning_tasks_rviz_plugin ${catkin_LIBRARIES} gtest) + motion_planning_tasks_rviz_plugin) + add_launch_test(test_task_model.launch.py + ARGS "test_binary_dir:=$") endif() diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 86909c20..53f625ad 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/visualization/motion_planning_tasks/test/test_task_model.cpp @@ -40,7 +40,7 @@ #include #include -#include +#include #include #include #include @@ -50,19 +50,18 @@ using namespace moveit::task_constructor; class TaskListModelTest : public ::testing::Test { protected: - ros::NodeHandle nh; moveit_rviz_plugin::TaskListModel model; int children = 0; int num_inserts = 0; int num_updates = 0; - moveit_task_constructor_msgs::TaskDescription genMsg(const std::string& name, - const std::string& task_id = std::string()) { - moveit_task_constructor_msgs::TaskDescription t; + moveit_task_constructor_msgs::msg::TaskDescription genMsg(const std::string& name, + const std::string& task_id = std::string()) { + moveit_task_constructor_msgs::msg::TaskDescription t; uint id = 0, root_id; t.task_id = task_id.empty() ? name : task_id; - moveit_task_constructor_msgs::StageDescription desc; + moveit_task_constructor_msgs::msg::StageDescription desc; desc.parent_id = id; desc.id = root_id = ++id; desc.name = name; @@ -129,7 +128,7 @@ protected: SCOPED_TRACE("first i=" + std::to_string(i)); num_inserts = 0; num_updates = 0; - model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); + model.processTaskDescriptionMessage(genMsg("first"), "get_solution"); if (i == 0) EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task @@ -143,7 +142,7 @@ protected: SCOPED_TRACE("second i=" + std::to_string(i)); num_inserts = 0; num_updates = 0; - model.processTaskDescriptionMessage(genMsg("second"), nh, "get_solution"); // 1 notify for inserted task + model.processTaskDescriptionMessage(genMsg("second"), "get_solution"); // 1 notify for inserted task if (i == 0) EXPECT_EQ(num_inserts, 1); @@ -165,17 +164,13 @@ protected: TEST_F(TaskListModelTest, remoteTaskModel) { children = 3; planning_scene::PlanningSceneConstPtr scene; - moveit_rviz_plugin::RemoteTaskModel m(nh, "get_solution", scene, nullptr); + moveit_rviz_plugin::RemoteTaskModel m("get_solution", scene, nullptr); m.processStageDescriptions(genMsg("first").stages); SCOPED_TRACE("first"); validate(m, { "first" }); } TEST_F(TaskListModelTest, localTaskModel) { - int argc = 0; - char* argv = nullptr; - ros::init(argc, &argv, "testLocalTaskModel"); - children = 3; const char* task_name = "task pipeline"; moveit_rviz_plugin::LocalTaskModel m(std::make_unique(task_name), @@ -187,6 +182,9 @@ TEST_F(TaskListModelTest, localTaskModel) { SCOPED_TRACE("localTaskModel"); validate(m, { task_name }); } + // There's a bug where cancelling the executor in IntrospectionPrivate is happening before the call to spin causing + // the it to stuck in the destructor when calling executor_thread_.join() + rclcpp::sleep_for(std::chrono::milliseconds(100)); } TEST_F(TaskListModelTest, noChildren) { @@ -202,13 +200,13 @@ TEST_F(TaskListModelTest, threeChildren) { TEST_F(TaskListModelTest, visitedPopulate) { // first population without children children = 0; - model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); + model.processTaskDescriptionMessage(genMsg("first"), "get_solution"); validate(model, { "first" }); // validation visits root node EXPECT_EQ(num_inserts, 1); children = 3; num_inserts = 0; - model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); + model.processTaskDescriptionMessage(genMsg("first"), "get_solution"); validate(model, { "first" }); // second population with children should emit insert notifies for them EXPECT_EQ(num_inserts, 3); @@ -217,7 +215,7 @@ TEST_F(TaskListModelTest, visitedPopulate) { TEST_F(TaskListModelTest, deletion) { children = 3; - model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); + model.processTaskDescriptionMessage(genMsg("first"), "get_solution"); auto m = model.getModel(model.index(0, 0)).first; int num_deletes = 0; QObject::connect(m, &QObject::destroyed, [&num_deletes]() { ++num_deletes; }); @@ -232,6 +230,6 @@ TEST_F(TaskListModelTest, deletion) { int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_task_model"); + rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } diff --git a/visualization/motion_planning_tasks/test/test_task_model.launch b/visualization/motion_planning_tasks/test/test_task_model.launch deleted file mode 100644 index 3aebf298..00000000 --- a/visualization/motion_planning_tasks/test/test_task_model.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - diff --git a/visualization/motion_planning_tasks/test/test_task_model.launch.py b/visualization/motion_planning_tasks/test/test_task_model.launch.py new file mode 100644 index 00000000..40cf5cbc --- /dev/null +++ b/visualization/motion_planning_tasks/test/test_task_model.launch.py @@ -0,0 +1,52 @@ +import unittest + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +import launch_testing +from launch_testing.asserts import assertExitCodes +from launch_testing.util import KeepAliveProc +from launch_testing.actions import ReadyToTest, GTest + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + test_task_model = GTest( + path=[ + PathJoinSubstitution( + [ + LaunchConfiguration("test_binary_dir"), + "moveit_task_constructor_visualization-test-task_model", + ] + ) + ], + output="screen", + ) + return ( + LaunchDescription( + [ + DeclareLaunchArgument( + name="test_binary_dir", + description="Binary directory of package containing test executables", + ), + test_task_model, + KeepAliveProc(), + ReadyToTest(), + ] + ), + {"test_task_model": test_task_model}, + ) + + +class TestTerminatingProcessStops(unittest.TestCase): + def test_gtest_run_complete(self, proc_info, test_task_model): + proc_info.assertWaitForShutdown(process=test_task_model, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TaskModelTestAfterShutdown(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that all processes in the launch exit with code 0 + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/visualization/motion_planning_tasks/utils/CMakeLists.txt b/visualization/motion_planning_tasks/utils/CMakeLists.txt index cab82cae..436145dc 100644 --- a/visualization/motion_planning_tasks/utils/CMakeLists.txt +++ b/visualization/motion_planning_tasks/utils/CMakeLists.txt @@ -12,9 +12,8 @@ target_link_libraries(${MOVEIT_LIB_NAME} ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC $ - PRIVATE ${catkin_INCLUDE_DIRS} ) install(TARGETS ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) diff --git a/visualization/motion_planning_tasks_rviz_plugin_description.xml b/visualization/motion_planning_tasks_rviz_plugin_description.xml index 0dd6e4c2..fd8f3f78 100644 --- a/visualization/motion_planning_tasks_rviz_plugin_description.xml +++ b/visualization/motion_planning_tasks_rviz_plugin_description.xml @@ -1,14 +1,14 @@ - + + base_class_type="rviz_common::Panel"> A panel widget to monitor and edit motion planning tasks + base_class_type="rviz_common::Display"> Monitor motion planning tasks and animate their solution trajectories diff --git a/visualization/package.xml b/visualization/package.xml index 196503bf..08838900 100644 --- a/visualization/package.xml +++ b/visualization/package.xml @@ -7,21 +7,24 @@ Robert Haschke Michael Goerner - catkin + ament_cmake qtbase5-dev moveit_core moveit_task_constructor_msgs moveit_task_constructor_core moveit_ros_visualization - roscpp - rviz + rclcpp + rviz2 - rosunit - rostest - moveit_resources_fanuc_moveit_config + ament_cmake_gmock + ament_cmake_gtest + launch + launch_testing + launch_testing_ament_cmake + launch_testing_ros - - + ament_cmake + diff --git a/visualization/visualization_tools/CMakeLists.txt b/visualization/visualization_tools/CMakeLists.txt index 49116a1e..8c35ec20 100644 --- a/visualization/visualization_tools/CMakeLists.txt +++ b/visualization/visualization_tools/CMakeLists.txt @@ -2,11 +2,6 @@ set(MOVEIT_LIB_NAME moveit_task_visualization_tools) set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/visualization_tools) -# TODO: Remove when Kinetic support is dropped -if(rviz_VERSION VERSION_LESS 1.13.1) # Does rviz supports TF2? - add_definitions(-DRVIZ_TF1) -endif() - set(HEADERS ${PROJECT_INCLUDE}/display_solution.h ${PROJECT_INCLUDE}/marker_visualization.h @@ -14,7 +9,7 @@ set(HEADERS ${PROJECT_INCLUDE}/task_solution_visualization.h ) -add_library(${MOVEIT_LIB_NAME} +add_library(${MOVEIT_LIB_NAME} SHARED ${HEADERS} src/display_solution.cpp @@ -23,25 +18,27 @@ add_library(${MOVEIT_LIB_NAME} src/task_solution_visualization.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - target_link_libraries(${MOVEIT_LIB_NAME} - ${catkin_LIBRARIES} - ${rviz_DEFAULT_PLUGIN_LIBRARIES} - ${OGRE_LIBRARIES} ${QT_LIBRARIES} - ${Boost_LIBRARIES} + rviz_ogre_vendor::OgreMain ) target_include_directories(${MOVEIT_LIB_NAME} PUBLIC include - PRIVATE ${catkin_INCLUDE_DIRS} ) -target_include_directories(${MOVEIT_LIB_NAME} SYSTEM - PUBLIC ${rviz_OGRE_INCLUDE_DIRS} +ament_target_dependencies(${MOVEIT_LIB_NAME} + Boost + pluginlib + moveit_task_constructor_msgs + moveit_ros_visualization + moveit_core + rclcpp + rviz_common + rviz_default_plugins ) -add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) +install(DIRECTORY include/ DESTINATION include) install(TARGETS ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + EXPORT export_${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h index 408b3022..fae2d866 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h @@ -36,7 +36,7 @@ #pragma once -#include +#include #include namespace moveit { @@ -53,7 +53,7 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory); namespace Ogre { class SceneNode; } -namespace rviz { +namespace rviz_common { class DisplayContext; } @@ -125,7 +125,7 @@ public: const MarkerVisualizationPtr markersOfSubTrajectory(size_t index) const { return data_.at(index).markers_; } void setFromMessage(const planning_scene::PlanningScenePtr& start_scene, - const moveit_task_constructor_msgs::Solution& msg); - void fillMessage(moveit_task_constructor_msgs::Solution& msg) const; + const moveit_task_constructor_msgs::msg::Solution& msg); + void fillMessage(moveit_task_constructor_msgs::msg::Solution& msg) const; }; } // namespace moveit_rviz_plugin diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h index fe743bf0..1db0d028 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/marker_visualization.h @@ -1,8 +1,8 @@ #pragma once -#include +#include #include -#include +#include #include #include #include @@ -11,10 +11,17 @@ namespace Ogre { class SceneNode; } -namespace rviz { +namespace rviz_common { class DisplayContext; +} // namespace rviz_common + +namespace rviz_default_plugins { +namespace displays { +namespace markers { class MarkerBase; -} // namespace rviz +} +} // namespace displays +} // namespace rviz_default_plugins namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene); @@ -41,10 +48,10 @@ class MarkerVisualization // list of all markers, attached to scene nodes in namespaces_ struct MarkerData { - visualization_msgs::MarkerPtr msg_; - std::shared_ptr marker_; + visualization_msgs::msg::Marker::SharedPtr msg_; + std::shared_ptr marker_; - MarkerData(const visualization_msgs::Marker& marker); + MarkerData(const visualization_msgs::msg::Marker& marker); }; struct NamespaceData { @@ -64,14 +71,14 @@ class MarkerVisualization bool markers_created_ = false; public: - MarkerVisualization(const std::vector& markers, + MarkerVisualization(const std::vector& markers, const planning_scene::PlanningScene& end_scene); ~MarkerVisualization(); /// did we successfully created all markers (and scene nodes)? bool created() const { return markers_created_; } /// create markers (placed at planning frame of scene) - bool createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* scene_node); + bool createMarkers(rviz_common::DisplayContext* context, Ogre::SceneNode* scene_node); /// update marker position/orientation based on frames of given scene + robot_state void update(const planning_scene::PlanningScene& end_scene, const moveit::core::RobotState& robot_state); @@ -88,22 +95,22 @@ private: * The class remembers which MarkerVisualization instances are currently hosted * and provides the user interaction to toggle marker visibility by namespace. */ -class MarkerVisualizationProperty : public rviz::BoolProperty +class MarkerVisualizationProperty : public rviz_common::properties::BoolProperty { Q_OBJECT - rviz::DisplayContext* context_ = nullptr; + rviz_common::DisplayContext* context_ = nullptr; Ogre::SceneNode* parent_scene_node_ = nullptr; // scene node provided externally Ogre::SceneNode* marker_scene_node_ = nullptr; // scene node all markers are attached to - std::map namespaces_; // rviz properties for encountered namespaces + std::map namespaces_; // rviz properties for encountered namespaces std::list hosted_markers_; // list of hosted MarkerVisualization instances - rviz::BoolProperty* all_markers_at_once_; + rviz_common::properties::BoolProperty* all_markers_at_once_; public: MarkerVisualizationProperty(const QString& name, Property* parent = nullptr); ~MarkerVisualizationProperty() override; - void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context); + void onInitialize(Ogre::SceneNode* scene_node, rviz_common::DisplayContext* context); /// remove all hosted markers from display void clearMarkers(); diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h index e74aeb82..4e6e31ee 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_panel.h @@ -37,17 +37,17 @@ #pragma once #ifndef Q_MOC_RUN -#include +#include #endif -#include +#include #include #include #include namespace moveit_rviz_plugin { -class TaskSolutionPanel : public rviz::Panel +class TaskSolutionPanel : public rviz_common::Panel { Q_OBJECT diff --git a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h index d78dab5f..101903ce 100644 --- a/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h +++ b/visualization/visualization_tools/include/moveit/visualization_tools/task_solution_visualization.h @@ -37,7 +37,7 @@ #pragma once #include -#include +#include #include #include @@ -47,10 +47,13 @@ namespace Ogre { class SceneNode; } -namespace rviz { -class Display; -class DisplayContext; +namespace rviz_default_plugins { +namespace robot { class Robot; +} +} // namespace rviz_default_plugins +namespace rviz_common { +namespace properties { class Property; class IntProperty; class StringProperty; @@ -60,8 +63,11 @@ class RosTopicProperty; class EnumProperty; class EditableEnumProperty; class ColorProperty; +} // namespace properties +class Display; +class DisplayContext; class PanelDockWidget; -} // namespace rviz +} // namespace rviz_common namespace moveit { namespace core { @@ -91,24 +97,24 @@ class TaskSolutionVisualization : public QObject public: /** * \brief Playback a trajectory from a planned path - * \param parent - either a rviz::Display or rviz::Property + * \param parent - either a rviz::Display _commonorproperties:: rviz::Property * \param display - the rviz::Display from the parent * \return true on success */ - TaskSolutionVisualization(rviz::Property* parent, rviz::Display* display); + TaskSolutionVisualization(rviz_common::properties::Property* parent, rviz_common::Display* display); ~TaskSolutionVisualization() override; virtual void update(float wall_dt, float ros_dt); virtual void reset(); - void onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context); + void onInitialize(Ogre::SceneNode* scene_node, rviz_common::DisplayContext* context); void onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model); void onEnable(); void onDisable(); void setName(const QString& name); planning_scene::PlanningSceneConstPtr getScene() const { return scene_; } - void showTrajectory(const moveit_task_constructor_msgs::Solution& msg); + void showTrajectory(const moveit_task_constructor_msgs::msg::Solution& msg); void showTrajectory(const moveit_rviz_plugin::DisplaySolutionPtr& s, bool lock); void unlock(); @@ -156,12 +162,12 @@ protected: MarkerVisualizationProperty* marker_visual_; // Handle colouring of robot - void setRobotColor(rviz::Robot* robot, const QColor& color); - void unsetRobotColor(rviz::Robot* robot); + void setRobotColor(rviz_default_plugins::robot::Robot* robot, const QColor& color); + void unsetRobotColor(rviz_default_plugins::robot::Robot* robot); DisplaySolutionPtr displaying_solution_; DisplaySolutionPtr next_solution_to_display_; - std::vector trail_; + std::vector trail_; bool animating_ = false; // auto-progressing the current waypoint? bool drop_displaying_solution_ = false; bool locked_ = false; @@ -172,36 +178,36 @@ protected: planning_scene::PlanningScenePtr scene_; // Pointers from parent display that we save - rviz::Display* display_; // the parent display that this class populates + rviz_common::Display* display_; // the parent display that this class populates Ogre::SceneNode* parent_scene_node_; // parent scene node provided by display Ogre::SceneNode* main_scene_node_; // to be added/removed to/from scene_node_ Ogre::SceneNode* trail_scene_node_; // to be added/removed to/from scene_node_ - rviz::DisplayContext* context_; + rviz_common::DisplayContext* context_; TaskSolutionPanel* slider_panel_ = nullptr; - rviz::PanelDockWidget* slider_dock_panel_ = nullptr; + rviz_common::PanelDockWidget* slider_dock_panel_ = nullptr; bool slider_panel_was_visible_ = false; // Trajectory Properties - rviz::Property* robot_property_; - rviz::BoolProperty* robot_visual_enabled_property_; - rviz::BoolProperty* robot_collision_enabled_property_; - rviz::FloatProperty* robot_alpha_property_; - rviz::ColorProperty* robot_color_property_; - rviz::BoolProperty* enable_robot_color_property_; + rviz_common::properties::Property* robot_property_; + rviz_common::properties::BoolProperty* robot_visual_enabled_property_; + rviz_common::properties::BoolProperty* robot_collision_enabled_property_; + rviz_common::properties::FloatProperty* robot_alpha_property_; + rviz_common::properties::ColorProperty* robot_color_property_; + rviz_common::properties::BoolProperty* enable_robot_color_property_; - rviz::EditableEnumProperty* state_display_time_property_; - rviz::BoolProperty* loop_display_property_; - rviz::BoolProperty* trail_display_property_; - rviz::BoolProperty* interrupt_display_property_; - rviz::IntProperty* trail_step_size_property_; + rviz_common::properties::EditableEnumProperty* state_display_time_property_; + rviz_common::properties::BoolProperty* loop_display_property_; + rviz_common::properties::BoolProperty* trail_display_property_; + rviz_common::properties::BoolProperty* interrupt_display_property_; + rviz_common::properties::IntProperty* trail_step_size_property_; // PlanningScene Properties - rviz::BoolProperty* scene_enabled_property_; - rviz::FloatProperty* scene_alpha_property_; - rviz::ColorProperty* scene_color_property_; - rviz::ColorProperty* attached_body_color_property_; - rviz::EnumProperty* octree_render_property_; - rviz::EnumProperty* octree_coloring_property_; + rviz_common::properties::BoolProperty* scene_enabled_property_; + rviz_common::properties::FloatProperty* scene_alpha_property_; + rviz_common::properties::ColorProperty* scene_color_property_; + rviz_common::properties::ColorProperty* attached_body_color_property_; + rviz_common::properties::EnumProperty* octree_render_property_; + rviz_common::properties::EnumProperty* octree_coloring_property_; }; } // namespace moveit_rviz_plugin diff --git a/visualization/visualization_tools/src/display_solution.cpp b/visualization/visualization_tools/src/display_solution.cpp index 3d5dee7a..ce3ac3bf 100644 --- a/visualization/visualization_tools/src/display_solution.cpp +++ b/visualization/visualization_tools/src/display_solution.cpp @@ -38,8 +38,10 @@ #include #include #include -#include #include +#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.display_solution"); namespace moveit_rviz_plugin { @@ -65,7 +67,7 @@ float DisplaySolution::getWayPointDurationFromPrevious(const IndexPair& idx_pair return data_[idx_pair.first].trajectory_->getWayPointDurationFromPrevious(idx_pair.second); } -const robot_state::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair& idx_pair) const { +const moveit::core::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair& idx_pair) const { return data_[idx_pair.first].trajectory_->getWayPointPtr(idx_pair.second); } @@ -87,7 +89,7 @@ const MarkerVisualizationPtr DisplaySolution::markers(const DisplaySolution::Ind } void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& start_scene, - const moveit_task_constructor_msgs::Solution& msg) { + const moveit_task_constructor_msgs::msg::Solution& msg) { if (msg.start_scene.robot_model_name != start_scene->getRobotModel()->getName()) { static boost::format fmt("Solution for model '%s' but model '%s' was expected"); fmt % msg.start_scene.robot_model_name.c_str() % start_scene->getRobotModel()->getName().c_str(); @@ -127,7 +129,7 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& sta } } -void DisplaySolution::fillMessage(moveit_task_constructor_msgs::Solution& msg) const { +void DisplaySolution::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg) const { start_scene_->getPlanningSceneMsg(msg.start_scene); msg.sub_trajectory.resize(data_.size()); auto traj_it = msg.sub_trajectory.begin(); diff --git a/visualization/visualization_tools/src/marker_visualization.cpp b/visualization/visualization_tools/src/marker_visualization.cpp index b3671772..e7a88cfb 100644 --- a/visualization/visualization_tools/src/marker_visualization.cpp +++ b/visualization/visualization_tools/src/marker_visualization.cpp @@ -1,36 +1,41 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include -#ifndef RVIZ_TF1 -#include +#include +#include +#if __has_include() +#include +#else +#include #endif -#include -#include -#include + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.marker_visualization"); namespace moveit_rviz_plugin { // create MarkerData with nil marker_ pointer, just with a copy of message -MarkerVisualization::MarkerData::MarkerData(const visualization_msgs::Marker& marker) { - msg_.reset(new visualization_msgs::Marker(marker)); - msg_->header.stamp = ros::Time(); +MarkerVisualization::MarkerData::MarkerData(const visualization_msgs::msg::Marker& marker) { + msg_.reset(new visualization_msgs::msg::Marker(marker)); + msg_->header.stamp = rclcpp::Time(RCL_ROS_TIME); msg_->frame_locked = false; } -MarkerVisualization::MarkerVisualization(const std::vector& markers, +MarkerVisualization::MarkerVisualization(const std::vector& markers, const planning_scene::PlanningScene& end_scene) { planning_frame_ = end_scene.getPlanningFrame(); // remember marker message, postpone rviz::MarkerBase creation until later for (const auto& marker : markers) { if (!end_scene.knowsFrameTransform(marker.header.frame_id)) { - ROS_WARN_ONCE("unknown frame '%s' for solution marker in namespace '%s'", marker.header.frame_id.c_str(), - marker.ns.c_str()); + RCLCPP_WARN_ONCE(LOGGER, "unknown frame '%s' for solution marker in namespace '%s'", + marker.header.frame_id.c_str(), marker.ns.c_str()); continue; // ignore markers with unknown frame } @@ -62,7 +67,7 @@ void MarkerVisualization::setVisible(const QString& ns, Ogre::SceneNode* parent_ setVisibility(it->second.ns_node_, parent_scene_node, visible); } -bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::SceneNode* parent_scene_node) { +bool MarkerVisualization::createMarkers(rviz_common::DisplayContext* context, Ogre::SceneNode* parent_scene_node) { if (markers_created_) return true; // already called before @@ -72,25 +77,18 @@ bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::Sce Ogre::Vector3 pos; try { -#ifdef RVIZ_TF1 - tf::TransformListener* tf = context->getFrameManager()->getTFClient(); - tf::StampedTransform tm; - tf->lookupTransform(planning_frame_, fixed_frame, ros::Time(), tm); - auto q = tm.getRotation(); - auto p = tm.getOrigin(); - quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z()); - pos = Ogre::Vector3(p.x(), p.y(), p.z()); -#else - std::shared_ptr tf = context->getFrameManager()->getTF2BufferPtr(); - geometry_msgs::TransformStamped tm; - tm = tf->lookupTransform(planning_frame_, fixed_frame, ros::Time()); - auto q = tm.transform.rotation; - auto p = tm.transform.translation; - quat = Ogre::Quaternion(q.w, -q.x, -q.y, -q.z); - pos = Ogre::Vector3(p.x, p.y, p.z); -#endif + auto tf_wrapper = std::dynamic_pointer_cast( + context->getFrameManager()->getConnector().lock()); + if (tf_wrapper) { + geometry_msgs::msg::TransformStamped tm; + tm = tf_wrapper->lookupTransform(planning_frame_, fixed_frame, tf2::TimePointZero); + auto q = tm.transform.rotation; + auto p = tm.transform.translation; + quat = Ogre::Quaternion(q.w, -q.x, -q.y, -q.z); + pos = Ogre::Vector3(p.x, p.y, p.z); + } } catch (const tf2::TransformException& e) { - ROS_WARN_STREAM_NAMED("MarkerVisualization", e.what()); + RCLCPP_WARN_STREAM(LOGGER, e.what()); return false; } @@ -110,7 +108,9 @@ bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::Sce frame_it->second = node->createChildSceneNode(); node = frame_it->second; - data.marker_.reset(rviz::createMarker(data.msg_->type, nullptr, context, node)); + rviz_default_plugins::displays::markers::MarkerFactory marker_factory; + marker_factory.initialize(nullptr, context, node); + data.marker_ = marker_factory.createMarkerForType(data.msg_->type); if (!data.marker_) continue; // failed to create marker @@ -136,7 +136,7 @@ void MarkerVisualization::update(MarkerData& data, const planning_scene::Plannin const moveit::core::RobotState& robot_state) const { Q_ASSERT(scene.getPlanningFrame() == planning_frame_); - const visualization_msgs::Marker& marker = *data.msg_; + const visualization_msgs::msg::Marker& marker = *data.msg_; if (marker.header.frame_id == scene.getPlanningFrame()) return; // no need to transform nodes placed at planning frame @@ -147,8 +147,8 @@ void MarkerVisualization::update(MarkerData& data, const planning_scene::Plannin else if (scene.knowsFrameTransform(marker.header.frame_id)) pose = scene.getFrameTransform(marker.header.frame_id); else { - ROS_WARN_ONCE_NAMED("MarkerVisualization", "unknown frame '%s' for solution marker in namespace '%s'", - marker.header.frame_id.c_str(), marker.ns.c_str()); + RCLCPP_WARN_ONCE(LOGGER, "unknown frame '%s' for solution marker in namespace '%s'", + marker.header.frame_id.c_str(), marker.ns.c_str()); return; // ignore markers with unknown frame } @@ -169,11 +169,11 @@ void MarkerVisualization::update(const planning_scene::PlanningScene& end_scene, update(data, end_scene, robot_state); } -MarkerVisualizationProperty::MarkerVisualizationProperty(const QString& name, rviz::Property* parent) - : rviz::BoolProperty(name, true, "Enable/disable markers", parent) { - all_markers_at_once_ = - new rviz::BoolProperty("All at once?", false, "Show all markers of multiple subsolutions at once?", this, - SLOT(onAllAtOnceChanged()), this); +MarkerVisualizationProperty::MarkerVisualizationProperty(const QString& name, rviz_common::properties::Property* parent) + : rviz_common::properties::BoolProperty(name, true, "Enable/disable markers", parent) { + all_markers_at_once_ = new rviz_common::properties::BoolProperty( + "All at once?", false, "Show all markers of multiple subsolutions at once?", this, SLOT(onAllAtOnceChanged()), + this); connect(this, SIGNAL(changed()), this, SLOT(onEnableChanged())); } @@ -183,7 +183,7 @@ MarkerVisualizationProperty::~MarkerVisualizationProperty() { marker_scene_node_->getCreator()->destroySceneNode(marker_scene_node_); } -void MarkerVisualizationProperty::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context) { +void MarkerVisualizationProperty::onInitialize(Ogre::SceneNode* scene_node, rviz_common::DisplayContext* context) { context_ = context; parent_scene_node_ = scene_node; marker_scene_node_ = parent_scene_node_->createChildSceneNode(); @@ -212,8 +212,8 @@ void MarkerVisualizationProperty::addMarkers(const MarkerVisualizationPtr& marke // create sub property for newly encountered namespace, enabling visibility by default auto ns_it = namespaces_.insert(std::make_pair(ns, nullptr)).first; if (ns_it->second == nullptr) { - ns_it->second = new rviz::BoolProperty(ns, true, "Show/hide markers of this namespace", this, - SLOT(onNSEnableChanged()), this); + ns_it->second = new rviz_common::properties::BoolProperty(ns, true, "Show/hide markers of this namespace", + this, SLOT(onNSEnableChanged()), this); } Q_ASSERT(pair.second.ns_node_); // nodes should have been created in createMarkers() @@ -241,7 +241,7 @@ void MarkerVisualizationProperty::onEnableChanged() { } void MarkerVisualizationProperty::onNSEnableChanged() { - rviz::BoolProperty* ns_property = static_cast(sender()); + rviz_common::properties::BoolProperty* ns_property = static_cast(sender()); const QString& ns = ns_property->getName(); bool visible = ns_property->getBool(); // for all hosted markers, set visibility of given namespace diff --git a/visualization/visualization_tools/src/task_solution_visualization.cpp b/visualization/visualization_tools/src/task_solution_visualization.cpp index 1c666a52..d5af657a 100644 --- a/visualization/visualization_tools/src/task_solution_visualization.cpp +++ b/visualization/visualization_tools/src/task_solution_visualization.cpp @@ -47,111 +47,122 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include +#include + +static const rclcpp::Logger LOGGER = + rclcpp::get_logger("moveit_task_constructor_visualization.task_solution_visualization"); namespace moveit_rviz_plugin { -TaskSolutionVisualization::TaskSolutionVisualization(rviz::Property* parent, rviz::Display* display) +TaskSolutionVisualization::TaskSolutionVisualization(rviz_common::properties::Property* parent, + rviz_common::Display* display) : display_(display) { // trajectory properties - interrupt_display_property_ = new rviz::BoolProperty("Interrupt Display", false, - "Immediately show newly planned trajectory, " - "interrupting the currently displayed one.", - parent); + interrupt_display_property_ = new rviz_common::properties::BoolProperty("Interrupt Display", false, + "Immediately show newly planned trajectory, " + "interrupting the currently displayed one.", + parent); - loop_display_property_ = new rviz::BoolProperty( + loop_display_property_ = new rviz_common::properties::BoolProperty( "Loop Animation", false, "Indicates whether the last received path is to be animated in a loop", parent, SLOT(changedLoopDisplay()), this); - trail_display_property_ = - new rviz::BoolProperty("Show Trail", false, "Show a path trail", parent, SLOT(changedTrail()), this); + trail_display_property_ = new rviz_common::properties::BoolProperty("Show Trail", false, "Show a path trail", parent, + SLOT(changedTrail()), this); state_display_time_property_ = - new rviz::EditableEnumProperty("State Display Time", "0.05 s", - "The amount of wall-time to wait in between displaying " - "states along a received trajectory path", - parent); + new rviz_common::properties::EditableEnumProperty("State Display Time", "0.05 s", + "The amount of wall-time to wait in between displaying " + "states along a received trajectory path", + parent); state_display_time_property_->addOptionStd("REALTIME"); state_display_time_property_->addOptionStd("0.05 s"); state_display_time_property_->addOptionStd("0.1 s"); state_display_time_property_->addOptionStd("0.5 s"); - trail_step_size_property_ = new rviz::IntProperty( + trail_step_size_property_ = new rviz_common::properties::IntProperty( "Trail Step Size", 1, "Specifies the step size of the samples shown in the trajectory trail.", parent, SLOT(changedTrail()), this); trail_step_size_property_->setMin(1); // robot properties - robot_property_ = new rviz::Property("Robot", QString(), QString(), parent); - robot_visual_enabled_property_ = new rviz::BoolProperty("Show Robot Visual", true, - "Indicates whether the geometry of the robot as defined for " - "visualisation purposes should be displayed", - robot_property_, SLOT(changedRobotVisualEnabled()), this); + robot_property_ = new rviz_common::properties::Property("Robot", QString(), QString(), parent); + robot_visual_enabled_property_ = + new rviz_common::properties::BoolProperty("Show Robot Visual", true, + "Indicates whether the geometry of the robot as defined for " + "visualisation purposes should be displayed", + robot_property_, SLOT(changedRobotVisualEnabled()), this); robot_collision_enabled_property_ = - new rviz::BoolProperty("Show Robot Collision", false, - "Indicates whether the geometry of the robot as defined " - "for collision detection purposes should be displayed", - robot_property_, SLOT(changedRobotCollisionEnabled()), this); + new rviz_common::properties::BoolProperty("Show Robot Collision", false, + "Indicates whether the geometry of the robot as defined " + "for collision detection purposes should be displayed", + robot_property_, SLOT(changedRobotCollisionEnabled()), this); - robot_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links", - robot_property_, SLOT(changedRobotAlpha()), this); + robot_alpha_property_ = + new rviz_common::properties::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links", + robot_property_, SLOT(changedRobotAlpha()), this); robot_alpha_property_->setMin(0.0); robot_alpha_property_->setMax(1.0); - robot_color_property_ = - new rviz::ColorProperty("Fixed Robot Color", QColor(150, 50, 150), "The color of the animated robot", - robot_property_, SLOT(changedRobotColor()), this); + robot_color_property_ = new rviz_common::properties::ColorProperty("Fixed Robot Color", QColor(150, 50, 150), + "The color of the animated robot", + robot_property_, SLOT(changedRobotColor()), this); - enable_robot_color_property_ = new rviz::BoolProperty("Use Fixed Robot Color", false, - "Specifies whether the fixed robot color should be used." - " If not, the original color is used.", - robot_property_, SLOT(enabledRobotColor()), this); + enable_robot_color_property_ = + new rviz_common::properties::BoolProperty("Use Fixed Robot Color", false, + "Specifies whether the fixed robot color should be used." + " If not, the original color is used.", + robot_property_, SLOT(enabledRobotColor()), this); // planning scene properties - scene_enabled_property_ = - new rviz::BoolProperty("Scene", true, "Show Planning Scene", parent, SLOT(changedSceneEnabled()), this); + scene_enabled_property_ = new rviz_common::properties::BoolProperty("Scene", true, "Show Planning Scene", parent, + SLOT(changedSceneEnabled()), this); - scene_alpha_property_ = new rviz::FloatProperty("Scene Alpha", 0.9f, "Specifies the alpha for the scene geometry", - scene_enabled_property_, SLOT(renderCurrentScene()), this); + scene_alpha_property_ = + new rviz_common::properties::FloatProperty("Scene Alpha", 0.9f, "Specifies the alpha for the scene geometry", + scene_enabled_property_, SLOT(renderCurrentScene()), this); scene_alpha_property_->setMin(0.0); scene_alpha_property_->setMax(1.0); - scene_color_property_ = new rviz::ColorProperty( + scene_color_property_ = new rviz_common::properties::ColorProperty( "Scene Color", QColor(50, 230, 50), "The color for the planning scene obstacles (if a color is not defined)", scene_enabled_property_, SLOT(renderCurrentScene()), this); - attached_body_color_property_ = - new rviz::ColorProperty("Attached Body Color", QColor(150, 50, 150), "The color for the attached bodies", - scene_enabled_property_, SLOT(changedAttachedBodyColor()), this); + attached_body_color_property_ = new rviz_common::properties::ColorProperty( + "Attached Body Color", QColor(150, 50, 150), "The color for the attached bodies", scene_enabled_property_, + SLOT(changedAttachedBodyColor()), this); - octree_render_property_ = new rviz::EnumProperty("Voxel Rendering", "Occupied Voxels", "Select voxel type.", - scene_enabled_property_, SLOT(renderCurrentScene()), this); + octree_render_property_ = + new rviz_common::properties::EnumProperty("Voxel Rendering", "Occupied Voxels", "Select voxel type.", + scene_enabled_property_, SLOT(renderCurrentScene()), this); octree_render_property_->addOption("Occupied Voxels", OCTOMAP_OCCUPIED_VOXELS); octree_render_property_->addOption("Free Voxels", OCTOMAP_FREE_VOXELS); octree_render_property_->addOption("All Voxels", OCTOMAP_FREE_VOXELS | OCTOMAP_OCCUPIED_VOXELS); - octree_coloring_property_ = new rviz::EnumProperty("Voxel Coloring", "Z-Axis", "Select voxel coloring mode", - scene_enabled_property_, SLOT(renderCurrentScene()), this); + octree_coloring_property_ = + new rviz_common::properties::EnumProperty("Voxel Coloring", "Z-Axis", "Select voxel coloring mode", + scene_enabled_property_, SLOT(renderCurrentScene()), this); octree_coloring_property_->addOption("Z-Axis", OCTOMAP_Z_AXIS_COLOR); octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR); @@ -174,7 +185,7 @@ TaskSolutionVisualization::~TaskSolutionVisualization() { main_scene_node_->getCreator()->destroySceneNode(main_scene_node_); } -void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::DisplayContext* context) { +void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz_common::DisplayContext* context) { // Save pointers for later use parent_scene_node_ = scene_node; context_ = context; @@ -194,7 +205,7 @@ void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz:: marker_visual_->onInitialize(main_scene_node_, context_); - rviz::WindowManagerInterface* window_context = context_->getWindowManager(); + rviz_common::WindowManagerInterface* window_context = context_->getWindowManager(); if (window_context) { slider_panel_ = new TaskSolutionPanel(window_context->getParentWindow()); slider_dock_panel_ = window_context->addPane(display_->getName() + " - Slider", slider_panel_); @@ -209,10 +220,10 @@ void TaskSolutionVisualization::setName(const QString& name) { slider_dock_panel_->setWindowTitle(name + " - Slider"); } -void TaskSolutionVisualization::onRobotModelLoaded(const robot_model::RobotModelConstPtr& robot_model) { +void TaskSolutionVisualization::onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model) { // Error check if (!robot_model) { - ROS_ERROR_STREAM_NAMED("task_solution_visualization", "No robot model found"); + RCLCPP_ERROR(LOGGER, "No robot model found"); return; } @@ -268,8 +279,8 @@ void TaskSolutionVisualization::changedTrail() { trail_.resize(t->getWayPointCount() / stepsize); for (std::size_t i = 0; i < trail_.size(); i++) { int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point - rviz::Robot* r = - new rviz::Robot(trail_scene_node_, context_, "Trail Robot " + boost::lexical_cast(i), nullptr); + rviz_default_plugins::robot::Robot* r = new rviz_default_plugins::robot::Robot( + trail_scene_node_, context_, "Trail Robot " + boost::lexical_cast(i), nullptr); r->load(*scene_->getRobotModel()->getURDF()); r->setVisualVisible(robot_visual_enabled_property_->getBool()); r->setCollisionVisible(robot_collision_enabled_property_->getBool()); @@ -486,7 +497,7 @@ void TaskSolutionVisualization::renderWayPoint(size_t index, int previous_index) } QColor attached_color = attached_body_color_property_->getColor(); - std_msgs::ColorRGBA color; + std_msgs::msg::ColorRGBA color; color.r = attached_color.redF(); color.g = attached_color.greenF(); color.b = attached_color.blueF(); @@ -506,16 +517,16 @@ void TaskSolutionVisualization::renderPlanningScene(const planning_scene::Planni return; QColor color = scene_color_property_->getColor(); - rviz::Color env_color(color.redF(), color.greenF(), color.blueF()); + Ogre::ColourValue env_color(color.redF(), color.greenF(), color.blueF()); color = attached_body_color_property_->getColor(); - rviz::Color attached_color(color.redF(), color.greenF(), color.blueF()); + Ogre::ColourValue attached_color(color.redF(), color.greenF(), color.blueF()); scene_render_->renderPlanningScene( scene, env_color, attached_color, static_cast(octree_render_property_->getOptionInt()), static_cast(octree_coloring_property_->getOptionInt()), scene_alpha_property_->getFloat()); } -void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msgs::Solution& msg) { +void TaskSolutionVisualization::showTrajectory(const moveit_task_constructor_msgs::msg::Solution& msg) { DisplaySolutionPtr s(new DisplaySolution); s->setFromMessage(scene_, msg); showTrajectory(s, false); @@ -565,12 +576,12 @@ void TaskSolutionVisualization::changedAttachedBodyColor() { renderCurrentWayPoint(); } -void TaskSolutionVisualization::unsetRobotColor(rviz::Robot* robot) { +void TaskSolutionVisualization::unsetRobotColor(rviz_default_plugins::robot::Robot* robot) { for (auto& link : robot->getLinks()) link.second->unsetColor(); } -void TaskSolutionVisualization::setRobotColor(rviz::Robot* robot, const QColor& color) { +void TaskSolutionVisualization::setRobotColor(rviz_default_plugins::robot::Robot* robot, const QColor& color) { for (auto& link : robot->getLinks()) link.second->setColor(color.redF(), color.greenF(), color.blueF()); }