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)
find_package(catkin REQUIRED COMPONENTS
moveit_core
moveit_ros_visualization
moveit_task_constructor_core
moveit_task_constructor_msgs
roscpp
rviz
)
# rviz transitively includes OGRE headers which break with `-Wall -Werror`
# so isolate these include dirs and add them as SYSTEM include where needed.
set(rviz_OGRE_INCLUDE_DIRS)
foreach(header IN ITEMS OgreRoot.h OgreOverlay.h)
find_path(include_dir ${header}
HINTS ${catkin_INCLUDE_DIRS}
NO_DEFAULT_PATH)
list(REMOVE_ITEM catkin_INCLUDE_DIRS "${include_dir}")
list(APPEND rviz_OGRE_INCLUDE_DIRS "${include_dir}")
endforeach()
find_package(ament_cmake REQUIRED)
find_package(Boost REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_visualization REQUIRED)
find_package(moveit_task_constructor_core REQUIRED)
find_package(moveit_task_constructor_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rviz_common REQUIRED)
find_package(rviz_default_plugins REQUIRED)
find_package(rviz_ogre_vendor REQUIRED)
# definition needed for boost/math/constants/constants.hpp included by Ogre to compile
add_definitions(-DBOOST_MATH_DISABLE_FLOAT128)
# Qt Stuff
if("${rviz_QT_VERSION}" VERSION_LESS "5")
find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui)
include(${QT_USE_FILE})
macro(qt_wrap_ui)
qt4_wrap_ui(${ARGN})
endmacro()
else()
find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets)
find_package(Qt5 REQUIRED COMPONENTS 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_AUTOMOC ON)
set(CMAKE_AUTORCC ON)
add_definitions(-DQT_NO_KEYWORDS)
catkin_package(
LIBRARIES
moveit_task_visualization_tools
motion_planning_tasks_utils
INCLUDE_DIRS
visualization_tools/include
CATKIN_DEPENDS
moveit_core
moveit_task_constructor_msgs
roscpp
rviz
)
set(CMAKE_CXX_STANDARD 14)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
add_subdirectory(visualization_tools)
add_subdirectory(motion_planning_tasks)
install(FILES
motion_planning_tasks_rviz_plugin_description.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY icons DESTINATION share)
install(DIRECTORY icons DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
pluginlib_export_plugin_description_file(rviz_common motion_planning_tasks_rviz_plugin_description.xml)
ament_export_include_directories(include)
ament_export_libraries(motion_planning_tasks_utils
motion_planning_tasks_properties
motion_planning_tasks_rviz_plugin
moveit_task_visualization_tools
)
ament_export_dependencies(ament_cmake)
ament_export_dependencies(Boost)
ament_export_dependencies(moveit_core)
ament_export_dependencies(moveit_ros_visualization)
ament_export_dependencies(moveit_task_constructor_core)
ament_export_dependencies(moveit_task_constructor_msgs)
ament_export_dependencies(rclcpp)
ament_export_dependencies(rviz_common)
ament_export_dependencies(rviz_default_plugins)
ament_export_dependencies(rviz_ogre_vendor)
ament_package()

View File

@ -20,9 +20,14 @@ target_link_libraries(${MOVEIT_LIB_NAME}
)
target_include_directories(${MOVEIT_LIB_NAME}
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}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
EXPORT export_${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)

View File

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

View File

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

View File

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

View File

@ -6,7 +6,7 @@ qt_wrap_ui(UIC_FILES
global_settings.ui
)
add_library(${MOVEIT_LIB_NAME}
add_library(${MOVEIT_LIB_NAME} SHARED
factory_model.cpp
icons.cpp
job_queue.cpp
@ -28,7 +28,7 @@ add_library(${MOVEIT_LIB_NAME}
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
target_link_libraries(${MOVEIT_LIB_NAME}
motion_planning_tasks_utils motion_planning_tasks_properties moveit_task_visualization_tools
${catkin_LIBRARIES} ${QT_LIBRARIES}
${QT_LIBRARIES}
)
target_include_directories(${MOVEIT_LIB_NAME}
PUBLIC $<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_properties,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}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
EXPORT export_${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)

View File

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

View File

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

View File

@ -51,7 +51,7 @@
</widget>
</item>
<item>
<widget class="rviz::PropertyTreeWidget" name="view"/>
<widget class="rviz_common::properties::PropertyTreeWidget" name="view"/>
</item>
</layout>
</item>
@ -59,9 +59,9 @@
</widget>
<customwidgets>
<customwidget>
<class>rviz::PropertyTreeWidget</class>
<class>rviz_common::properties::PropertyTreeWidget</class>
<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>
</customwidgets>
<resources/>

View File

@ -35,7 +35,9 @@
/* Author: Robert Haschke */
#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 tools {
@ -71,7 +73,7 @@ void JobQueue::executeJobs() {
try {
fn();
} catch (std::exception& ex) {
ROS_ERROR("Exception caught executing main loop job: %s", ex.what());
RCLCPP_ERROR(LOGGER, "Exception caught executing main loop job: %s", ex.what());
}
ulock.lock();
}

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -38,7 +38,8 @@
#include "task_list_model.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 <limits>
@ -54,7 +55,11 @@ class RemoteTaskModel : public BaseTaskModel
Q_OBJECT
struct Node;
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, DisplaySolutionPtr> id_to_solution_;
@ -64,12 +69,11 @@ class RemoteTaskModel : public BaseTaskModel
Node* node(uint32_t stage_id) const;
inline RemoteSolutionModel* getSolutionModel(uint32_t stage_id) const;
void setSolutionData(const moveit_task_constructor_msgs::SolutionInfo& info);
void setSolutionData(const moveit_task_constructor_msgs::msg::SolutionInfo& info);
public:
RemoteTaskModel(ros::NodeHandle& nh, const std::string& service_name,
const planning_scene::PlanningSceneConstPtr& scene, rviz::DisplayContext* display_context,
QObject* parent = nullptr);
RemoteTaskModel(const std::string& service_name, const planning_scene::PlanningSceneConstPtr& scene,
rviz_common::DisplayContext* display_context, QObject* parent = nullptr);
~RemoteTaskModel() override;
int rowCount(const QModelIndex& parent = QModelIndex()) const override;
@ -81,14 +85,14 @@ public:
bool setData(const QModelIndex& index, const QVariant& value, int role = Qt::EditRole) override;
QModelIndex indexFromStageId(size_t id) const override;
void processStageDescriptions(const moveit_task_constructor_msgs::TaskDescription::_stages_type& msg);
void processStageStatistics(const moveit_task_constructor_msgs::TaskStatistics::_stages_type& msg);
DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::Solution& msg);
void processStageDescriptions(const moveit_task_constructor_msgs::msg::TaskDescription::_stages_type& msg);
void processStageStatistics(const moveit_task_constructor_msgs::msg::TaskStatistics::_stages_type& msg);
DisplaySolutionPtr processSolutionMessage(const moveit_task_constructor_msgs::msg::Solution& msg);
QAbstractItemModel* getSolutionModel(const QModelIndex& index) override;
DisplaySolutionPtr getSolution(const QModelIndex& index) override;
rviz::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override;
rviz_common::properties::PropertyTreeModel* getPropertyModel(const QModelIndex& index) override;
};
/** Model representing solutions of a remote task */

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@ -129,7 +129,7 @@
</widget>
</item>
<item>
<widget class="rviz::PropertyTreeWidget" name="property_view"/>
<widget class="rviz_common::properties::PropertyTreeWidget" name="property_view"/>
</item>
</layout>
</widget>
@ -174,9 +174,9 @@
<header>task_list_model.h</header>
</customwidget>
<customwidget>
<class>rviz::PropertyTreeWidget</class>
<class>rviz_common::properties::PropertyTreeWidget</class>
<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>
<class>moveit_rviz_plugin::SolutionListView</class>

View File

@ -3,18 +3,22 @@
#############
## Add gtest based cpp test target and link libraries
if (CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
if (BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_cmake_gmock REQUIRED)
find_package(launch_testing_ament_cmake REQUIRED)
catkin_add_gtest(${PROJECT_NAME}-test-merge-models test_merge_models.cpp)
ament_add_gtest(${PROJECT_NAME}-test-merge-models test_merge_models.cpp)
target_link_libraries(${PROJECT_NAME}-test-merge-models
motion_planning_tasks_utils gtest_main)
motion_planning_tasks_utils)
catkin_add_gmock(${PROJECT_NAME}-test-solution-models test_solution_models.cpp)
ament_add_gmock(${PROJECT_NAME}-test-solution-models test_solution_models.cpp)
target_link_libraries(${PROJECT_NAME}-test-solution-models
motion_planning_tasks_rviz_plugin gtest_main)
motion_planning_tasks_rviz_plugin)
add_rostest_gtest(${PROJECT_NAME}-test-task_model test_task_model.launch test_task_model.cpp)
ament_add_gtest_executable(${PROJECT_NAME}-test-task_model test_task_model.cpp)
target_link_libraries(${PROJECT_NAME}-test-task_model
motion_planning_tasks_rviz_plugin ${catkin_LIBRARIES} gtest)
motion_planning_tasks_rviz_plugin)
add_launch_test(test_task_model.launch.py
ARGS "test_binary_dir:=$<TARGET_FILE_DIR:${PROJECT_NAME}-test-task_model>")
endif()

