correctly sort upstream_solutions_ in ComputeIK and GeneratePose

Reworked cost_queue to correctly sort pointer-like types.
Added unittests for new ValueOrPointeeLess<T> less operator, ordered<T>, and rviz cost ordering.
This commit is contained in:
Robert Haschke 2019-02-10 20:55:07 +01:00
parent 8392ed5ad3
commit 527ec8edaa
10 changed files with 207 additions and 48 deletions

View File

@ -6,6 +6,18 @@
#include <iostream>
#include <algorithm>
/// ValueOrPointeeLess provides correct comparison for plain and pointer-like types
template <typename T, typename Default = void>
struct ValueOrPointeeLess : public std::less<T> {};
/// The following template-specialization is for pointer-like types
template <typename T>
struct ValueOrPointeeLess<T, decltype(*std::declval<T>(), std::declval<void>())> {
bool operator()(const T& x, const T& y) const {
return *x < *y;
}
};
/**
* @brief ordered<ValueType> provides an adapter for a std::list to allow sorting.
*
@ -14,7 +26,7 @@
* Sorted insertion has logarithmic complexity.
*/
template <typename T,
typename Compare = std::less<T>>
typename Compare = ValueOrPointeeLess<T>>
class ordered
{
public:
@ -50,7 +62,7 @@ public:
reference top() { return c.front(); }
const_reference top() const { return c.front(); }
reference pop() { reference result = top(); c.pop_front(); return result; }
value_type pop() { value_type result(top()); c.pop_front(); return result; }
reference front() { return c.front(); }
const_reference front() const { return c.front(); }
@ -81,6 +93,8 @@ public:
iterator at = std::upper_bound(c.begin(), c.end(), item, comp);
return c.insert(at, std::move(item));
}
inline void push(const value_type& item) { insert(item); }
inline void push(value_type&& item) { insert(std::move(item)); }
iterator erase(const_iterator pos) { return c.erase(pos); }

View File

@ -37,6 +37,7 @@
#pragma once
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/cost_queue.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Geometry>
@ -66,7 +67,8 @@ class ComputeIK : public WrapperBase {
public:
ComputeIK(const std::string &name, Stage::pointer &&child = Stage::pointer());
void init(const core::RobotModelConstPtr &robot_model);
void reset() override;
void init(const core::RobotModelConstPtr &robot_model) override;
void onNewSolution(const SolutionBase &s) override;
bool canCompute() const override;
@ -113,7 +115,7 @@ public:
}
protected:
std::queue<const SolutionBase*> targets_;
ordered<const SolutionBase*> upstream_solutions_;
};
} } }

View File

@ -39,6 +39,7 @@
#pragma once
#include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/cost_queue.h>
#include <geometry_msgs/PoseStamped.h>
namespace moveit { namespace task_constructor { namespace stages {
@ -57,8 +58,7 @@ public:
protected:
void onNewSolution(const SolutionBase& s) override;
std::deque<planning_scene::PlanningScenePtr> scenes_;
ordered<const SolutionBase*> upstream_solutions_;
};
} } }

View File

@ -134,17 +134,9 @@ private:
Interface* owner_ = nullptr; // allow update of priority
};
/// compare InterfaceState* by value
struct InterfaceStateLess {
inline bool operator()(const InterfaceState* x, const InterfaceState* y) const {
return *x < *y;
}
};
/** Interface provides a cost-sorted list of InterfaceStates available as input for a stage. */
class Interface : public ordered<InterfaceState*, InterfaceStateLess> {
typedef ordered<InterfaceState*, InterfaceStateLess> base_type;
class Interface : public ordered<InterfaceState*> {
typedef ordered<InterfaceState*> base_type;
public:
// iterators providing convinient access to stored InterfaceState

View File

@ -179,6 +179,12 @@ bool validateGroup(const PropertyMap& props, const moveit::core::RobotModelConst
} // anonymous namespace
void ComputeIK::reset()
{
upstream_solutions_.clear();
WrapperBase::reset();
}
void ComputeIK::init(const moveit::core::RobotModelConstPtr& robot_model)
{
InitStageException errors;
@ -204,12 +210,12 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
assert(s.start() && s.end());
assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator
// TODO: don't take pointers, this is dangerous
targets_.push(&s);
// It's safe to store a pointer to the solution, as the generating stage stores it
upstream_solutions_.push(&s);
}
bool ComputeIK::canCompute() const {
return !targets_.empty() || WrapperBase::canCompute();
return !upstream_solutions_.empty() || WrapperBase::canCompute();
}
void ComputeIK::compute()
@ -217,11 +223,10 @@ void ComputeIK::compute()
if(WrapperBase::canCompute())
WrapperBase::compute();
if(targets_.empty())
if(upstream_solutions_.empty())
return;
const SolutionBase& s = *targets_.front();
targets_.pop();
const SolutionBase& s = *upstream_solutions_.pop();
// -1 TODO: this should not be necessary in my opinion: Why do you think so?
// It is, because the properties on the interface might change from call to call...

View File

@ -90,16 +90,9 @@ void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model)
void GenerateGraspPose::onNewSolution(const SolutionBase& s)
{
planning_scene::PlanningScenePtr scene = s.end()->scene()->diff();
planning_scene::PlanningSceneConstPtr scene = s.end()->scene();
// set end effector pose
const auto& props = properties();
const std::string& eef = props.get<std::string>("eef");
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef);
robot_state::RobotState &robot_state = scene->getCurrentStateNonConst();
robot_state.setToDefaultValues(jmg , props.get<std::string>("pregrasp"));
const std::string& object = props.get<std::string>("object");
if (!scene->knowsFrameTransform(object)) {
const std::string msg = "object '" + object + "' not in scene";
@ -114,16 +107,21 @@ void GenerateGraspPose::onNewSolution(const SolutionBase& s)
return;
}
scenes_.push_back(scene);
upstream_solutions_.push(&s);
}
void GenerateGraspPose::compute() {
if (scenes_.empty())
if (upstream_solutions_.empty())
return;
planning_scene::PlanningScenePtr scene = scenes_[0];
scenes_.pop_front();
planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff();
// set end effector pose
const auto& props = properties();
const std::string& eef = props.get<std::string>("eef");
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef);
robot_state::RobotState &robot_state = scene->getCurrentStateNonConst();
robot_state.setToDefaultValues(jmg , props.get<std::string>("pregrasp"));
geometry_msgs::PoseStamped target_pose_msg;
target_pose_msg.header.frame_id = props.get<std::string>("object");

