subt: add rospy+zmq based communication with cloudsim

[zbynekwinkler]

general changes:

• cloudsim2osgar.py:
  • use rospy to get data from ROS
  • serialize using msgpack
  • send over zmq.PUSH socket as multipart message (channel, data)
• pull.py:
  • receive multipart (channel, data) message over zmq.PULL socket
  • republish on osgar channel

advantages

• no hand-parsing of ROS serialization format
• logging or compression can be set per channel
• allows multiple zmq.PUSH nodes using channel for identification

specific changes:

• cloudsim2osgar.py + ros_proxy_node.cc: move rot, orientation and acc from cc to py

[zbynekwinkler]

With #525 we can use #511 here.

[zbynekwinkler]

Here is an example data for acc. Stream number 15 is rosmsg and 34 is rospy:

0:00:25.668112 34 [199, -306, 9701]
0:00:25.668809 15 [199, -306, 9701]
0:00:25.684960 15 [170, -249, 9709]
0:00:25.685255 34 [170, -249, 9709]
0:00:25.709631 34 [135, -181, 9687]
0:00:25.710096 15 [135, -181, 9687]
0:00:25.733484 15 [149, -174, 9694]
0:00:25.733745 34 [149, -174, 9694]
0:00:25.746197 15 [1172, -323, 9696]
0:00:25.746351 34 [1172, -323, 9696]
0:00:25.771339 34 [132, -273, 9713]
0:00:25.772314 15 [132, -273, 9713]
0:00:25.794065 34 [145, -235, 9690]
0:00:25.794566 15 [145, -235, 9690]
0:00:25.817642 34 [147, -198, 9692]
0:00:25.818202 15 [147, -198, 9692]
0:00:25.834615 34 [1124, -197, 9697]
0:00:25.835417 15 [1124, -197, 9697]
0:00:25.851889 15 [145, -322, 9713]
0:00:25.852200 34 [145, -322, 9713]
0:00:25.872012 15 [149, -251, 9715]
0:00:25.872238 34 [149, -251, 9715]
0:00:25.891666 15 [134, -244, 9697]
0:00:25.891838 34 [134, -244, 9697]
0:00:26.070499 15 [131, -208, 9697]
0:00:26.070706 34 [131, -208, 9697]
0:00:26.104830 34 [136, -207, 9695]
0:00:26.105862 15 [136, -207, 9695]
0:00:26.117981 15 [117, -197, 9702]
0:00:26.123908 34 [117, -197, 9702]
0:00:26.133076 15 [140, -199, 9697]
0:00:26.133659 34 [140, -199, 9697]
0:00:26.153651 34 [1072, -364, 9698]
0:00:26.160901 15 [1072, -364, 9698]
0:00:26.182055 34 [1193, -1705, 8304]
0:00:26.186116 15 [1193, -1705, 8304]
0:00:26.201037 15 [155, -1453, 2708]
0:00:26.201228 34 [155, -1453, 2708]
0:00:26.228026 34 [32, -1142, 2823]
0:00:26.228922 15 [32, -1142, 2823]

The values are the same and the difference in the timestamp seems to be under 1ms. The other streams - rot and orientation - show the same behavior.

In the next commit I am going to remove the imu-based outputs from ros_proxy_node.cc.

[zbynekwinkler]

$ logger ../logs/zmq-subt-x2-200529_110655.log | egrep '(acc|rot|orientation)'
14            rosmsg.rot    183031 |  22319 |  38.7Hz
15            rosmsg.acc    157086 |  22319 |  38.7Hz
24       rosmsg.t265_rot    132553 |  22312 |  38.7Hz
25    rosmsg.orientation    825803 |  22319 |  38.7Hz
33             rospy.rot    183345 |  22371 |  38.8Hz
34             rospy.acc    157392 |  22370 |  38.8Hz
35     rospy.orientation    827690 |  22370 |  38.8Hz

rospy starts sending data a bit earlier than ros_proxy_node.

[zbynekwinkler]

rosmsg has hard-coded outputs so as long as we have it in the module list, it will create streams rot, acc and orientation in the logfile but they will be empty.

[zbynekwinkler]

14            rosmsg.rot         0 |      0 |   0.0Hz
15            rosmsg.acc         0 |      0 |   0.0Hz
24       rosmsg.t265_rot    133790 |  22322 |  41.2Hz
25    rosmsg.orientation         0 |      0 |   0.0Hz
33             rospy.rot    183641 |  22369 |  41.3Hz
34             rospy.acc    167766 |  22368 |  41.3Hz
35     rospy.orientation    827616 |  22368 |  41.3Hz

[m3d]

It looks good, thanks. I am not sure if the subt/pull.py should not be in some common part (i.e. osgar), but maybe later. I would double check assumptions regarding angles from quaternions as this is 2nd copy and it is WIP in different branch.

[zbynekwinkler]

The quaternions I see are normalized. I just tested it with:

/osgar-ws/env/bin/python -m osgar.logger /osgar-ws/logs/zmq-subt-x2-200529_120128.log --stream rospy.orientation --format "{sum(a*a for a in data)}" 

and it returns all 1.0 or 0.9999...

It would be great if we can just remove rot and use orientation everywhere. To get the euler angles when we need them, we have the quaternion library (with tests).

[m3d]

I am currently testing build ver64mdrospy on cave practice worlds .... (with this branch)

[m3d]

I looks like it was not really working - score 0 for each practice world and in P3 it did not move far (unfortunately I have insufficient connectivity to download and check the results). The problem could be also caused by some other bits already in master??

[m3d]

p.s. I am running now also former ver62mddnn2 for reference ...

[m3d]

The results are comparable to former (not scoring) version, so feel free to go ahead ... I do not know what is Jirka's opinion?

[m3d]

p.s. maybe side note, that all ROS projects using IMU will have to add parsing and conversion to OSGAR while older version shared one place ... it is both + & -.

[m3d]

sample example looks OK:

32             rospy.imu          0 |      0 |   0.0Hz
33             rospy.rot     222773 |  22852 |  21.9Hz
34             rospy.acc     218934 |  22851 |  21.9Hz
35     rospy.orientation     845487 |  22851 |  21.9Hz