Ask Your Question

Write/Improve a ROS node to publish encoder data

asked 2017-11-23 11:07:28 -0600

Marcus Barnet gravatar image

updated 2017-11-24 03:32:00 -0600

Hi to all,

I'm using two incremental encoders (SICK DFS60B) on my skid steering robot. A full wheel turn on each side generates about 2241114 pulses.

I wrote a ROS node in C language in order to publish the encoders data and the wheel velocity. The node works well, but the problem is that the encoders data are too big. Is there any ROS package or library which can properly handle the encoders data? What can I do to improve the encoder topic and my code?
Is there any better method to publish odometry data?

EDIT 1: it seems that the motor controller that I'm using to read the encoder pulses cannot handle the current PPR. I'm going to decrease the PPR for my encoders and check if the problem goes away. In the meanwhile, how do you think I can improve my code? Is there anything I can change? Is there any ROS library which I can include in my code to optimize the velocity calculation?

    int main(int argc, char **argv)
  ros::init(argc, argv, "robot_base");
  ros::NodeHandle n;

  std::string port = "";
  int sys=0, enc=0, io=0, velocity=0, rate=10;

  n.getParam("/robot_base/port", port);
  n.getParam("/robot_base/sys", sys);
  n.getParam("/robot_base/enc", enc);
  n.getParam("/robot_base/io", io);
  n.getParam("/robot_base/velocity", velocity);
  n.getParam("/robot_base/rate", rate);

  //ROS_INFO("%d - %d - %d - %s", sys, enc, io, port.c_str());

  long long unsigned int count = 0; //Frame counter

  RoboteqDevice device;
  int status = device.Connect(port.c_str());

  ros::Publisher sys_pub = n.advertise<std_msgs::String>("robot/sys", 1);
  ros::Publisher enc_pub = n.advertise<std_msgs::String>("robot/enc", 1);
  ros::Publisher vel_pub = n.advertise<std_msgs::String>("robot/velocity", 1);
  ros::Publisher io_pub = n.advertise<std_msgs::String>("robot/io", 1);

  ros::Subscriber cmd_sub = n.subscribe("robot/cmd_vel", 1, &RoboteqDevice::cmdCallback, &device);

  double current_time =ros::Time::now().toSec();
  double last_time =ros::Time::now().toSec();

  ros::Rate loop_rate(rate);

 while (ros::ok())
    std_msgs::String msg_sys, msg_enc, msg_io, msg_vel;
    std:string temp="", volt="", amotor="", enco="", aio="", dio=""; //Strings that receives data from roboteq device
    std::stringstream ss, ss2, ss3, ssvel;  //Strings used to build topics message

    //System Topic
    if (sys == 1){
      device.GetValue(_T, 0, temp);
      device.GetValue(_V, 0, volt);
      device.GetValue(_A, amotor);

    //printf("Debug: %s -- %llu \n", temp.c_str(), count);

      if (temp != "" && volt != "" && amotor != ""){

        std::replace(temp.begin(), temp.end(), ':', ',');
        std::replace(volt.begin(), volt.end(), ':', ',');
        std::replace(amotor.begin(), amotor.end(), ':', ',');

        ros::Time time = ros::Time::now();
        ss << time << "," << temp << "," << volt << "," << amotor << "," << count;
        //msg_sys.header.stamp = ros::Time::now(); = ss.str();



    //Encoder Topic
    if (enc == 1){
      current_time =ros::Time::now().toSec();
      device.GetValue(_C, 0, enco);
       //enco ="15000:16000";

      if (enco != ""){

        if (velocity == 1){
        //Convert encoder string in long long integer
        std::string vel_enco=enco;
        std::replace(vel_enco.begin(), vel_enco.end(), ':', ' ');

        const char * c_enco = vel_enco.c_str();
        char* pEnd;
        double Encoder_sx, Encoder_dx;
        double Distance_sx, Distance_dx, Distance_prev_sx, Distance_prev_dx, Vel_sx, Vel_dx;
        float Vel_linear, w;

        Encoder_sx = std ...
edit retag flag offensive close merge delete



Hi, can you be more specific ? Is it a problem with the size of the data (memory size) or the latency (Low rate of publishing) or computing speed (CPU)?

for the first case, try Boost.Multiprecision

for the second, try nodelet if you can.

Or if this is a CPU problem, maybe try GPU computing ?

lmathieu gravatar image lmathieu  ( 2017-11-23 12:19:00 -0600 )edit

At high speed and for distances greater than 8 meters, the encoder data seem to saturate in some way. On the contrary, at low speed, everything works great. I cannot understand if it is an encoder problem or a problem with my code. The input velocity is 2200RPM at max at high speed.

Marcus Barnet gravatar image Marcus Barnet  ( 2017-11-23 13:04:47 -0600 )edit

I use a good CPU, so i do not think it's a CPU problem. I cannot understand if it is related to my encoders which cannot handle high speed or to my code which is not optimized.

Marcus Barnet gravatar image Marcus Barnet  ( 2017-11-23 13:05:45 -0600 )edit

Sounds like a sampling rate issue, how often do you sample the inputs and at what rate do you expect your 2241114 pulses to change?

Humpelstilzchen gravatar image Humpelstilzchen  ( 2017-11-24 02:09:34 -0600 )edit

It seems that the motor controller that I'm using to read the pulses cannot handle the current PPR. I'm going to decrease the PPR for my encoders and check if the problem goes away. In the meanwhile, how do you think I can improve my code? Is there anything I can change?

Marcus Barnet gravatar image Marcus Barnet  ( 2017-11-24 03:27:10 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2017-11-24 10:46:54 -0600

VictorLamoine gravatar image

Two cases:

  • Your encoder is "smart" and is counting for you, so you only fetch the current value of the encoder, you are not counting the pulses.
  • You are counting the pulses from your encoder and

First case

Fetch/publish the encoder value at any rate you need, or fetch the value only when you require it. Use time-stamps and everything should be ok.

Second case

You really shouldn't rely on "software" for this kind of task, you should set-up an interrupt on your micro-controller / microprocessor so that every-time a pulse appears from the encoder; a variable is changed to reflect the new encoder position.

The interrupt should do as least a possible so that you are sure every-time a pulse is triggered, your micro-controller is ready to enter the interrupt again. (don't do anything else than updating the value inside this interrupt, maybe just set the time-stamp of the data).

In an other thread, you should set up a loop that will get the current encoder position and publish it with a time-stamp. Make sure you don't have data races because the encoder position value is shared between the interrupt and the loop thread.

edit flag offensive delete link more

Your Answer

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

Add Answer

Question Tools

1 follower


Asked: 2017-11-23 11:07:28 -0600

Seen: 1,150 times

Last updated: Nov 24 '17