subt: add rospy+zmq based communication with cloudsim

subt/cloudsim2osgar.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python2
+
+import errno
+import math
+import socket
+import sys
+import threading
+import time
+
+import rospy
+import zmq
+import msgpack
+
+from sensor_msgs.msg import Imu
+
+
+def py3round(f):
+    if abs(round(f) - f) == 0.5:
+        return int(2.0 * round(f / 2.0))
+    return int(round(f))
+
+
+def wait_for_master():
+    """Return when /use_sim_time is set in rosparams. If rospy.init_node is called before /use_sim_time
+    is set, it uses real time."""
+    print("Waiting for rosmaster")
+    start = time.time()
+    last_params = []
+    while not rospy.is_shutdown():
+        try:
+            params = rospy.get_param_names()
+            if params != last_params:
+                print(time.time()-start, params)
+                if '/use_sim_time' in params:
+                    return True
+                last_params = params
+            time.sleep(0.01)
+        except socket.error as serr:
+            if serr.errno != errno.ECONNREFUSED:
+                raise serr  # re-raise error if its not the one we want
+            time.sleep(0.5)
+
+class Bus:
+    def __init__(self):
+        self.lock = threading.Lock()
+        context = zmq.Context.instance()
+        self.push = context.socket(zmq.PUSH)
+        self.push.setsockopt(zmq.LINGER, 100)  # milliseconds
+        self.push.bind('tcp://*:5565')
+
+    def register(self, *outputs):
+        pass
+
+    def publish(self, channel, data):
+        raw = msgpack.packb(data, use_bin_type=True)
+        with self.lock:
+            self.push.send_multipart([channel, raw])
+
+
+class main:
+    def __init__(self):
+        # get cloudsim ready
+        robot_name = sys.argv[1]
+        imu_name = '/'+robot_name+'/imu/data'
+        wait_for_master()
+        rospy.init_node('imu2osgar', log_level=rospy.DEBUG)
+        rospy.loginfo("waiting for {}".format(imu_name))
+        rospy.wait_for_message(imu_name, Imu)
+        rospy.sleep(2)
+        self.imu_count = 0
+
+        # start
+        self.bus = Bus()
+        self.bus.register('rot', 'acc', 'orientation')
+        rospy.Subscriber(imu_name, Imu, self.imu)
+        rospy.spin()
+
+    def imu(self, msg):
+        self.imu_count += 1
+        rospy.loginfo_throttle(10, "imu callback: {}".format(self.imu_count))
+        acc = [msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z]
+        orientation = [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w]
+
+        # copy & paste from rosmsg
+        q0, q1, q2, q3 = orientation  # quaternion
+        x = math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))
+        y = math.asin(2 * (q0 * q2 - q3 * q1))
+        z = math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3))
+        rot = [x, y, z]
+        # end of copy & paste from rosmsg
+
+        self.bus.publish('rot', [py3round(math.degrees(angle) * 100) for angle in rot])
+        self.bus.publish('acc', [py3round(x * 1000) for x in acc])
+        self.bus.publish('orientation', orientation)
+        # preliminary suggestion for combined message
+        #angular_velocity = [msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z]
+        #data = [
+        #    msg.header.stamp.to_nsec()/1000000, # time in milliseconds
+        #    orientation,
+        #    angular_velocity,
+        #    acc,
+        #]
+        #self.bus.publish('imu', data)
+
+
+if __name__ == '__main__':
+    if len(sys.argv) < 2:
+        print("need robot name as argument")
+        raise SystemExit(1)
+    main()
+

subt/docker/base/Dockerfile

@@ -97,3 +97,10 @@ RUN ./env/bin/pip install --no-cache-dir \
 
 ENV NVIDIA_VISIBLE_DEVICES ${NVIDIA_VISIBLE_DEVICES:-all}
 ENV NVIDIA_DRIVER_CAPABILITIES ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics,compat32,utility
+
+RUN apt-get update \
+  && DEBIAN_FRONTEND=noninteractive apt-get install -y --no-install-recommends python-pip python-wheel \
+  && apt-get clean \
+  && pip2 install --no-cache-dir --user \
+    "msgpack==1.0.0" \
+    "pyzmq==19.0.0"

subt/docker/robotika/Dockerfile

@@ -1,4 +1,4 @@
-FROM robotika/subt-base:2020-05-18
+FROM robotika/subt-base:2020-05-26
 
 RUN sudo apt-get update && sudo apt install -y ros-melodic-teleop-twist-keyboard
 

subt/docker/robotika/run_solution.bash

@@ -43,6 +43,8 @@ echo "Starting ros<->osgar proxy"
 # http://wiki.ros.org/roslaunch/Commandline%20Tools#line-45 
 roslaunch $LAUNCH_FILE --wait robot_name:=$ROBOT_NAME &
 
+/osgar-ws/src/osgar/subt/cloudsim2osgar.py $ROBOT_NAME &
+
 echo "Starting osgar"
 export OSGAR_LOGS=/osgar-ws/logs
 /osgar-ws/env/bin/python3 -m subt run /osgar-ws/src/osgar/subt/$CONFIG_FILE --side auto --walldist 0.8 --timeout 100 --speed 1.0 --note "run_solution.bash"

subt/pull.py

