Does an finished odometry node exists?

edit

Hello,

i ended up doing it by myself. And since maybe someone is interested in (or what to make suggestions). Anything i could/should change ?

#include "ros/ros.h"                                    
#include "std_msgs/Int64MultiArray.h"   
#include "geometry_msgs/Vector3.h"      
#include "math.h"               

// -------------------------------------------------------------

// prototype + global Var
void encoderCallback(std_msgs::Int64MultiArray array);
#define TIME        0
#define E_L         1
#define E_R         2

bool new_data;

geometry_msgs::Vector3 encoder;     // time, left, right
#define ENC_T               encoder.x
#define ENC_L               encoder.y
#define ENC_R               encoder.z

geometry_msgs::Vector3 encoder_new;// time, left, right
#define ENC_T_NEW       encoder_new.x
#define ENC_L_NEW       encoder_new.y
#define ENC_R_NEW       encoder_new.z

geometry_msgs::Vector3 position;        // x, y, theta
#define POS_X               position.x
#define POS_Y               position.y
#define POS_THETA           position.z

#define R   75      // [mm] wheel radius
#define D   530     // [mm] distance between wheels
#define C   148000  // [] counts per turn, ecperimental value 

// -------------------------------------------------------------

int main(int argc, char **argv)
{

    // init node
    ros::init(argc, argv, "OdometryNode");      
    ros::NodeHandle n;                  
    ros::Rate loop_rate(10);    

    // init topics
    ros::Publisher  Position  = n.advertise<geometry_msgs::Vector3>("Odometry_Pos", 1);
    ros::Subscriber Encoders  = n.subscribe<std_msgs::Int64MultiArray>("MotorDrive_encoder", 1, encoderCallback);

    // init global vars
    POS_X       = 0;
    POS_Y       = 0;
    POS_THETA   = 0;
    new_data    = false;

    // wait for first encoder values arrive
    while (new_data == false && ros::ok()){
        ros::spinOnce();
        loop_rate.sleep();}
    ENC_T = ENC_T_NEW;
    ENC_L = ENC_L_NEW;
    ENC_R = ENC_R_NEW;
    new_data = false;

    // values independent from encoders
    float M_PI_2 = 2.0*M_PI;
    float d_2 = D/2;
    float r_PI_c = R*2*M_PI/C;          // orientation in: rad (=2*PI) or deg (=360)
    float r_PI_d_c = R*2*M_PI/(D*C);    // constant for the angle

    while (ros::ok())
    {
        // just when needed
        if(new_data == true)
        {
            // get driven distance in counts
            long e_l = ENC_L_NEW - ENC_L;
            long e_r = ENC_R_NEW - ENC_R;
            ROS_INFO("e_l %li, e_r %li", e_l, e_r);

            // save new data as current ones
            ENC_T = ENC_T_NEW;
            ENC_L = ENC_L_NEW;
            ENC_R = ENC_R_NEW;

            // rotation + maybe transation
            float dx, dy, alpha;
            if((e_l - e_r) > 10 || (e_l - e_r) < -10)
            {
                // radius + angle of the movement   
                float e_temp = (e_l + e_r) / (e_l - e_r);
                float r = d_2 * e_temp;
                alpha = (e_l - e_r) * r_PI_d_c;

                // move in robot frame
                dx = r * sin(alpha);
                dy = r * (1-cos(alpha));

            }
            else    // just translation
            {
                dx = e_l * r_PI_c;
                dy = 0.00;
                alpha = 0.00;
            }

            // prepare transformation
            float temp_cos = cos(POS_THETA);
            float temp_sin = sin(POS_THETA);

            // transform to global frame (rotation matrix)
            POS_X       += dx*temp_cos - dy*temp_sin;
            POS_Y       += dx*temp_sin + dy*temp_cos;
            POS_THETA    += alpha;
            POS_THETA   %= M_PI_2;

            // publish new estimate of position
            Position.publish(position);
            new_data = false;
        }

        // tell ROS to repeat
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

// -------------------------------------------------------------

void encoderCallback(std_msgs::Int64MultiArray data_new)
{
    // just save the data, rest is in main loop
    ENC_T_NEW = data_new.data[TIME];
    ENC_L_NEW = data_new.data[E_L];
    ENC_R_NEW = data_new.data[E_R];

    // flag to process new data
    new_data = true;
    return;
}