fix rviz solution execution

Store the set of joints involved in a received trajectories and
use this set to constrain serialization of solutions for execution to those joints only.
If this is not done, all joints of the RobotModel are considered for the trajectory,
but we might not have controllers defined for all joints, so execution will fail.

Maybe, a better approach might be to just ask the MTC planner to execute a specific solution id.
However, this requires that the planner node is still available.

Author: rhaschke

visualization/visualization_tools/include/moveit/visualization_tools/display_solution.h

@@ -76,6 +76,8 @@ class DisplaySolution
 		planning_scene::PlanningSceneConstPtr scene_;
 		/// sub trajectories, might be empty
 		robot_trajectory::RobotTrajectoryPtr trajectory_;
+		/// joints involved in the trajectory
+		std::vector<std::string> joints_;
 		/// comment of the trajectory
 		std::string comment_;
 		/// id of creating stage

visualization/visualization_tools/src/display_solution.cpp

@@ -103,8 +103,11 @@ void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& sta
 	steps_ = 0;
 	size_t i = 0;
 	for (const auto& sub : msg.sub_trajectory) {
-		data_[i].trajectory_.reset(new robot_trajectory::RobotTrajectory(ref_scene->getRobotModel(), ""));
+		data_[i].trajectory_.reset(new robot_trajectory::RobotTrajectory(ref_scene->getRobotModel(), nullptr));
 		data_[i].trajectory_->setRobotTrajectoryMsg(ref_scene->getCurrentState(), sub.trajectory);
+		data_[i].joints_ = sub.trajectory.joint_trajectory.joint_names;
+		data_[i].joints_.insert(data_[i].joints_.end(), sub.trajectory.multi_dof_joint_trajectory.joint_names.begin(),
+		                        sub.trajectory.multi_dof_joint_trajectory.joint_names.end());
 		data_[i].comment_ = sub.info.comment;
 		data_[i].creator_id_ = sub.info.stage_id;
 		steps_ += data_[i].trajectory_->getWayPointCount();
@@ -129,7 +132,7 @@ void DisplaySolution::fillMessage(moveit_task_constructor_msgs::Solution& msg) c
 	auto traj_it = msg.sub_trajectory.begin();
 	for (const auto& sub : data_) {
 		sub.scene_->getPlanningSceneDiffMsg(traj_it->scene_diff);
-		sub.trajectory_->getRobotTrajectoryMsg(traj_it->trajectory);
+		sub.trajectory_->getRobotTrajectoryMsg(traj_it->trajectory, sub.joints_);
 		++traj_it;
 	}
 }