create a single timestamp in Controller::do_publishing()
There are multiple calls to ros::Time::now()
inside Controller::do_publishing()
but all the points to be published correspond to the same timestamp.
Examples:
if (panda_rundata_publisher.trylock())
{
panda_rundata_publisher.msg_.header.stamp = ros::Time::now();
if (pose_curr_publisher.trylock())
{
tf::poseKDLToMsg(X_curr_, X_curr_msg_);
geometry_msgs::PoseStamped X_curr_stamp;
pose_curr_publisher.msg_.header.stamp = ros::Time::now();