Revision history [back]

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 last "echoed" message published to /trajectory_controller/command. Someone correct me if this is wrong.

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/

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 ~~last~~ previous "echoed" message published to /trajectory_controller/command. Someone correct me if this is wrong.

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/

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 = newHeaderTime;

      int a = (int)output.header.stamp.sec;
      int b = (int)output.header.stamp.nsec;

     pub_.publish(output);
     ROS_INFO("sending message with %i seconds and %i nsec", a, b);
   }

   void spin()
   {
     while (ros::ok())
     {

       ros::spinOnce();
       last_point_time_.sleep();
     }
   }

  private:
    ros::NodeHandle n_;
    ros::Publisher pub_;
    ros::Subscriber sub_;

    ros::Duration last_point_time_;
};

int main(int argc, char **argv)
{
  //Initiate ROS
  ros::init(argc, argv, "steady_traj_publisher");

  //Create an object of class SubscribeAndPublish that will take care of everything
  SubscribeAndPublish SAPObject;
  SAPObject.spin();

  return 0;
}

This will work "most/all" of the time (I guess you call always receive a new "outside" msg in your Subscriber queue while the Subscriber callback is executing in which case it'd get overwritten).