mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
8392ed5ad3
commit
527ec8edaa
@ -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); }
|
||||
|
||||
|
||||
@ -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_;
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -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_;
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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...
|
||||
|
||||
@ -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");
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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});
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user