Ask Your Question
0

Vector3 deserialize problem

asked 2015-03-09 05:51:54 -0500

updated 2015-03-10 05:21:49 -0500

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...

edit retag flag offensive close merge delete

Comments

Good to hear that you solved it, but we don't normally close questions on ROS Answers. If an answer solved your issue, click the checkmark to the left of it to mark it as accepted. The question can remain open to allow others to comment on it.

gvdhoorn gravatar image gvdhoorn  ( 2015-03-10 05:38:52 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2015-03-09 06:28:44 -0500

gvdhoorn gravatar image

updated 2015-03-10 05:40:18 -0500

I'm not sure, but this might be related to rosserial/issues/86 (which was fixed in rosserial/pull/144, but only on indigo-devel).

Are you running rosserial from debs, or have you built it from source? The debs perhaps don't include the necessary fixes.


Edit:

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..

You could also try and see whether you can build rosserial from source, that would allow you to run the newest version of rosserial under Hydro.

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

Stats

Asked: 2015-03-09 05:51:54 -0500

Seen: 91 times

Last updated: Mar 10 '15