Issue when periodically republishing Joint Trajectory messages
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;
}
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...
.. 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...
...and stop republishing message A?
yes the node is starting to republish both messages A and B, however, he should stop republishing A when B arrives