mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
RemoteTaskModel: insert solution also when received on solution topic
This commit is contained in:
parent
ca899d6a43
commit
3b26d2777a
@ -370,9 +370,7 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex &index)
|
|||||||
}
|
}
|
||||||
|
|
||||||
namespace detail {
|
namespace detail {
|
||||||
// SFINAE magic selects matching template: http://en.cppreference.com/w/cpp/language/sfinae
|
// method used by sorted_ container (requiring an additional dereference to access id)
|
||||||
|
|
||||||
// method used for sorted_ container, requiring an additional dereference to access id
|
|
||||||
template <class T>
|
template <class T>
|
||||||
typename T::iterator findById(T& c, decltype((*c.cbegin())->id) id)
|
typename T::iterator findById(T& c, decltype((*c.cbegin())->id) id)
|
||||||
{
|
{
|
||||||
@ -381,16 +379,15 @@ typename T::iterator findById(T& c, decltype((*c.cbegin())->id) id)
|
|||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
// method used for data_ container, allowing for binary search
|
// insert new Data item into data_ container
|
||||||
template <class T>
|
template <class T>
|
||||||
typename T::iterator findById(T& c, decltype((*c.cbegin()).id) id)
|
typename T::iterator insert(T& c, typename T::value_type&& item)
|
||||||
{
|
{
|
||||||
typedef decltype((*c.cbegin()).id) val_type;
|
auto p = std::equal_range(c.begin(), c.end(), item);
|
||||||
auto p = std::__equal_range(c.begin(), c.end(), id,
|
if (p.first == p.second) // new item
|
||||||
[](typename T::iterator it, val_type val){ return it->id < val; },
|
return c.insert(p.second, std::move(item));
|
||||||
[](val_type val, typename T::iterator it){ return val < it->id; });
|
else
|
||||||
if (p.first == p.second) return c.end(); // id not found
|
return p.first;
|
||||||
return p.first;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -464,11 +461,7 @@ void RemoteSolutionModel::setSolutionData(uint32_t id, float cost, const QString
|
|||||||
// retrieve iterator and row corresponding to id
|
// retrieve iterator and row corresponding to id
|
||||||
auto sit = detail::findById(sorted_, id);
|
auto sit = detail::findById(sorted_, id);
|
||||||
int row = (sit != sorted_.end()) ? sit - sorted_.begin() : -1;
|
int row = (sit != sorted_.end()) ? sit - sorted_.begin() : -1;
|
||||||
auto it = (sit != sorted_.end()) ? *sit : detail::findById(data_, id);
|
auto it = (sit != sorted_.end()) ? *sit : detail::insert(data_, Data(id, cost, 0, name));
|
||||||
if (it == data_.end()) {
|
|
||||||
ROS_WARN("solution id not found: %d", id);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
QModelIndex tl, br;
|
QModelIndex tl, br;
|
||||||
Data &item = *it;
|
Data &item = *it;
|
||||||
@ -484,6 +477,9 @@ void RemoteSolutionModel::setSolutionData(uint32_t id, float cost, const QString
|
|||||||
}
|
}
|
||||||
if (tl.isValid())
|
if (tl.isValid())
|
||||||
Q_EMIT dataChanged(tl, br);
|
Q_EMIT dataChanged(tl, br);
|
||||||
|
|
||||||
|
if (row < 0 && isVisible(*it)) // item was newly created: inform views
|
||||||
|
sortInternal();
|
||||||
}
|
}
|
||||||
|
|
||||||
void RemoteSolutionModel::sort(int column, Qt::SortOrder order)
|
void RemoteSolutionModel::sort(int column, Qt::SortOrder order)
|
||||||
@ -548,17 +544,14 @@ void RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t> &succes
|
|||||||
const std::vector<uint32_t> &failed,
|
const std::vector<uint32_t> &failed,
|
||||||
size_t num_failed)
|
size_t num_failed)
|
||||||
{
|
{
|
||||||
bool was_empty = data_.empty();
|
|
||||||
auto last = --data_.end();
|
|
||||||
|
|
||||||
// append new items to the end of data_
|
// append new items to the end of data_
|
||||||
processSolutionIDs(successful, true);
|
processSolutionIDs(successful, true);
|
||||||
processSolutionIDs(failed, false);
|
processSolutionIDs(failed, false);
|
||||||
|
|
||||||
// assign creation rank to new items
|
// assign consecutive creation ranks
|
||||||
uint32_t rank = was_empty ? 0 : last->creation_rank;
|
uint32_t rank = 0;
|
||||||
for (auto it = was_empty ? data_.begin() : ++last, end = data_.end(); it != end; ++it)
|
for (auto& item: data_)
|
||||||
it->creation_rank = ++rank;
|
item.creation_rank = ++rank;
|
||||||
|
|
||||||
// the task may not report failure ids (in failed),
|
// the task may not report failure ids (in failed),
|
||||||
// but it may report the overall number of failures
|
// but it may report the overall number of failures
|
||||||
@ -575,15 +568,10 @@ void RemoteSolutionModel::processSolutionIDs(const std::vector<uint32_t> &ids, b
|
|||||||
: std::numeric_limits<double>::infinity();
|
: std::numeric_limits<double>::infinity();
|
||||||
uint32_t cost_rank = 0;
|
uint32_t cost_rank = 0;
|
||||||
for (const uint32_t id : ids) {
|
for (const uint32_t id : ids) {
|
||||||
Data item(id, default_cost, successful ? ++cost_rank : std::numeric_limits<uint32_t>::max());
|
uint32_t rank = successful ? ++cost_rank : std::numeric_limits<uint32_t>::max();
|
||||||
// find id in available data_
|
auto it = detail::insert(data_, Data(id, default_cost, rank));
|
||||||
auto p = std::equal_range(data_.begin(), data_.end(), item);
|
Q_ASSERT(it->id == id);
|
||||||
if (p.first == p.second) { // new item
|
it->cost_rank = rank;
|
||||||
data_.insert(p.second, std::move(item));
|
|
||||||
} else { // existing item: update cost rank
|
|
||||||
Q_ASSERT(p.first->id == id);
|
|
||||||
p.first->cost_rank = item.cost_rank;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -100,8 +100,8 @@ class RemoteSolutionModel : public QAbstractTableModel {
|
|||||||
uint32_t creation_rank; // rank, ordered by creation
|
uint32_t creation_rank; // rank, ordered by creation
|
||||||
uint32_t cost_rank; // rank, ordering by cost
|
uint32_t cost_rank; // rank, ordering by cost
|
||||||
|
|
||||||
Data(uint32_t id, float cost, uint32_t cost_rank)
|
Data(uint32_t id, float cost, uint32_t cost_rank, const QString& name=QString())
|
||||||
: id(id), cost(cost), creation_rank(0), cost_rank(cost_rank) {}
|
: id(id), cost(cost), name(name), creation_rank(0), cost_rank(cost_rank) {}
|
||||||
|
|
||||||
inline bool operator<(const Data& other) const {
|
inline bool operator<(const Data& other) const {
|
||||||
return this->id < other.id;
|
return this->id < other.id;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user