ros is publishing incorrect values?

edit

I am currently working with rosserial, and trying to publish the number of steps my motor has moved.

The hardware moves correctly, but for some reason i am not able to publish the right number.. somehow am I publishing a prior value, than the last incremented value.

I confirmed this by publishing within my ISR an saw how it incremented..

Here is my code:

#include "speed_profile.h"


volatile speed_profile profile;

ros::NodeHandle nh;

std_msgs::Int16 status_step_count;
std_msgs::Int16 status_tipper_availability;
ros::Publisher chatter_status("tipper_status", &status_step_count);
ros::Publisher chatter_availabilty("tipper_availability", &status_tipper_availability);

bool global_state = false;
bool moved_to_position = true;
int received_data = 0;
int receive_percentage = 0;


void call_back_control( const std_msgs::Empty & msg)
{
  global_state = true;
  // For HMI
  // received_data  = msg.data
}

ros::Subscriber<std_msgs::Empty> sub_control("tipper_control", &call_back_control );

void output_pin_setup()
{
  pinMode(en_pin, OUTPUT);
  pinMode(dir_pin, OUTPUT);
  pinMode(step_pin, OUTPUT);
  nh.initNode();
  nh.advertise(chatter_status);
  nh.advertise(chatter_availabilty);
  nh.subscribe(sub_control);
  //nh.subscribe(sub_command);
  profile.moved_steps = 0;
  profile.current_step_position = 0;
  delay(1000);
  nh.getHardware()->setBaud(57600);

}

void timer1_setup()
{
  // Tells what part of speed ramp we are in.
  profile.run_state = STOP;

  // Timer/Counter 1 in mode 4 CTC (Not running).
  TCCR1B = (1 << WGM12);

  // Timer/Counter 1 Output Compare A Match Interrupt enable.
  TIMSK1 = (1 << OCIE1A);
}


void compute_speed_profile(signed int motor_steps, unsigned int motor_accel, unsigned int motor_decel, unsigned int motor_speed)
{
  while (global_state == false)
  {
    //do nothing
    status_step_count.data = profile.current_step_position; //profile.current_step_position;
    status_tipper_availability.data = 0;

    chatter_status.publish( &status_step_count);
    chatter_availabilty.publish(&status_tipper_availability);
    nh.spinOnce();
    //moved_to_position = true;
    //motor_steps = received_data  // not currently added for testing purposes
    delay(100);

  }

  digitalWrite(en_pin, HIGH);
  delay(1);

  if (motor_steps < 0)
  {
    profile.dir = CCW;
    motor_steps = -motor_steps;
    digitalWrite(dir_pin, LOW);
  }
  else
  {
    profile.dir = CW;
    digitalWrite(dir_pin, HIGH);
  }

  delay(1);

  computation_of_speed_profile(motor_steps, motor_accel, motor_decel, motor_speed); // some process not related 

  OCR1A = 10;
  // Set Timer/Counter to divide clock by 8
  TCCR1B |= ((0 << CS12) | (1 << CS11) | (0 << CS10));


  while (global_state == true)
  { 
    cli();
    int temp =  profile.moved_steps;
    sei();

    if (profile.dir == CCW)
    {
      profile.current_step_position =   temp;
    }
    else
    {
      profile.current_step_position =  temp;
    }        // Keeping ISR disable shortest time possible

    //status_step_count.data = ((float)(profile.current_step_position / 23400.0)) * 100.0; //map(profile.current_step_position,0,23400,0,100); // could be expensive -- Nice flowing is only available with float
    status_step_count.data = profile.current_step_position;
    status_tipper_availability.data = 1;

    chatter_availabilty.publish(&status_tipper_availability);
    chatter_status.publish( &status_step_count);
    nh.spinOnce();
    delay(100);
  }
}





ISR(TIMER1_COMPA_vect)
{
  // Holds next delay period.
  unsigned int new_first_step_delay;

  // Remember the last step delay used when accelrating.
  static int last_accel_delay;

  // Counting steps when moving.
  //static unsigned int step_count = 0;

  // Keep track of remainder from new_step-delay calculation to incrase accurancy
  static unsigned int rest = 0;
  OCR1A = profile.first_step_delay;
  switch (profile.run_state)
  {

    case STOP:
      //profile.moved_steps = step_count;
      //step_count = 0;
      //profile.moved_steps = 0;

      global_state = false;
      rest = 0;
      TCCR1B &= ~((1 << CS12) | (1 << CS11) | (1 << CS10)); // Stop the timer,  No clock source
      break;

    case ACCEL:
      digitalWrite(step_pin, !digitalRead(step_pin));
      //delay(1);
      //digitalWrite(step_pin,LOW);
      //step_count++;
      profile.moved_steps++;
      //profile.moved_steps = step_count;
      profile.accel_count++;
      new_first_step_delay = profile.first_step_delay - (((2 * (long)profile.first_step_delay) + rest) / (4 * profile.accel_count + 1));
      rest = ((2 * (long)profile.first_step_delay) + rest) % (4 * profile.accel_count + 1);

      // Chech if we should start decelration.
      //if (step_count >= profile.decel_start)
      if(profile.moved_steps >= profile.decel_start)
      {
        profile.accel_count ...