Issue when periodically republishing Joint Trajectory messages

edit

I have one planner-node, which sends single messages of type "trajectory_msgs/JointTrajectory" to the topic "/trajectory_controller/command".

Makes sense.

Now I try to create another node, which reads that message and reproduces it and publishes it back on the same topic. This creates a periodic loop since the published messages are read again by the same node.

This seems like a really bad idea.

...this program is not able to react to new messages send by the planner-node in between.

This is to be expected. First, your example code is currently echoing itself (with no explicit delay), probably causing congestion on that topic. Second, since ROS uses TCP delivery then there's no guarantee that a new "outside" message published to the /trajectory_controller/command topic will be received "last" or after the previous "echoed" message published to /trajectory_controller/command. Someone correct me if this is wrong.

^ When I wrote this I hadn't noticed last_point_time.sleep();
^ Also, I got mixed up. While TCP doesn't guarantee that packets will be delivered in the same order that they were sent, the higher level data constructs (like ROS messages) should be "received" in the same order they were sent - of course it is possible for a message(s) to be completely lost on an imperfect network but that's another topic...

Somehow the reproducing loop needs to be stopped and restarted again based on the new message from the planner-node.

What are you ultimately trying to accomplish? I'm confident that we can find a better solution. See: http://xyproblem.info/

EDIT:

Testing your code as is, it will echo the first msg published to the /trajectory_controller/command topic and generally ignore subsequent "outside" msgs published to that topic.

"outside" publisher: ..A...........B...............B......B........................
steady_traj_publisher: .cb------>.cb------>.cb------>.cb------>.cb------>.cb------>
pub_ output: ...................A.........A.........A.........A.........A.........A
sub_ queue: ...........A.........A..B//////A........BA.....B///A.........A.........

This is because your Subscriber's queue_size is set to 1. See the roscpp Subscriber documentation.

Now if we increase the Subscriber's queue_size to 2, your node will echo two of the previous "outside" messages received.

"outside" publisher: ..A...........B...............C.........................D.....
steady_traj_publisher: .cb------>.cb------>.cb------>.cb------>.cb------>.cb------>
pub_ output: ...................A.........A.........B.........A.........C.........A
sub_ queue[0]: .........A........A..B///////A/////////C/////////A/////////C///D////
sub_ queue[1]: ............................A........C/.........A.........C.........

Quick & Dirty (and not necessarily good) Hack...

Move the delay out of your callback function (like so):

#include "ros/ros.h"
#include "trajectory_msgs/JointTrajectory.h"

class SubscribeAndPublish
{
  public:
    SubscribeAndPublish()
    {
      //Topic you want to publish
      pub_ = n_.advertise<trajectory_msgs::JointTrajectory>("/trajectory_controller/command", 1);

      //Topic you want to subscribe
      sub_ = n_.subscribe("/trajectory_controller/command", 1, &SubscribeAndPublish::callback, this);

      last_point_time_ = ros::Duration(1);  //Initial value
    }

    void callback(const trajectory_msgs::JointTrajectory::ConstPtr& msg)
    {
      ROS_INFO("msg received: %f", msg->points[0].positions[0]);
      std::vector<int>::size_type size = msg->points.size();
      last_point_time_ = msg->points[size-1].time_from_start;

      ros::Time newHeaderTime = msg->header.stamp + last_point_time_;

      trajectory_msgs::JointTrajectory output;
      output = *msg;
      output.header.stamp ...