MoveItVisualTools::publishTrajectoryPath rejects valid Multi-DoF paths

[werner291]

Hello,
the function bool MoveItVisualTools::publishTrajectoryPath(const moveit_msgs::RobotTrajectory& trajectory_msg, const robot_state::RobotStateConstPtr robot_state, bool blocking) has this bit in it:

// Check if we have enough points
if (!trajectory_msg.joint_trajectory.points.size())
{
    ROS_WARN_STREAM_NAMED(name_, "Unable to publish trajectory path because trajectory has zero points");
    return false;
}

Trajectories that have multi-DoF points, but no regular points, get rejected by that check.

[simonschmeisser]

Please test if adding something like || (trajectory_msg.joint_trajectory.multi_dof.size() != 0) fixes your problem and create a pull request (PR) for it. Ideally also provide a small example to test this (does not need to be a full unit-test, just something to run quickly)