Fix SolutionBase::fillMessage(): also write start_scene

This method was only doing half of the job, namely adding subsolutions
to the message fields. However, the start_scene was not yet written.
This was handled manually in some but not all callers.
To avoid this inconsistency, the new method toMsg() takes care of both
actions now, while the old fillMessage() method was renamed to appendTo().

Author: rhaschke

core/include/moveit/task_constructor/storage.h

@@ -311,9 +311,11 @@ class SolutionBase
 	auto& markers() { return markers_; }
 	const auto& markers() const { return markers_; }
 
+	/// convert solution to message
+	void toMsg(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const;
 	/// append this solution to Solution msg
-	virtual void fillMessage(moveit_task_constructor_msgs::Solution& solution,
-	                         Introspection* introspection = nullptr) const = 0;
+	virtual void appendTo(moveit_task_constructor_msgs::Solution& solution,
+	                      Introspection* introspection = nullptr) const = 0;
 	void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const;
 
 	/// required to dispatch to type-specific CostTerm methods via vtable
@@ -354,7 +356,7 @@ class SubTrajectory : public SolutionBase
 	robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
 	void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; }
 
-	void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override;
+	void appendTo(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override;
 
 	double computeCost(const CostTerm& cost, std::string& comment) const override;
 
@@ -387,7 +389,7 @@ class SolutionSequence : public SolutionBase
 	void push_back(const SolutionBase& solution);
 
 	/// append all subsolutions to solution
-	void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override;
+	void appendTo(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override;
 
 	double computeCost(const CostTerm& cost, std::string& comment) const override;
 
@@ -418,8 +420,8 @@ class WrappedSolution : public SolutionBase
 	  : SolutionBase(creator, cost), wrapped_(wrapped) {}
 	explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped)
 	  : WrappedSolution(creator, wrapped, wrapped->cost()) {}
-	void fillMessage(moveit_task_constructor_msgs::Solution& solution,
-	                 Introspection* introspection = nullptr) const override;
+	void appendTo(moveit_task_constructor_msgs::Solution& solution,
+	              Introspection* introspection = nullptr) const override;
 
 	double computeCost(const CostTerm& cost, std::string& comment) const override;
 

core/python/bindings/src/core.cpp

@@ -124,9 +124,9 @@ void export_core(pybind11::module& m) {
 	        ":visualization_msgs:`Marker`: Markers to visualize important aspects of the trajectory (read-only)")
 	    .def(
 	        "toMsg",
-	        [](const SolutionBasePtr& s) {
+	        [](const SolutionBase& self) {
 		        moveit_task_constructor_msgs::Solution msg;
-		        s->fillMessage(msg);
+		        self.toMsg(msg);
 		        return msg;
 	        },
 	        "Convert to the ROS message ``Solution``");
@@ -495,7 +495,7 @@ void export_core(pybind11::module& m) {
 
 		        MoveGroupInterface::Plan plan;
 		        moveit_task_constructor_msgs::Solution serialized;
-		        solution->fillMessage(serialized);
+		        solution->toMsg(serialized);
 
 		        for (const moveit_task_constructor_msgs::SubTrajectory& traj : serialized.sub_trajectory) {
 			        if (!traj.trajectory.joint_trajectory.points.empty()) {

core/src/introspection.cpp

@@ -146,9 +146,7 @@ void Introspection::registerSolution(const SolutionBase& s) {
 }
 
 void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, const SolutionBase& s) {
-	s.fillMessage(msg, this);
-	s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
-
+	s.toMsg(msg, this);
 	msg.task_id = impl->task_id_;
 }
 

core/src/storage.cpp

@@ -204,6 +204,11 @@ void SolutionBase::markAsFailure(const std::string& msg) {
 	}
 }
 
+void SolutionBase::toMsg(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const {
+	appendTo(msg, introspection);
+	start()->scene()->getPlanningSceneMsg(msg.start_scene);
+}
+
 void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection) const {
 	info.id = introspection ? introspection->solutionId(*this) : 0;
 	info.cost = this->cost();
@@ -216,7 +221,7 @@ void SolutionBase::fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, In
 	std::copy(markers.begin(), markers.end(), info.markers.begin());
 }
 
-void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const {
+void SubTrajectory::appendTo(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const {
 	msg.sub_trajectory.emplace_back();
 	moveit_task_constructor_msgs::SubTrajectory& t = msg.sub_trajectory.back();
 	SolutionBase::fillInfo(t.info, introspection);
@@ -235,7 +240,7 @@ void SolutionSequence::push_back(const SolutionBase& solution) {
 	subsolutions_.push_back(&solution);
 }
 
-void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const {
+void SolutionSequence::appendTo(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const {
 	moveit_task_constructor_msgs::SubSolution sub_msg;
 	SolutionBase::fillInfo(sub_msg.info, introspection);
 
@@ -257,7 +262,7 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg,
 	msg.sub_trajectory.reserve(msg.sub_trajectory.size() + subsolutions_.size());
 	for (const SolutionBase* s : subsolutions_) {
 		size_t current = msg.sub_trajectory.size();
-		s->fillMessage(msg, introspection);
+		s->appendTo(msg, introspection);
 
 		// zero IDs of sub solutions with same creator as this
 		if (s->creator() == this->creator()) {
@@ -274,9 +279,8 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co
 	return f(*this, comment);
 }
 
-void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution,
-                                  Introspection* introspection) const {
-	wrapped_->fillMessage(solution, introspection);
+void WrappedSolution::appendTo(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection) const {
+	wrapped_->appendTo(solution, introspection);
 
 	// prepend this solutions info as a SubSolution msg
 	moveit_task_constructor_msgs::SubSolution sub_msg;

core/src/task.cpp

@@ -270,8 +270,7 @@ moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
 	ac.waitForServer();
 
 	moveit_task_constructor_msgs::ExecuteTaskSolutionGoal goal;
-	s.fillMessage(goal.solution, pimpl()->introspection_.get());
-	s.start()->scene()->getPlanningSceneMsg(goal.solution.start_scene);
+	s.toMsg(goal.solution, pimpl()->introspection_.get());
 
 	ac.sendGoal(goal);
 	ac.waitForResult();

demo/src/pick_place_task.cpp

@@ -502,7 +502,7 @@ bool PickPlaceTask::execute() {
 	// actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>
 	// execute("execute_task_solution", true); execute.waitForServer();
 	// moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal;
-	// task_->solutions().front()->fillMessage(execute_goal.solution);
+	// task_->solutions().front()->toMsg(execute_goal.solution);
 	// execute.sendGoalAndWait(execute_goal);
 	// execute_result = execute.getResult()->error_code;