0
$\begingroup$

Rosanswers logo

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

Originally posted by Masoud on ROS Answers with karma: 41 on 2016-09-18

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

After running roscore make sure to set the parameter use_sim_time to true:

$ rosparam set use_sim_time true

Then, launch your node.

Hope it helps!


Originally posted by jacobperron with karma: 1870 on 2016-09-18

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.