rviz: catch + display invalid-robot-model exception

Author: rhaschke

visualization/motion_planning_tasks/src/remote_task_model.cpp

@@ -422,17 +422,13 @@ DisplaySolutionPtr RemoteTaskModel::getSolution(const QModelIndex& index) {
 			// request solution via service
 			moveit_task_constructor_msgs::GetSolution srv;
 			srv.request.solution_id = id;
-			try {
-				if (get_solution_client_.call(srv)) {
-					id_to_solution_[id] = result = processSolutionMessage(srv.response.solution);
-					return result;
-				} else {  // on failure mark remote task as destroyed: don't retrieve more solutions
-					get_solution_client_.shutdown();
-					flags_ |= IS_DESTROYED;
-				}
-			} catch (const std::exception& e) {
-				ROS_ERROR("exception: %s", e.what());
+			if (get_solution_client_.call(srv)) {
+				id_to_solution_[id] = result = processSolutionMessage(srv.response.solution);
+				return result;
 			}
+			// on failure mark remote task as destroyed: don't retrieve more solutions
+			get_solution_client_.shutdown();
+			flags_ |= IS_DESTROYED;
 		}
 		return result;
 	}

visualization/motion_planning_tasks/src/task_display.cpp

@@ -216,10 +216,16 @@ void TaskDisplay::taskStatisticsCB(const moveit_task_constructor_msgs::TaskStati
 
 void TaskDisplay::taskSolutionCB(const moveit_task_constructor_msgs::SolutionConstPtr& msg) {
 	setStatus(rviz::StatusProperty::Ok, "Task Monitor", "OK");
-	const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(*msg);
-	if (s)
-		trajectory_visual_->showTrajectory(s, false);
-	return;
+	try {
+		const DisplaySolutionPtr& s = task_list_model_->processSolutionMessage(*msg);
+		if (s)
+			trajectory_visual_->showTrajectory(s, false);
+		else
+			setSolutionStatus(false);
+	} catch (const std::invalid_argument& e) {
+		ROS_ERROR_STREAM(e.what());
+		setSolutionStatus(false, e.what());
+	}
 }
 
 void TaskDisplay::changedTaskSolutionTopic() {
@@ -249,11 +255,11 @@ void TaskDisplay::changedTaskSolutionTopic() {
 	setStatus(rviz::StatusProperty::Warn, "Task Monitor", "No messages received");
 }
 
-void TaskDisplay::setSolutionStatus(bool ok) {
+void TaskDisplay::setSolutionStatus(bool ok, const char* msg) {
 	if (ok)
 		setStatus(rviz::StatusProperty::Ok, "Solution", "Ok");
 	else
-		setStatus(rviz::StatusProperty::Warn, "Solution", "Retrieval failed");
+		setStatus(rviz::StatusProperty::Warn, "Solution", msg ? msg : "Retrieval failed");
 }
 
 void TaskDisplay::onTasksInserted(const QModelIndex& parent, int first, int last) {

visualization/motion_planning_tasks/src/task_display.h

@@ -85,7 +85,7 @@ class TaskDisplay : public rviz::Display
 	void load(const rviz::Config& config) override;
 
 	void setName(const QString& name) override;
-	void setSolutionStatus(bool ok);
+	void setSolutionStatus(bool ok, const char* msg = "");
 
 	TaskListModel& getTaskListModel() { return *task_list_model_; }
 	TaskSolutionVisualization* visualization() const { return trajectory_visual_.get(); }

visualization/motion_planning_tasks/src/task_panel.cpp

@@ -483,8 +483,14 @@ void TaskView::onCurrentSolutionChanged(const QModelIndex& current, const QModel
 	Q_ASSERT(task);
 
 	TaskSolutionVisualization* vis = display->visualization();
-	const DisplaySolutionPtr& solution = task->getSolution(current);
-	display->setSolutionStatus(bool(solution));
+	DisplaySolutionPtr solution;
+	try {
+		solution = task->getSolution(current);
+		display->setSolutionStatus(bool(solution));
+	} catch (const std::invalid_argument& e) {
+		ROS_ERROR_STREAM(e.what());
+		display->setSolutionStatus(false, e.what());
+	}
 	vis->interruptCurrentDisplay();
 	vis->showTrajectory(solution, true);
 }
@@ -500,8 +506,14 @@ void TaskView::onSolutionSelectionChanged(const QItemSelection& selected, const
 
 	display->clearMarkers();
 	for (const auto& index : selected_rows) {
-		const DisplaySolutionPtr& solution = task->getSolution(index);
-		display->setSolutionStatus(bool(solution));
+		DisplaySolutionPtr solution;
+		try {
+			solution = task->getSolution(index);
+			display->setSolutionStatus(bool(solution));
+		} catch (const std::invalid_argument& e) {
+			ROS_ERROR_STREAM(e.what());
+			display->setSolutionStatus(false, e.what());
+		}
 		display->addMarkers(solution);
 	}
 }

visualization/visualization_tools/src/display_solution.cpp

@@ -39,6 +39,7 @@
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/robot_trajectory/robot_trajectory.h>
 #include <ros/console.h>
+#include <boost/format.hpp>
 
 namespace moveit_rviz_plugin {
 
@@ -88,9 +89,9 @@ const MarkerVisualizationPtr DisplaySolution::markers(const DisplaySolution::Ind
 void DisplaySolution::setFromMessage(const planning_scene::PlanningScenePtr& start_scene,
                                      const moveit_task_constructor_msgs::Solution& msg) {
 	if (msg.start_scene.robot_model_name != start_scene->getRobotModel()->getName()) {
-		ROS_ERROR("Solution for model '%s' but model '%s' was expected", msg.start_scene.robot_model_name.c_str(),
-		          start_scene->getRobotModel()->getName().c_str());
-		throw std::runtime_error("invalid robot model");
+		static boost::format fmt("Solution for model '%s' but model '%s' was expected");
+		fmt % msg.start_scene.robot_model_name.c_str() % start_scene->getRobotModel()->getName().c_str();
+		throw std::invalid_argument(fmt.str());
 	}
 
 	// initialize parent scene from solution's start scene