diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 4d1a6706f..e7f74f316 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -184,6 +184,7 @@ class Stage void setName(const std::string& name); uint32_t introspectionId() const; + Introspection* introspection() const; /** set computation timeout (in seconds) * diff --git a/core/src/introspection.cpp b/core/src/introspection.cpp index 9820efedf..d46542e1a 100644 --- a/core/src/introspection.cpp +++ b/core/src/introspection.cpp @@ -55,6 +55,8 @@ bool isValidCharInName(char c); // unfortunately this is not declared in ros/na } // namespace names } // namespace ros +static const char* LOGGER = "introspection"; + namespace moveit { namespace task_constructor { @@ -206,6 +208,9 @@ uint32_t Introspection::stageId(const Stage* const s) const { uint32_t Introspection::solutionId(const SolutionBase& s) { auto result = impl->id_solution_bimap_.left.insert(std::make_pair(1 + impl->id_solution_bimap_.size(), &s)); + if (result.second) // new entry + ROS_DEBUG_STREAM_NAMED(LOGGER, "new solution #" << result.first->first << " (" << s.creator()->name() + << "): " << s.cost() << " " << s.comment()); return result.first->first; } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 3298b3ad5..2f7fcb1c1 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -381,6 +381,9 @@ uint32_t Stage::introspectionId() const { throw std::runtime_error("Task is not initialized yet or Introspection was disabled."); return const_cast(pimpl_->introspection_)->stageId(this); } +Introspection* Stage::introspection() const { + return pimpl_->introspection_; +} void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) { const PropertyMap& src = source.properties(); diff --git a/core/test/stage_mockups.cpp b/core/test/stage_mockups.cpp index b084faf3b..9397be569 100644 --- a/core/test/stage_mockups.cpp +++ b/core/test/stage_mockups.cpp @@ -99,6 +99,7 @@ void ConnectMockup::compute(const InterfaceState& from, const InterfaceState& to auto solution{ std::make_shared() }; solution->setCost(costs_.cost()); + solution->setComment(std::to_string(from.priority().cost()) + " -> " + std::to_string(to.priority().cost())); connect(from, to, solution); } diff --git a/core/test/test_serial.cpp b/core/test/test_serial.cpp index 406b6663d..c6aecd2f6 100644 --- a/core/test/test_serial.cpp +++ b/core/test/test_serial.cpp @@ -74,6 +74,13 @@ TEST_F(ConnectConnect, FailSucc) { // TODO: Solutions are enumerated multiple times // TODO: invalid solutions leak into enumeration TEST_F(ConnectConnect, UniqueEnumeration) { +#if 1 // enable solution introspection + int argc = 0; + ros::init(argc, NULL, "debug"); + t.enableIntrospection(true); + ros::console::set_logger_level(ROSCONSOLE_NAME_PREFIX, ros::console::levels::Debug); +#endif + add(t, new GeneratorMockup({ 1.0, 2.0, 3.0 })); auto con1 = add(t, new ConnectMockup()); add(t, new GeneratorMockup({ 10.0, 20.0 })); diff --git a/visualization/motion_planning_tasks/src/remote_task_model.cpp b/visualization/motion_planning_tasks/src/remote_task_model.cpp index d34cb40be..8ab980476 100644 --- a/visualization/motion_planning_tasks/src/remote_task_model.cpp +++ b/visualization/motion_planning_tasks/src/remote_task_model.cpp @@ -501,7 +501,15 @@ QVariant RemoteSolutionModel::data(const QModelIndex& index, int role) const { return item.id; case Qt::ToolTipRole: - return item.comment; + switch (index.column()) { +#if 1 // show internal solution id in first column return item + case 0: + return item.id; + default: + return item.comment; +#endif + } + break; case Qt::DisplayRole: switch (index.column()) {