View File

@ -51,25 +51,25 @@ GeneratePose::GeneratePose(const std::string& name)
void GeneratePose::reset()
{
upstream_solutions_.clear();
MonitoringGenerator::reset();
scenes_.clear();
}
void GeneratePose::onNewSolution(const SolutionBase& s)
{
scenes_.push_back(s.end()->scene()->diff());
// It's safe to store a pointer to this solution, as the generating stage stores it
upstream_solutions_.push(&s);
}
bool GeneratePose::canCompute() const {
return scenes_.size() > 0;
return upstream_solutions_.size() > 0;
}
void GeneratePose::compute() {
if (scenes_.empty())
if (upstream_solutions_.empty())
return;
planning_scene::PlanningScenePtr scene = scenes_[0];
scenes_.pop_front();
planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff();
geometry_msgs::PoseStamped target_pose = properties().get<geometry_msgs::PoseStamped>("pose");
if (target_pose.header.frame_id.empty())
target_pose.header.frame_id = scene->getPlanningFrame();

View File

@ -1,6 +1,100 @@
#include <list>
#include <memory>
#include <moveit/task_constructor/cost_queue.h>
#include <moveit/task_constructor/storage.h>
#include <gtest/gtest.h>
#include <gmock/gmock-matchers.h>
namespace mtc = moveit::task_constructor;
// type-trait functions for OrderedTest<T>
template <typename T> T create(int cost);
template <> int create(int cost) { return cost; }
template <> int* create(int cost) { return new int(cost); }
template <> mtc::SolutionBasePtr create(int cost) {
mtc::SolutionBasePtr s = std::make_shared<mtc::SubTrajectory>();
s->setCost(cost);
return s;
}
template <> mtc::SolutionBaseConstPtr create(int cost) {
return create<mtc::SolutionBasePtr>(cost);
}
template <typename T> int value(const T& item);
template <> int value(const int& item) { return item; }
template <> int value(int* const& item) { return *item; }
template <> int value(const mtc::SolutionBasePtr& item) { return item->cost(); }
template <> int value(const mtc::SolutionBaseConstPtr& item) { return item->cost(); }
template <typename T>
class ValueOrPointeeLessTest : public ::testing::Test {
protected:
bool less(int lhs, int rhs) {
ValueOrPointeeLess<T> comp;
return comp(create<T>(lhs), create<T>(rhs));
}
};
// set of template types to test for
typedef ::testing::Types<int, int*, mtc::SolutionBasePtr, mtc::SolutionBaseConstPtr> TypeInstances;
TYPED_TEST_CASE(ValueOrPointeeLessTest, TypeInstances);
TYPED_TEST(ValueOrPointeeLessTest, less) {
EXPECT_TRUE(this->less(2, 3));
EXPECT_FALSE(this->less(1, 1));
EXPECT_FALSE(this->less(2, 1));
}
template <typename T>
class OrderedTest : public ::testing::Test, public ordered<T> {
protected:
void pushAndValidate(int cost, const std::vector<int>& expected) {
this->insert(create<T>(cost));
std::vector<int> actual;
std::transform(this->c.begin(), this->c.end(), std::back_inserter(actual),
[](const T& item) -> int { return value<T>(item); });
EXPECT_THAT(actual, ::testing::ElementsAreArray(expected));
}
void validatePop() {
std::vector<int> expected;
std::vector<int> actual;
std::transform(this->c.begin(), this->c.end(), std::back_inserter(expected),
[](const T& item) -> int { return value<T>(item); });
while(!this->empty())
actual.push_back(value<T>(this->pop()));
EXPECT_THAT(actual, ::testing::ElementsAreArray(expected));
EXPECT_TRUE(this->empty());
}
};
#define pushAndValidate(cost, ...) { \
SCOPED_TRACE("pushAndValidate(" #cost ", " #__VA_ARGS__ ")"); \
this->pushAndValidate(cost, __VA_ARGS__); \
}
TYPED_TEST_CASE(OrderedTest, TypeInstances);
TYPED_TEST(OrderedTest, sorting) {
pushAndValidate(2, {2});
pushAndValidate(1, {1,2});
pushAndValidate(3, {1,2,3});
this->validatePop();
pushAndValidate(1, {1});
pushAndValidate(2, {1,2});
pushAndValidate(3, {1,2,3});
pushAndValidate(4, {1,2,3,4});
pushAndValidate(5, {1,2,3,4,5});
this->validatePop();
pushAndValidate(5, {5});
pushAndValidate(4, {4,5});
pushAndValidate(3, {3,4,5});
pushAndValidate(1, {1,3,4,5});
pushAndValidate(2, {1,2,3,4,5});
this->validatePop();
}
template<typename ValueType, typename CostType>
std::ostream& operator<<(std::ostream &os, const cost_ordered<ValueType, CostType> &queue) {

View File

@ -10,13 +10,11 @@ if (CATKIN_ENABLE_TESTING)
target_link_libraries(${PROJECT_NAME}-test-merge-models
motion_planning_tasks_utils gtest_main)
catkin_add_gtest(${PROJECT_NAME}-test-solution-models test_solution_models.cpp)
target_link_libraries(${PROJECT_NAME}-test-solution-models
motion_planning_tasks_rviz_plugin gtest_main)
if(NOT "${rviz_QT_VERSION}" VERSION_LESS "5")
# This tests uses Qt5's generic function slots
add_rostest_gtest(${PROJECT_NAME}-test-task_model test_task_model.launch test_task_model.cpp)
target_link_libraries(${PROJECT_NAME}-test-task_model
motion_planning_tasks_rviz_plugin
${catkin_LIBRARIES}
gtest_main)
endif()
add_rostest_gtest(${PROJECT_NAME}-test-task_model test_task_model.launch test_task_model.cpp)
target_link_libraries(${PROJECT_NAME}-test-task_model
motion_planning_tasks_rviz_plugin ${catkin_LIBRARIES} gtest_main)
endif()

View File

@ -0,0 +1,56 @@
#include <remote_task_model.h>
#include <algorithm>
#include <gtest/gtest.h>
#include <gmock/gmock-matchers.h>
using namespace moveit_rviz_plugin;
class SolutionModelTest : public ::testing::Test {
protected:
template <typename T>
inline std::vector<T> reversed(const std::vector<T> &in) {
return std::vector<T>(in.rbegin(), in.rend());
}
void validateSorting(QAbstractTableModel& model, int sort_column, Qt::SortOrder sort_order,
const std::vector<uint32_t>& expected_id_order) {
SCOPED_TRACE(qPrintable(QString("sorting in %1 %2 order")
.arg(sort_order == Qt::AscendingOrder ? "ascending" : "descending")
.arg(sort_column == 0 ? "creation" : "cost")));
model.sort(sort_column, sort_order);
std::vector<uint32_t> actual_id_order(model.rowCount());
for (int row = 0; row < model.rowCount(); ++row)
actual_id_order[row] = model.data(model.index(row, 0), Qt::UserRole).toInt();
EXPECT_THAT(actual_id_order, ::testing::ElementsAreArray(expected_id_order));
}
void processAndValidate(RemoteSolutionModel& model,
const std::vector<uint32_t>& success_ids,
const std::vector<uint32_t>& failure_ids) {
model.processSolutionIDs(success_ids, failure_ids, failure_ids.size());
std::vector<uint32_t> cost_ordered_ids(success_ids.begin(), success_ids.end());
std::vector<uint32_t> sorted_failure_ids(failure_ids.begin(), failure_ids.end());
std::sort(sorted_failure_ids.begin(), sorted_failure_ids.end());
std::copy(sorted_failure_ids.begin(), sorted_failure_ids.end(), std::back_inserter(cost_ordered_ids));
std::vector<uint32_t> creation_ordered_ids(cost_ordered_ids.size());
for (size_t i=0; i < cost_ordered_ids.size(); ++i)
creation_ordered_ids[i] = i+1;
validateSorting(model, 0, Qt::AscendingOrder, creation_ordered_ids);
validateSorting(model, 0, Qt::DescendingOrder, reversed(creation_ordered_ids));
validateSorting(model, 1, Qt::AscendingOrder, cost_ordered_ids);
validateSorting(model, 1, Qt::DescendingOrder, reversed(cost_ordered_ids));
}
};
#define processAndValidate(...) { \
SCOPED_TRACE("processSolutionIDs(" #__VA_ARGS__ ")"); \
processAndValidate(model, __VA_ARGS__); \
}
TEST_F(SolutionModelTest, sorting) {
RemoteSolutionModel model;
processAndValidate({1,3}, {2});
processAndValidate({4,1,6,3}, {5,2});
}