@@ -0,0 +1,45 @@
+import contextlib
+
+from threading import Thread
+
+import zmq
+
+from osgar.bus import BusShutdownException
+import osgar.lib.serialize
+
+class Pull:
+
+    def __init__(self, config, bus):
+        bus.register(*config['outputs'])
+        self.endpoint = config.get('endpoint', 'tcp://127.0.0.1:5565')
+        self.timeout = config.get('timeout', 1) # default recv timeout 1s
+        self.thread = Thread(target=self.run)
+        self.thread.name = bus.name
+        self.bus = bus
+
+    def start(self):
+        self.thread.start()
+
+    def join(self, timeout=None):
+        self.thread.join(timeout=timeout)
+
+    def run(self):
+        context = zmq.Context.instance()
+        socket = context.socket(zmq.PULL)
+        # https://stackoverflow.com/questions/7538988/zeromq-how-to-prevent-infinite-wait
+        socket.RCVTIMEO = int(self.timeout * 1000)  # convert to milliseconds
+        socket.connect(self.endpoint)
+
+        with contextlib.closing(socket):
+            while self.bus.is_alive():
+                try:
+                    channel, raw = socket.recv_multipart()
+                    message = osgar.lib.serialize.deserialize(raw)
+                    self.bus.publish(channel.decode('ascii'), message)
+                except zmq.error.Again:
+                    pass
+
+    def request_stop(self):
+        self.bus.shutdown()
+
+

subt/ros/proxy/ros_proxy_node.cc

@@ -18,15 +18,14 @@
 // Goal:
 //   - handle all artifacts, ROS communication
 //   - send speed commands
-//   - redirect IMU, Odom, Scan and Image messages to Python3 (via ZeroMQ?)
+//   - redirect Odom, Scan and Image messages to Python3 (via ZeroMQ?)
 
 #include <chrono>
 #include <thread>
 #include <fstream>
 
 #include <geometry_msgs/Twist.h>
 #include <rosgraph_msgs/Clock.h>
-#include <sensor_msgs/Imu.h>
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/CompressedImage.h>
 #include <nav_msgs/Odometry.h>
@@ -56,7 +55,6 @@
 
 
 int g_countClock = 0;
-int g_countImu = 0;
 int g_countScan = 0;
 int g_countImage = 0;
 int g_countOdom = 0;
@@ -88,15 +86,6 @@ void clockCallback(const rosgraph_msgs::Clock::ConstPtr& msg)
   g_countClock++;
 }
 
-void imuCallback(const sensor_msgs::Imu::ConstPtr& msg)
-{
-  ros::SerializedMessage sm = ros::serialization::serializeMessage(*msg);
-  zmq_send(g_responder, sm.buf.get(), sm.num_bytes, 0);
-  if(g_countImu % 100 == 0)
-    ROS_INFO("received Imu %d ", g_countImu);
-  g_countImu++;
-}
-
 void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
 {
   ros::SerializedMessage sm = ros::serialization::serializeMessage(*msg);
@@ -219,7 +208,6 @@ class Controller
   subt_msgs::PoseFromArtifact originSrv;
 
   ros::Subscriber subClock;
-  ros::Subscriber subImu;
   ros::Subscriber subScan;
   ros::Subscriber subImage;
   ros::Subscriber subImage2;  // workaround for two identical topics with different name
@@ -283,9 +271,6 @@ Controller::Controller(const std::string &_name)
   ros::service::waitForService("/subt/pose_from_artifact_origin", -1);
 
   // Waiting from some message related to robot so that we know the robot is already in the simulation
-  ROS_INFO_STREAM("Waiting for " << this->name << "/imu/data");
-  ros::topic::waitForMessage<sensor_msgs::Imu>(this->name + "/imu/data", this->n);
-
   ROS_INFO_STREAM("Waiting for " << this->name << "/front/depth");
   ros::topic::waitForMessage<sensor_msgs::Image>(this->name + "/front/depth", this->n);
 
@@ -354,7 +339,6 @@ void Controller::Update(const ros::TimerEvent&)
           "/robot_data", 1);
 
       this->subClock  = n.subscribe("/clock", 1000, clockCallback);
-      this->subImu  = n.subscribe(this->name + "/imu/data", 1000, imuCallback);
       this->subScan = n.subscribe(this->name + "/front_scan", 1000, scanCallback);
       this->subImage = n.subscribe(this->name + "/front/image_raw/compressed", 1000, imageCallback);
       this->subImage2 = n.subscribe(this->name + "/image_raw/compressed", 1000, imageCallback);

subt/zmq-subt-x2.json

@@ -59,6 +59,12 @@
           "in": ["depth", "scan"],
           "out": ["scan"],
           "init": {}
+      },
+      "rospy": {
+          "driver": "subt.pull:Pull",
+	      "init": {
+	        "outputs": ["rot", "acc", "orientation"]
+	      }
       }
     },
     "links": [["app.desired_speed", "rosmsg.desired_speed"],
@@ -69,10 +75,10 @@
               ["receiver.raw", "rosmsg.raw"],
               ["rosmsg.cmd", "transmitter.raw"],
 
-              ["rosmsg.rot", "app.rot"],
-              ["rosmsg.rot", "depth2scan.rot"],
-              ["rosmsg.acc", "app.acc"],
-              ["rosmsg.orientation", "app.orientation"],
+              ["rospy.rot", "app.rot"],
+              ["rospy.rot", "depth2scan.rot"],
+              ["rospy.acc", "app.acc"],
+              ["rospy.orientation", "app.orientation"],
 
               ["rosmsg.scan", "detector.scan"],
               ["rosmsg.scan", "depth2scan.scan"],