mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Port visualization to ROS2
This commit is contained in:
parent
d7ceaa01dd
commit
29703d0d6a
@ -1,69 +1,56 @@
|
||||
cmake_minimum_required(VERSION 3.1.3)
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(moveit_task_constructor_visualization)
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
moveit_core
|
||||
moveit_ros_visualization
|
||||
moveit_task_constructor_core
|
||||
moveit_task_constructor_msgs
|
||||
roscpp
|
||||
rviz
|
||||
)
|
||||
|
||||
# rviz transitively includes OGRE headers which break with `-Wall -Werror`
|
||||
# so isolate these include dirs and add them as SYSTEM include where needed.
|
||||
set(rviz_OGRE_INCLUDE_DIRS)
|
||||
foreach(header IN ITEMS OgreRoot.h OgreOverlay.h)
|
||||
find_path(include_dir ${header}
|
||||
HINTS ${catkin_INCLUDE_DIRS}
|
||||
NO_DEFAULT_PATH)
|
||||
list(REMOVE_ITEM catkin_INCLUDE_DIRS "${include_dir}")
|
||||
list(APPEND rviz_OGRE_INCLUDE_DIRS "${include_dir}")
|
||||
endforeach()
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(Boost REQUIRED)
|
||||
find_package(moveit_core REQUIRED)
|
||||
find_package(moveit_ros_visualization REQUIRED)
|
||||
find_package(moveit_task_constructor_core REQUIRED)
|
||||
find_package(moveit_task_constructor_msgs REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rviz_common REQUIRED)
|
||||
find_package(rviz_default_plugins REQUIRED)
|
||||
find_package(rviz_ogre_vendor REQUIRED)
|
||||
|
||||
# definition needed for boost/math/constants/constants.hpp included by Ogre to compile
|
||||
add_definitions(-DBOOST_MATH_DISABLE_FLOAT128)
|
||||
|
||||
# Qt Stuff
|
||||
if("${rviz_QT_VERSION}" VERSION_LESS "5")
|
||||
find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui)
|
||||
include(${QT_USE_FILE})
|
||||
macro(qt_wrap_ui)
|
||||
qt4_wrap_ui(${ARGN})
|
||||
endmacro()
|
||||
else()
|
||||
find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets)
|
||||
set(QT_LIBRARIES Qt5::Widgets)
|
||||
macro(qt_wrap_ui)
|
||||
find_package(Qt5 REQUIRED COMPONENTS Core Widgets)
|
||||
set(QT_LIBRARIES Qt5::Widgets)
|
||||
macro(qt_wrap_ui)
|
||||
qt5_wrap_ui(${ARGN})
|
||||
endmacro()
|
||||
endif()
|
||||
endmacro()
|
||||
|
||||
set(CMAKE_INCLUDE_CURRENT_DIR ON)
|
||||
set(CMAKE_AUTOMOC ON)
|
||||
set(CMAKE_AUTORCC ON)
|
||||
add_definitions(-DQT_NO_KEYWORDS)
|
||||
|
||||
catkin_package(
|
||||
LIBRARIES
|
||||
moveit_task_visualization_tools
|
||||
motion_planning_tasks_utils
|
||||
INCLUDE_DIRS
|
||||
visualization_tools/include
|
||||
CATKIN_DEPENDS
|
||||
moveit_core
|
||||
moveit_task_constructor_msgs
|
||||
roscpp
|
||||
rviz
|
||||
)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
endif()
|
||||
|
||||
add_subdirectory(visualization_tools)
|
||||
add_subdirectory(motion_planning_tasks)
|
||||
|
||||
install(FILES
|
||||
motion_planning_tasks_rviz_plugin_description.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
install(DIRECTORY icons DESTINATION share)
|
||||
|
||||
install(DIRECTORY icons DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
pluginlib_export_plugin_description_file(rviz_common motion_planning_tasks_rviz_plugin_description.xml)
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(motion_planning_tasks_utils
|
||||
motion_planning_tasks_properties
|
||||
motion_planning_tasks_rviz_plugin
|
||||
moveit_task_visualization_tools
|
||||
)
|
||||
ament_export_dependencies(ament_cmake)
|
||||
ament_export_dependencies(Boost)
|
||||
ament_export_dependencies(moveit_core)
|
||||
ament_export_dependencies(moveit_ros_visualization)
|
||||
ament_export_dependencies(moveit_task_constructor_core)
|
||||
ament_export_dependencies(moveit_task_constructor_msgs)
|
||||
ament_export_dependencies(rclcpp)
|
||||
ament_export_dependencies(rviz_common)
|
||||
ament_export_dependencies(rviz_default_plugins)
|
||||
ament_export_dependencies(rviz_ogre_vendor)
|
||||
ament_package()
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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/>
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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());
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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 */
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
@ -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>
|
||||
@ -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)
|
||||
@ -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)
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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());
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user