Port visualization to ROS2

This commit is contained in:
JafarAbdi 2021-11-23 17:00:01 +03:00
parent d7ceaa01dd
commit 29703d0d6a
39 changed files with 854 additions and 698 deletions

View File

@ -1,69 +1,56 @@
cmake_minimum_required(VERSION 3.1.3) cmake_minimum_required(VERSION 3.5)
project(moveit_task_constructor_visualization) project(moveit_task_constructor_visualization)
find_package(catkin REQUIRED COMPONENTS find_package(ament_cmake REQUIRED)
moveit_core find_package(Boost REQUIRED)
moveit_ros_visualization find_package(moveit_core REQUIRED)
moveit_task_constructor_core find_package(moveit_ros_visualization REQUIRED)
moveit_task_constructor_msgs find_package(moveit_task_constructor_core REQUIRED)
roscpp find_package(moveit_task_constructor_msgs REQUIRED)
rviz find_package(rclcpp REQUIRED)
) find_package(rviz_common REQUIRED)
find_package(rviz_default_plugins REQUIRED)
# rviz transitively includes OGRE headers which break with `-Wall -Werror` find_package(rviz_ogre_vendor REQUIRED)
# 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()
# definition needed for boost/math/constants/constants.hpp included by Ogre to compile # definition needed for boost/math/constants/constants.hpp included by Ogre to compile
add_definitions(-DBOOST_MATH_DISABLE_FLOAT128) add_definitions(-DBOOST_MATH_DISABLE_FLOAT128)
# Qt Stuff # Qt Stuff
if("${rviz_QT_VERSION}" VERSION_LESS "5") find_package(Qt5 REQUIRED COMPONENTS Core Widgets)
find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui) set(QT_LIBRARIES Qt5::Widgets)
include(${QT_USE_FILE}) macro(qt_wrap_ui)
macro(qt_wrap_ui) qt5_wrap_ui(${ARGN})
qt4_wrap_ui(${ARGN}) endmacro()
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()
set(CMAKE_INCLUDE_CURRENT_DIR ON) set(CMAKE_INCLUDE_CURRENT_DIR ON)
set(CMAKE_AUTOMOC ON) set(CMAKE_AUTOMOC ON)
set(CMAKE_AUTORCC ON) set(CMAKE_AUTORCC ON)
add_definitions(-DQT_NO_KEYWORDS)
catkin_package( if(NOT CMAKE_CXX_STANDARD)
LIBRARIES set(CMAKE_CXX_STANDARD 17)
moveit_task_visualization_tools endif()
motion_planning_tasks_utils
INCLUDE_DIRS
visualization_tools/include
CATKIN_DEPENDS
moveit_core
moveit_task_constructor_msgs
roscpp
rviz
)
set(CMAKE_CXX_STANDARD 14)
add_subdirectory(visualization_tools) add_subdirectory(visualization_tools)
add_subdirectory(motion_planning_tasks) add_subdirectory(motion_planning_tasks)
install(FILES install(DIRECTORY icons DESTINATION share)
motion_planning_tasks_rviz_plugin_description.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
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()

View File

@ -20,9 +20,14 @@ target_link_libraries(${MOVEIT_LIB_NAME}
) )
target_include_directories(${MOVEIT_LIB_NAME} target_include_directories(${MOVEIT_LIB_NAME}
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..> PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>
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} install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} EXPORT export_${MOVEIT_LIB_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)

View File

