mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
show properties based on ros-type-introspection
This commit is contained in:
parent
4ca794cb86
commit
ecb8c733b5
@ -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
|
||||
|
@ -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}/..>
|
||||
|
@ -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));
|
||||
|
@ -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>
|
||||
|
@ -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}")
|
||||
|
||||
|
@ -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
|
191
visualization/visualization_tools/src/type_introspection.cpp
Normal file
191
visualization/visualization_tools/src/type_introspection.cpp
Normal 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
|
||||
|
Loading…
Reference in New Issue
Block a user