Display Trajectory Message has less wp & wrong timing

[MichalisPapadakis]

Description

When I am publishing a trajectory in display_planned_path with 307 waypoints, of total duration of around 32.5s (which i can check by printing 1. time_from_start of the last waypoint of the published trajectory msg, and 2. by using getDuration from the trajectory object of moveit) I get an unexpected output.

ROS Distro

Humble

OS and version

Ubuntu 22.04.5 LTS

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

main

Which RMW are you using?

CycloneDDS

Steps to Reproduce

After generating the trajectory, I am publishing it with: Relevant Code:

moveit_msgs::msg::DisplayTrajectory disp;
disp.model_id = robot_model_ptr->getName();
disp.trajectory.resize(1);
downsampled_traj->getRobotTrajectoryMsg(disp.trajectory[0]);
moveit::core::robotStateToRobotStateMsg(downsampled_traj->getFirstWayPoint(), disp.trajectory_start);
display_path_publisher_->publish(disp);
auto time_start =  disp.trajectory[0].joint_trajectory.points.back().time_from_start;
double time_start_double = static_cast<double>(time_start.sec) + static_cast<double>(time_start.nanosec)*1e-9 ;
RCLCPP_INFO(LOGGER, "Total Trajectory duration after conversion to msg: %.3f ",time_start_double ); //prints out 32.5
RCLCPP_INFO(LOGGER,"Num JS %ld", disp.trajectory[0].joint_trajectory.points.size()); //prints out 307

Expected behavior

Using ros2 topic echo /display_planned_path, I would expect to get a trajectory of 307 points, with a duration of 32.5s and that using the rviz plugin, i could play the trajectory back at actual speed using 1x State Display Time.

Actual behavior

Unexpected Behavior Description:

1. Specifically using ros2 topic echo /display_planned_path I get a message of around 128 waypoints, with a total duration of 13.5s.
2. Also, the time_from_start in this echoed message is not strictly ascending.
3. Finally, when i play the trajectory with moveit_ros_visualization::Trajectory rviz plugin, I cannot play it with State Display Time 1x but only with 0.1s, 0.05s etc.
4. It must be noted, that the robotic arm seems to travels the total desired path, but at a different speed.

Backtrace or Console output

No response

[github-actions[bot]]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[github-actions[bot]]

This issue was closed because it has been stalled for 45 days with no activity.