View File

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

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}
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>
PRIVATE ${catkin_INCLUDE_DIRS}
)
install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)

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"
type="moveit_rviz_plugin::TaskPanel"
base_class_type="rviz::Panel">
base_class_type="rviz_common::Panel">
<description>
A panel widget to monitor and edit motion planning tasks
</description>
</class>
<class name="moveit_task_constructor/Motion Planning Tasks"
type="moveit_rviz_plugin::TaskDisplay"
base_class_type="rviz::Display">
base_class_type="rviz_common::Display">
<description>
Monitor motion planning tasks and animate their solution trajectories
</description>

View File

@ -7,21 +7,24 @@
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</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>
<depend>moveit_core</depend>
<depend>moveit_task_constructor_msgs</depend>
<depend>moveit_task_constructor_core</depend>
<depend>moveit_ros_visualization</depend>
<depend>roscpp</depend>
<depend>rviz</depend>
<depend>rclcpp</depend>
<depend>rviz2</depend>
<test_depend>rosunit</test_depend>
<test_depend>rostest</test_depend>
<test_depend>moveit_resources_fanuc_moveit_config</test_depend>
<test_depend>ament_cmake_gmock</test_depend>
<test_depend>ament_cmake_gtest</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>
<rviz plugin="${prefix}/motion_planning_tasks_rviz_plugin_description.xml"/>
<build_type>ament_cmake</build_type>
</export>
</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)
# TODO: Remove when Kinetic support is dropped
if(rviz_VERSION VERSION_LESS 1.13.1) # Does rviz supports TF2?
add_definitions(-DRVIZ_TF1)
endif()
set(HEADERS
${PROJECT_INCLUDE}/display_solution.h
${PROJECT_INCLUDE}/marker_visualization.h
@ -14,7 +9,7 @@ set(HEADERS
${PROJECT_INCLUDE}/task_solution_visualization.h
)
add_library(${MOVEIT_LIB_NAME}
add_library(${MOVEIT_LIB_NAME} SHARED
${HEADERS}
src/display_solution.cpp
@ -23,25 +18,27 @@ add_library(${MOVEIT_LIB_NAME}
src/task_solution_visualization.cpp
)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
target_link_libraries(${MOVEIT_LIB_NAME}
${catkin_LIBRARIES}
${rviz_DEFAULT_PLUGIN_LIBRARIES}
${OGRE_LIBRARIES}
${QT_LIBRARIES}
${Boost_LIBRARIES}
rviz_ogre_vendor::OgreMain
)
target_include_directories(${MOVEIT_LIB_NAME}
PUBLIC include
PRIVATE ${catkin_INCLUDE_DIRS}
)
target_include_directories(${MOVEIT_LIB_NAME} SYSTEM
PUBLIC ${rviz_OGRE_INCLUDE_DIRS}
ament_target_dependencies(${MOVEIT_LIB_NAME}
Boost
pluginlib
moveit_task_constructor_msgs
moveit_ros_visualization
moveit_core
rclcpp
rviz_common
rviz_default_plugins
)
add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
install(DIRECTORY include/ DESTINATION include)
install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
EXPORT export_${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)

View File

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

View File

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

View File

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

View File

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

View File

@ -38,8 +38,10 @@
#include <moveit/visualization_tools/marker_visualization.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <ros/console.h>
#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 {
@ -65,7 +67,7 @@ float DisplaySolution::getWayPointDurationFromPrevious(const IndexPair& idx_pair
return data_[idx_pair.first].trajectory_->getWayPointDurationFromPrevious(idx_pair.second);
}
const robot_state::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair& idx_pair) const {
const moveit::core::RobotStatePtr& DisplaySolution::getWayPointPtr(const IndexPair& idx_pair) const {
return data_[idx_pair.first].trajectory_->getWayPointPtr(idx_pair.second);
}
@ -87,7 +89,7 @@ const MarkerVisualizationPtr DisplaySolution::markers(const DisplaySolution::Ind
}
void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& start_scene,
const moveit_task_constructor_msgs::Solution& msg) {
const moveit_task_constructor_msgs::msg::Solution& msg) {
if (msg.start_scene.robot_model_name != start_scene->getRobotModel()->getName()) {
static boost::format fmt("Solution for model '%s' but model '%s' was expected");
fmt % msg.start_scene.robot_model_name.c_str() % start_scene->getRobotModel()->getName().c_str();
@ -127,7 +129,7 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& sta
}
}
void DisplaySolution::fillMessage(moveit_task_constructor_msgs::Solution& msg) const {
void DisplaySolution::fillMessage(moveit_task_constructor_msgs::msg::Solution& msg) const {
start_scene_->getPlanningSceneMsg(msg.start_scene);
msg.sub_trajectory.resize(data_.size());
auto traj_it = msg.sub_trajectory.begin();

View File

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

View File

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