Rosanswers logo

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();
        msg_sys.data = ss.str();

        //ROS_INFO("%s", msg_sys.data.c_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::strtod (c_enco,&pEnd);
        Encoder_dx = std::strtod (pEnd,&pEnd);
        //printf("E1 %lf, E2 %lf", Encoder_sx, Encoder_dx);

        Distance_sx = (Encoder_sx)*125;
        Distance_dx = (Encoder_dx)*125;

        last_time =ros::Time::now().toSec();
        Vel_sx = ((Distance_prev_sx - Distance_sx) / (last_time - current_time))/1000; // m / sec 
        Vel_dx = ((Distance_prev_dx - Distance_dx) / (last_time - current_time))/1000;  

        Distance_prev_sx = Distance_sx;
        Distance_prev_dx = Distance_dx;

        Vel_linear = (Vel_dx + Vel_sx)/2;
        w = (Vel_dx - Vel_sx)/0.945;

        ros::Time time2 = ros::Time::now();

        //Publish velocity topic
        ssvel << time2 << "," << Distance_sx << "," << Distance_dx << "," << Vel_sx << "," << Vel_dx << "," << Vel_linear << "," << w;
        msg_vel.data = ssvel.str();

        ros::Time time3 = ros::Time::now();

        std::replace(enco.begin(), enco.end(), ':', ',');
        //Publish enco topic
        ss2 << time3 << "," << enco << "," << count;
        msg_enc.data = ss2.str();




  return 0;

Originally posted by Marcus Barnet on ROS Answers with karma: 287 on 2017-11-23

Post score: 0

Original comments

Comment by lmathieu on 2017-11-23:
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 ?

Comment by Marcus Barnet on 2017-11-23:
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.

Comment by Marcus Barnet on 2017-11-23:
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.

Comment by Humpelstilzchen on 2017-11-24:
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?

Comment by Marcus Barnet on 2017-11-24:
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?


1 Answer 1


Rosanswers logo

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.

Originally posted by VictorLamoine with karma: 1505 on 2017-11-24

This answer was ACCEPTED on the original site

Post score: 2


Your Answer

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