From a912dd7a6c34229bd164e36c5e606026a34fbe41 Mon Sep 17 00:00:00 2001 From: Zbynek Winkler Date: Tue, 26 May 2020 11:25:15 +0200 Subject: [PATCH 1/7] subt: add rospy+zmq based communication with cloudsim --- subt/cloudsim2osgar.py | 79 ++++++++++++++++++++++++++ subt/docker/robotika/run_solution.bash | 2 + subt/pull.py | 45 +++++++++++++++ subt/zmq-subt-x2.json | 6 ++ 4 files changed, 132 insertions(+) create mode 100755 subt/cloudsim2osgar.py create mode 100644 subt/pull.py diff --git a/subt/cloudsim2osgar.py b/subt/cloudsim2osgar.py new file mode 100755 index 000000000..95f131c9e --- /dev/null +++ b/subt/cloudsim2osgar.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python2 + +import errno +import socket +import sys +import threading +import time + +import rospy +import zmq +import msgpack + +from sensor_msgs.msg import Imu + +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 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.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') + rospy.Subscriber(imu_name, Imu, self.imu) + rospy.spin() + + def imu(self, msg): + self.imu_count += 1 + rospy.loginfo_throttle(60, "imu callback: {}".format(self.imu_count)) + data = [ + msg.header.stamp.to_nsec()/1000, # time in microseconds + [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w], + [msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z], + [msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z], + ] + self.send('imu', data) + + def send(self, channel, data): + raw = msgpack.packb(data, use_bin_type=True) + with self.lock: + self.push.send_multipart([channel, raw]) + + + +if __name__ == '__main__': + if len(sys.argv) < 2: + print("need robot name as argument") + raise SystemExit(1) + main() + diff --git a/subt/docker/robotika/run_solution.bash b/subt/docker/robotika/run_solution.bash index 75c514055..435b3b69b 100755 --- a/subt/docker/robotika/run_solution.bash +++ b/subt/docker/robotika/run_solution.bash @@ -32,6 +32,8 @@ echo "Starting ros<->osgar proxy" # http://wiki.ros.org/roslaunch/Commandline%20Tools#line-45 roslaunch proxy sim.launch --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/zmq-subt-x2.json --side auto --walldist 0.8 --timeout 100 --speed 1.0 --note "run_solution.bash" diff --git a/subt/pull.py b/subt/pull.py new file mode 100644 index 000000000..dd6ca6cb5 --- /dev/null +++ b/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() + + diff --git a/subt/zmq-subt-x2.json b/subt/zmq-subt-x2.json index 87ef70c8c..eaff8aa95 100644 --- a/subt/zmq-subt-x2.json +++ b/subt/zmq-subt-x2.json @@ -59,6 +59,12 @@ "in": ["depth", "scan"], "out": ["scan"], "init": {} + }, + "pull": { + "driver": "subt.pull:Pull", + "init": { + "outputs": ["imu"] + } } }, "links": [["app.desired_speed", "rosmsg.desired_speed"], From dca11c31c51c046e7210665d1d932502322b3cb5 Mon Sep 17 00:00:00 2001 From: Zbynek Winkler Date: Tue, 26 May 2020 15:15:40 +0200 Subject: [PATCH 2/7] Update base docker with msgpack and pyzmq for python2. --- subt/docker/base/Dockerfile | 7 +++++++ subt/docker/robotika/Dockerfile | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/subt/docker/base/Dockerfile b/subt/docker/base/Dockerfile index 40fa44a8d..2fe01f9b7 100644 --- a/subt/docker/base/Dockerfile +++ b/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" diff --git a/subt/docker/robotika/Dockerfile b/subt/docker/robotika/Dockerfile index e91f5a302..94cd2921f 100644 --- a/subt/docker/robotika/Dockerfile +++ b/subt/docker/robotika/Dockerfile @@ -1,4 +1,4 @@ -FROM robotika/subt-base:2020-05-18 +FROM robotika/subt-base:2020-05-26 ENTRYPOINT ["./src/osgar/subt/docker/robotika/entrypoint.bash"] From b1ac06c12351c918f2a7a0f4581a7b75c3fd3796 Mon Sep 17 00:00:00 2001 From: Zbynek Winkler Date: Fri, 29 May 2020 13:30:59 +0200 Subject: [PATCH 3/7] subt.cloudsim2osgar: introduce acc, rot and orientation outputs. --- subt/cloudsim2osgar.py | 65 +++++++++++++++++++++++++++++++----------- subt/zmq-subt-x2.json | 12 ++++---- 2 files changed, 54 insertions(+), 23 deletions(-) diff --git a/subt/cloudsim2osgar.py b/subt/cloudsim2osgar.py index 95f131c9e..fe9733ca5 100755 --- a/subt/cloudsim2osgar.py +++ b/subt/cloudsim2osgar.py @@ -1,6 +1,7 @@ #!/usr/bin/env python2 import errno +import math import socket import sys import threading @@ -12,6 +13,13 @@ 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.""" @@ -32,6 +40,23 @@ def wait_for_master(): 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 @@ -45,30 +70,36 @@ def __init__(self): self.imu_count = 0 # start - 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') + self.bus = Bus() + self.bus.register('imu', 'rot', 'acc', 'orientation') rospy.Subscriber(imu_name, Imu, self.imu) rospy.spin() def imu(self, msg): self.imu_count += 1 - rospy.loginfo_throttle(60, "imu callback: {}".format(self.imu_count)) - data = [ - msg.header.stamp.to_nsec()/1000, # time in microseconds - [msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w], - [msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z], - [msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z], - ] - self.send('imu', data) + 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] + angular_velocity = [msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z] - def send(self, channel, data): - raw = msgpack.packb(data, use_bin_type=True) - with self.lock: - self.push.send_multipart([channel, raw]) + # 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 + data = [ + msg.header.stamp.to_nsec()/1000000, # time in milliseconds + orientation, + angular_velocity, + acc, + ] + 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) + self.bus.publish('imu', data) if __name__ == '__main__': diff --git a/subt/zmq-subt-x2.json b/subt/zmq-subt-x2.json index eaff8aa95..f0895aaec 100644 --- a/subt/zmq-subt-x2.json +++ b/subt/zmq-subt-x2.json @@ -60,10 +60,10 @@ "out": ["scan"], "init": {} }, - "pull": { + "rospy": { "driver": "subt.pull:Pull", "init": { - "outputs": ["imu"] + "outputs": ["imu", "rot", "acc", "orientation"] } } }, @@ -75,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"], From 35feb57af7e253e352280af14ca408dbaf994c52 Mon Sep 17 00:00:00 2001 From: Zbynek Winkler Date: Fri, 29 May 2020 14:10:23 +0200 Subject: [PATCH 4/7] subt/ros/proxy/ros_proxy_node.cc: remove imu-related messages --- subt/ros/proxy/ros_proxy_node.cc | 18 +----------------- 1 file changed, 1 insertion(+), 17 deletions(-) diff --git a/subt/ros/proxy/ros_proxy_node.cc b/subt/ros/proxy/ros_proxy_node.cc index 35559e3d9..947d38061 100644 --- a/subt/ros/proxy/ros_proxy_node.cc +++ b/subt/ros/proxy/ros_proxy_node.cc @@ -18,7 +18,7 @@ // 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 #include @@ -26,7 +26,6 @@ #include #include -#include #include #include #include @@ -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(this->name + "/imu/data", this->n); - ROS_INFO_STREAM("Waiting for " << this->name << "/front/depth"); ros::topic::waitForMessage(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); From 5bfb8a2f154f22557499dc307c80ad251793137e Mon Sep 17 00:00:00 2001 From: Zbynek Winkler Date: Fri, 29 May 2020 14:10:52 +0200 Subject: [PATCH 5/7] subt.cloudsim2osgar: remove combined imu message --- subt/cloudsim2osgar.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/subt/cloudsim2osgar.py b/subt/cloudsim2osgar.py index fe9733ca5..e2f729c9b 100755 --- a/subt/cloudsim2osgar.py +++ b/subt/cloudsim2osgar.py @@ -80,7 +80,6 @@ def imu(self, msg): 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] - angular_velocity = [msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z] # copy & paste from rosmsg q0, q1, q2, q3 = orientation # quaternion @@ -90,16 +89,18 @@ def imu(self, msg): rot = [x, y, z] # end of copy & paste from rosmsg - data = [ - msg.header.stamp.to_nsec()/1000000, # time in milliseconds - orientation, - angular_velocity, - acc, - ] 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) - self.bus.publish('imu', data) + # 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__': From 3e204d6b92f304cb5c8ce30ae3de4b6852daf02e Mon Sep 17 00:00:00 2001 From: Zbynek Winkler Date: Mon, 1 Jun 2020 13:11:16 +0200 Subject: [PATCH 6/7] subt/zmq-subt-x2.json: remove the "imu" output (not used yet) --- subt/zmq-subt-x2.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/subt/zmq-subt-x2.json b/subt/zmq-subt-x2.json index f0895aaec..41a27c137 100644 --- a/subt/zmq-subt-x2.json +++ b/subt/zmq-subt-x2.json @@ -63,7 +63,7 @@ "rospy": { "driver": "subt.pull:Pull", "init": { - "outputs": ["imu", "rot", "acc", "orientation"] + "outputs": ["rot", "acc", "orientation"] } } }, From e885071d64738579c105de56b5c6c1ece3f1c016 Mon Sep 17 00:00:00 2001 From: Zbynek Winkler Date: Mon, 1 Jun 2020 14:58:01 +0200 Subject: [PATCH 7/7] subt.cloudsim2osgar: remove 'imu' output --- subt/cloudsim2osgar.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/subt/cloudsim2osgar.py b/subt/cloudsim2osgar.py index e2f729c9b..205c18a59 100755 --- a/subt/cloudsim2osgar.py +++ b/subt/cloudsim2osgar.py @@ -71,7 +71,7 @@ def __init__(self): # start self.bus = Bus() - self.bus.register('imu', 'rot', 'acc', 'orientation') + self.bus.register('rot', 'acc', 'orientation') rospy.Subscriber(imu_name, Imu, self.imu) rospy.spin()