Add more debugging output

- Add Stage::introspection() accessor
- Introspection: debug-log solution registration
- RemoteSolutionModel: show internal solution id as tooltip in 1st column

Author: rhaschke

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)
 	 *

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;
 }
 

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<const moveit::task_constructor::Introspection*>(pimpl_->introspection_)->stageId(this);
 }
+Introspection* Stage::introspection() const {
+	return pimpl_->introspection_;
+}
 
 void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) {
 	const PropertyMap& src = source.properties();

core/test/stage_mockups.cpp

@@ -99,6 +99,7 @@ void ConnectMockup::compute(const InterfaceState& from, const InterfaceState& to
 
 	auto solution{ std::make_shared<SubTrajectory>() };
 	solution->setCost(costs_.cost());
+	solution->setComment(std::to_string(from.priority().cost()) + " -> " + std::to_string(to.priority().cost()));
 	connect(from, to, solution);
 }
 

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()) {