@ -40,34 +40,34 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/properties.h> #include <moveit/task_constructor/properties.h>
#include <rviz/properties/property_tree_model.h> #include <rviz_common/properties/property_tree_model.hpp>
#include <rviz/properties/string_property.h> #include <rviz_common/properties/string_property.hpp>
#include <rviz/properties/float_property.h> #include <rviz_common/properties/float_property.hpp>
namespace mtc = ::moveit::task_constructor; namespace mtc = ::moveit::task_constructor;
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
static rviz::StringProperty* stringFactory(const QString& name, mtc::Property& mtc_prop, static rviz_common::properties::StringProperty* stringFactory(const QString& name, mtc::Property& mtc_prop,
const planning_scene::PlanningScene* /*unused*/, const planning_scene::PlanningScene* /*unused*/,
rviz::DisplayContext* /*unused*/) { rviz_common::DisplayContext* /*unused*/) {
std::string value; std::string value;
if (!mtc_prop.value().empty()) if (!mtc_prop.value().empty())
value = boost::any_cast<std::string>(mtc_prop.value()); value = boost::any_cast<std::string>(mtc_prop.value());
rviz::StringProperty* rviz_prop = rviz_common::properties::StringProperty* rviz_prop = new rviz_common::properties::StringProperty(
new rviz::StringProperty(name, QString::fromStdString(value), QString::fromStdString(mtc_prop.description())); name, QString::fromStdString(value), QString::fromStdString(mtc_prop.description()));
QObject::connect(rviz_prop, &rviz::StringProperty::changed, QObject::connect(rviz_prop, &rviz_common::properties::StringProperty::changed,
[rviz_prop, &mtc_prop]() { mtc_prop.setValue(rviz_prop->getStdString()); }); [rviz_prop, &mtc_prop]() { mtc_prop.setValue(rviz_prop->getStdString()); });
return rviz_prop; return rviz_prop;
} }
template <typename T> template <typename T>
static rviz::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc_prop, static rviz_common::properties::FloatProperty* floatFactory(const QString& name, mtc::Property& mtc_prop,
const planning_scene::PlanningScene* /*unused*/, const planning_scene::PlanningScene* /*unused*/,
rviz::DisplayContext* /*unused*/) { rviz_common::DisplayContext* /*unused*/) {
T value = !mtc_prop.value().empty() ? boost::any_cast<T>(mtc_prop.value()) : T(); T value = !mtc_prop.value().empty() ? boost::any_cast<T>(mtc_prop.value()) : T();
rviz::FloatProperty* rviz_prop = rviz_common::properties::FloatProperty* rviz_prop =
new rviz::FloatProperty(name, value, QString::fromStdString(mtc_prop.description())); new rviz_common::properties::FloatProperty(name, value, QString::fromStdString(mtc_prop.description()));
QObject::connect(rviz_prop, &rviz::FloatProperty::changed, QObject::connect(rviz_prop, &rviz_common::properties::FloatProperty::changed,
[rviz_prop, &mtc_prop]() { mtc_prop.setValue(T(rviz_prop->getFloat())); }); [rviz_prop, &mtc_prop]() { mtc_prop.setValue(T(rviz_prop->getFloat())); });
return rviz_prop; 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)); stage_registry_.insert(std::make_pair(type_index, f));
} }
rviz::Property* PropertyFactory::create(const std::string& prop_name, mtc::Property& prop, rviz_common::properties::Property* PropertyFactory::create(const std::string& prop_name, mtc::Property& prop,
const planning_scene::PlanningScene* scene, const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context) const { rviz_common::DisplayContext* display_context) const {
auto it = property_registry_.find(prop.typeName()); auto it = property_registry_.find(prop.typeName());
if (it == property_registry_.end()) if (it == property_registry_.end())
return createDefault(prop_name, prop.typeName(), prop.description(), prop.serialize()); return createDefault(prop_name, prop.typeName(), prop.description(), prop.serialize());
return it->second(QString::fromStdString(prop_name), prop, scene, display_context); return it->second(QString::fromStdString(prop_name), prop, scene, display_context);
} }
rviz::PropertyTreeModel* PropertyFactory::createPropertyTreeModel(moveit::task_constructor::Stage& stage, rviz_common::properties::PropertyTreeModel*
const planning_scene::PlanningScene* scene, PropertyFactory::createPropertyTreeModel(moveit::task_constructor::Stage& stage,
rviz::DisplayContext* display_context) { const planning_scene::PlanningScene* scene,
rviz_common::DisplayContext* display_context) {
auto it = stage_registry_.find(typeid(stage)); auto it = stage_registry_.find(typeid(stage));
if (it == stage_registry_.end()) if (it == stage_registry_.end())
return defaultPropertyTreeModel(stage.properties(), scene, display_context); return defaultPropertyTreeModel(stage.properties(), scene, display_context);
return it->second(stage.properties(), scene, display_context); return it->second(stage.properties(), scene, display_context);
} }
rviz::PropertyTreeModel* PropertyFactory::defaultPropertyTreeModel(mtc::PropertyMap& properties, rviz_common::properties::PropertyTreeModel*
const planning_scene::PlanningScene* scene, PropertyFactory::defaultPropertyTreeModel(mtc::PropertyMap& properties, const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context) { rviz_common::DisplayContext* display_context) {
auto root = new rviz::Property(); auto root = new rviz_common::properties::Property();
addRemainingProperties(root, properties, scene, display_context); 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) { for (int i = 0, end = root->numChildren(); i != end; ++i) {
if (root->childAt(i)->getName() == name) if (root->childAt(i)->getName() == name)
return true; return true;
@ -128,37 +129,38 @@ static bool hasChild(rviz::Property* root, const QString& name) {
return false; 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, const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context) { rviz_common::DisplayContext* display_context) {
for (auto& prop : properties) { for (auto& prop : properties) {
const QString& name = QString::fromStdString(prop.first); const QString& name = QString::fromStdString(prop.first);
if (hasChild(root, name)) if (hasChild(root, name))
continue; 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) if (!rviz_prop)
rviz_prop = new rviz::Property(name); rviz_prop = new rviz_common::properties::Property(name);
root->addChild(rviz_prop); root->addChild(rviz_prop);
} }
// just to see something, when no properties are defined // just to see something, when no properties are defined
if (root->numChildren() == 0) 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 #ifndef HAVE_YAML
rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type, rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& type,
const std::string& description, const std::string& value, const std::string& description,
rviz::Property* old) { const std::string& value,
rviz_common::properties::Property* old) {
if (old) { // reuse existing Property? if (old) { // reuse existing Property?
assert(old->getNameStd() == name); assert(old->getNameStd() == name);
old->setDescription(QString::fromStdString(description)); old->setDescription(QString::fromStdString(description));
old->setValue(QString::fromStdString(value)); old->setValue(QString::fromStdString(value));
return old; return old;
} else { // create new Property? } else { // create new Property?
rviz::Property* result = new rviz::StringProperty(QString::fromStdString(name), QString::fromStdString(value), rviz_common::properties::Property* result = new rviz::StringProperty(
QString::fromStdString(description)); QString::fromStdString(name), QString::fromStdString(value), QString::fromStdString(description));
result->setReadOnly(true); result->setReadOnly(true);
return result; return result;
} }

View File

@ -44,11 +44,14 @@
#include <moveit/task_constructor/properties.h> #include <moveit/task_constructor/properties.h>
namespace rviz { namespace rviz_common {
class DisplayContext;
namespace properties {
class Property; class Property;
class PropertyTreeModel; class PropertyTreeModel;
class DisplayContext; } // namespace properties
} // namespace rviz } // namespace rviz_common
namespace planning_scene { namespace planning_scene {
class PlanningScene; class PlanningScene;
} }
@ -60,10 +63,10 @@ class Stage;
namespace moveit_rviz_plugin { 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, * 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 * 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 * 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 * the display of stage properties, e.g. hiding specific properties, or returning a subclassed
@ -75,11 +78,11 @@ class PropertyFactory
public: public:
static PropertyFactory& instance(); static PropertyFactory& instance();
using PropertyFactoryFunction = using PropertyFactoryFunction = std::function<rviz_common::properties::Property*(
std::function<rviz::Property*(const QString&, moveit::task_constructor::Property&, const QString&, moveit::task_constructor::Property&, const planning_scene::PlanningScene*,
const planning_scene::PlanningScene*, rviz::DisplayContext*)>; rviz_common::DisplayContext*)>;
using TreeFactoryFunction = std::function<rviz::PropertyTreeModel*( using TreeFactoryFunction = std::function<rviz_common::properties::PropertyTreeModel*(
moveit::task_constructor::PropertyMap&, const planning_scene::PlanningScene*, rviz::DisplayContext*)>; moveit::task_constructor::PropertyMap&, const planning_scene::PlanningScene*, rviz_common::DisplayContext*)>;
/// register a factory function for type T /// register a factory function for type T
template <typename T> template <typename T>
@ -94,27 +97,30 @@ public:
registerStage(typeid(T), f); registerStage(typeid(T), f);
} }
/// create rviz::Property for given MTC Property /// create rviz_common::properties::Property for given MTC Property
rviz::Property* create(const std::string& prop_name, moveit::task_constructor::Property& prop, rviz_common::properties::Property* create(const std::string& prop_name, moveit::task_constructor::Property& prop,
const planning_scene::PlanningScene* scene, rviz::DisplayContext* display_context) const; const planning_scene::PlanningScene* scene,
/// create rviz::Property for property of given name, type, description, and value rviz_common::DisplayContext* display_context) const;
static rviz::Property* createDefault(const std::string& name, const std::string& type, /// create rviz_common::properties::Property for property of given name, type, description, and value
const std::string& description, const std::string& value, static rviz_common::properties::Property* createDefault(const std::string& name, const std::string& type,
rviz::Property* old = nullptr); const std::string& description, const std::string& value,
rviz_common::properties::Property* old = nullptr);
/// create PropertyTreeModel for given Stage /// create PropertyTreeModel for given Stage
rviz::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage& stage, rviz_common::properties::PropertyTreeModel* createPropertyTreeModel(moveit::task_constructor::Stage& stage,
const planning_scene::PlanningScene* scene, const planning_scene::PlanningScene* scene,
rviz::DisplayContext* display_context); rviz_common::DisplayContext* display_context);
/// turn a PropertyMap into an rviz::PropertyTreeModel /// turn a PropertyMap into an rviz_common::properties::PropertyTreeModel
rviz::PropertyTreeModel* defaultPropertyTreeModel(moveit::task_constructor::PropertyMap& properties, rviz_common::properties::PropertyTreeModel*
const planning_scene::PlanningScene* scene, defaultPropertyTreeModel(moveit::task_constructor::PropertyMap& properties,
rviz::DisplayContext* display_context); const planning_scene::PlanningScene* scene, rviz_common::DisplayContext* display_context);
/// add all properties from map that are not yet in root /// add all properties from map that are not yet in root
void addRemainingProperties(rviz::Property* root, moveit::task_constructor::PropertyMap& properties, void addRemainingProperties(rviz_common::properties::Property* root,
const planning_scene::PlanningScene* scene, rviz::DisplayContext* display_context); moveit::task_constructor::PropertyMap& properties,
const planning_scene::PlanningScene* scene,
rviz_common::DisplayContext* display_context);
private: private:
std::map<std::string, PropertyFactoryFunction> property_registry_; std::map<std::string, PropertyFactoryFunction> property_registry_;

View File

@ -36,12 +36,12 @@
#include "property_factory.h" #include "property_factory.h"
#include <yaml.h> #include <yaml.h>
#include <rviz/properties/string_property.h> #include <rviz_common/properties/string_property.hpp>
#include <rviz/properties/float_property.h> #include <rviz_common/properties/float_property.hpp>
namespace mtc = ::moveit::task_constructor; 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. * from a YAML-serialized string.
* As we cannot know the required data type for a field from YAML parsing, * 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). * we only distinguish numbers (FloatProperty) and all other YAML scalars (StringProperty).
@ -60,7 +60,7 @@ private:
yaml_event_t event_; 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 // https://www.wpsoftware.net/andrew/pages/libyaml.html
class Parser class Parser
{ {
@ -70,30 +70,35 @@ public:
Parser(const std::string& value); Parser(const std::string& value);
~Parser(); ~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: private:
static rviz::Property* createScalar(const QString& name, const QString& description, const QByteArray& value, static rviz_common::properties::Property* createScalar(const QString& name, const QString& description,
rviz::Property* old); const QByteArray& value,
rviz_common::properties::Property* old);
// return true if there was no error so far // return true if there was no error so far
bool noError() const { return parser_.error == YAML_NO_ERROR; } bool noError() const { return parser_.error == YAML_NO_ERROR; }
// parse a single event and return it's type, YAML_ERROR_EVENT on parsing error // parse a single event and return it's type, YAML_ERROR_EVENT on parsing error
int parse(yaml_event_t& event) const; int parse(yaml_event_t& event) const;
// process events: scalar, start mapping, start sequence // process events: scalar, start mapping, start sequence
rviz::Property* process(const yaml_event_t& event, const QString& name, const QString& description, rviz_common::properties::Property* process(const yaml_event_t& event, const QString& name,
rviz::Property* old) const; const QString& description, rviz_common::properties::Property* old) const;
inline static QByteArray byteArray(const yaml_event_t& event) { inline static QByteArray byteArray(const yaml_event_t& event) {
assert(event.type == YAML_SCALAR_EVENT); assert(event.type == YAML_SCALAR_EVENT);
return QByteArray::fromRawData(reinterpret_cast<const char*>(event.data.scalar.value), event.data.scalar.length); return QByteArray::fromRawData(reinterpret_cast<const char*>(event.data.scalar.value), event.data.scalar.length);
} }
// Try to set value of existing rviz::Property (expecting matching types). Return false on error. // Try to set value of existing rviz_common::properties::Property (expecting matching types). Return false on error.
static bool setValue(rviz::Property* old, const QByteArray& value); static bool setValue(rviz_common::properties::Property* old, const QByteArray& value);
static rviz::Property* createParent(const QString& name, const QString& description, rviz::Property* old); static rviz_common::properties::Property* createParent(const QString& name, const QString& description,
rviz::Property* processMapping(const QString& name, const QString& description, rviz::Property* old) const; rviz_common::properties::Property* old);
rviz::Property* processSequence(const QString& name, const QString& description, rviz::Property* old) const; 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: private:
mutable yaml_parser_t parser_; mutable yaml_parser_t parser_;
@ -116,7 +121,8 @@ int Parser::parse(yaml_event_t& event) const {
} }
// main processing function // 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; bool stop = false;
while (!stop) { while (!stop) {
ScopedYamlEvent event; 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 // 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_common::properties::Property* Parser::process(const yaml_event_t& event, const QString& name,
rviz::Property* old) const { const QString& description,
rviz_common::properties::Property* old) const {
switch (event.type) { switch (event.type) {
case YAML_SEQUENCE_START_EVENT: case YAML_SEQUENCE_START_EVENT:
return processSequence(name, description, old); return processSequence(name, description, old);
@ -153,18 +160,20 @@ rviz::Property* Parser::process(const yaml_event_t& event, const QString& name,
default: default:
throw std::runtime_error("Unhandled YAML event"); 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. // 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) { bool Parser::setValue(rviz_common::properties::Property* old, const QByteArray& value) {
if (rviz::FloatProperty* p = dynamic_cast<rviz::FloatProperty*>(old)) { if (rviz_common::properties::FloatProperty* p = dynamic_cast<rviz_common::properties::FloatProperty*>(old)) {
bool ok = true; bool ok = true;
double v = value.toDouble(&ok); double v = value.toDouble(&ok);
if (ok) if (ok)
p->setValue(v); p->setValue(v);
return ok; return ok;
} }
if (rviz::StringProperty* p = dynamic_cast<rviz::StringProperty*>(old)) { if (rviz_common::properties::StringProperty* p = dynamic_cast<rviz_common::properties::StringProperty*>(old)) {
// value should be an arbitrary string. If not throws YAML::BadConversion // value should be an arbitrary string. If not throws YAML::BadConversion
p->setValue(value); p->setValue(value);
return true; 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 // 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_common::properties::Property* Parser::createScalar(const QString& name, const QString& description,
rviz::Property* old) { const QByteArray& value,
// try to update value, expecting matching rviz::Property rviz_common::properties::Property* old) {
// try to update value, expecting matching rviz_common::properties::Property
if (old && setValue(old, value)) { if (old && setValue(old, value)) {
// only if setValue succeeded, also update the rest // only if setValue succeeded, also update the rest
old->setName(name); old->setName(name);
@ -186,21 +196,23 @@ rviz::Property* Parser::createScalar(const QString& name, const QString& descrip
bool ok = true; bool ok = true;
double v = value.toDouble(&ok); double v = value.toDouble(&ok);
if (ok) // if value is a number, create a FloatProperty 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 else // otherwise create a StringProperty
old = new rviz::StringProperty(name, value, description); old = new rviz_common::properties::StringProperty(name, value, description);
old->setReadOnly(true); old->setReadOnly(true);
return old; return old;
} }
// Reuse old property (or create new one) as parent for a sequence or map // 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) // don't reuse float or string properties (they are for scalars)
if (dynamic_cast<rviz::FloatProperty*>(old) || dynamic_cast<rviz::StringProperty*>(old)) if (dynamic_cast<rviz_common::properties::FloatProperty*>(old) ||
dynamic_cast<rviz_common::properties::StringProperty*>(old))
old = nullptr; old = nullptr;
if (!old) { if (!old) {
old = new rviz::Property(name, QVariant(), description); old = new rviz_common::properties::Property(name, QVariant(), description);
old->setReadOnly(true); old->setReadOnly(true);
} else { } else {
old->setName(name); old->setName(name);
@ -210,7 +222,8 @@ rviz::Property* Parser::createParent(const QString& name, const QString& descrip
} }
// Hierarchically create property from YAML map node // 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); root = createParent(name, description, root);
int index = 0; // current child index in root int index = 0; // current child index in root
bool stop = false; bool stop = false;
@ -233,11 +246,11 @@ rviz::Property* Parser::processMapping(const QString& name, const QString& descr
num = root->numChildren(); num = root->numChildren();
// if names differ, insert a new child, otherwise reuse existing // 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) if (old_child && old_child->getName() != key)
old_child = nullptr; old_child = nullptr;
rviz::Property* new_child = nullptr; rviz_common::properties::Property* new_child = nullptr;
switch (parse(event)) { // parse value switch (parse(event)) { // parse value
case YAML_MAPPING_START_EVENT: case YAML_MAPPING_START_EVENT:
case YAML_SEQUENCE_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 [#]. // 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); root = createParent(name, description, root);
int index = 0; // current child index in root int index = 0; // current child index in root
bool stop = false; bool stop = false;
@ -282,8 +296,8 @@ rviz::Property* Parser::processSequence(const QString& name, const QString& desc
case YAML_MAPPING_START_EVENT: case YAML_MAPPING_START_EVENT:
case YAML_SEQUENCE_START_EVENT: case YAML_SEQUENCE_START_EVENT:
case YAML_SCALAR_EVENT: { case YAML_SCALAR_EVENT: {
rviz::Property* old_child = root->childAt(index); // nullptr for invalid index rviz_common::properties::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* new_child = process(event, QString("[%1]").arg(index), "", old_child);
if (new_child != old_child) if (new_child != old_child)
root->addChild(new_child, index); root->addChild(new_child, index);
if (++index >= 10) if (++index >= 10)
@ -305,9 +319,10 @@ rviz::Property* Parser::processSequence(const QString& name, const QString& desc
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
rviz::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/, rviz_common::properties::Property* PropertyFactory::createDefault(const std::string& name, const std::string& /*type*/,
const std::string& description, const std::string& value, const std::string& description,
rviz::Property* old) { const std::string& value,
rviz_common::properties::Property* old) {
QString qname = QString::fromStdString(name); QString qname = QString::fromStdString(name);
QString qdesc = QString::fromStdString(description); QString qdesc = QString::fromStdString(description);
Parser parser(value); Parser parser(value);

View File

@ -6,7 +6,7 @@ qt_wrap_ui(UIC_FILES
global_settings.ui global_settings.ui
) )
add_library(${MOVEIT_LIB_NAME} add_library(${MOVEIT_LIB_NAME} SHARED
factory_model.cpp factory_model.cpp
icons.cpp icons.cpp
job_queue.cpp job_queue.cpp
@ -28,7 +28,7 @@ add_library(${MOVEIT_LIB_NAME}
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
target_link_libraries(${MOVEIT_LIB_NAME} target_link_libraries(${MOVEIT_LIB_NAME}
motion_planning_tasks_utils motion_planning_tasks_properties moveit_task_visualization_tools motion_planning_tasks_utils motion_planning_tasks_properties moveit_task_visualization_tools
${catkin_LIBRARIES} ${QT_LIBRARIES} ${QT_LIBRARIES}
) )
target_include_directories(${MOVEIT_LIB_NAME} target_include_directories(${MOVEIT_LIB_NAME}
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..> PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>
@ -37,12 +37,9 @@ target_include_directories(${MOVEIT_LIB_NAME}
PUBLIC $<TARGET_PROPERTY:motion_planning_tasks_utils,INTERFACE_INCLUDE_DIRECTORIES> PUBLIC $<TARGET_PROPERTY:motion_planning_tasks_utils,INTERFACE_INCLUDE_DIRECTORIES>
PUBLIC $<TARGET_PROPERTY:motion_planning_tasks_properties,INTERFACE_INCLUDE_DIRECTORIES> PUBLIC $<TARGET_PROPERTY:motion_planning_tasks_properties,INTERFACE_INCLUDE_DIRECTORIES>
PUBLIC $<TARGET_PROPERTY:moveit_task_visualization_tools,INTERFACE_INCLUDE_DIRECTORIES> PUBLIC $<TARGET_PROPERTY:moveit_task_visualization_tools,INTERFACE_INCLUDE_DIRECTORIES>
PUBLIC ${catkin_INCLUDE_DIRS}
)
target_include_directories(${MOVEIT_LIB_NAME} SYSTEM
PUBLIC ${rviz_OGRE_INCLUDE_DIRS}
) )
install(TARGETS ${MOVEIT_LIB_NAME} install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} EXPORT export_${MOVEIT_LIB_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)

View File

@ -35,29 +35,29 @@
/* Author: Robert Haschke */ /* Author: Robert Haschke */
#include "factory_model.h" #include "factory_model.h"
#include <rviz/load_resource.h> #include <rviz_common/load_resource.hpp>
#include <QMimeData> #include <QMimeData>
#include <QSet> #include <QSet>
namespace moveit_rviz_plugin { 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) { : QStandardItemModel(parent), mime_type_(mime_type) {
setHorizontalHeaderLabels({ tr("Name") }); setHorizontalHeaderLabels({ tr("Name") });
fillTree(factory); fillTree(factory);
} }
void FactoryModel::fillTree(rviz::Factory& factory) { void FactoryModel::fillTree(rviz_common::Factory& factory) {
QIcon default_package_icon = rviz::loadPixmap("package://rviz/icons/default_package_icon.png"); QIcon default_package_icon = rviz_common::loadPixmap("package://rviz/icons/default_package_icon.png");
QStringList classes = factory.getDeclaredClassIds(); auto plugins = factory.getDeclaredPlugins();
classes.sort(); std::sort(plugins.begin(), plugins.end());
// Map from package names to the corresponding top-level tree widget items. // Map from package names to the corresponding top-level tree widget items.
std::map<QString, QStandardItem*> package_items; std::map<QString, QStandardItem*> package_items;
for (const QString& lookup_name : classes) { for (const auto& plugin : plugins) {
QString package = factory.getClassPackage(lookup_name); const QString& package = plugin.package;
QStandardItem* package_item; QStandardItem* package_item;
auto mi = package_items.find(package); auto mi = package_items.find(package);
@ -68,9 +68,9 @@ void FactoryModel::fillTree(rviz::Factory& factory) {
} else { } else {
package_item = mi->second; package_item = mi->second;
} }
QStandardItem* class_item = new QStandardItem(factory.getIcon(lookup_name), factory.getClassName(lookup_name)); QStandardItem* class_item = new QStandardItem(plugin.icon, plugin.name);
class_item->setWhatsThis(factory.getClassDescription(lookup_name)); class_item->setWhatsThis(plugin.description);
class_item->setData(lookup_name, Qt::UserRole); class_item->setData(plugin.id, Qt::UserRole);
class_item->setDragEnabled(true); class_item->setDragEnabled(true);
package_item->appendRow(class_item); package_item->appendRow(class_item);
} }

View File

@ -36,7 +36,7 @@
#pragma once #pragma once
#include <rviz/factory.h> #include <rviz_common/factory/factory.hpp>
#include <QStandardItemModel> #include <QStandardItemModel>
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
@ -47,10 +47,10 @@ namespace moveit_rviz_plugin {
class FactoryModel : public QStandardItemModel class FactoryModel : public QStandardItemModel
{ {
QString mime_type_; QString mime_type_;
void fillTree(rviz::Factory& factory); void fillTree(rviz_common::Factory& factory);
public: 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; QStringList mimeTypes() const override;
QMimeData* mimeData(const QModelIndexList& indexes) const override; QMimeData* mimeData(const QModelIndexList& indexes) const override;

View File

@ -51,7 +51,7 @@
</widget> </widget>
</item> </item>
<item> <item>
<widget class="rviz::PropertyTreeWidget" name="view"/> <widget class="rviz_common::properties::PropertyTreeWidget" name="view"/>
</item> </item>
</layout> </layout>
</item> </item>
@ -59,9 +59,9 @@
</widget> </widget>
<customwidgets> <customwidgets>
<customwidget> <customwidget>
<class>rviz::PropertyTreeWidget</class> <class>rviz_common::properties::PropertyTreeWidget</class>
<extends>QTreeView</extends> <extends>QTreeView</extends>
<header location="global">rviz/properties/property_tree_widget.h</header> <header location="global">rviz_common/properties/property_tree_widget.hpp</header>
</customwidget> </customwidget>
</customwidgets> </customwidgets>
<resources/> <resources/>

View File

@ -35,7 +35,9 @@
/* Author: Robert Haschke */ /* Author: Robert Haschke */
#include "job_queue.h" #include "job_queue.h"
#include <ros/console.h> #include <rclcpp/logging.hpp>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.job_queue");
namespace moveit { namespace moveit {
namespace tools { namespace tools {
@ -71,7 +73,7 @@ void JobQueue::executeJobs() {
try { try {
fn(); fn();
} catch (std::exception& ex) { } 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(); ulock.lock();
} }

View File

@ -37,9 +37,7 @@
#include "local_task_model.h" #include "local_task_model.h"
#include "factory_model.h" #include "factory_model.h"
#include "properties/property_factory.h" #include "properties/property_factory.h"
#include <rviz/properties/property_tree_model.h> #include <rviz_common/properties/property_tree_model.hpp>
#include <ros/console.h>
#include <QMimeData> #include <QMimeData>
@ -78,7 +76,7 @@ QModelIndex LocalTaskModel::index(Node* n) const {
} }
LocalTaskModel::LocalTaskModel(ContainerBase::pointer&& container, const planning_scene::PlanningSceneConstPtr& scene, 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)) { : BaseTaskModel(scene, display_context, parent), Task("", true, std::move(container)) {
root_ = this; root_ = this;
flags_ |= LOCAL_MODEL; flags_ |= LOCAL_MODEL;
@ -235,7 +233,7 @@ DisplaySolutionPtr LocalTaskModel::getSolution(const QModelIndex& /*index*/) {
return DisplaySolutionPtr(); return DisplaySolutionPtr();
} }
rviz::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex& index) { rviz_common::properties::PropertyTreeModel* LocalTaskModel::getPropertyModel(const QModelIndex& index) {
Node* n = node(index); Node* n = node(index);
if (!n) if (!n)
return nullptr; return nullptr;

View File

@ -47,14 +47,14 @@ class LocalTaskModel : public BaseTaskModel, public moveit::task_constructor::Ta
using Node = moveit::task_constructor::Stage; using Node = moveit::task_constructor::Stage;
Node* root_; Node* root_;
StageFactoryPtr stage_factory_; StageFactoryPtr stage_factory_;
std::map<Node*, rviz::PropertyTreeModel*> properties_; std::map<Node*, rviz_common::properties::PropertyTreeModel*> properties_;
inline Node* node(const QModelIndex& index) const; inline Node* node(const QModelIndex& index) const;
QModelIndex index(Node* n) const; QModelIndex index(Node* n) const;
public: public:
LocalTaskModel(ContainerBase::pointer&& container, const planning_scene::PlanningSceneConstPtr& scene, 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; int rowCount(const QModelIndex& parent = QModelIndex()) const override;
QModelIndex index(int row, int column, 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; QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
DisplaySolutionPtr getSolution(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 } // namespace moveit_rviz_plugin

View File

@ -34,9 +34,9 @@
/* Author: Robert Haschke */ /* Author: Robert Haschke */
#include <pluginlib/class_list_macros.h> #include <pluginlib/class_list_macros.hpp>
#include "task_display.h" #include "task_display.h"
#include "task_panel.h" #include "task_panel.h"
PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskDisplay, rviz::Display) PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskDisplay, rviz_common::Display)
PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskPanel, rviz::Panel) PLUGINLIB_EXPORT_CLASS(moveit_rviz_plugin::TaskPanel, rviz_common::Panel)

View File

@ -45,12 +45,13 @@
#include <vector> #include <vector>
#ifndef Q_MOC_RUN #ifndef Q_MOC_RUN
#include <pluginlib/class_loader.h> #include <pluginlib/class_loader.hpp>
#include <rviz/load_resource.h> #include <rviz_common/load_resource.hpp>
#include <functional> #include <functional>
#endif #endif
#include <rviz/factory.h> #include <rclcpp/logging.hpp>
#include <rviz_common/factory/factory.hpp>
namespace moveit_rviz_plugin { 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. * This is a slightly modified version of rviz::PluginlibFactory, providing a custom mime type.
*/ */
template <class Type> template <class Type>
class PluginlibFactory : public rviz::Factory class PluginlibFactory : public rviz_common::Factory
{ {
private: private:
struct BuiltInClassRecord struct BuiltInClassRecord
@ -80,41 +81,39 @@ public:
/// retrieve mime type used for given factory /// retrieve mime type used for given factory
QString mimeType() const { return mime_type_; } QString mimeType() const { return mime_type_; }
QStringList getDeclaredClassIds() override { std::vector<rviz_common::PluginInfo> getDeclaredPlugins() override {
QStringList ids; std::vector<rviz_common::PluginInfo> plugins;
for (const auto& record : built_ins_) for (auto iter = built_ins_.cbegin(); iter != built_ins_.cend(); ++iter)
ids.push_back(record.class_id_); plugins.emplace_back(getPluginInfo(iter.key()));
for (const auto& id : class_loader_->getDeclaredClasses()) { for (const auto& id : class_loader_->getDeclaredClasses()) {
QString sid = QString::fromStdString(id); auto sid = QString::fromStdString(id);
if (ids.contains(sid)) 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 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 { rviz_common::PluginInfo getPluginInfo(const QString& class_id) const override {
auto it = built_ins_.find(class_id); rviz_common::PluginInfo info;
if (it != built_ins_.end()) { const auto iter = built_ins_.find(class_id);
return it->description_; 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())); auto class_id_std = class_id.toStdString();
} info.id = class_id;
info.name = QString::fromStdString(class_loader_->getName(class_id_std));
QString getClassName(const QString& class_id) const override { info.package = QString::fromStdString(class_loader_->getClassPackage(class_id_std));
auto it = built_ins_.find(class_id); info.description = QString::fromStdString(class_loader_->getClassDescription(class_id_std));
if (it != built_ins_.end()) { info.icon = getIcon(info);
return it->name_; return info;
}
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()));
} }
virtual QString getPluginManifestPath(const QString& class_id) const { virtual QString getPluginManifestPath(const QString& class_id) const {
@ -125,14 +124,12 @@ public:
return QString::fromStdString(class_loader_->getPluginManifestPath(class_id.toStdString())); return QString::fromStdString(class_loader_->getPluginManifestPath(class_id.toStdString()));
} }
QIcon getIcon(const QString& class_id) const override { QIcon getIcon(const rviz_common::PluginInfo& info) const {
QString package = getClassPackage(class_id); QIcon icon = rviz_common::loadPixmap("package://" + info.package + "/icons/classes/" + info.name + ".svg");
QString class_name = getClassName(class_id);
QIcon icon = rviz::loadPixmap("package://" + package + "/icons/classes/" + class_name + ".svg");
if (icon.isNull()) { 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()) { 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; return icon;
@ -175,8 +172,9 @@ public:
try { try {
return class_loader_->createUnmanagedInstance(class_id.toStdString()); return class_loader_->createUnmanagedInstance(class_id.toStdString());
} catch (pluginlib::PluginlibException& ex) { } catch (pluginlib::PluginlibException& ex) {
ROS_ERROR("PluginlibFactory: The plugin for class '%s' failed to load. Error: %s", qPrintable(class_id), RCLCPP_ERROR(rclcpp::get_logger("moveit_task_constructor_visualization.pluginlib_factory"),
ex.what()); "PluginlibFactory: The plugin for class '%s' failed to load. Error: %s", qPrintable(class_id),
ex.what());
if (error_return) { if (error_return) {
*error_return = QString::fromStdString(ex.what()); *error_return = QString::fromStdString(ex.what());
} }

View File

@ -41,15 +41,16 @@
#include <moveit/task_constructor/container.h> #include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/properties.h> #include <moveit/task_constructor/properties.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit_task_constructor_msgs/GetSolution.h> #include <rviz_common/properties/property_tree_model.hpp>
#include <rviz/properties/property_tree_model.h> #include <rviz_common/properties/string_property.hpp>
#include <rviz/properties/string_property.h> #include <rclcpp/logging.hpp>
#include <ros/console.h>
#include <QApplication> #include <QApplication>
#include <QPalette> #include <QPalette>
#include <qglobal.h> #include <qglobal.h>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.task_list_model");
using namespace moveit::task_constructor; using namespace moveit::task_constructor;
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
@ -69,12 +70,12 @@ struct RemoteTaskModel::Node
InterfaceFlags interface_flags_; InterfaceFlags interface_flags_;
NodeFlags node_flags_; NodeFlags node_flags_;
std::unique_ptr<RemoteSolutionModel> solutions_; std::unique_ptr<RemoteSolutionModel> solutions_;
std::unique_ptr<rviz::PropertyTreeModel> property_tree_; std::unique_ptr<rviz_common::properties::PropertyTreeModel> property_tree_;
std::map<std::string, Property> properties_; std::map<std::string, Property> properties_;
inline Node(Node* parent) : parent_(parent) { inline Node(Node* parent) : parent_(parent) {
solutions_.reset(new RemoteSolutionModel()); 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) { bool setName(const QString& name) {
@ -84,18 +85,20 @@ struct RemoteTaskModel::Node
return true; return true;
} }
void setProperties(const std::vector<moveit_task_constructor_msgs::Property>& props, void setProperties(const std::vector<moveit_task_constructor_msgs::msg::Property>& props,
const planning_scene::PlanningSceneConstPtr& scene_, rviz::DisplayContext* display_context_); const planning_scene::PlanningSceneConstPtr& scene_,
rviz::Property* createProperty(const moveit_task_constructor_msgs::Property& prop, rviz::Property* old, rviz_common::DisplayContext* display_context_);
const planning_scene::PlanningSceneConstPtr& scene_, rviz_common::properties::Property* createProperty(const moveit_task_constructor_msgs::msg::Property& prop,
rviz::DisplayContext* display_context_); rviz_common::properties::Property* old,
const planning_scene::PlanningSceneConstPtr& scene_,
rviz_common::DisplayContext* display_context_);
}; };
void RemoteTaskModel::Node::setProperties(const std::vector<moveit_task_constructor_msgs::Property>& props, void RemoteTaskModel::Node::setProperties(const std::vector<moveit_task_constructor_msgs::msg::Property>& props,
const planning_scene::PlanningSceneConstPtr& scene_, const planning_scene::PlanningSceneConstPtr& scene_,
rviz::DisplayContext* display_context_) { rviz_common::DisplayContext* display_context_) {
// insert properties in same order as reported in description // 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 int index = 0; // current child index in root
for (const auto& prop : props) { for (const auto& prop : props) {
int num = root->numChildren(); int num = root->numChildren();
@ -108,11 +111,11 @@ void RemoteTaskModel::Node::setProperties(const std::vector<moveit_task_construc
num = root->numChildren(); num = root->numChildren();
// if names differ, insert a new child, otherwise reuse existing // 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) if (old_child && old_child->getName().toStdString() != prop.name)
old_child = nullptr; 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) if (new_child != old_child)
root->addChild(new_child, index); root->addChild(new_child, index);
++index; ++index;
@ -121,25 +124,26 @@ void RemoteTaskModel::Node::setProperties(const std::vector<moveit_task_construc
root->removeChildren(index, root->numChildren() - index); root->removeChildren(index, root->numChildren() - index);
} }
rviz::Property* RemoteTaskModel::Node::createProperty(const moveit_task_constructor_msgs::Property& prop, rviz_common::properties::Property* RemoteTaskModel::Node::createProperty(
rviz::Property* old, const moveit_task_constructor_msgs::msg::Property& prop, rviz_common::properties::Property* old,
const planning_scene::PlanningSceneConstPtr& scene_, const planning_scene::PlanningSceneConstPtr& scene_, rviz_common::DisplayContext* display_context_) {
rviz::DisplayContext* display_context_) {
auto& factory = PropertyFactory::instance(); auto& factory = PropertyFactory::instance();
// try to deserialize from msg (using registered functions) // try to deserialize from msg (using registered functions)
boost::any value = Property::deserialize(prop.type, prop.value); 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; auto it = properties_.insert(std::make_pair(prop.name, Property())).first;
it->second.setDescription(prop.description); it->second.setDescription(prop.description);
it->second.setValue(value); 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); rviz_prop->setReadOnly(true);
return rviz_prop; return rviz_prop;
} else } else
properties_.erase(it); 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); 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_; return root_;
if (index.model() != this) { if (index.model() != this) {
ROS_ERROR_NAMED("TaskModel", "invalid model in QModelIndex"); RCLCPP_ERROR(LOGGER, "invalid model in QModelIndex");
return nullptr; return nullptr;
} }
@ -180,13 +184,18 @@ QModelIndex RemoteTaskModel::index(const Node* n) const {
return QModelIndex(); return QModelIndex();
} }
RemoteTaskModel::RemoteTaskModel(ros::NodeHandle& nh, const std::string& service_name, RemoteTaskModel::RemoteTaskModel(const std::string& service_name, const planning_scene::PlanningSceneConstPtr& scene,
const planning_scene::PlanningSceneConstPtr& scene, rviz_common::DisplayContext* display_context, QObject* parent)
rviz::DisplayContext* display_context, QObject* parent)
: BaseTaskModel(scene, display_context, parent), root_(new Node(nullptr)) { : BaseTaskModel(scene, display_context, parent), root_(new Node(nullptr)) {
id_to_stage_[0] = root_; // root node has ID 0 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<std::size_t>(this)), "-r",
"__ns:=/moveit_task_constructor/remote_task_model" });
node_ = rclcpp::Node::make_shared("_", options);
// service to request solutions // service to request solutions
get_solution_client_ = nh.serviceClient<moveit_task_constructor_msgs::GetSolution>(service_name); get_solution_client_ = node_->create_client<moveit_task_constructor_msgs::srv::GetSolution>(service_name);
} }
RemoteTaskModel::~RemoteTaskModel() { RemoteTaskModel::~RemoteTaskModel() {
@ -277,13 +286,14 @@ QModelIndex RemoteTaskModel::indexFromStageId(size_t id) const {
return n ? index(n) : QModelIndex(); 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 // iterate over descriptions and create new / update existing nodes where needed
for (const auto& s : msg) { for (const auto& s : msg) {
// find parent node for stage s, this should always exist // find parent node for stage s, this should always exist
auto parent_it = id_to_stage_.find(s.parent_id); auto parent_it = id_to_stage_.find(s.parent_id);
if (parent_it == id_to_stage_.end()) { 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; continue;
} }
Node* parent = parent_it->second; 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 // iterate over statistics and update node's solutions where needed
for (const auto& s : msg) { for (const auto& s : msg) {
// find node for stage s, this should always exist // find node for stage s, this should always exist
auto it = id_to_stage_.find(s.id); auto it = id_to_stage_.find(s.id);
if (it == id_to_stage_.end()) { if (it == id_to_stage_.end()) {
ROS_ERROR_NAMED("TaskListModel", "No stage %d", s.id); RCLCPP_ERROR(LOGGER, "No stage %d", s.id);
continue; continue;
} }
Node* n = it->second; 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) if (info.id == 0)
return; return;
if (RemoteSolutionModel* m = getSolutionModel(info.stage_id)) if (RemoteSolutionModel* m = getSolutionModel(info.stage_id))
m->setSolutionData(info.id, info.cost, QString::fromStdString(info.comment)); 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); DisplaySolutionPtr s(new DisplaySolution);
s->setFromMessage(scene_->diff(), msg); s->setFromMessage(scene_->diff(), msg);
@ -416,18 +427,21 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex& index) {
if (it == id_to_solution_.cend()) { if (it == id_to_solution_.cend()) {
// TODO: try to assemble (and cache) the solution from known leaves // TODO: try to assemble (and cache) the solution from known leaves
// to avoid some communication overhead // to avoid some communication overhead
DisplaySolutionPtr result; DisplaySolutionPtr result;
if (!(flags_ & IS_DESTROYED)) { if (!(flags_ & IS_DESTROYED)) {
// request solution via service if (get_solution_client_->service_is_ready()) {
moveit_task_constructor_msgs::GetSolution srv; // request solution via service
srv.request.solution_id = id; auto request = std::make_shared<moveit_task_constructor_msgs::srv::GetSolution::Request>();
if (get_solution_client_.call(srv)) { request->solution_id = id;
id_to_solution_[id] = result = processSolutionMessage(srv.response.solution); auto result_future = get_solution_client_->async_send_request(request);
return result; 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 // 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; flags_ |= IS_DESTROYED;
} }
return result; return result;
@ -435,7 +449,7 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex& index) {
return it->second; return it->second;
} }
rviz::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex& index) { rviz_common::properties::PropertyTreeModel* RemoteTaskModel::getPropertyModel(const QModelIndex& index) {
Node* n = node(index); Node* n = node(index);
if (!n) if (!n)
return nullptr; return nullptr;

View File

@ -38,7 +38,8 @@
#include "task_list_model.h" #include "task_list_model.h"
#include <moveit/visualization_tools/display_solution.h> #include <moveit/visualization_tools/display_solution.h>
#include <ros/service_client.h> #include <moveit_task_constructor_msgs/srv/get_solution.hpp>
#include <rclcpp/client.hpp>
#include <memory> #include <memory>
#include <limits> #include <limits>
@ -54,7 +55,11 @@ class RemoteTaskModel : public BaseTaskModel
Q_OBJECT Q_OBJECT
struct Node; struct Node;
Node* const root_; Node* const root_;
ros::ServiceClient get_solution_client_; rclcpp::Client<moveit_task_constructor_msgs::srv::GetSolution>::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<uint32_t, Node*> id_to_stage_; std::map<uint32_t, Node*> id_to_stage_;
std::map<uint32_t, DisplaySolutionPtr> id_to_solution_; std::map<uint32_t, DisplaySolutionPtr> id_to_solution_;
@ -64,12 +69,11 @@ class RemoteTaskModel : public BaseTaskModel
Node* node(uint32_t stage_id) const; Node* node(uint32_t stage_id) const;
inline RemoteSolutionModel* getSolutionModel(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: public:
RemoteTaskModel(ros::NodeHandle& nh, const std::string& service_name, RemoteTaskModel(const std::string& service_name, const planning_scene::PlanningSceneConstPtr& scene,
const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context, rviz_common::DisplayContext* display_context, QObject* parent = nullptr);
QObject* parent = nullptr);
~RemoteTaskModel() override; ~RemoteTaskModel() override;
int rowCount(const QModelIndex& parent = QModelIndex()) const 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; bool setData(const QModelIndex& index, const QVariant& value, int role = Qt::EditRole) override;
QModelIndex indexFromStageId(size_t id) const override; QModelIndex indexFromStageId(size_t id) const override;
void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type& msg); void processStageDescriptions(const moveit_task_constructor_msgs::msg::TaskDescription::_stages_type& msg);
void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type& msg); void processStageStatistics(const moveit_task_constructor_msgs::msg::TaskStatistics::_stages_type& msg);
DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg); DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg);
QAbstractItemModel* getSolutionModel(const QModelIndex& index) override; QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
DisplaySolutionPtr getSolution(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 */ /** Model representing solutions of a remote task */

View File

@ -44,19 +44,22 @@
#include <moveit/visualization_tools/task_solution_visualization.h> #include <moveit/visualization_tools/task_solution_visualization.h>
#include <moveit/visualization_tools/marker_visualization.h> #include <moveit/visualization_tools/marker_visualization.h>
#include <moveit/visualization_tools/display_solution.h> #include <moveit/visualization_tools/display_solution.h>
#include <moveit_task_constructor_msgs/GetSolution.h> #include <moveit_task_constructor_msgs/srv/get_solution.hpp>
#include <moveit/rdf_loader/rdf_loader.h> #include <moveit/rdf_loader/rdf_loader.h>
#include <moveit/robot_model/robot_model.h> #include <moveit/robot_model/robot_model.h>
#include <rviz/display_context.h> #include <rviz_common/display_context.hpp>
#include <rviz/properties/string_property.h> #include <rviz_common/properties/string_property.hpp>
#include <rviz/properties/ros_topic_property.h> #include <rviz_common/properties/ros_topic_property.hpp>
#include <rviz/properties/status_property.h> #include <rviz_common/properties/status_property.hpp>
#include <rviz/frame_manager.h> #include <rviz_common/frame_manager_iface.hpp>
#include <rosidl_runtime_cpp/traits.hpp>
#include <OgreSceneNode.h> #include <OgreSceneNode.h>
#include <QTimer> #include <QTimer>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.task_display");
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
TaskDisplay::TaskDisplay() : Display(), panel_requested_(false), received_task_description_(false) { 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, connect(task_list_model_.get(), SIGNAL(dataChanged(QModelIndex, QModelIndex)), this,
SLOT(onTaskDataChanged(QModelIndex, QModelIndex))); 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", "Robot Description", "robot_description", "The name of the ROS parameter where the URDF for the robot is loaded",
this, SLOT(changedRobotDescription()), this); this, SLOT(changedRobotDescription()), this);
task_solution_topic_property_ = new rviz::RosTopicProperty( task_solution_topic_property_ = new rviz_common::properties::RosTopicProperty(
"Task Solution Topic", "", ros::message_traits::datatype<moveit_task_constructor_msgs::Solution>(), "Task Solution Topic", "", rosidl_generator_traits::data_type<moveit_task_constructor_msgs::msg::Solution>(),
"The topic on which task solutions (moveit_msgs::Solution messages) are received", this, "The topic on which task solutions (moveit_msgs::Solution messages) are received", this,
SLOT(changedTaskSolutionTopic()), 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(), connect(trajectory_visual_.get(), SIGNAL(activeStageChanged(size_t)), task_list_model_.get(),
SLOT(highlightStage(size_t))); 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() { TaskDisplay::~TaskDisplay() {
@ -94,6 +98,8 @@ TaskDisplay::~TaskDisplay() {
void TaskDisplay::onInitialize() { void TaskDisplay::onInitialize() {
Display::onInitialize(); Display::onInitialize();
rviz_ros_node_ = context_->getRosNodeAbstraction();
task_solution_topic_property_->initialize(rviz_ros_node_);
trajectory_visual_->onInitialize(scene_node_, context_); trajectory_visual_->onInitialize(scene_node_, context_);
task_list_model_->setDisplayContext(context_); task_list_model_->setDisplayContext(context_);
} }
@ -109,18 +115,19 @@ inline void TaskDisplay::requestPanel() {
} }
void TaskDisplay::loadRobotModel() { 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()) { 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()); "Failed to load from parameter " + robot_description_property_->getString());
return; 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 = const srdf::ModelSharedPtr& srdf =
rdf_loader_->getSRDF() ? rdf_loader_->getSRDF() : srdf::ModelSharedPtr(new srdf::Model()); 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 // Send to child class
trajectory_visual_->onRobotModelLoaded(robot_model_); trajectory_visual_->onRobotModelLoaded(robot_model_);
@ -139,11 +146,11 @@ void TaskDisplay::reset() {
trajectory_visual_->reset(); trajectory_visual_->reset();
} }
void TaskDisplay::save(rviz::Config config) const { void TaskDisplay::save(rviz_common::Config config) const {
Display::save(config); Display::save(config);
} }
void TaskDisplay::load(const rviz::Config& config) { void TaskDisplay::load(const rviz_common::Config& config) {
Display::load(config); Display::load(config);
} }
@ -170,7 +177,8 @@ void TaskDisplay::calculateOffsetPosition() {
Ogre::Vector3 position; Ogre::Vector3 position;
Ogre::Quaternion orientation; 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_->setPosition(position);
scene_node_->setOrientation(orientation); scene_node_->setOrientation(orientation);
@ -183,11 +191,6 @@ void TaskDisplay::update(float wall_dt, float ros_dt) {
trajectory_visual_->update(wall_dt, 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() { void TaskDisplay::changedRobotDescription() {
if (isEnabled()) if (isEnabled())
reset(); reset();
@ -195,28 +198,37 @@ void TaskDisplay::changedRobotDescription() {
loadRobotModel(); loadRobotModel();
} }
void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg) { void TaskDisplay::taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr msg) {
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK");
requestPanel(); requestPanel();
task_list_model_->processTaskDescriptionMessage(*msg, update_nh_, task_list_model_->processTaskDescriptionMessage(*msg, base_ns_ + GET_SOLUTION_SERVICE "_" + msg->task_id);
base_ns_ + GET_SOLUTION_SERVICE "_" + msg->task_id);
// Start listening to other topics if this is the first description // 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 // Waiting for the description ensures we do not receive data that cannot be interpreted yet
if (!received_task_description_ && !msg->stages.empty()) { 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; received_task_description_ = true;
task_statistics_sub = update_nh_.subscribe(base_ns_ + STATISTICS_TOPIC, 2, &TaskDisplay::taskStatisticsCB, this); task_statistics_sub = node->create_subscription<moveit_task_constructor_msgs::msg::TaskStatistics>(
task_solution_sub = update_nh_.subscribe(base_ns_ + SOLUTION_TOPIC, 2, &TaskDisplay::taskSolutionCB, this); base_ns_ + STATISTICS_TOPIC, rclcpp::QoS(2).transient_local(),
std::bind(&TaskDisplay::taskStatisticsCB, this, std::placeholders::_1));
task_solution_sub = node->create_subscription<moveit_task_constructor_msgs::msg::Solution>(
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) { void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr msg) {
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK");
task_list_model_->processTaskStatisticsMessage(*msg); task_list_model_->processTaskStatisticsMessage(*msg);
} }
void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg) { void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr msg) {
setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK"); setStatus(rviz_common::properties::StatusProperty::Ok, "Task Monitor", "OK");
try { try {
const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(*msg); const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(*msg);
if (s) if (s)
@ -224,7 +236,7 @@ void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionCon
else else
setSolutionStatus(false); setSolutionStatus(false);
} catch (const std::invalid_argument& e) { } catch (const std::invalid_argument& e) {
ROS_ERROR_STREAM(e.what()); RCLCPP_ERROR_STREAM(LOGGER, e.what());
setSolutionStatus(false, e.what()); setSolutionStatus(false, e.what());
} }
} }
@ -234,33 +246,41 @@ void TaskDisplay::changedTaskSolutionTopic() {
if (!trajectory_visual_->getScene()) if (!trajectory_visual_->getScene())
return; return;
task_description_sub.shutdown(); task_description_sub.reset();
task_statistics_sub.shutdown(); task_statistics_sub.reset();
task_solution_sub.shutdown(); task_solution_sub.reset();
received_task_description_ = false; received_task_description_ = false;
// generate task monitoring topics from solution topic // generate task monitoring topics from solution topic
const QString& solution_topic = task_solution_topic_property_->getString(); const QString& solution_topic = task_solution_topic_property_->getString();
if (!solution_topic.endsWith(QString("/").append(SOLUTION_TOPIC))) { 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)); QString("Invalid topic. Expecting a name ending on \"/%1\"").arg(SOLUTION_TOPIC));
return; return;
} }
base_ns_ = solution_topic.toStdString().substr(0, solution_topic.length() - strlen(SOLUTION_TOPIC)); 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 // 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<moveit_task_constructor_msgs::msg::TaskDescription>(
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) { void TaskDisplay::setSolutionStatus(bool ok, const char* msg) {
if (ok) if (ok)
setStatus(rviz::StatusProperty::Ok, "Solution", "Ok"); setStatus(rviz_common::properties::StatusProperty::Ok, "Solution", "Ok");
else 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) { 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<TaskListModel*>(sender()); TaskListModel* m = static_cast<TaskListModel*>(sender());
for (; first <= last; ++first) { for (; first <= last; ++first) {
QModelIndex idx = m->index(first, 0, parent); 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 return; // only handle top-level items
for (int row = topLeft.row(); row <= bottomRight.row(); ++row) { 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); assert(child);
if (topLeft.column() <= 0 && 0 <= bottomRight.column()) // name changed if (topLeft.column() <= 0 && 0 <= bottomRight.column()) // name changed

View File

@ -38,22 +38,26 @@
#pragma once #pragma once
#include <rviz/display.h> #include <rviz_common/display.hpp>
#include <rviz_common/ros_integration/ros_client_abstraction_iface.hpp>
#include <moveit/visualization_tools/task_solution_visualization.h> #include <moveit/visualization_tools/task_solution_visualization.h>
#ifndef Q_MOC_RUN #ifndef Q_MOC_RUN
#include "job_queue.h" #include "job_queue.h"
#include "local_task_model.h"
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <ros/subscriber.h> #include <rclcpp/subscription.hpp>
#include <moveit_task_constructor_msgs/TaskDescription.h> #include <moveit_task_constructor_msgs/msg/task_description.hpp>
#include <moveit_task_constructor_msgs/TaskStatistics.h> #include <moveit_task_constructor_msgs/msg/task_statistics.hpp>
#include <moveit_task_constructor_msgs/Solution.h> #include <moveit_task_constructor_msgs/msg/solution.hpp>
#endif #endif
namespace rviz { namespace rviz_common {
namespace properties {
class StringProperty; class StringProperty;
class RosTopicProperty; class RosTopicProperty;
} // namespace rviz } // namespace properties
} // namespace rviz_common
namespace moveit { namespace moveit {
namespace core { namespace core {
@ -69,7 +73,7 @@ namespace moveit_rviz_plugin {
MOVEIT_CLASS_FORWARD(DisplaySolution); MOVEIT_CLASS_FORWARD(DisplaySolution);
class TaskListModel; class TaskListModel;
class TaskDisplay : public rviz::Display class TaskDisplay : public rviz_common::Display
{ {
Q_OBJECT Q_OBJECT
@ -81,10 +85,9 @@ public:
void update(float wall_dt, float ros_dt) override; void update(float wall_dt, float ros_dt) override;
void reset() override; void reset() override;
void save(rviz::Config config) const override; void save(rviz_common::Config config) const override;
void load(const rviz::Config& config) override; void load(const rviz_common::Config& config) override;
void setName(const QString& name) override;
void setSolutionStatus(bool ok, const char* msg = ""); void setSolutionStatus(bool ok, const char* msg = "");
TaskListModel& getTaskListModel() { return *task_list_model_; } TaskListModel& getTaskListModel() { return *task_list_model_; }
@ -113,14 +116,19 @@ private Q_SLOTS:
void onTasksRemoved(const QModelIndex& parent, int first, int last); void onTasksRemoved(const QModelIndex& parent, int first, int last);
void onTaskDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight); void onTaskDataChanged(const QModelIndex& topLeft, const QModelIndex& bottomRight);
void taskDescriptionCB(const moveit_task_constructor_msgs::TaskDescriptionConstPtr& msg); void taskDescriptionCB(const moveit_task_constructor_msgs::msg::TaskDescription::ConstSharedPtr msg);
void taskStatisticsCB(const moveit_task_constructor_msgs::TaskStatisticsConstPtr& msg); void taskStatisticsCB(const moveit_task_constructor_msgs::msg::TaskStatistics::ConstSharedPtr msg);
void taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg); void taskSolutionCB(const moveit_task_constructor_msgs::msg::Solution::ConstSharedPtr msg);
protected: protected:
ros::Subscriber task_solution_sub; /** @brief A Node which is registered with the main executor (used in the "update" thread).
ros::Subscriber task_description_sub; *
ros::Subscriber task_statistics_sub; * This is configured after the constructor within the initialize() method of Display. */
rviz_common::ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node_;
rclcpp::Subscription<moveit_task_constructor_msgs::msg::Solution>::SharedPtr task_solution_sub;
rclcpp::Subscription<moveit_task_constructor_msgs::msg::TaskDescription>::SharedPtr task_description_sub;
rclcpp::Subscription<moveit_task_constructor_msgs::msg::TaskStatistics>::SharedPtr task_statistics_sub;
// The trajectory playback component // The trajectory playback component
std::unique_ptr<TaskSolutionVisualization> trajectory_visual_; std::unique_ptr<TaskSolutionVisualization> trajectory_visual_;
@ -138,9 +146,9 @@ protected:
bool received_task_description_; bool received_task_description_;
// Properties // Properties
rviz::StringProperty* robot_description_property_; rviz_common::properties::StringProperty* robot_description_property_;
rviz::RosTopicProperty* task_solution_topic_property_; rviz_common::properties::RosTopicProperty* task_solution_topic_property_;
rviz::Property* tasks_property_; rviz_common::properties::Property* tasks_property_;
}; };
} // namespace moveit_rviz_plugin } // namespace moveit_rviz_plugin

View File

@ -42,8 +42,6 @@
#include "factory_model.h" #include "factory_model.h"
#include "icons.h" #include "icons.h"
#include <ros/console.h>
#include <QMimeData> #include <QMimeData>
#include <QHeaderView> #include <QHeaderView>
#include <QScrollBar> #include <QScrollBar>
@ -54,7 +52,7 @@ using namespace moveit::task_constructor;
namespace moveit_rviz_plugin { 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) { QVariant TaskListModel::horizontalHeader(int column, int role) {
switch (role) { switch (role) {
@ -145,7 +143,7 @@ StageFactoryPtr getStageFactory() {
factory = result; // remember for future uses factory = result; // remember for future uses
return result; return result;
} catch (const std::exception& e) { } catch (const std::exception& e) {
ROS_ERROR("Failed to initialize StageFactory"); RCLCPP_ERROR(LOGGER, "Failed to initialize StageFactory");
return StageFactoryPtr(); return StageFactoryPtr();
} }
} }
@ -164,12 +162,12 @@ void TaskListModel::onRemoveModel(QAbstractItemModel* model) {
TaskListModel::TaskListModel(QObject* parent) TaskListModel::TaskListModel(QObject* parent)
: FlatMergeProxyModel(parent), old_task_handling_(TaskView::OLD_TASK_REPLACE) { : 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()); setStageFactory(getStageFactory());
} }
TaskListModel::~TaskListModel() { 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 // inform MetaTaskListModel that we will remove our stuff
removeRows(0, rowCount()); removeRows(0, rowCount());
// free RemoteTaskModels // free RemoteTaskModels
@ -181,7 +179,7 @@ void TaskListModel::setScene(const planning_scene::PlanningSceneConstPtr& scene)
scene_ = scene; scene_ = scene;
} }
void TaskListModel::setDisplayContext(rviz::DisplayContext* display_context) { void TaskListModel::setDisplayContext(rviz_common::DisplayContext* display_context) {
display_context_ = display_context; display_context_ = display_context;
} }
@ -241,8 +239,8 @@ QVariant TaskListModel::data(const QModelIndex& index, int role) const {
// process a task description message: // process a task description message:
// update existing RemoteTask, create a new one, or (if msg.stages is empty) delete an existing one // 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, void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_msgs::msg::TaskDescription& msg,
ros::NodeHandle& nh, const std::string& service_name) { const std::string& service_name) {
// retrieve existing or insert new remote task for given task id // retrieve existing or insert new remote task for given task id
auto it_inserted = remote_tasks_.insert(std::make_pair(msg.task_id, nullptr)); auto it_inserted = remote_tasks_.insert(std::make_pair(msg.task_id, nullptr));
const auto& task_it = it_inserted.first; const auto& task_it = it_inserted.first;
@ -267,9 +265,9 @@ void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_
remote_task->processStageDescriptions(msg.stages); remote_task->processStageDescriptions(msg.stages);
} else if (!remote_task) { // create new task model, if ID was not known before } 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 // 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); 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 // insert newly created model into this' model instance
insertModel(remote_task, -1); insertModel(remote_task, -1);
@ -280,10 +278,10 @@ void TaskListModel::processTaskDescriptionMessage(const moveit_task_constructor_
} }
// process a task statistics message // 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); auto it = remote_tasks_.find(msg.task_id);
if (it == remote_tasks_.cend()) { 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; return;
} }
@ -294,7 +292,7 @@ void TaskListModel::processTaskStatisticsMessage(const moveit_task_constructor_m
remote_task->processStageStatistics(msg.stages); 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); auto it = remote_tasks_.find(msg.task_id);
if (it == remote_tasks_.cend()) if (it == remote_tasks_.cend())
return DisplaySolutionPtr(); // unkown task return DisplaySolutionPtr(); // unkown task

View File

@ -42,10 +42,10 @@
#include <utils/flat_merge_proxy_model.h> #include <utils/flat_merge_proxy_model.h>
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <ros/node_handle.h> #include <rclcpp/node.hpp>
#include <moveit_task_constructor_msgs/TaskDescription.h> #include <moveit_task_constructor_msgs/msg/task_description.hpp>
#include <moveit_task_constructor_msgs/TaskStatistics.h> #include <moveit_task_constructor_msgs/msg/task_statistics.hpp>
#include <moveit_task_constructor_msgs/Solution.h> #include <moveit_task_constructor_msgs/msg/solution.hpp>
#include <QAbstractItemModel> #include <QAbstractItemModel>
#include <QTreeView> #include <QTreeView>
@ -53,10 +53,12 @@
#include <memory> #include <memory>
#include <QPointer> #include <QPointer>
namespace rviz { namespace rviz_common {
namespace properties {
class PropertyTreeModel; class PropertyTreeModel;
}
class DisplayContext; class DisplayContext;
} // namespace rviz } // namespace rviz_common
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
@ -74,7 +76,7 @@ class BaseTaskModel : public QAbstractItemModel
protected: protected:
unsigned int flags_ = 0; unsigned int flags_ = 0;
planning_scene::PlanningSceneConstPtr scene_; planning_scene::PlanningSceneConstPtr scene_;
rviz::DisplayContext* display_context_; rviz_common::DisplayContext* display_context_;
public: public:
enum TaskModelFlag enum TaskModelFlag
@ -85,7 +87,7 @@ public:
IS_RUNNING = 0x08, 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) QObject* parent = nullptr)
: QAbstractItemModel(parent), scene_(scene), display_context_(display_context) {} : QAbstractItemModel(parent), scene_(scene), display_context_(display_context) {}
@ -106,7 +108,7 @@ public:
virtual DisplaySolutionPtr getSolution(const QModelIndex& index) = 0; virtual DisplaySolutionPtr getSolution(const QModelIndex& index) = 0;
/// get property model for given stage index /// 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. /** 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 / robot model used by all tasks in this model
planning_scene::PlanningSceneConstPtr scene_; planning_scene::PlanningSceneConstPtr scene_;
// rviz::DisplayContext used to show (interactive) markers by the property models // 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 // map from remote task IDs to (active) tasks
// if task is destroyed remotely, it is marked with flag IS_DESTROYED // if task is destroyed remotely, it is marked with flag IS_DESTROYED
@ -146,7 +148,7 @@ public:
~TaskListModel() override; ~TaskListModel() override;
void setScene(const planning_scene::PlanningSceneConstPtr& scene); 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; } void setActiveTaskModel(BaseTaskModel* model) { active_task_model_ = model; }
int columnCount(const QModelIndex& /*parent*/ = QModelIndex()) const override { return 4; } int columnCount(const QModelIndex& /*parent*/ = QModelIndex()) const override { return 4; }
@ -155,12 +157,12 @@ public:
QVariant data(const QModelIndex& index, int role) const override; QVariant data(const QModelIndex& index, int role) const override;
/// process an incoming task description message - only call in Qt's main loop /// 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); const std::string& service_name);
/// process an incoming task description message - only call in Qt's main loop /// 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 /// 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() /// insert a TaskModel, pos is relative to modelCount()
bool insertModel(BaseTaskModel* model, int pos = -1); bool insertModel(BaseTaskModel* model, int pos = -1);

View File

@ -48,21 +48,25 @@
#include <moveit/visualization_tools/display_solution.h> #include <moveit/visualization_tools/display_solution.h>
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <rviz/properties/property.h> #include <rviz_common/properties/property.hpp>
#include <rviz/properties/enum_property.h> #include <rviz_common/properties/enum_property.hpp>
#include <rviz/display_group.h> #include <rviz_common/properties/property_tree_model.hpp>
#include <rviz/visualization_manager.h> #include <rviz_common/display_group.hpp>
#include <rviz/window_manager_interface.h> #include <rviz_common/visualization_manager.hpp>
#include <rviz/visualization_frame.h> #include <rviz_common/window_manager_interface.hpp>
#include <rviz/panel_dock_widget.h> #include <rviz_common/visualization_frame.hpp>
#include <ros/console.h> #include <rviz_common/panel_dock_widget.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <QPointer> #include <QPointer>
#include <QButtonGroup> #include <QButtonGroup>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.task_view");
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
rviz::PanelDockWidget* getStageDockWidget(rviz::WindowManagerInterface* mgr) { rviz_common::PanelDockWidget* getStageDockWidget(rviz_common::WindowManagerInterface* mgr) {
static QPointer<rviz::PanelDockWidget> widget = nullptr; static QPointer<rviz_common::PanelDockWidget> widget = nullptr;
if (!widget && mgr) { // create widget if (!widget && mgr) { // create widget
StageFactoryPtr factory = getStageFactory(); StageFactoryPtr factory = getStageFactory();
if (!factory) if (!factory)
@ -83,7 +87,7 @@ static QPointer<TaskPanel> SINGLETON;
// count active TaskDisplays // count active TaskDisplays
static uint DISPLAY_COUNT = 0; 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); Q_D(TaskPanel);
// sync checked tool button with displayed widget // 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... * 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; ++DISPLAY_COUNT;
rviz::VisualizationFrame* vis_frame = dynamic_cast<rviz::VisualizationFrame*>(window_manager); rviz_common::VisualizationFrame* vis_frame = dynamic_cast<rviz_common::VisualizationFrame*>(window_manager);
if (SINGLETON || !vis_frame) if (SINGLETON || !vis_frame)
return; // already defined, nothing to do return; // already defined, nothing to do
@ -173,23 +177,23 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel* panel) : q_ptr(panel) {
tool_buttons_group->setExclusive(true); tool_buttons_group->setExclusive(true);
button_show_stage_dock_widget->setEnabled(bool(getStageFactory())); button_show_stage_dock_widget->setEnabled(bool(getStageFactory()));
button_show_stage_dock_widget->setToolTip(QStringLiteral("Show available stages")); 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() { void TaskPanel::onInitialize() {
d_ptr->window_manager_ = vis_manager_->getWindowManager(); d_ptr->window_manager_ = getDisplayContext()->getWindowManager();
} }
void TaskPanel::save(rviz::Config config) const { void TaskPanel::save(rviz_common::Config config) const {
rviz::Panel::save(config); rviz_common::Panel::save(config);
for (int i = 0; i < d_ptr->stackedWidget->count(); ++i) { for (int i = 0; i < d_ptr->stackedWidget->count(); ++i) {
SubPanel* w = static_cast<SubPanel*>(d_ptr->stackedWidget->widget(i)); SubPanel* w = static_cast<SubPanel*>(d_ptr->stackedWidget->widget(i));
w->save(config.mapMakeChild(w->windowTitle())); w->save(config.mapMakeChild(w->windowTitle()));
} }
} }
void TaskPanel::load(const rviz::Config& config) { void TaskPanel::load(const rviz_common::Config& config) {
rviz::Panel::load(config); rviz_common::Panel::load(config);
for (int i = 0; i < d_ptr->stackedWidget->count(); ++i) { for (int i = 0; i < d_ptr->stackedWidget->count(); ++i) {
SubPanel* w = static_cast<SubPanel*>(d_ptr->stackedWidget->widget(i)); SubPanel* w = static_cast<SubPanel*>(d_ptr->stackedWidget->widget(i));
w->load(config.mapGetChild(w->windowTitle())); w->load(config.mapGetChild(w->windowTitle()));
@ -197,7 +201,7 @@ void TaskPanel::load(const rviz::Config& config) {
} }
void TaskPanel::showStageDockWidget() { void TaskPanel::showStageDockWidget() {
rviz::PanelDockWidget* dock = getStageDockWidget(d_ptr->window_manager_); rviz_common::PanelDockWidget* dock = getStageDockWidget(d_ptr->window_manager_);
if (dock) if (dock)
dock->show(); dock->show();
} }
@ -216,9 +220,15 @@ void setExpanded(QTreeView* view, const QModelIndex& index, bool expand, int dep
view->setExpanded(index, expand); 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); 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<moveit_task_constructor_msgs::action::ExecuteTaskSolution>(
node_, "execute_task_solution");
MetaTaskListModel* meta_model = &MetaTaskListModel::instance(); MetaTaskListModel* meta_model = &MetaTaskListModel::instance();
StageFactoryPtr factory = getStageFactory(); StageFactoryPtr factory = getStageFactory();
if (factory) if (factory)
@ -292,7 +302,7 @@ void TaskViewPrivate::lock(TaskDisplay* display) {
locked_display_ = 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)) { : SubPanel(parent), d_ptr(new TaskViewPrivate(this)) {
Q_D(TaskView); Q_D(TaskView);
@ -318,23 +328,24 @@ TaskView::TaskView(moveit_rviz_plugin::TaskPanel* parent, rviz::Property* root)
SIGNAL(configChanged())); SIGNAL(configChanged()));
// configuration settings // configuration settings
auto configs = new rviz::Property("Task View Settings", QVariant(), QString(), root); auto configs = new rviz_common::properties::Property("Task View Settings", QVariant(), QString(), root);
initial_task_expand = initial_task_expand = new rviz_common::properties::EnumProperty(
new rviz::EnumProperty("Task Expansion", "All Expanded", "Configure how to initially expand new tasks", configs); "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("Top-level Expanded", EXPAND_TOP);
initial_task_expand->addOption("All Expanded", EXPAND_ALL); initial_task_expand->addOption("All Expanded", EXPAND_ALL);
initial_task_expand->addOption("All Closed", EXPAND_NONE); initial_task_expand->addOption("All Closed", EXPAND_NONE);
old_task_handling = old_task_handling = new rviz_common::properties::EnumProperty(
new rviz::EnumProperty("Old task handling", "Keep", "Old task handling", "Keep", "Configure what to do with old tasks whose solutions cannot be queried anymore",
"Configure what to do with old tasks whose solutions cannot be queried anymore", configs); configs);
old_task_handling->addOption("Keep", OLD_TASK_KEEP); old_task_handling->addOption("Keep", OLD_TASK_KEEP);
old_task_handling->addOption("Replace", OLD_TASK_REPLACE); old_task_handling->addOption("Replace", OLD_TASK_REPLACE);
old_task_handling->addOption("Remove", OLD_TASK_REMOVE); 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); show_time_column =
connect(show_time_column, &rviz::Property::changed, this, &TaskView::onShowTimeChanged); 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(); d_ptr->configureExistingModels();
} }
@ -343,11 +354,11 @@ TaskView::~TaskView() {
delete d_ptr; 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) { 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()) { for (int s : splitter->sizes()) {
rviz::Config item = group.listAppendNew(); rviz_common::Config item = group.listAppendNew();
item.setValue(s); item.setValue(s);
} }
}; };
@ -355,9 +366,9 @@ void TaskView::save(rviz::Config config) {
write_splitter_sizes(d_ptr->tasks_solutions_splitter, "solutions_splitter"); write_splitter_sizes(d_ptr->tasks_solutions_splitter, "solutions_splitter");
auto write_column_sizes = [&config](QHeaderView* view, const QString& key) { 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) { 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)); 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"); write_column_sizes(d_ptr->solutions_view->header(), "solutions_view_columns");
const QHeaderView* view = d_ptr->solutions_view->header(); 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("column", view->sortIndicatorSection());
group.mapSetValue("order", view->sortIndicatorOrder()); group.mapSetValue("order", view->sortIndicatorOrder());
} }
void TaskView::load(const rviz::Config& config) { void TaskView::load(const rviz_common::Config& config) {
if (!config.isValid()) if (!config.isValid())
return; return;
auto read_sizes = [&config](const QString& key) { auto read_sizes = [&config](const QString& key) {
rviz::Config group = config.mapGetChild(key); rviz_common::Config group = config.mapGetChild(key);
QList<int> sizes, empty; QList<int> sizes, empty;
for (int i = 0; i < group.listLength(); ++i) { for (int i = 0; i < group.listLength(); ++i) {
rviz::Config item = group.listChildAt(i); rviz_common::Config item = group.listChildAt(i);
if (item.getType() != rviz::Config::Value) if (item.getType() != rviz_common::Config::Value)
return empty; return empty;
QVariant value = item.getValue(); QVariant value = item.getValue();
bool ok = false; bool ok = false;
@ -401,7 +412,7 @@ void TaskView::load(const rviz::Config& config) {
d_ptr->tasks_view->setColumnWidth(++column, w); d_ptr->tasks_view->setColumnWidth(++column, w);
QTreeView* view = d_ptr->solutions_view; QTreeView* view = d_ptr->solutions_view;
rviz::Config group = config.mapGetChild("solution_sorting"); rviz_common::Config group = config.mapGetChild("solution_sorting");
int order = 0; int order = 0;
if (group.mapGetInt("column", &column) && group.mapGetInt("order", &order)) if (group.mapGetInt("column", &column) && group.mapGetInt("order", &order))
view->sortByColumn(column, static_cast<Qt::SortOrder>(order)); view->sortByColumn(column, static_cast<Qt::SortOrder>(order));
@ -488,7 +499,7 @@ void TaskView::onCurrentSolutionChanged(const QModelIndex& current, const QModel
solution = task->getSolution(current); solution = task->getSolution(current);
display->setSolutionStatus(bool(solution)); display->setSolutionStatus(bool(solution));
} catch (const std::invalid_argument& e) { } catch (const std::invalid_argument& e) {
ROS_ERROR_STREAM(e.what()); RCLCPP_ERROR_STREAM(LOGGER, e.what());
display->setSolutionStatus(false, e.what()); display->setSolutionStatus(false, e.what());
} }
vis->interruptCurrentDisplay(); vis->interruptCurrentDisplay();
@ -511,7 +522,7 @@ void TaskView::onSolutionSelectionChanged(const QItemSelection& /*selected*/, co
solution = task->getSolution(index); solution = task->getSolution(index);
display->setSolutionStatus(bool(solution)); display->setSolutionStatus(bool(solution));
} catch (const std::invalid_argument& e) { } catch (const std::invalid_argument& e) {
ROS_ERROR_STREAM(e.what()); RCLCPP_ERROR_STREAM(LOGGER, e.what());
display->setSolutionStatus(false, e.what()); display->setSolutionStatus(false, e.what());
} }
display->addMarkers(solution); display->addMarkers(solution);
@ -528,14 +539,21 @@ void TaskView::onExecCurrentSolution() const {
const DisplaySolutionPtr& solution = task->getSolution(current); const DisplaySolutionPtr& solution = task->getSolution(current);
if (!d_ptr->exec_action_client_.waitForServer(ros::Duration(0.1))) { if (!d_ptr->exec_action_client_->wait_for_action_server(std::chrono::milliseconds(100))) {
ROS_ERROR("Failed to connect to task execution action"); RCLCPP_ERROR(LOGGER, "Failed to connect to task execution action");
return; return;
} }
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal; moveit_task_constructor_msgs::action::ExecuteTaskSolution::Goal goal;
solution->fillMessage(goal.solution); 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() { void TaskView::onShowTimeChanged() {
@ -550,30 +568,33 @@ void TaskView::onOldTaskHandlingChanged() {
Q_EMIT oldTaskHandlingChanged(old_task_handling->getOptionInt()); 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) { : q_ptr(widget) {
setupUi(widget); setupUi(widget);
properties = new rviz::PropertyTreeModel(root, widget); properties = new rviz_common::properties::PropertyTreeModel(root, widget);
view->setModel(properties); 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)) { : SubPanel(parent), d_ptr(new GlobalSettingsWidgetPrivate(this, root)) {
Q_D(GlobalSettingsWidget); Q_D(GlobalSettingsWidget);
d->view->expandAll(); 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() { GlobalSettingsWidget::~GlobalSettingsWidget() {
delete d_ptr; delete d_ptr;
} }
void GlobalSettingsWidget::save(rviz::Config config) { void GlobalSettingsWidget::save(rviz_common::Config config) {
d_ptr->properties->getRoot()->save(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); d_ptr->properties->getRoot()->load(config);
} }
} // namespace moveit_rviz_plugin } // namespace moveit_rviz_plugin

View File

@ -38,18 +38,21 @@
#pragma once #pragma once
#include <rviz/panel.h> #include <rviz_common/panel.hpp>
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <QModelIndex> #include <QModelIndex>
class QItemSelection; class QItemSelection;
class QIcon; class QIcon;
namespace rviz { namespace rviz_common {
class WindowManagerInterface; class WindowManagerInterface;
class VisualizationManager;
namespace properties {
class Property; class Property;
class BoolProperty; class BoolProperty;
class EnumProperty; class EnumProperty;
} // namespace rviz } // namespace properties
} // namespace rviz_common
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
@ -64,16 +67,15 @@ class SubPanel : public QWidget
public: public:
SubPanel(QWidget* parent = nullptr) : QWidget(parent) {} SubPanel(QWidget* parent = nullptr) : QWidget(parent) {}
virtual void save(rviz::Config /*config*/) {} // NOLINT(performance-unnecessary-value-param) virtual void save(rviz_common::Config /*config*/) {} // NOLINT(performance-unnecessary-value-param)
virtual void load(const rviz::Config& /*config*/) {} virtual void load(const rviz_common::Config& /*config*/) {}
Q_SIGNALS: Q_SIGNALS:
void configChanged(); void configChanged();
}; };
/** The TaskPanel is the central panel of this plugin, collecting various sub panels. */ /** The TaskPanel is the central panel of this plugin, collecting various sub panels. */
class TaskPanelPrivate; class TaskPanelPrivate;
class TaskPanel : public rviz::Panel class TaskPanel : public rviz_common::Panel
{ {
Q_OBJECT Q_OBJECT
Q_DECLARE_PRIVATE(TaskPanel) Q_DECLARE_PRIVATE(TaskPanel)
@ -91,12 +93,12 @@ public:
* If not yet done, an instance is created. If use count drops to zero, * If not yet done, an instance is created. If use count drops to zero,
* the global instance is destroyed. * the global instance is destroyed.
*/ */
static void request(rviz::WindowManagerInterface* window_manager); static void request(rviz_common::WindowManagerInterface* window_manager);
static void release(); static void release();
void onInitialize() override; void onInitialize() override;
void load(const rviz::Config& config) override; void load(const rviz_common::Config& config) override;
void save(rviz::Config config) const override; void save(rviz_common::Config config) const override;
protected Q_SLOTS: protected Q_SLOTS:
void showStageDockWidget(); void showStageDockWidget();
@ -125,9 +127,9 @@ protected:
EXPAND_NONE EXPAND_NONE
}; };
rviz::EnumProperty* initial_task_expand; rviz_common::properties::EnumProperty* initial_task_expand;
rviz::EnumProperty* old_task_handling; rviz_common::properties::EnumProperty* old_task_handling;
rviz::BoolProperty* show_time_column; rviz_common::properties::BoolProperty* show_time_column;
public: public:
enum OldTaskHandling enum OldTaskHandling
@ -137,11 +139,11 @@ public:
OLD_TASK_REMOVE OLD_TASK_REMOVE
}; };
TaskView(TaskPanel* parent, rviz::Property* root); TaskView(TaskPanel* parent, rviz_common::properties::Property* root);
~TaskView() override; ~TaskView() override;
void save(rviz::Config config) override; void save(rviz_common::Config config) override;
void load(const rviz::Config& config) override; void load(const rviz_common::Config& config) override;
public Q_SLOTS: public Q_SLOTS:
void addTask(); void addTask();
@ -170,10 +172,10 @@ class GlobalSettingsWidget : public SubPanel
GlobalSettingsWidgetPrivate* d_ptr; GlobalSettingsWidgetPrivate* d_ptr;
public: public:
GlobalSettingsWidget(TaskPanel* parent, rviz::Property* root); GlobalSettingsWidget(TaskPanel* parent, rviz_common::properties::Property* root);
~GlobalSettingsWidget() override; ~GlobalSettingsWidget() override;
void save(rviz::Config config) override; void save(rviz_common::Config config) override;
void load(const rviz::Config& config) override; void load(const rviz_common::Config& config) override;
}; };
} // namespace moveit_rviz_plugin } // namespace moveit_rviz_plugin

View File

@ -42,11 +42,11 @@
#include "ui_task_panel.h" #include "ui_task_panel.h"
#include "ui_task_view.h" #include "ui_task_view.h"
#include "ui_global_settings.h" #include "ui_global_settings.h"
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h> #include <moveit_task_constructor_msgs/action/execute_task_solution.hpp>
#include <actionlib/client/simple_action_client.h> #include <rclcpp_action/client.hpp>
#include <rviz/panel.h> #include <rviz_common/panel.hpp>
#include <rviz/properties/property_tree_model.h> #include <rviz_common/properties/property_tree_model.hpp>
#include <QPointer> #include <QPointer>
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
@ -62,9 +62,9 @@ public:
TaskPanel* q_ptr; TaskPanel* q_ptr;
QButtonGroup* tool_buttons_group; 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 class TaskViewPrivate : public Ui_TaskView
@ -91,15 +91,16 @@ public:
TaskView* q_ptr; TaskView* q_ptr;
QPointer<TaskDisplay> locked_display_; QPointer<TaskDisplay> locked_display_;
actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> exec_action_client_; rclcpp::Node::SharedPtr node_;
rclcpp_action::Client<moveit_task_constructor_msgs::action::ExecuteTaskSolution>::SharedPtr exec_action_client_;
}; };
class GlobalSettingsWidgetPrivate : public Ui_GlobalSettingsWidget class GlobalSettingsWidgetPrivate : public Ui_GlobalSettingsWidget
{ {
public: public:
GlobalSettingsWidgetPrivate(GlobalSettingsWidget* q_ptr, rviz::Property* root); GlobalSettingsWidgetPrivate(GlobalSettingsWidget* q_ptr, rviz_common::properties::Property* root);
GlobalSettingsWidget* q_ptr; GlobalSettingsWidget* q_ptr;
rviz::PropertyTreeModel* properties; rviz_common::properties::PropertyTreeModel* properties;
}; };
} // namespace moveit_rviz_plugin } // namespace moveit_rviz_plugin

View File

@ -129,7 +129,7 @@
</widget> </widget>
</item> </item>
<item> <item>
<widget class="rviz::PropertyTreeWidget" name="property_view"/> <widget class="rviz_common::properties::PropertyTreeWidget" name="property_view"/>
</item> </item>
</layout> </layout>
</widget> </widget>
@ -174,9 +174,9 @@
<header>task_list_model.h</header> <header>task_list_model.h</header>
</customwidget> </customwidget>
<customwidget> <customwidget>
<class>rviz::PropertyTreeWidget</class> <class>rviz_common::properties::PropertyTreeWidget</class>
<extends>QTreeView</extends> <extends>QTreeView</extends>
<header location="global">rviz/properties/property_tree_widget.h</header> <header location="global">rviz_common/properties/property_tree_widget.hpp</header>
</customwidget> </customwidget>
<customwidget> <customwidget>
<class>moveit_rviz_plugin::SolutionListView</class> <class>moveit_rviz_plugin::SolutionListView</class>

View File

@ -3,18 +3,22 @@
############# #############
## Add gtest based cpp test target and link libraries ## Add gtest based cpp test target and link libraries
if (CATKIN_ENABLE_TESTING) if (BUILD_TESTING)
find_package(rostest REQUIRED) 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 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 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 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:=$<TARGET_FILE_DIR:${PROJECT_NAME}-test-task_model>")
endif() endif()

View File

@ -40,7 +40,7 @@
#include <moveit/task_constructor/container.h> #include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/stages/current_state.h> #include <moveit/task_constructor/stages/current_state.h>
#include <ros/init.h> #include <rclcpp/utilities.hpp>
#include <gtest/gtest.h> #include <gtest/gtest.h>
#include <initializer_list> #include <initializer_list>
#include <qcoreapplication.h> #include <qcoreapplication.h>
@ -50,19 +50,18 @@ using namespace moveit::task_constructor;
class TaskListModelTest : public ::testing::Test class TaskListModelTest : public ::testing::Test
{ {
protected: protected:
ros::NodeHandle nh;
moveit_rviz_plugin::TaskListModel model; moveit_rviz_plugin::TaskListModel model;
int children = 0; int children = 0;
int num_inserts = 0; int num_inserts = 0;
int num_updates = 0; int num_updates = 0;
moveit_task_constructor_msgs::TaskDescription genMsg(const std::string& name, moveit_task_constructor_msgs::msg::TaskDescription genMsg(const std::string& name,
const std::string& task_id = std::string()) { const std::string& task_id = std::string()) {
moveit_task_constructor_msgs::TaskDescription t; moveit_task_constructor_msgs::msg::TaskDescription t;
uint id = 0, root_id; uint id = 0, root_id;
t.task_id = task_id.empty() ? name : task_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.parent_id = id;
desc.id = root_id = ++id; desc.id = root_id = ++id;
desc.name = name; desc.name = name;
@ -129,7 +128,7 @@ protected:
SCOPED_TRACE("first i=" + std::to_string(i)); SCOPED_TRACE("first i=" + std::to_string(i));
num_inserts = 0; num_inserts = 0;
num_updates = 0; num_updates = 0;
model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); model.processTaskDescriptionMessage(genMsg("first"), "get_solution");
if (i == 0) if (i == 0)
EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task EXPECT_EQ(num_inserts, 1); // 1 notify for inserted task
@ -143,7 +142,7 @@ protected:
SCOPED_TRACE("second i=" + std::to_string(i)); SCOPED_TRACE("second i=" + std::to_string(i));
num_inserts = 0; num_inserts = 0;
num_updates = 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) if (i == 0)
EXPECT_EQ(num_inserts, 1); EXPECT_EQ(num_inserts, 1);
@ -165,17 +164,13 @@ protected:
TEST_F(TaskListModelTest, remoteTaskModel) { TEST_F(TaskListModelTest, remoteTaskModel) {
children = 3; children = 3;
planning_scene::PlanningSceneConstPtr scene; 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); m.processStageDescriptions(genMsg("first").stages);
SCOPED_TRACE("first"); SCOPED_TRACE("first");
validate(m, { "first" }); validate(m, { "first" });
} }
TEST_F(TaskListModelTest, localTaskModel) { TEST_F(TaskListModelTest, localTaskModel) {
int argc = 0;
char* argv = nullptr;
ros::init(argc, &argv, "testLocalTaskModel");
children = 3; children = 3;
const char* task_name = "task pipeline"; const char* task_name = "task pipeline";
moveit_rviz_plugin::LocalTaskModel m(std::make_unique<SerialContainer>(task_name), moveit_rviz_plugin::LocalTaskModel m(std::make_unique<SerialContainer>(task_name),
@ -187,6 +182,9 @@ TEST_F(TaskListModelTest, localTaskModel) {
SCOPED_TRACE("localTaskModel"); SCOPED_TRACE("localTaskModel");
validate(m, { task_name }); 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) { TEST_F(TaskListModelTest, noChildren) {
@ -202,13 +200,13 @@ TEST_F(TaskListModelTest, threeChildren) {
TEST_F(TaskListModelTest, visitedPopulate) { TEST_F(TaskListModelTest, visitedPopulate) {
// first population without children // first population without children
children = 0; children = 0;
model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); model.processTaskDescriptionMessage(genMsg("first"), "get_solution");
validate(model, { "first" }); // validation visits root node validate(model, { "first" }); // validation visits root node
EXPECT_EQ(num_inserts, 1); EXPECT_EQ(num_inserts, 1);
children = 3; children = 3;
num_inserts = 0; num_inserts = 0;
model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); model.processTaskDescriptionMessage(genMsg("first"), "get_solution");
validate(model, { "first" }); validate(model, { "first" });
// second population with children should emit insert notifies for them // second population with children should emit insert notifies for them
EXPECT_EQ(num_inserts, 3); EXPECT_EQ(num_inserts, 3);
@ -217,7 +215,7 @@ TEST_F(TaskListModelTest, visitedPopulate) {
TEST_F(TaskListModelTest, deletion) { TEST_F(TaskListModelTest, deletion) {
children = 3; children = 3;
model.processTaskDescriptionMessage(genMsg("first"), nh, "get_solution"); model.processTaskDescriptionMessage(genMsg("first"), "get_solution");
auto m = model.getModel(model.index(0, 0)).first; auto m = model.getModel(model.index(0, 0)).first;
int num_deletes = 0; int num_deletes = 0;
QObject::connect(m, &QObject::destroyed, [&num_deletes]() { ++num_deletes; }); QObject::connect(m, &QObject::destroyed, [&num_deletes]() { ++num_deletes; });
@ -232,6 +230,6 @@ TEST_F(TaskListModelTest, deletion) {
int main(int argc, char** argv) { int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_task_model"); rclcpp::init(argc, argv);
return RUN_ALL_TESTS(); return RUN_ALL_TESTS();
} }

View File

@ -1,4 +0,0 @@
<launch>
<test pkg="moveit_task_constructor_visualization"
type="moveit_task_constructor_visualization-test-task_model" test-name="test_task_model" />
</launch>

View File

@ -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)

View File

@ -12,9 +12,8 @@ target_link_libraries(${MOVEIT_LIB_NAME}
) )
target_include_directories(${MOVEIT_LIB_NAME} target_include_directories(${MOVEIT_LIB_NAME}
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..> PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>
PRIVATE ${catkin_INCLUDE_DIRS}
) )
install(TARGETS ${MOVEIT_LIB_NAME} install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} ARCHIVE DESTINATION lib
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) LIBRARY DESTINATION lib)

View File

@ -1,14 +1,14 @@
<library path="libmotion_planning_tasks_rviz_plugin"> <library path="motion_planning_tasks_rviz_plugin">
<class name="moveit_task_constructor/Motion Planning Tasks" <class name="moveit_task_constructor/Motion Planning Tasks"
type="moveit_rviz_plugin::TaskPanel" type="moveit_rviz_plugin::TaskPanel"
base_class_type="rviz::Panel"> base_class_type="rviz_common::Panel">
<description> <description>
A panel widget to monitor and edit motion planning tasks A panel widget to monitor and edit motion planning tasks
</description> </description>
</class> </class>
<class name="moveit_task_constructor/Motion Planning Tasks" <class name="moveit_task_constructor/Motion Planning Tasks"
type="moveit_rviz_plugin::TaskDisplay" type="moveit_rviz_plugin::TaskDisplay"
base_class_type="rviz::Display"> base_class_type="rviz_common::Display">
<description> <description>
Monitor motion planning tasks and animate their solution trajectories Monitor motion planning tasks and animate their solution trajectories
</description> </description>

View File

@ -7,21 +7,24 @@
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer> <maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
<maintainer email="me@v4hn.de">Michael Goerner</maintainer> <maintainer email="me@v4hn.de">Michael Goerner</maintainer>
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>qtbase5-dev</build_depend> <build_depend>qtbase5-dev</build_depend>
<depend>moveit_core</depend> <depend>moveit_core</depend>
<depend>moveit_task_constructor_msgs</depend> <depend>moveit_task_constructor_msgs</depend>
<depend>moveit_task_constructor_core</depend> <depend>moveit_task_constructor_core</depend>
<depend>moveit_ros_visualization</depend> <depend>moveit_ros_visualization</depend>
<depend>roscpp</depend> <depend>rclcpp</depend>
<depend>rviz</depend> <depend>rviz2</depend>
<test_depend>rosunit</test_depend> <test_depend>ament_cmake_gmock</test_depend>
<test_depend>rostest</test_depend> <test_depend>ament_cmake_gtest</test_depend>
<test_depend>moveit_resources_fanuc_moveit_config</test_depend> <test_depend>launch</test_depend>
<test_depend>launch_testing</test_depend>
<test_depend>launch_testing_ament_cmake</test_depend>
<test_depend>launch_testing_ros</test_depend>
<export> <export>
<rviz plugin="${prefix}/motion_planning_tasks_rviz_plugin_description.xml"/> <build_type>ament_cmake</build_type>
</export> </export>
</package> </package>

View File

@ -2,11 +2,6 @@ set(MOVEIT_LIB_NAME moveit_task_visualization_tools)
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/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 set(HEADERS
${PROJECT_INCLUDE}/display_solution.h ${PROJECT_INCLUDE}/display_solution.h
${PROJECT_INCLUDE}/marker_visualization.h ${PROJECT_INCLUDE}/marker_visualization.h
@ -14,7 +9,7 @@ set(HEADERS
${PROJECT_INCLUDE}/task_solution_visualization.h ${PROJECT_INCLUDE}/task_solution_visualization.h
) )
add_library(${MOVEIT_LIB_NAME} add_library(${MOVEIT_LIB_NAME} SHARED
${HEADERS} ${HEADERS}
src/display_solution.cpp src/display_solution.cpp
@ -23,25 +18,27 @@ add_library(${MOVEIT_LIB_NAME}
src/task_solution_visualization.cpp src/task_solution_visualization.cpp
) )
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
target_link_libraries(${MOVEIT_LIB_NAME} target_link_libraries(${MOVEIT_LIB_NAME}
${catkin_LIBRARIES}
${rviz_DEFAULT_PLUGIN_LIBRARIES}
${OGRE_LIBRARIES}
${QT_LIBRARIES} ${QT_LIBRARIES}
${Boost_LIBRARIES} rviz_ogre_vendor::OgreMain
) )
target_include_directories(${MOVEIT_LIB_NAME} target_include_directories(${MOVEIT_LIB_NAME}
PUBLIC include PUBLIC include
PRIVATE ${catkin_INCLUDE_DIRS}
) )
target_include_directories(${MOVEIT_LIB_NAME} SYSTEM ament_target_dependencies(${MOVEIT_LIB_NAME}
PUBLIC ${rviz_OGRE_INCLUDE_DIRS} 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} install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} EXPORT export_${MOVEIT_LIB_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)

View File

@ -36,7 +36,7 @@
#pragma once #pragma once
#include <moveit_task_constructor_msgs/Solution.h> #include <moveit_task_constructor_msgs/msg/solution.hpp>
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
namespace moveit { namespace moveit {
@ -53,7 +53,7 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory);
namespace Ogre { namespace Ogre {
class SceneNode; class SceneNode;
} }
namespace rviz { namespace rviz_common {
class DisplayContext; class DisplayContext;
} }
@ -125,7 +125,7 @@ public:
const MarkerVisualizationPtr markersOfSubTrajectory(size_t index) const { return data_.at(index).markers_; } const MarkerVisualizationPtr markersOfSubTrajectory(size_t index) const { return data_.at(index).markers_; }
void setFromMessage(const planning_scene::PlanningScenePtr& start_scene, void setFromMessage(const planning_scene::PlanningScenePtr& start_scene,
const moveit_task_constructor_msgs::Solution& msg); const moveit_task_constructor_msgs::msg::Solution& msg);
void fillMessage(moveit_task_constructor_msgs::Solution& msg) const; void fillMessage(moveit_task_constructor_msgs::msg::Solution& msg) const;
}; };
} // namespace moveit_rviz_plugin } // namespace moveit_rviz_plugin

View File

@ -1,8 +1,8 @@
#pragma once #pragma once
#include <rviz/properties/bool_property.h> #include <rviz_common/properties/bool_property.hpp>
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <visualization_msgs/Marker.h> #include <visualization_msgs/msg/marker.hpp>
#include <deque> #include <deque>
#include <list> #include <list>
#include <memory> #include <memory>
@ -11,10 +11,17 @@ namespace Ogre {
class SceneNode; class SceneNode;
} }
namespace rviz { namespace rviz_common {
class DisplayContext; class DisplayContext;
} // namespace rviz_common
namespace rviz_default_plugins {
namespace displays {
namespace markers {
class MarkerBase; class MarkerBase;
} // namespace rviz }
} // namespace displays
} // namespace rviz_default_plugins
namespace planning_scene { namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene); MOVEIT_CLASS_FORWARD(PlanningScene);
@ -41,10 +48,10 @@ class MarkerVisualization
// list of all markers, attached to scene nodes in namespaces_ // list of all markers, attached to scene nodes in namespaces_
struct MarkerData struct MarkerData
{ {
visualization_msgs::MarkerPtr msg_; visualization_msgs::msg::Marker::SharedPtr msg_;
std::shared_ptr<rviz::MarkerBase> marker_; std::shared_ptr<rviz_default_plugins::displays::markers::MarkerBase> marker_;
MarkerData(const visualization_msgs::Marker& marker); MarkerData(const visualization_msgs::msg::Marker& marker);
}; };
struct NamespaceData struct NamespaceData
{ {
@ -64,14 +71,14 @@ class MarkerVisualization
bool markers_created_ = false; bool markers_created_ = false;
public: public:
MarkerVisualization(const std::vector<visualization_msgs::Marker>& markers, MarkerVisualization(const std::vector<visualization_msgs::msg::Marker>& markers,
const planning_scene::PlanningScene& end_scene); const planning_scene::PlanningScene& end_scene);
~MarkerVisualization(); ~MarkerVisualization();
/// did we successfully created all markers (and scene nodes)? /// did we successfully created all markers (and scene nodes)?
bool created() const { return markers_created_; } bool created() const { return markers_created_; }
/// create markers (placed at planning frame of scene) /// 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 /// 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); 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 * The class remembers which MarkerVisualization instances are currently hosted
* and provides the user interaction to toggle marker visibility by namespace. * 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 Q_OBJECT
rviz::DisplayContext* context_ = nullptr; rviz_common::DisplayContext* context_ = nullptr;
Ogre::SceneNode* parent_scene_node_ = nullptr; // scene node provided externally Ogre::SceneNode* parent_scene_node_ = nullptr; // scene node provided externally
Ogre::SceneNode* marker_scene_node_ = nullptr; // scene node all markers are attached to Ogre::SceneNode* marker_scene_node_ = nullptr; // scene node all markers are attached to
std::map<QString, rviz::BoolProperty*> namespaces_; // rviz properties for encountered namespaces std::map<QString, rviz_common::properties::BoolProperty*> namespaces_; // rviz properties for encountered namespaces
std::list<MarkerVisualizationPtr> hosted_markers_; // list of hosted MarkerVisualization instances std::list<MarkerVisualizationPtr> hosted_markers_; // list of hosted MarkerVisualization instances
rviz::BoolProperty* all_markers_at_once_; rviz_common::properties::BoolProperty* all_markers_at_once_;
public: public:
MarkerVisualizationProperty(const QString& name, Property* parent = nullptr); MarkerVisualizationProperty(const QString& name, Property* parent = nullptr);
~MarkerVisualizationProperty() override; ~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 /// remove all hosted markers from display
void clearMarkers(); void clearMarkers();

View File

@ -37,17 +37,17 @@
#pragma once #pragma once
#ifndef Q_MOC_RUN #ifndef Q_MOC_RUN
#include <ros/ros.h> #include <rclcpp/rclcpp.hpp>
#endif #endif
#include <rviz/panel.h> #include <rviz_common/panel.hpp>
#include <QSlider> #include <QSlider>
#include <QLabel> #include <QLabel>
#include <QPushButton> #include <QPushButton>
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
class TaskSolutionPanel : public rviz::Panel class TaskSolutionPanel : public rviz_common::Panel
{ {
Q_OBJECT Q_OBJECT

View File

@ -37,7 +37,7 @@
#pragma once #pragma once
#include <moveit/macros/class_forward.h> #include <moveit/macros/class_forward.h>
#include <moveit_task_constructor_msgs/Solution.h> #include <moveit_task_constructor_msgs/msg/solution.hpp>
#include <QObject> #include <QObject>
#include <boost/thread/mutex.hpp> #include <boost/thread/mutex.hpp>
@ -47,10 +47,13 @@ namespace Ogre {
class SceneNode; class SceneNode;
} }
namespace rviz { namespace rviz_default_plugins {
class Display; namespace robot {
class DisplayContext;
class Robot; class Robot;
}
} // namespace rviz_default_plugins
namespace rviz_common {
namespace properties {
class Property; class Property;
class IntProperty; class IntProperty;
class StringProperty; class StringProperty;
@ -60,8 +63,11 @@ class RosTopicProperty;
class EnumProperty; class EnumProperty;
class EditableEnumProperty; class EditableEnumProperty;
class ColorProperty; class ColorProperty;
} // namespace properties
class Display;
class DisplayContext;
class PanelDockWidget; class PanelDockWidget;
} // namespace rviz } // namespace rviz_common
namespace moveit { namespace moveit {
namespace core { namespace core {
@ -91,24 +97,24 @@ class TaskSolutionVisualization : public QObject
public: public:
/** /**
* \brief Playback a trajectory from a planned path * \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 * \param display - the rviz::Display from the parent
* \return true on success * \return true on success
*/ */
TaskSolutionVisualization(rviz::Property* parent, rviz::Display* display); TaskSolutionVisualization(rviz_common::properties::Property* parent, rviz_common::Display* display);
~TaskSolutionVisualization() override; ~TaskSolutionVisualization() override;
virtual void update(float wall_dt, float ros_dt); virtual void update(float wall_dt, float ros_dt);
virtual void reset(); 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 onRobotModelLoaded(const moveit::core::RobotModelConstPtr& robot_model);
void onEnable(); void onEnable();
void onDisable(); void onDisable();
void setName(const QString& name); void setName(const QString& name);
planning_scene::PlanningSceneConstPtr getScene() const { return scene_; } 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 showTrajectory(const moveit_rviz_plugin::DisplaySolutionPtr& s, bool lock);
void unlock(); void unlock();
@ -156,12 +162,12 @@ protected:
MarkerVisualizationProperty* marker_visual_; MarkerVisualizationProperty* marker_visual_;
// Handle colouring of robot // Handle colouring of robot
void setRobotColor(rviz::Robot* robot, const QColor& color); void setRobotColor(rviz_default_plugins::robot::Robot* robot, const QColor& color);
void unsetRobotColor(rviz::Robot* robot); void unsetRobotColor(rviz_default_plugins::robot::Robot* robot);
DisplaySolutionPtr displaying_solution_; DisplaySolutionPtr displaying_solution_;
DisplaySolutionPtr next_solution_to_display_; DisplaySolutionPtr next_solution_to_display_;
std::vector<rviz::Robot*> trail_; std::vector<rviz_default_plugins::robot::Robot*> trail_;
bool animating_ = false; // auto-progressing the current waypoint? bool animating_ = false; // auto-progressing the current waypoint?
bool drop_displaying_solution_ = false; bool drop_displaying_solution_ = false;
bool locked_ = false; bool locked_ = false;
@ -172,36 +178,36 @@ protected:
planning_scene::PlanningScenePtr scene_; planning_scene::PlanningScenePtr scene_;
// Pointers from parent display that we save // 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* parent_scene_node_; // parent scene node provided by display
Ogre::SceneNode* main_scene_node_; // to be added/removed to/from scene_node_ 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_ Ogre::SceneNode* trail_scene_node_; // to be added/removed to/from scene_node_
rviz::DisplayContext* context_; rviz_common::DisplayContext* context_;
TaskSolutionPanel* slider_panel_ = nullptr; TaskSolutionPanel* slider_panel_ = nullptr;
rviz::PanelDockWidget* slider_dock_panel_ = nullptr; rviz_common::PanelDockWidget* slider_dock_panel_ = nullptr;
bool slider_panel_was_visible_ = false; bool slider_panel_was_visible_ = false;
// Trajectory Properties // Trajectory Properties
rviz::Property* robot_property_; rviz_common::properties::Property* robot_property_;
rviz::BoolProperty* robot_visual_enabled_property_; rviz_common::properties::BoolProperty* robot_visual_enabled_property_;
rviz::BoolProperty* robot_collision_enabled_property_; rviz_common::properties::BoolProperty* robot_collision_enabled_property_;
rviz::FloatProperty* robot_alpha_property_; rviz_common::properties::FloatProperty* robot_alpha_property_;
rviz::ColorProperty* robot_color_property_; rviz_common::properties::ColorProperty* robot_color_property_;
rviz::BoolProperty* enable_robot_color_property_; rviz_common::properties::BoolProperty* enable_robot_color_property_;
rviz::EditableEnumProperty* state_display_time_property_; rviz_common::properties::EditableEnumProperty* state_display_time_property_;
rviz::BoolProperty* loop_display_property_; rviz_common::properties::BoolProperty* loop_display_property_;
rviz::BoolProperty* trail_display_property_; rviz_common::properties::BoolProperty* trail_display_property_;
rviz::BoolProperty* interrupt_display_property_; rviz_common::properties::BoolProperty* interrupt_display_property_;
rviz::IntProperty* trail_step_size_property_; rviz_common::properties::IntProperty* trail_step_size_property_;
// PlanningScene Properties // PlanningScene Properties
rviz::BoolProperty* scene_enabled_property_; rviz_common::properties::BoolProperty* scene_enabled_property_;
rviz::FloatProperty* scene_alpha_property_; rviz_common::properties::FloatProperty* scene_alpha_property_;
rviz::ColorProperty* scene_color_property_; rviz_common::properties::ColorProperty* scene_color_property_;
rviz::ColorProperty* attached_body_color_property_; rviz_common::properties::ColorProperty* attached_body_color_property_;
rviz::EnumProperty* octree_render_property_; rviz_common::properties::EnumProperty* octree_render_property_;
rviz::EnumProperty* octree_coloring_property_; rviz_common::properties::EnumProperty* octree_coloring_property_;
}; };
} // namespace moveit_rviz_plugin } // namespace moveit_rviz_plugin

View File

@ -38,8 +38,10 @@
#include <moveit/visualization_tools/marker_visualization.h> #include <moveit/visualization_tools/marker_visualization.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_trajectory/robot_trajectory.h> #include <moveit/robot_trajectory/robot_trajectory.h>
#include <ros/console.h>
#include <boost/format.hpp> #include <boost/format.hpp>
#include <rclcpp/logging.hpp>
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.display_solution");
namespace moveit_rviz_plugin { 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); 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); 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, 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()) { if (msg.start_scene.robot_model_name != start_scene->getRobotModel()->getName()) {
static boost::format fmt("Solution for model '%s' but model '%s' was expected"); 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(); 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); start_scene_->getPlanningSceneMsg(msg.start_scene);
msg.sub_trajectory.resize(data_.size()); msg.sub_trajectory.resize(data_.size());
auto traj_it = msg.sub_trajectory.begin(); auto traj_it = msg.sub_trajectory.begin();

View File

@ -1,36 +1,41 @@
#include <moveit/visualization_tools/marker_visualization.h> #include <moveit/visualization_tools/marker_visualization.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
#include <rviz/default_plugin/markers/marker_base.h> #include <rviz_default_plugins/displays/marker/markers/marker_base.hpp>
#include <rviz/default_plugin/marker_utils.h> #include <rviz_default_plugins/displays/marker/markers/marker_factory.hpp>
#include <rviz/display_context.h> #include <rviz_default_plugins/displays/marker/marker_common.hpp>
#include <rviz/frame_manager.h> #include <rviz_default_plugins/transformation/tf_wrapper.hpp>
#include <rviz_common/display_context.hpp>
#include <rviz_common/frame_manager_iface.hpp>
#include <OgreSceneManager.h> #include <OgreSceneManager.h>
#include <OgreSceneNode.h> #include <OgreSceneNode.h>
#ifndef RVIZ_TF1 #include <tf2_msgs/msg/tf2_error.hpp>
#include <tf/tf.h> #include <rclcpp/logging.hpp>
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif #endif
#include <tf2_msgs/TF2Error.h>
#include <ros/console.h> static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_visualization.marker_visualization");
#include <eigen_conversions/eigen_msg.h>
namespace moveit_rviz_plugin { namespace moveit_rviz_plugin {
// create MarkerData with nil marker_ pointer, just with a copy of message // create MarkerData with nil marker_ pointer, just with a copy of message
MarkerVisualization::MarkerData::MarkerData(const visualization_msgs::Marker& marker) { MarkerVisualization::MarkerData::MarkerData(const visualization_msgs::msg::Marker& marker) {
msg_.reset(new visualization_msgs::Marker(marker)); msg_.reset(new visualization_msgs::msg::Marker(marker));
msg_->header.stamp = ros::Time(); msg_->header.stamp = rclcpp::Time(RCL_ROS_TIME);
msg_->frame_locked = false; msg_->frame_locked = false;
} }
MarkerVisualization::MarkerVisualization(const std::vector<visualization_msgs::Marker>& markers, MarkerVisualization::MarkerVisualization(const std::vector<visualization_msgs::msg::Marker>& markers,
const planning_scene::PlanningScene& end_scene) { const planning_scene::PlanningScene& end_scene) {
planning_frame_ = end_scene.getPlanningFrame(); planning_frame_ = end_scene.getPlanningFrame();
// remember marker message, postpone rviz::MarkerBase creation until later // remember marker message, postpone rviz::MarkerBase creation until later
for (const auto& marker : markers) { for (const auto& marker : markers) {
if (!end_scene.knowsFrameTransform(marker.header.frame_id)) { 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(), RCLCPP_WARN_ONCE(LOGGER, "unknown frame '%s' for solution marker in namespace '%s'",
marker.ns.c_str()); marker.header.frame_id.c_str(), marker.ns.c_str());
continue; // ignore markers with unknown frame 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); 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_) if (markers_created_)
return true; // already called before return true; // already called before
@ -72,25 +77,18 @@ bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::Sce
Ogre::Vector3 pos; Ogre::Vector3 pos;
try { try {
#ifdef RVIZ_TF1 auto tf_wrapper = std::dynamic_pointer_cast<rviz_default_plugins::transformation::TFWrapper>(
tf::TransformListener* tf = context->getFrameManager()->getTFClient(); context->getFrameManager()->getConnector().lock());
tf::StampedTransform tm; if (tf_wrapper) {
tf->lookupTransform(planning_frame_, fixed_frame, ros::Time(), tm); geometry_msgs::msg::TransformStamped tm;
auto q = tm.getRotation(); tm = tf_wrapper->lookupTransform(planning_frame_, fixed_frame, tf2::TimePointZero);
auto p = tm.getOrigin(); auto q = tm.transform.rotation;
quat = Ogre::Quaternion(q.w(), -q.x(), -q.y(), -q.z()); auto p = tm.transform.translation;
pos = Ogre::Vector3(p.x(), p.y(), p.z()); quat = Ogre::Quaternion(q.w, -q.x, -q.y, -q.z);
#else pos = Ogre::Vector3(p.x, p.y, p.z);
std::shared_ptr<tf2_ros::Buffer> 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
} catch (const tf2::TransformException& e) { } catch (const tf2::TransformException& e) {
ROS_WARN_STREAM_NAMED("MarkerVisualization", e.what()); RCLCPP_WARN_STREAM(LOGGER, e.what());
return false; return false;
} }
@ -110,7 +108,9 @@ bool MarkerVisualization::createMarkers(rviz::DisplayContext* context, Ogre::Sce
frame_it->second = node->createChildSceneNode(); frame_it->second = node->createChildSceneNode();
node = frame_it->second; 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_) if (!data.marker_)
continue; // failed to create 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 { const moveit::core::RobotState& robot_state) const {
Q_ASSERT(scene.getPlanningFrame() == planning_frame_); 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()) if (marker.header.frame_id == scene.getPlanningFrame())
return; // no need to transform nodes placed at planning frame 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)) else if (scene.knowsFrameTransform(marker.header.frame_id))
pose = scene.getFrameTransform(marker.header.frame_id); pose = scene.getFrameTransform(marker.header.frame_id);
else { else {
ROS_WARN_ONCE_NAMED("MarkerVisualization", "unknown frame '%s' for solution marker in namespace '%s'", RCLCPP_WARN_ONCE(LOGGER, "unknown frame '%s' for solution marker in namespace '%s'",
marker.header.frame_id.c_str(), marker.ns.c_str()); marker.header.frame_id.c_str(), marker.ns.c_str());
return; // ignore markers with unknown frame 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); update(data, end_scene, robot_state);
} }
MarkerVisualizationProperty::MarkerVisualizationProperty(const QString& name, rviz::Property* parent) MarkerVisualizationProperty::MarkerVisualizationProperty(const QString& name, rviz_common::properties::Property* parent)
: rviz::BoolProperty(name, true, "Enable/disable markers", parent) { : rviz_common::properties::BoolProperty(name, true, "Enable/disable markers", parent) {
all_markers_at_once_ = all_markers_at_once_ = new rviz_common::properties::BoolProperty(
new rviz::BoolProperty("All at once?", false, "Show all markers of multiple subsolutions at once?", this, "All at once?", false, "Show all markers of multiple subsolutions at once?", this, SLOT(onAllAtOnceChanged()),
SLOT(onAllAtOnceChanged()), this); this);
connect(this, SIGNAL(changed()), this, SLOT(onEnableChanged())); connect(this, SIGNAL(changed()), this, SLOT(onEnableChanged()));
} }
@ -183,7 +183,7 @@ MarkerVisualizationProperty::~MarkerVisualizationProperty() {
marker_scene_node_->getCreator()->destroySceneNode(marker_scene_node_); 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; context_ = context;
parent_scene_node_ = scene_node; parent_scene_node_ = scene_node;
marker_scene_node_ = parent_scene_node_->createChildSceneNode(); 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 // create sub property for newly encountered namespace, enabling visibility by default
auto ns_it = namespaces_.insert(std::make_pair(ns, nullptr)).first; auto ns_it = namespaces_.insert(std::make_pair(ns, nullptr)).first;
if (ns_it->second == nullptr) { if (ns_it->second == nullptr) {
ns_it->second = new rviz::BoolProperty(ns, true, "Show/hide markers of this namespace", this, ns_it->second = new rviz_common::properties::BoolProperty(ns, true, "Show/hide markers of this namespace",
SLOT(onNSEnableChanged()), this); this, SLOT(onNSEnableChanged()), this);
} }
Q_ASSERT(pair.second.ns_node_); // nodes should have been created in createMarkers() Q_ASSERT(pair.second.ns_node_); // nodes should have been created in createMarkers()
@ -241,7 +241,7 @@ void MarkerVisualizationProperty::onEnableChanged() {
} }
void MarkerVisualizationProperty::onNSEnableChanged() { void MarkerVisualizationProperty::onNSEnableChanged() {
rviz::BoolProperty* ns_property = static_cast<rviz::BoolProperty*>(sender()); rviz_common::properties::BoolProperty* ns_property = static_cast<rviz_common::properties::BoolProperty*>(sender());
const QString& ns = ns_property->getName(); const QString& ns = ns_property->getName();
bool visible = ns_property->getBool(); bool visible = ns_property->getBool();
// for all hosted markers, set visibility of given namespace // for all hosted markers, set visibility of given namespace

View File

@ -47,111 +47,122 @@
#include <moveit/robot_trajectory/robot_trajectory.h> #include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/trajectory_tools.h> #include <moveit/trajectory_processing/trajectory_tools.h>
#include <rviz/robot/robot.h> #include <rviz_default_plugins/robot/robot.hpp>
#include <rviz/robot/robot_link.h> #include <rviz_default_plugins/robot/robot_link.hpp>
#include <rviz/properties/property.h> #include <rviz_common/properties/property.hpp>
#include <rviz/properties/int_property.h> #include <rviz_common/properties/int_property.hpp>
#include <rviz/properties/string_property.h> #include <rviz_common/properties/string_property.hpp>
#include <rviz/properties/bool_property.h> #include <rviz_common/properties/bool_property.hpp>
#include <rviz/properties/float_property.h> #include <rviz_common/properties/float_property.hpp>
#include <rviz/properties/ros_topic_property.h> #include <rviz_common/properties/ros_topic_property.hpp>
#include <rviz/properties/enum_property.h> #include <rviz_common/properties/enum_property.hpp>
#include <rviz/properties/editable_enum_property.h> #include <rviz_common/properties/editable_enum_property.hpp>
#include <rviz/properties/color_property.h> #include <rviz_common/properties/color_property.hpp>
#include <rviz/display.h> #include <rviz_common/display.hpp>
#include <rviz/display_context.h> #include <rviz_common/display_context.hpp>
#include <rviz/window_manager_interface.h> #include <rviz_common/window_manager_interface.hpp>
#include <rviz/panel_dock_widget.h> #include <rviz_common/panel_dock_widget.hpp>
#include <OgreSceneNode.h> #include <OgreSceneNode.h>
#include <boost/algorithm/string/replace.hpp> #include <boost/algorithm/string/replace.hpp>
#include <boost/algorithm/string/trim.hpp> #include <boost/algorithm/string/trim.hpp>
#include <boost/lexical_cast.hpp>
static const rclcpp::Logger LOGGER =
rclcpp::get_logger("moveit_task_constructor_visualization.task_solution_visualization");
namespace moveit_rviz_plugin { 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) { : display_(display) {
// trajectory properties // trajectory properties
interrupt_display_property_ = new rviz::BoolProperty("Interrupt Display", false, interrupt_display_property_ = new rviz_common::properties::BoolProperty("Interrupt Display", false,
"Immediately show newly planned trajectory, " "Immediately show newly planned trajectory, "
"interrupting the currently displayed one.", "interrupting the currently displayed one.",
parent); 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, "Loop Animation", false, "Indicates whether the last received path is to be animated in a loop", parent,
SLOT(changedLoopDisplay()), this); SLOT(changedLoopDisplay()), this);
trail_display_property_ = trail_display_property_ = new rviz_common::properties::BoolProperty("Show Trail", false, "Show a path trail", parent,
new rviz::BoolProperty("Show Trail", false, "Show a path trail", parent, SLOT(changedTrail()), this); SLOT(changedTrail()), this);
state_display_time_property_ = state_display_time_property_ =
new rviz::EditableEnumProperty("State Display Time", "0.05 s", new rviz_common::properties::EditableEnumProperty("State Display Time", "0.05 s",
"The amount of wall-time to wait in between displaying " "The amount of wall-time to wait in between displaying "
"states along a received trajectory path", "states along a received trajectory path",
parent); parent);
state_display_time_property_->addOptionStd("REALTIME"); state_display_time_property_->addOptionStd("REALTIME");
state_display_time_property_->addOptionStd("0.05 s"); state_display_time_property_->addOptionStd("0.05 s");
state_display_time_property_->addOptionStd("0.1 s"); state_display_time_property_->addOptionStd("0.1 s");
state_display_time_property_->addOptionStd("0.5 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, "Trail Step Size", 1, "Specifies the step size of the samples shown in the trajectory trail.", parent,
SLOT(changedTrail()), this); SLOT(changedTrail()), this);
trail_step_size_property_->setMin(1); trail_step_size_property_->setMin(1);
// robot properties // robot properties
robot_property_ = new rviz::Property("Robot", QString(), QString(), parent); robot_property_ = new rviz_common::properties::Property("Robot", QString(), QString(), parent);
robot_visual_enabled_property_ = new rviz::BoolProperty("Show Robot Visual", true, robot_visual_enabled_property_ =
"Indicates whether the geometry of the robot as defined for " new rviz_common::properties::BoolProperty("Show Robot Visual", true,
"visualisation purposes should be displayed", "Indicates whether the geometry of the robot as defined for "
robot_property_, SLOT(changedRobotVisualEnabled()), this); "visualisation purposes should be displayed",
robot_property_, SLOT(changedRobotVisualEnabled()), this);
robot_collision_enabled_property_ = robot_collision_enabled_property_ =
new rviz::BoolProperty("Show Robot Collision", false, new rviz_common::properties::BoolProperty("Show Robot Collision", false,
"Indicates whether the geometry of the robot as defined " "Indicates whether the geometry of the robot as defined "
"for collision detection purposes should be displayed", "for collision detection purposes should be displayed",
robot_property_, SLOT(changedRobotCollisionEnabled()), this); robot_property_, SLOT(changedRobotCollisionEnabled()), this);
robot_alpha_property_ = new rviz::FloatProperty("Robot Alpha", 0.5f, "Specifies the alpha for the robot links", robot_alpha_property_ =
robot_property_, SLOT(changedRobotAlpha()), this); 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_->setMin(0.0);
robot_alpha_property_->setMax(1.0); robot_alpha_property_->setMax(1.0);
robot_color_property_ = robot_color_property_ = new rviz_common::properties::ColorProperty("Fixed Robot Color", QColor(150, 50, 150),
new rviz::ColorProperty("Fixed Robot Color", QColor(150, 50, 150), "The color of the animated robot", "The color of the animated robot",
robot_property_, SLOT(changedRobotColor()), this); robot_property_, SLOT(changedRobotColor()), this);
enable_robot_color_property_ = new rviz::BoolProperty("Use Fixed Robot Color", false, enable_robot_color_property_ =
"Specifies whether the fixed robot color should be used." new rviz_common::properties::BoolProperty("Use Fixed Robot Color", false,
" If not, the original color is used.", "Specifies whether the fixed robot color should be used."
robot_property_, SLOT(enabledRobotColor()), this); " If not, the original color is used.",
robot_property_, SLOT(enabledRobotColor()), this);
// planning scene properties // planning scene properties
scene_enabled_property_ = scene_enabled_property_ = new rviz_common::properties::BoolProperty("Scene", true, "Show Planning Scene", parent,
new rviz::BoolProperty("Scene", true, "Show Planning Scene", parent, SLOT(changedSceneEnabled()), this); SLOT(changedSceneEnabled()), this);
scene_alpha_property_ = new rviz::FloatProperty("Scene Alpha", 0.9f, "Specifies the alpha for the scene geometry", scene_alpha_property_ =
scene_enabled_property_, SLOT(renderCurrentScene()), this); 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_->setMin(0.0);
scene_alpha_property_->setMax(1.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 Color", QColor(50, 230, 50), "The color for the planning scene obstacles (if a color is not defined)",
scene_enabled_property_, SLOT(renderCurrentScene()), this); scene_enabled_property_, SLOT(renderCurrentScene()), this);
attached_body_color_property_ = attached_body_color_property_ = new rviz_common::properties::ColorProperty(
new rviz::ColorProperty("Attached Body Color", QColor(150, 50, 150), "The color for the attached bodies", "Attached Body Color", QColor(150, 50, 150), "The color for the attached bodies", scene_enabled_property_,
scene_enabled_property_, SLOT(changedAttachedBodyColor()), this); SLOT(changedAttachedBodyColor()), this);
octree_render_property_ = new rviz::EnumProperty("Voxel Rendering", "Occupied Voxels", "Select voxel type.", octree_render_property_ =
scene_enabled_property_, SLOT(renderCurrentScene()), this); 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("Occupied Voxels", OCTOMAP_OCCUPIED_VOXELS);
octree_render_property_->addOption("Free Voxels", OCTOMAP_FREE_VOXELS); octree_render_property_->addOption("Free Voxels", OCTOMAP_FREE_VOXELS);
octree_render_property_->addOption("All Voxels", OCTOMAP_FREE_VOXELS | OCTOMAP_OCCUPIED_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", octree_coloring_property_ =
scene_enabled_property_, SLOT(renderCurrentScene()), this); 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("Z-Axis", OCTOMAP_Z_AXIS_COLOR);
octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR); octree_coloring_property_->addOption("Cell Probability", OCTOMAP_PROBABLILTY_COLOR);
@ -174,7 +185,7 @@ TaskSolutionVisualization::~TaskSolutionVisualization() {
main_scene_node_->getCreator()->destroySceneNode(main_scene_node_); 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 // Save pointers for later use
parent_scene_node_ = scene_node; parent_scene_node_ = scene_node;
context_ = context; context_ = context;
@ -194,7 +205,7 @@ void TaskSolutionVisualization::onInitialize(Ogre::SceneNode* scene_node, rviz::
marker_visual_->onInitialize(main_scene_node_, context_); marker_visual_->onInitialize(main_scene_node_, context_);
rviz::WindowManagerInterface* window_context = context_->getWindowManager(); rviz_common::WindowManagerInterface* window_context = context_->getWindowManager();
if (window_context) { if (window_context) {
slider_panel_ = new TaskSolutionPanel(window_context->getParentWindow()); slider_panel_ = new TaskSolutionPanel(window_context->getParentWindow());
slider_dock_panel_ = window_context->addPane(display_->getName() + " - Slider", slider_panel_); 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"); 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 // Error check
if (!robot_model) { if (!robot_model) {
ROS_ERROR_STREAM_NAMED("task_solution_visualization", "No robot model found"); RCLCPP_ERROR(LOGGER, "No robot model found");
return; return;
} }
@ -268,8 +279,8 @@ void TaskSolutionVisualization::changedTrail() {
trail_.resize(t->getWayPointCount() / stepsize); trail_.resize(t->getWayPointCount() / stepsize);
for (std::size_t i = 0; i < trail_.size(); i++) { 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 int waypoint_i = std::min(i * stepsize, t->getWayPointCount() - 1); // limit to last trajectory point
rviz::Robot* r = rviz_default_plugins::robot::Robot* r = new rviz_default_plugins::robot::Robot(
new rviz::Robot(trail_scene_node_, context_, "Trail Robot " + boost::lexical_cast<std::string>(i), nullptr); trail_scene_node_, context_, "Trail Robot " + boost::lexical_cast<std::string>(i), nullptr);
r->load(*scene_->getRobotModel()->getURDF()); r->load(*scene_->getRobotModel()->getURDF());
r->setVisualVisible(robot_visual_enabled_property_->getBool()); r->setVisualVisible(robot_visual_enabled_property_->getBool());
r->setCollisionVisible(robot_collision_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(); QColor attached_color = attached_body_color_property_->getColor();
std_msgs::ColorRGBA color; std_msgs::msg::ColorRGBA color;
color.r = attached_color.redF(); color.r = attached_color.redF();
color.g = attached_color.greenF(); color.g = attached_color.greenF();
color.b = attached_color.blueF(); color.b = attached_color.blueF();
@ -506,16 +517,16 @@ void TaskSolutionVisualization::renderPlanningScene(const planning_scene::Planni
return; return;
QColor color = scene_color_property_->getColor(); 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(); 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_render_->renderPlanningScene(
scene, env_color, attached_color, static_cast<OctreeVoxelRenderMode>(octree_render_property_->getOptionInt()), scene, env_color, attached_color, static_cast<OctreeVoxelRenderMode>(octree_render_property_->getOptionInt()),
static_cast<OctreeVoxelColorMode>(octree_coloring_property_->getOptionInt()), scene_alpha_property_->getFloat()); static_cast<OctreeVoxelColorMode>(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); DisplaySolutionPtr s(new DisplaySolution);
s->setFromMessage(scene_, msg); s->setFromMessage(scene_, msg);
showTrajectory(s, false); showTrajectory(s, false);
@ -565,12 +576,12 @@ void TaskSolutionVisualization::changedAttachedBodyColor() {
renderCurrentWayPoint(); renderCurrentWayPoint();
} }
void TaskSolutionVisualization::unsetRobotColor(rviz::Robot* robot) { void TaskSolutionVisualization::unsetRobotColor(rviz_default_plugins::robot::Robot* robot) {
for (auto& link : robot->getLinks()) for (auto& link : robot->getLinks())
link.second->unsetColor(); 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()) for (auto& link : robot->getLinks())
link.second->setColor(color.redF(), color.greenF(), color.blueF()); link.second->setColor(color.redF(), color.greenF(), color.blueF());
} }