How to synchronize a ROS publisher node with gazebo simulation time?
Hi everybody
I have an offline trajectory generation program which is written in MATLAB that produce joint trajectories for my humanoid robot. I saved the trajectory in a text file and load it through a ROS publisher node that read the text file line by line and command the desired angle of each joint to corresponding topic of PID controller. Because time step of saved trajectory is 0.02 I set loop rate of publisher to 50Hz.
This publisher work well as long as the simulation runs at normal speed, but when the simulation run slower than normal the PID and simulation are not sync.
I'm wondering if there is a way to send simulation time to publisher in order to command the desired angle of that specific time?
Here is the source code of joint1 publisher node:
#include "ros/ros.h"
#include "std_msgs/Float64.h"
#include <iostream>
#include <fstream>
#include <sstream>
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<std_msgs::Float64>("/HumanoidBot/joint1_position_controller/command", 1000);
ros::Rate loop_rate(50);
int count = 0;
std::ifstream inFile;
inFile.open("example.txt");
std::string LNN;
while (ros::ok())
{
std_msgs::Float64 msg2;
getline(inFile,LNN);
float temp = ::atof(LNN.c_str());
msg2.data = temp;
chatter_pub.publish(msg2);
ros::spinOnce();
loop_rate.sleep();
++count;
}
return 0;
}