show properties based on ros-type-introspection

This commit is contained in:
janEbert 2018-11-13 11:57:11 +01:00 committed by Robert Haschke
parent 4ca794cb86
commit ecb8c733b5
7 changed files with 287 additions and 1 deletions

View File

@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
moveit_task_constructor_msgs
roscpp
rviz
ros_type_introspection
)
# definition needed for boost/math/constants/constants.hpp included by Ogre to compile

View File

@ -6,7 +6,8 @@ set(SOURCES
add_library(${MOVEIT_LIB_NAME} SHARED ${SOURCES})
target_link_libraries(${MOVEIT_LIB_NAME}
${QT_LIBRARIES}
PUBLIC ${QT_LIBRARIES}
PRIVATE moveit_task_visualization_tools
)
target_include_directories(${MOVEIT_LIB_NAME}
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/..>

View File

@ -40,6 +40,7 @@
#include "properties/property_factory.h"
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/properties.h>
#include <moveit/visualization_tools/type_introspection.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_task_constructor_msgs/GetSolution.h>
#include <rviz/properties/property_tree_model.h>
@ -140,6 +141,9 @@ RemoteTaskModel::Node::createProperty(const moveit_task_constructor_msgs::Proper
properties_.erase(it);
}
const ri::RenamedValues& flat_dict = TypeIntrospector::instance().extract(prop.type, prop.value);
return TreeConstructor::parseToTree(flat_dict, prop.name, prop.description, old);
if (old) { // reuse existing Property?
old->setDescription(QString::fromStdString(prop.description));
old->setValue(QString::fromStdString(prop.value));

View File

@ -15,6 +15,7 @@
<build_depend>moveit_ros_visualization</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rviz</build_depend>
<build_depend>ros_type_introspection</build_depend>
<run_depend>moveit_core</run_depend>
<run_depend>moveit_task_constructor_msgs</run_depend>
@ -22,6 +23,7 @@
<run_depend>moveit_ros_visualization</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rviz</run_depend>
<run_depend>ros_type_introspection</run_depend>
<test_depend>rosunit</test_depend>
<test_depend>rostest</test_depend>

View File

@ -7,6 +7,7 @@ set(HEADERS
${PROJECT_INCLUDE}/marker_visualization.h
${PROJECT_INCLUDE}/task_solution_panel.h
${PROJECT_INCLUDE}/task_solution_visualization.h
${PROJECT_INCLUDE}/type_introspection.h
)
add_library(${MOVEIT_LIB_NAME}
@ -16,6 +17,7 @@ add_library(${MOVEIT_LIB_NAME}
src/marker_visualization.cpp
src/task_solution_panel.cpp
src/task_solution_visualization.cpp
src/type_introspection.cpp
)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

View File

@ -0,0 +1,85 @@
#pragma once
#include <ros/message_traits.h>
#include <rviz/properties/property.h>
#include <rviz/properties/string_property.h>
#include <ros_type_introspection/ros_introspection.hpp>
#include <moveit_task_constructor_msgs/Property.h>
namespace ri = RosIntrospection;
namespace moveit_rviz_plugin {
class TreeConstructor {
public:
static rviz::Property* parseToTree(const ri::RenamedValues& msgValues,
const std::string& name, const std::string& description,
rviz::Property* old = 0);
private:
static void treeFromValue(const std::pair<std::string, ri::Variant>& value,
rviz::Property* root, const size_t startIx);
static rviz::Property* getOrCreateChild(const std::string& name, rviz::Property* parent);
static rviz::Property* createLeaf(const std::pair<std::string, ri::Variant>& value,
rviz::Property* parent, const size_t startIx);
template <typename T, typename V>
static rviz::Property* createLeafOfType(const std::pair<std::string, ri::Variant>& value,
rviz::Property* parent, const size_t startIx);
template <typename V>
static rviz::StringProperty* createStringLeaf(const std::pair<std::string, ri::Variant>& value,
rviz::Property* parent, const size_t startIx);
static rviz::StringProperty* timeLeaf(const std::pair<std::string, ri::Variant>& value,
rviz::Property* parent, const size_t startIx);
static rviz::StringProperty* durationLeaf(const std::pair<std::string, ri::Variant>& value,
rviz::Property* parent, const size_t startIx);
};
class TypeIntrospector {
public:
/// Return singleton instance
inline static TypeIntrospector& instance() {
static TypeIntrospector instance_;
return instance_;
}
/// Register the given type to the parser. Registering the same message twice is allowed.
template <typename T>
inline void registerMsgType() {
parser.registerMessageDefinition(ros::message_traits::datatype<T>(),
ri::ROSType(ros::message_traits::datatype<T>()),
ros::message_traits::definition<T>());
}
/// Return extracted values of the message contained in the given property.
inline ri::RenamedValues extractFromPropertyMsg(const moveit_task_constructor_msgs::Property& prop) {
return extract(prop.type, prop.value);
}
/** Return extracted values of a serialized message as a flat vector.
* `name` is the name by which the message was registered, while
* `sermsg` is the serialized message. */
ri::RenamedValues extract(const std::string& msg_id, const std::string& serialized);
/** Return extracted values of a serialized msg in a buffer as a flat vector.
* `name` is the name by which the message was registered, while
* `sermsg` is a buffer containing the serialized message. */
ri::RenamedValues extract(const std::string& name, std::vector<uint8_t>& serialized);
private:
TypeIntrospector() {}
/// The parser containing all registered message types.
ri::Parser parser;
};
} // namespace moveit_rviz_plugin

View File

@ -0,0 +1,191 @@
#include <moveit/visualization_tools/type_introspection.h>
#include <iostream>
#include <time.h>
#include <rviz/properties/bool_property.h>
#include <rviz/properties/int_property.h>
#include <rviz/properties/float_property.h>
namespace moveit_rviz_plugin {
rviz::Property* TreeConstructor::parseToTree(const ri::RenamedValues& msgValues,
const std::string& name, const std::string& description,
rviz::Property* old) {
rviz::Property* root;
if (old) {
root = old;
root->setName(QString::fromStdString(name));
root->setDescription(QString::fromStdString(description));
} else {
root = new rviz::StringProperty(QString::fromStdString(name), QString(),
QString::fromStdString(description));
root->setReadOnly(true);
}
if (msgValues.size() == 0)
return root;
const size_t startIx = name.size() + 1; // skip the slash '/'
for (const std::pair<std::string, ri::Variant>& value : msgValues) {
treeFromValue(value, root, startIx);
}
return root;
}
void TreeConstructor::treeFromValue(const std::pair<std::string, ri::Variant>& value,
rviz::Property* root, const size_t startIx) {
size_t oldStartIx = startIx;
size_t newStartIx = value.first.find('/', startIx);
rviz::Property* parent = root;
if (newStartIx != std::string::npos) {
newStartIx += 1; // skip the slash '/'
while (true) { // while (newStartIx != std::string::npos)
// subtract 2 from newStartIx to get take everything excluding the slash
parent = getOrCreateChild(value.first.substr(oldStartIx, newStartIx - 2), parent);
oldStartIx = newStartIx;
newStartIx = value.first.find('/', newStartIx);
if (newStartIx != std::string::npos)
newStartIx += 1; // skip the slash '/'
else
break;
}
}
createLeaf(value, parent, oldStartIx);
}
rviz::Property* TreeConstructor::getOrCreateChild(const std::string& name, rviz::Property* parent) {
rviz::Property* possibleChild = NULL;
for (int i = 0; i < parent->numChildren(); ++i) {
if ((possibleChild = parent->childAt(i)) && possibleChild->getNameStd() == name)
return possibleChild;
}
return new rviz::Property(QString::fromStdString(name), QString(), QString(), parent);
}
rviz::Property* TreeConstructor::createLeaf(const std::pair<std::string, ri::Variant>& value,
rviz::Property* parent, const size_t startIx) {
// The reason for this check is that rviz::IntProperty takes an
// `int`. We want to check for this (see below).
constexpr size_t intsize = sizeof(int);
// Number types are separately handled as strings to avoid showing
// wrong numbers due to overflow.
ri::BuiltinType type = value.second.getTypeID();
switch (type) {
case ri::BOOL:
return createLeafOfType<rviz::BoolProperty, bool>(value, parent, startIx);
/* TODO these are currently not supported in RosTypeIntrospection
case ri::BYTE:
return createLeafOfType<rviz::IntProperty, TODO unsigned char>(value, parent, startIx);
case ri::CHAR:
return createLeafOfType<rviz::IntProperty, char>(value, parent, startIx);
*/
case ri::INT8:
return createLeafOfType<rviz::IntProperty, int8_t>(value, parent, startIx);
case ri::UINT8:
if (intsize > 1)
return createLeafOfType<rviz::IntProperty, uint8_t>(value, parent, startIx);
else
return createStringLeaf<uint8_t>(value, parent, startIx);
case ri::INT16:
if (intsize >= 2)
return createLeafOfType<rviz::IntProperty, int16_t>(value, parent, startIx);
else
return createStringLeaf<int16_t>(value, parent, startIx);
case ri::UINT16:
if (intsize > 2)
return createLeafOfType<rviz::IntProperty, uint16_t>(value, parent, startIx);
else
return createStringLeaf<uint16_t>(value, parent, startIx);
case ri::INT32:
if (intsize >= 4)
return createLeafOfType<rviz::IntProperty, int32_t>(value, parent, startIx);
else
return createStringLeaf<int32_t>(value, parent, startIx);
case ri::UINT32:
if (intsize > 4)
return createLeafOfType<rviz::IntProperty, uint32_t>(value, parent, startIx);
else
return createStringLeaf<uint32_t>(value, parent, startIx);
case ri::INT64:
if (intsize >= 8)
return createLeafOfType<rviz::IntProperty, int64_t>(value, parent, startIx);
else
return createStringLeaf<int64_t>(value, parent, startIx);
case ri::UINT64:
if (intsize > 8)
return createLeafOfType<rviz::IntProperty, uint64_t>(value, parent, startIx);
else
return createStringLeaf<uint64_t>(value, parent, startIx);
case ri::FLOAT32:
return createLeafOfType<rviz::FloatProperty, float>(value, parent, startIx);
case ri::FLOAT64:
return createStringLeaf<double>(value, parent, startIx);
case ri::STRING:
return new rviz::StringProperty(QString::fromStdString(value.first.substr(startIx)),
QString::fromStdString(value.second.extract<std::string>()),
QString(), parent);
case ri::TIME:
return timeLeaf(value, parent, startIx);
case ri::DURATION:
return durationLeaf(value, parent, startIx);
default:
throw std::runtime_error("Unsupported type");
}
return nullptr;
}
template <typename T, typename V>
rviz::Property* TreeConstructor::createLeafOfType(const std::pair<std::string, ri::Variant>& value,
rviz::Property* parent, const size_t startIx) {
rviz::Property* leaf = new T(QString::fromStdString(value.first.substr(startIx)),
value.second.extract<V>(), QString(), parent);
return leaf;
}
template <typename V>
rviz::StringProperty* TreeConstructor::createStringLeaf(const std::pair<std::string, ri::Variant>& value,
rviz::Property* parent, const size_t startIx) {
return new rviz::StringProperty(QString::fromStdString(value.first.substr(startIx)),
QString::fromStdString(std::to_string(value.second.extract<V>())),
QString(), parent);
}
rviz::StringProperty* TreeConstructor::timeLeaf(const std::pair<std::string, ri::Variant>& value,
rviz::Property* parent, const size_t startIx) {
ros::Time time = value.second.extract<ros::Time>();
std::ostringstream oss;
oss << time << " s";
return new rviz::StringProperty(QString::fromStdString(value.first.substr(startIx)),
QString::fromStdString(oss.str()),
QString("Time in Seconds"), parent);
}
rviz::StringProperty* TreeConstructor::durationLeaf(const std::pair<std::string, ri::Variant>& value,
rviz::Property* parent, const size_t startIx) {
ros::Duration duration = value.second.extract<ros::Duration>();
std::ostringstream oss;
oss << duration << " s";
return new rviz::StringProperty(QString::fromStdString(value.first.substr(startIx)),
QString::fromStdString(oss.str()),
QString("Duration in Seconds"), parent);
}
ri::RenamedValues TypeIntrospector::extract(const std::string& msg_id, const std::string& serialized) {
constexpr uint32_t MAX_ARRAY_SIZE = 10;
ri::FlatMessage msgContent;
ri::RenamedValues msgValues;
parser.deserializeIntoFlatContainer(msg_id, absl::Span<uint8_t>(reinterpret_cast<uint8_t*>(const_cast<char*>(serialized.data())),
serialized.size()),
&msgContent, MAX_ARRAY_SIZE);
parser.applyNameTransform(msg_id, msgContent, &msgValues);
return msgValues;
}
} // namespace moveit_rviz_plugin