Merge #154: Fix solution execution from rviz

Author: rhaschke

.travis.yml

@@ -8,18 +8,16 @@ compiler: gcc
 cache: ccache
 
 notifications:
-  email:
-    recipients:
-      - [email protected]
-      - [email protected]
+  email: true
+
 env:
   global:
-    - ROS_DISTRO=melodic
+    - DOCKER_IMAGE=moveit/moveit:melodic-source
     - UPSTREAM_WORKSPACE=.rosinstall
 
 jobs:
   - env: TEST=clang-format
-  - env: TEST=code-coverage ROS_REPO=ros-shadow-fixed
+  - env: TEST=code-coverage
   - compiler: clang
     env: DOCKER_IMAGE=moveit/moveit:master-source
 

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