Ask Your Question
0

Issue when periodically republishing Joint Trajectory messages

asked 2018-07-18 07:46:27 -0600

JaFeKl gravatar image

updated 2018-07-18 11:29:53 -0600

Hello ROS community,

I am trying the following thing: I have one planner-node, which sends single messages of type "trajectory_msgs/JointTrajectory" to the topic "/trajectory_controller/command". 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. So far so good, however, this program is not able to react to new messages send by the planner-node in between. Somehow the reproducing loop needs to be stopped and restarted again based on the new message from the planner-node. Do you guys have any suggestion on how to do this?

Here you can find a sketch, which illustrates what I am trying to achieve

Currently, the node is looking like this:

#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);
  }

 void callback(const trajectory_msgs::JointTrajectory::ConstPtr& msg)
  {
    std::vector<int>::size_type size = msg->points.size();
    ros::Duration 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;

    last_point_time.sleep();

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

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

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;

  ros::spin();

  return 0;
}
edit retag flag offensive close merge delete

Comments

2 questions: a) What is your ultimate goal with this scheme / why do you want to echo messages on a Topic? b) Your text description suggests that when a "new" message is published on the Topic, you want to start republishing the "new" message and stop republishing the "old" message...

josephcoombe gravatar imagejosephcoombe ( 2018-07-19 08:14:59 -0600 )edit

.. However, the image you attached appears to suggest that the node should simply republish ALL unique messages published to at set intervals. Which is it? If the node is republishing message A, and publish a new message B to that topic, does the node start republished message B...

josephcoombe gravatar imagejosephcoombe ( 2018-07-19 08:16:43 -0600 )edit

...and stop republishing message A?

josephcoombe gravatar imagejosephcoombe ( 2018-07-19 08:17:13 -0600 )edit

yes the node is starting to republish both messages A and B, however, he should stop republishing A when B arrives

JaFeKl gravatar imageJaFeKl ( 2018-07-19 12:49:10 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-07-18 10:13:39 -0600

updated 2018-07-20 16:26:48 -0600

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 ...
(more)
edit flag offensive delete link more

Comments

Thanks for your response, I am trying to achieve the following: see this picture the problem appears when the planner_node sends a second message. How can I change the architecture to be able to restart the reproducing process based on the new message?

JaFeKl gravatar imageJaFeKl ( 2018-07-18 11:13:42 -0600 )edit

I updated my answer with some hints.

josephcoombe gravatar imagejosephcoombe ( 2018-07-20 16:27:00 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-07-18 07:46:27 -0600

Seen: 169 times

Last updated: Jul 20 '18