From 443336cf5ac9f91fe056ef5b6e93fd5c73307280 Mon Sep 17 00:00:00 2001 From: Tyree Mitchell Date: Tue, 7 Jul 2020 13:57:12 -0400 Subject: [PATCH] added thread locking and try excpet. also removed a relay inside of the localization launch file because that is now fixed with mapping --- .../launch/autoware_localization.launch | 3 - cart_endpoints/scripts/network_client.py | 113 ------------------ cart_endpoints/scripts/network_node.py | 17 ++- .../scripts/cubic_spline_planner.pyc | Bin 8379 -> 8387 bytes cart_simulator/scripts/add_test_points.py | 3 - 5 files changed, 14 insertions(+), 122 deletions(-) delete mode 100644 cart_endpoints/scripts/network_client.py diff --git a/cart_control/launch/autoware_localization.launch b/cart_control/launch/autoware_localization.launch index 95cb9e31..9aa4d7a1 100644 --- a/cart_control/launch/autoware_localization.launch +++ b/cart_control/launch/autoware_localization.launch @@ -81,9 +81,6 @@ - - - diff --git a/cart_endpoints/scripts/network_client.py b/cart_endpoints/scripts/network_client.py deleted file mode 100644 index 05a83377..00000000 --- a/cart_endpoints/scripts/network_client.py +++ /dev/null @@ -1,113 +0,0 @@ -#!/usr/bin/env python - -import socketio -import rospy -import time -from navigation_msgs.msg import EmergencyStop, LatLongPoint -from std_msgs.msg import String, Bool - - -""" -This file contains the code needed to communicate -with the actually server. It is called by the ros node -network node. -""" - -id = '1bcadd2fea88' - -sio = socketio.Client() - -is_connected = False - - - -######################## -### Recieving Events ### -######################## -@sio.event(namespace='/cart') -def connect(): - rospy.loginfo('connection established') - send('connect', id) - is_connected = True - - -@sio.event(namespace='/cart') -def disconnect(): - #camera.cleanUp() - is_connected = False - rospy.loginfo('disconnected from server') - -def send(msg, data): - sio.emit(msg, data, namespace='/cart') - -@sio.on('cart_request', namespace='/cart') -def on_cart_req(data): - lat_long = json.loads(data) - rospy.loginfo("Latitude/Long received: " + str(data)) - msg = LatLongPoint() - msg.latitude = lat_long["latitude"] - msg.longitude = lat_long["longitude"] - gps_request_pub.publish(msg) - - -@sio.on('destination', namespace='/cart') -def on_dest(msg): - location_speech_pub.publish(False) - safety_constant_pub.publish(True) - - rospy.loginfo("RECIEVED GOAL: " + str(msg)) - location_string = str(msg) - # Delete White Spaces - location_string = location_string.replace(" ", "") - # Lowercase Entire String - location_string = location_string.lower() - #Process the string into a waypoint - calculated_waypoint = locationFinder(location_string) - #Send requested waypoint to planner - req_pub.publish(calculated_waypoint) - -@sio.on('pull_over',namespace='/cart') -def on_pull_over(): - rospy.loginfo("Recieved Pull Over") - stop_msg = EmergencyStop() - stop_msg.emergency_stop = True - stop_msg.sender_id = 1 - stop_pub.publish(stop_msg) - -@sio.on('resume_driving',namespace='/cart') -def onResume(): - rospy.loginfo("Received a resume signal") - stop_msg = EmergencyStop() - stop_msg.emergency_stop = False - stop_msg.sender_id = 1 - stop_pub.publish(stop_msg) - -@sio.on('stop',namespace='/cart') -def onStop(data): - rospy.loginfo("Received a stop signal") - stop_msg = EmergencyStop() - stop_msg.emergency_stop = True - stop_msg.sender_id = 1 - stop_pub.publish(stop_msg) - -@sio.on('transit_await',namespace='/cart') -def onTransitAwait(): - rospy.loginfo("TransitAwait") - time.sleep(4) - location_speech_pub.publish(True) - -if __name__ == "__main__": - req_pub = rospy.Publisher('/destination_request', String, queue_size=10) - gps_request_pub = rospy.Publisher('/gps_request', LatLongPoint, queue_size=10) - location_speech_pub = rospy.Publisher('/location_speech', Bool, queue_size=10) - stop_pub = rospy.Publisher('/emergency_stop', EmergencyStop, queue_size=10) - - rospy.loginfo("Attempting connection with socketio server") - sio.connect('http://35.238.125.238:8020', namespaces=['/cart'])#172.30.167.135 - - rate = rospy.Rate(10) - - while not rospy.is_shutdown(): - rate.sleep() - - \ No newline at end of file diff --git a/cart_endpoints/scripts/network_node.py b/cart_endpoints/scripts/network_node.py index fec07993..0651810a 100755 --- a/cart_endpoints/scripts/network_node.py +++ b/cart_endpoints/scripts/network_node.py @@ -2,6 +2,7 @@ import socketio import time +import threading import rospy import json import vlc @@ -46,7 +47,12 @@ def disconnect(): rospy.loginfo('disconnected from server') def send(msg, data): - sio.emit(msg, data, namespace='/cart') + server_lock.acquire() + try: + sio.emit(msg, data, namespace='/cart') + except: + rospy.loginfo('Was unable to send data to the server') + server_lock.release() @sio.on('cart_request', namespace='/cart') def on_cart_req(data): @@ -240,9 +246,13 @@ def front_image_callback(img_msg): if __name__ == "__main__": rospy.init_node('network_node') + server_lock = threading.Lock() - rospy.loginfo("Attempting connection with socketio server") - sio.connect('http://35.238.125.238:8020', namespaces=['/cart'])#172.30.167.135 + try: + rospy.loginfo("Attempting connection with socketio server") + sio.connect('http://35.238.125.238:8020', namespaces=['/cart'])#172.30.167.135 + except: + rospy.loginfo('Was unable to connect to the server') stop_pub = rospy.Publisher('/emergency_stop', EmergencyStop, queue_size=10) req_pub = rospy.Publisher('/destination_request', String, queue_size=10) @@ -250,6 +260,7 @@ def front_image_callback(img_msg): gps_request_pub = rospy.Publisher('/gps_request', LatLongPoint, queue_size=10) safety_constant_pub = rospy.Publisher('/safety_constant', Bool, queue_size=10) safety_exit_pub = rospy.Publisher('/safety_exit', Bool, queue_size=10) + rospy.Subscriber('/zed/image_raw', Image, passenger_image_callback) rospy.Subscriber('/front_facing/image_raw', Image, front_image_callback) diff --git a/cart_planning/scripts/cubic_spline_planner.pyc b/cart_planning/scripts/cubic_spline_planner.pyc index b8ca60dd014c884b3d4f4597b36ea38fddd7cde0..95f96db14129114665ae4029d5d812c87cc94daf 100644 GIT binary patch delta 900 zcmaKrze~eF9L3)yX;Yh~O*K~gLoEm@D0I-l#i3A_RxKjdAFJ1@(IQAENl@(IC~}M7 z?%<>Z2N4I+f5XK=+#K8l#cL`7!4qzIyw7{zFL&H+{vof*?|OK4;`2&Sp!pza74Ag-yj8 zqSB(P8_U9I(Y=q0x2>>x*PW=iDhf4`m230@#rOHV=?&r+yD-wvKggW>nwgYDPgP>hfF3At4(w#$T!fX`X*Mg5M#wiS Cq2lWR delta 893 zcmZ{jOG_J37>3_7Gr5?Uo2Jp+YETdtf}opHx(Ot0$w)&HGz&p0Us?^hXfhz^3|(Z= zMHgM1Md+f?Rkv-KRViIr`~&_2mt7R3`{I`ngTkE68P0j$_c zd=FgdGc^Ef2c1@ZZNJ+&sdroZ@@1jwZWwa^chG86u^W&5pb0bt96r-Q2FM5Ug94x+XbNuva0Dm>3WKIWkx_;N(Gg-p42}*< zgDqfd=xcBcSUMW4io-V0V4;Szt)s8Q@j+f^-jzRN$L4!kn?PS+ms=4yQ49~mV-?MS zWrYp*dO5-XnwMZgVORkn-U2b%x#3eig76;%ekc|*}J*i zWTP6#2Yc%F*8EWL_@3b5M8LBGf?w*^$^!obFM3g4w~m^J?RsIoRIutj%l(^sq