Markers of a SolutionSequence are not displayed by rviz

[leander2189]

I've customized Connect and MoveTo stages so they display collisions when they fail. I've done this by adding markers to the solution inside compute function.

MoveTo stages work perfectly, but Connect stages don't display anything, despite of containing some markers.

Is there anything preventing Connect stages to display the markers? A difference I noticed is that in MoveTo stage I am adding the markers to a SubTrajectory, while in Connect I am adding them to a SolutionBasePtr

[rhaschke]

Please share your code (e.g. as a pull request) and I can have a look.
I would expect that markers are handled in the same fashion for SubTrajectory and other SolutionBase classes.

[leander2189]

This is the piece of code

void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
	const auto& props = properties();
	double timeout = this->timeout();
	MergeMode mode = props.get<MergeMode>("merge_mode");
	const auto& path_constraints = props.get<moveit_msgs::msg::Constraints>("path_constraints");

	const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState();
	std::vector<robot_trajectory::RobotTrajectoryConstPtr> sub_trajectories;

	std::vector<planning_scene::PlanningSceneConstPtr> intermediate_scenes;
	planning_scene::PlanningSceneConstPtr start = from.scene();
	intermediate_scenes.push_back(start);

	bool success = false;
	std::vector<double> positions;
	moveit_msgs::msg::MotionPlanResponse plan_response;
	visualization_msgs::msg::MarkerArray arr;
	for (const GroupPlannerVector::value_type& pair : planner_) {
		// set intermediate goal state
		planning_scene::PlanningScenePtr end = start->diff();
		const moveit::core::JointModelGroup* jmg = final_goal_state.getJointModelGroup(pair.first);
		final_goal_state.copyJointGroupPositions(jmg, positions);
		moveit::core::RobotState& goal_state = end->getCurrentStateNonConst();
		goal_state.setJointGroupPositions(jmg, positions);
		goal_state.update();
		intermediate_scenes.push_back(end);

		robot_trajectory::RobotTrajectoryPtr trajectory;

		success = pair.second->plan(start, end, jmg, timeout, trajectory, plan_response, arr, path_constraints);
		sub_trajectories.push_back(trajectory);  // include failed trajectory

		if (!success)
			break;

		// continue from reached state
		start = end;
	}

	SolutionBasePtr solution;
	
	if (success && mode != SEQUENTIAL)  // try to merge
		solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
	if (!solution)  // success == false or merging failed: store sequentially
		solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
	if (!success)  // error during sequential planning
	{
		std::copy(arr.markers.begin(), arr.markers.end(), std::back_inserter(solution->markers()));
		solution->markAsFailure("Error code: " + std::to_string(plan_response.error_code.val));
		RCLCPP_WARN(rclcpp::get_logger("mtc"), "[%s] CONNECT MARKERS: %lu, markerNS=%s", this->name().c_str(),
		            solution->markers().size(), this->markerNS().c_str());
		for (auto& m : solution->markers()) {
			RCLCPP_WARN(rclcpp::get_logger("mtc"), "x=%.3f, y=%.3f, z=%.3f", m.pose.position.x, m.pose.position.y,
			            m.pose.position.z);
		}
	}

	connect(from, to, solution);
}

For this to work I've modified both moveit2 and PipelinePlanner::plan so they return the MotionPlanResponse and the MarkerArray (with collisions)

[rhaschke]

In principle this should work. Could you check whether the SolutionBasePtr returned by merge or makeSequential is a SubTrajectory or a SolutionSequence?

[leander2189]

In this particular case, solution was being built through make_sequential, that returns a SolutionSequence. Inserting the markers inside the subsolutions is apparently working

[rhaschke]

Looks like that rviz ignores markers of SolutionSequences then.

[rhaschke]

I confirmed that the rviz visualization code only considers markers of SubTrajectories for now.

Conceptually, I'm not yet sure, whether we should allow markers for something else. SubTrajectories represent real trajectories, while SolutionSequences just logically combine several SubTrajectories - possibly in a hierarchical fashion.
When displaying a solution in rviz, a complex hierarchical sequence is serialized into a flat vector of robot states from all subtrajectories, which are displayed one by one. If we switch to another subtrajectory, the corresponding markers are visualized.

If we allow markers for SolutionSequences (for example to visualize the whole path of the end-effector), we would need to hierarchically resolve which subset of sequence markers should be visualized at a given vector index.

For now, I'm tempted to remove markers from solution structures and messages other than SubTrajectories.
Any comments, @v4hn?

[leander2189]

For reference, knowing that SolutionSequences' markers are ignored, I managed to add the markers to the right SubTrajectory, which has helped me a lot to debug the Task I am planning for.

Also, to retrieve markers from the PipelinePlanner, I had to modify moveit2 sources (as plan method publishes that info into /display_contacts but does not return that info to the call stack). I guess what I am trying to say is that my code is not very suitable to do a Pull Request