Vector3 deserialize problem

edit

Dear all,

I found a strange problem with my robot running ROS on UDOO board. The cmd_vel published by teleop is subscribed by Arduino sketch that drive motors according to received speed. Arduino and ROS communicate using rosserial python node. When a cmd_vel is published with positive speed all works fine. Sending negative speed result in robot moving at smallest speed in back direction. After some test I found the problem on Vector3 message in Arduino ros_lib: deserialize method fails with negative float (I receive -0.0000 instead -0.200). I'm sure the published message is correct since using rostopic echo /cmd_vel print the right values.

At the end I success to make it works adding delays in deserialize method:

virtual int deserialize(unsigned char *inbuffer)
    {
      int offset = 0;
      uint32_t * val_x = (uint32_t*) &(this->x);
      offset += 3;
      *val_x = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
      *val_x |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
      *val_x |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
      uint32_t exp_x = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
      exp_x |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
      if(exp_x !=0)
        *val_x |= ((exp_x)-1023+127)<<23;
      delay(1);
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->x = -this->x;
      uint32_t * val_y = (uint32_t*) &(this->y);
      offset += 3;
      *val_y = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
      *val_y |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
      *val_y |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
      uint32_t exp_y = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
      exp_y |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
      if(exp_y !=0)
        *val_y |= ((exp_y)-1023+127)<<23;
      delay(1);
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->y = -this->y;
      uint32_t * val_z = (uint32_t*) &(this->z);
      offset += 3;
      *val_z = ((uint32_t)(*(inbuffer + offset++))>>5 & 0x07);
      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<3;
      *val_z |= ((uint32_t)(*(inbuffer + offset++)) & 0xff)<<11;
      *val_z |= ((uint32_t)(*(inbuffer + offset)) & 0x0f)<<19;
      uint32_t exp_z = ((uint32_t)(*(inbuffer + offset++))&0xf0)>>4;
      exp_z |= ((uint32_t)(*(inbuffer + offset)) & 0x7f)<<4;
      if(exp_z !=0)
        *val_z |= ((exp_z)-1023+127)<<23;
      delay(1);
      if( ((*(inbuffer+offset++)) & 0x80) > 0) this->z = -this->z;
     return offset;
    }

This is working now but I have no idea why delays are needed! I'm quite sure is as problem of Arduino but since I found this problem using ROS make sense to advertise the ROS community.

Thanks Alessandro

UPDATE

Thanks to gvdhoorn! I manually fix the ros_lib since I'm still using hydro and now it works fine. Still I cannot understand why the delay make it works too...