diff --git a/subt/cloudsim2osgar.py b/subt/cloudsim2osgar.py new file mode 100755 index 000000000..205c18a59 --- /dev/null +++ b/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() + 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 ee65e2bf3..049b3ee0e 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 RUN sudo apt-get update && sudo apt install -y ros-melodic-teleop-twist-keyboard diff --git a/subt/docker/robotika/run_solution.bash b/subt/docker/robotika/run_solution.bash index a9d23d311..e44c4887f 100755 --- a/subt/docker/robotika/run_solution.bash +++ b/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" 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/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); diff --git a/subt/zmq-subt-x2.json b/subt/zmq-subt-x2.json index 87ef70c8c..41a27c137 100644 --- a/subt/zmq-subt-x2.json +++ b/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"],