From 1ea94be7ba05bf543c90db41d608ccafc1ec58e8 Mon Sep 17 00:00:00 2001 From: len0rd Date: Wed, 30 Jan 2019 15:56:06 -0700 Subject: [PATCH 1/6] removed all the extra garbage for target submission --- .gitignore | 1 + scripts/InteropClient.py | 61 ++++------------------------------------ scripts/spawn_server.py | 4 +-- 3 files changed, 8 insertions(+), 58 deletions(-) diff --git a/.gitignore b/.gitignore index 0d20b64..6f44278 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,2 @@ *.pyc +.vscode \ No newline at end of file diff --git a/scripts/InteropClient.py b/scripts/InteropClient.py index 6702a18..9502cc7 100755 --- a/scripts/InteropClient.py +++ b/scripts/InteropClient.py @@ -49,26 +49,8 @@ def __init__(self, server_ip, server_port, server_url, retry_max, backup_path, def target_callback(self, data): # Setup target model and pass to post_target() and post_target_image() - # TODO: create thread for this - orientation_deg = data.orientation % 360 - if (orientation_deg <= 22.5 or orientation_deg >= 337.5): - orientation = "n" - elif (orientation_deg >= 22.5 and orientation_deg <= 67.5): - orientation = "nw" - elif (orientation_deg >= 67.5 and orientation_deg <= 112.5): - orientation = "w" - elif (orientation_deg >= 112.5 and orientation_deg <= 157.5): - orientation = "sw" - elif (orientation_deg >= 157.5 and orientation_deg <= 202.5): - orientation = "s" - elif (orientation_deg >= 202.5 and orientation_deg <= 247.5): - orientation = "se" - elif (orientation_deg >= 247.5 and orientation_deg <= 292.5): - orientation = "e" - else: - orientation = "ne" - - target = Target(data.type, data.gps_lati, data.gps_longit, orientation, data.target_shape, data.target_color, data.symbol, data.symbol_color, data.description, data.autonomous) + + target = Target(data.type, data.latitude, data.longitude, data.orientation, data.shape, data.background_color, data.alphanumeric, data.alphanumeric_color, data.description, data.autonomous) cv2_img = None try: @@ -82,46 +64,13 @@ def target_callback(self, data): try: target_id = self.post_target(target) # Throws if posting the target fails after retries except PostFailedException: - # Write target / target image to object file - target_id = self.pick_unique_id() - print("Writing target data and image to file with generated id: {}".format(target_id)) - self.write_target_data_to_file(target, target_id) - self.write_target_image_to_file(cv2_img, target_id) + # if the post failed, then dont post the image. + # imaging subsystem has already saved a copy of the image in odlc format return # If posting the target data succeeded, now post the target image self.post_target_image(target_id, cv2_img) - def write_target_data_to_file(self, target_data, target_id): - self.make_directory_if_not_exists(self.BACKUP_OBJECT_PATH) - - name = self.BACKUP_OBJECT_PATH + str(target_id) + ".json" - - # Write target_data as json to name path - params = {'type': target_data.type, 'latitude': target_data.latitude, 'longitude': target_data.longitude, - 'orientation': target_data.orientation, 'shape': target_data.shape, 'background_color': target_data.background_color, - 'alphanumeric': target_data.alphanumeric, 'alphanumeric_color': target_data.alphanumeric_color, - 'description': target_data.description, 'autonomous':target_data.autonomous} - - json_params = json.dumps(params) - with open(name, "w") as f: - f.write(json_params) - - def write_target_image_to_file(self, image, target_id): - self.make_directory_if_not_exists(self.BACKUP_OBJECT_PATH) - - name = self.BACKUP_OBJECT_PATH + str(target_id) + ".jpg" - cv2.imwrite(name, image) - - def make_directory_if_not_exists(self, directory): - if not os.path.exists(directory): - os.makedirs(directory) - - def pick_unique_id(self): - # global unique_id - self.unique_id += 1 - return self.unique_id - def state_callback(self, data): # These come in as NED, so convert to lat / lon latlon = self.nedToLatLon((data.position[0], data.position[1])) @@ -145,7 +94,7 @@ def nedToLatLon(self, ned): def listener(self): print('Listening') rospy.Subscriber("/state", State, self.state_callback) # state info from ros_plane - rospy.Subscriber("/plans", interopImages, self.target_callback) # images + metadata from imaging gui + rospy.Subscriber("/target_submission", interopImages, self.target_callback) # images + metadata from imaging gui # processing rospy.spin() diff --git a/scripts/spawn_server.py b/scripts/spawn_server.py index 498816c..eb694fc 100755 --- a/scripts/spawn_server.py +++ b/scripts/spawn_server.py @@ -8,7 +8,7 @@ disable_signals=True) os.system('docker start interop-server') - print 'SERVER RUNNING' + print('SERVER RUNNING') while not rospy.is_shutdown(): try: @@ -16,5 +16,5 @@ except KeyboardInterrupt: print('Kill signal received.') - print 'Shutting down interop server...' + print('Shutting down interop server...') os.system('docker stop interop-server') From 2d092a5a407c326bade33030566d0fa8efdd8556 Mon Sep 17 00:00:00 2001 From: len0rd Date: Thu, 31 Jan 2019 08:53:55 -0700 Subject: [PATCH 2/6] package now depends on uav_msgs as it should. update python 2 prints --- CMakeLists.txt | 11 ++++------- package.xml | 22 ++++++++-------------- scripts/InteropClient.py | 2 +- 3 files changed, 13 insertions(+), 22 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 13ce81f..7c10b83 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,16 +2,13 @@ cmake_minimum_required(VERSION 2.8.3) project(interop_pkg) find_package(catkin REQUIRED COMPONENTS - roscpp - rosflight_msgs rospy - sniper_cam + rosflight_msgs std_msgs + uav_msgs ) -catkin_package() - -include_directories( - ${catkin_INCLUDE_DIRS} +catkin_package( + CATKIN_DEPENDS rosflight_msgs std_msgs uav_msgs ) diff --git a/package.xml b/package.xml index a6c4ec2..609fce5 100644 --- a/package.xml +++ b/package.xml @@ -1,21 +1,15 @@ - - + interop_pkg - 0.0.0 + 0.0.1 The interop_pkg package michael - TODO + BSD catkin - roscpp - rosflight_msgs - rospy - sniper_cam - std_msgs - roscpp - rosflight_msgs - rospy - sniper_cam - std_msgs + + rospy + rosflight_msgs + std_msgs + uav_msgs diff --git a/scripts/InteropClient.py b/scripts/InteropClient.py index 9502cc7..5300113 100755 --- a/scripts/InteropClient.py +++ b/scripts/InteropClient.py @@ -396,4 +396,4 @@ def metersToFeet(self, meters): except KeyboardInterrupt: print('Kill signal received.') - print 'Shutting down interop client...' + print('Shutting down interop client...') From 2442ccaf60da639a6b1072b566502fe058864d6f Mon Sep 17 00:00:00 2001 From: len0rd Date: Fri, 1 Feb 2019 09:22:51 -0700 Subject: [PATCH 3/6] fix imports. add pip requirements --- scripts/InteropClient.py | 4 ++-- setup/pip-requirements.txt | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) create mode 100644 setup/pip-requirements.txt diff --git a/scripts/InteropClient.py b/scripts/InteropClient.py index 5300113..b0b5c3e 100755 --- a/scripts/InteropClient.py +++ b/scripts/InteropClient.py @@ -15,7 +15,7 @@ from std_msgs.msg import Float64 from sensor_msgs.msg import NavSatFix from rosplane_msgs.msg import State -from sniper_cam.msg import interopImages +from uav_msgs.msg import InteropImage from uav_msgs.srv import GetMissionWithId from uav_msgs.msg import * from ClientObjects import PostFailedException, Telemetry, Target @@ -94,7 +94,7 @@ def nedToLatLon(self, ned): def listener(self): print('Listening') rospy.Subscriber("/state", State, self.state_callback) # state info from ros_plane - rospy.Subscriber("/target_submission", interopImages, self.target_callback) # images + metadata from imaging gui + rospy.Subscriber("/target_submission", InteropImage, self.target_callback) # images + metadata from imaging gui # processing rospy.spin() diff --git a/setup/pip-requirements.txt b/setup/pip-requirements.txt new file mode 100644 index 0000000..8024749 --- /dev/null +++ b/setup/pip-requirements.txt @@ -0,0 +1 @@ +requests==2.21.0 \ No newline at end of file From cfa026b124746ea7bc2e103a3b9c87fc697c05d0 Mon Sep 17 00:00:00 2001 From: len0rd Date: Mon, 4 Feb 2019 13:08:59 -0700 Subject: [PATCH 4/6] removed old odlc stuff. prints arent totally broken. not printing request body, bc that's gross with images. --- param/client_params.yaml | 2 -- scripts/ClientObjects.pyc | Bin 1904 -> 1904 bytes scripts/InteropClient.py | 36 ++++++++---------------------------- 3 files changed, 8 insertions(+), 30 deletions(-) diff --git a/param/client_params.yaml b/param/client_params.yaml index f0a6d6f..de7f36f 100644 --- a/param/client_params.yaml +++ b/param/client_params.yaml @@ -6,5 +6,3 @@ MAX_RETRIES: 10 # number of times client will try to connect to server before gi init_lat: 38.144692 init_lon: -76.428007 r_earth: 6370027 -BACKUP_OBJECT_PATH: "~/Desktop/objects/" -unique_id: 0 # id that we will use to write images when we don't get one assigned from server diff --git a/scripts/ClientObjects.pyc b/scripts/ClientObjects.pyc index aacbbfb6ade393a19acd6b2a8766a68cb8654cb6..0c4141da3d03c1cdf20def4d75160167325e9437 100644 GIT binary patch delta 255 zcmeys_koXt`7L=!<6s4BymlmWKC8ngtCzh5KXU0!{&#JRo ufsv6BzZ&hyX-rQEDVe|=gkOoyW(}6L1Z--q*rfnnL}J4L diff --git a/scripts/InteropClient.py b/scripts/InteropClient.py index b0b5c3e..14331fd 100755 --- a/scripts/InteropClient.py +++ b/scripts/InteropClient.py @@ -21,8 +21,8 @@ from ClientObjects import PostFailedException, Telemetry, Target class InteropClient(object): - def __init__(self, server_ip, server_port, server_url, retry_max, backup_path, - unique_id, init_lat, init_lon, r_earth, sleep_sec): + def __init__(self, server_ip, server_port, server_url, retry_max, + init_lat, init_lon, r_earth, sleep_sec): self.SERVERADDR = server_ip self.SERVERPORT = server_port self.SERVERURL = server_url @@ -30,8 +30,6 @@ def __init__(self, server_ip, server_port, server_url, retry_max, backup_path, self.CONNECTED = False self.RETRY_MAX = retry_max self.SESSION = requests.Session() - self.BACKUP_OBJECT_PATH = backup_path - self.unique_id = unique_id self.connectedLock = threading.Lock() # For NED to LatLon conversions self.HOME = [init_lat, init_lon] @@ -94,7 +92,7 @@ def nedToLatLon(self, ned): def listener(self): print('Listening') rospy.Subscriber("/state", State, self.state_callback) # state info from ros_plane - rospy.Subscriber("/target_submission", InteropImage, self.target_callback) # images + metadata from imaging gui + rospy.Subscriber("/imaging/target", InteropImage, self.target_callback) # images + metadata from imaging gui # processing rospy.spin() @@ -206,16 +204,6 @@ def talker(self): json_obstacles = json.loads(string) print("Got obstacles!") - # def get_cookie(): - # global self.GLOBALCOOKIE - # return GLOBALCOOKIE - - - # def set_cookie(cookie): - # global GLOBALCOOKIE - # GLOBALCOOKIE = cookie - - def is_connected(self): # global CONNECTED @@ -225,7 +213,6 @@ def is_connected(self): return connected - def set_is_connected(self, connected): # global CONNECTED @@ -233,7 +220,6 @@ def set_is_connected(self, connected): self.CONNECTED = connected self.connectedLock.release() - def connect(self): # params = {"username": "testuser", "password": "testpass"} params = {"username": "testuser", "password": "testpass"} @@ -262,7 +248,6 @@ def connect(self): print("could not connect to server. (address=" + self.SERVERADDR + ", port=" + str(self.SERVERPORT) + ", " + ", ".join(str(params).split("&")) + "). exiting...") exit() - def send_request(self, method, resource, params, headers): response = None @@ -277,7 +262,7 @@ def send_request(self, method, resource, params, headers): response = self.SESSION.get(self.SERVERURL+resource, headers=headers) elif method == 'POST': response = self.SESSION.post(self.SERVERURL+resource, headers=headers, data=params) - print("Successfully posted: {}, {}, {}".format(resource,params,headers)) + print("Successfully posted: {}, {}".format(resource,headers)) elif method == 'PUT': response = self.SESSION.put(self.SERVERURL+resource, headers=headers, data=params) @@ -309,12 +294,10 @@ def send_request(self, method, resource, params, headers): return response - def get_obstacles(self): response = self.send_request('GET', '/api/obstacles', None, headers={'Cookie': self.GLOBALCOOKIE}) return response.text - def get_missions(self): response = self.send_request('GET', '/api/missions', None, headers={'Cookie': self.GLOBALCOOKIE}) return response.text @@ -326,7 +309,6 @@ def post_telemetry(self, telemetry): headers = {"Content-Type": "application/x-www-form-urlencoded", "Accept": "text/plain", 'Cookie': self.GLOBALCOOKIE} response = self.send_request('POST', '/api/telemetry', params, headers) - def post_target(self, target): params = {'type': target.type, 'latitude': target.latitude, 'longitude': target.longitude, 'orientation': target.orientation, 'shape': target.shape, 'background_color': target.background_color, @@ -340,7 +322,7 @@ def post_target(self, target): for retry in range(self.RETRY_MAX): if response.status_code == 201: - ROS_WARN("Target was submitted successfully on try {}!".format(retry + 1)) + print("Target was submitted successfully on try {}!".format(retry + 1)) return response.json()['id'] else: print("Something went wrong with posting a target, trying again") @@ -356,7 +338,7 @@ def post_target_image(self, target_id, image): for retry in range(self.RETRY_MAX): if response.status_code == 200: - ROS_WARN("Target image was submitted successfully on try {}!".format(retry + 1)) + print("Target image was submitted successfully on try {}!".format(retry + 1)) return else: print("Something went wrong with posting an image, trying again") @@ -378,16 +360,14 @@ def metersToFeet(self, meters): server_port = rospy.get_param("~SERVER_PORT") server_url = "http://" + server_ip + ":" + str(server_port) retry_max = rospy.get_param("~MAX_RETRIES") - backup_path = os.path.expanduser(rospy.get_param("~BACKUP_OBJECT_PATH")) - unique_id = rospy.get_param("~unique_id") init_lat = rospy.get_param("~init_lat") init_lon = rospy.get_param("~init_lon") r_earth = rospy.get_param("~r_earth") sleep_sec = rospy.get_param("~sleep_sec") # initialize client - client = InteropClient(server_ip, server_port, server_url, retry_max, backup_path, - unique_id, init_lat, init_lon, r_earth, sleep_sec) + client = InteropClient(server_ip, server_port, server_url, retry_max, + init_lat, init_lon, r_earth, sleep_sec) # keep the program running until manually killed while not rospy.is_shutdown(): From 3e1d01db9abfb54ca8848a6dea5408ef433ca4f4 Mon Sep 17 00:00:00 2001 From: len0rd Date: Tue, 12 Feb 2019 11:34:03 -0700 Subject: [PATCH 5/6] chenge image submission to a service. untested --- scripts/InteropClient.py | 41 ++++++++++++++++++++++++++++++---------- 1 file changed, 31 insertions(+), 10 deletions(-) diff --git a/scripts/InteropClient.py b/scripts/InteropClient.py index 14331fd..64805e8 100755 --- a/scripts/InteropClient.py +++ b/scripts/InteropClient.py @@ -15,7 +15,7 @@ from std_msgs.msg import Float64 from sensor_msgs.msg import NavSatFix from rosplane_msgs.msg import State -from uav_msgs.msg import InteropImage +from uav_msgs.srv import SubmitImage from uav_msgs.srv import GetMissionWithId from uav_msgs.msg import * from ClientObjects import PostFailedException, Telemetry, Target @@ -45,7 +45,7 @@ def __init__(self, server_ip, server_port, server_url, retry_max, self.listenerThread.start() self.talker() - def target_callback(self, data): + def target_submission_handler(self, data): # Setup target model and pass to post_target() and post_target_image() target = Target(data.type, data.latitude, data.longitude, data.orientation, data.shape, data.background_color, data.alphanumeric, data.alphanumeric_color, data.description, data.autonomous) @@ -62,12 +62,23 @@ def target_callback(self, data): try: target_id = self.post_target(target) # Throws if posting the target fails after retries except PostFailedException: - # if the post failed, then dont post the image. - # imaging subsystem has already saved a copy of the image in odlc format - return + # if the post failed, then dont put the image. + # imaging subsystem has already saved a copy of the image in odlc format on disk + # let them know that we failed to submit + return SubmitImageResponse(False) + + try: + # If posting the target data succeeded, now post the target image + self.post_target_image(target_id, cv2_img) + except PostFailedException: + # the image failed to post. remove the target from the judge server + # if possible and return failure + print("WARN: Failed to post image for target {}".format(target_id)) + if not delete_target(target_id): + print("WARN: Failed to remove target with id {} from judge server".format(target_id)) + return SubmitImageResponse(False) - # If posting the target data succeeded, now post the target image - self.post_target_image(target_id, cv2_img) + return SubmitImageResponse(True) def state_callback(self, data): # These come in as NED, so convert to lat / lon @@ -92,7 +103,7 @@ def nedToLatLon(self, ned): def listener(self): print('Listening') rospy.Subscriber("/state", State, self.state_callback) # state info from ros_plane - rospy.Subscriber("/imaging/target", InteropImage, self.target_callback) # images + metadata from imaging gui + rospy.Service("/imaging/target", SubmitImage, self.target_submission_handler) # images + metadata from imaging gui # processing rospy.spin() @@ -265,6 +276,8 @@ def send_request(self, method, resource, params, headers): print("Successfully posted: {}, {}".format(resource,headers)) elif method == 'PUT': response = self.SESSION.put(self.SERVERURL+resource, headers=headers, data=params) + elif method == 'DELETE': + response = self.SESSION.delete(self.SERVERURL+resource, headers=headers, data=params) if response.status_code == 200 or response.status_code == 201: break @@ -326,10 +339,17 @@ def post_target(self, target): return response.json()['id'] else: print("Something went wrong with posting a target, trying again") + # actually try again... + response = self.send_request('POST', '/api/odlcs', json_params, headers) print("Target failed after {} tries".format(self.RETRY_MAX)) raise PostFailedException() + def delete_target(self, target_id): + headers = {'Cookie': self.GLOBALCOOKIE} + response = self.send_request('DELETE', '/api/odlcs/' + str(target_id), None, headers) + return response.status_code == 200 + def post_target_image(self, target_id, image): encoded_image = cv2.imencode(".jpg", image)[1].tostring() @@ -342,8 +362,9 @@ def post_target_image(self, target_id, image): return else: print("Something went wrong with posting an image, trying again") - print("Writing target image to file with id: {}".format(target_id)) - self.write_target_image_to_file(image, target_id) + + print("Failed to post target image with id: {}".format(target_id)) + raise PostFailedException() def feetToMeters(self, feet): return feet * 0.3048 From 8e5b0b469782391fd443876ecc428d9c5445dcb5 Mon Sep 17 00:00:00 2001 From: len0rd Date: Wed, 13 Feb 2019 12:09:28 -0500 Subject: [PATCH 6/6] fixed import for imaging to also include response. works --- scripts/InteropClient.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/InteropClient.py b/scripts/InteropClient.py index 64805e8..0034a77 100755 --- a/scripts/InteropClient.py +++ b/scripts/InteropClient.py @@ -15,7 +15,7 @@ from std_msgs.msg import Float64 from sensor_msgs.msg import NavSatFix from rosplane_msgs.msg import State -from uav_msgs.srv import SubmitImage +from uav_msgs.srv import SubmitImage, SubmitImageResponse from uav_msgs.srv import GetMissionWithId from uav_msgs.msg import * from ClientObjects import PostFailedException, Telemetry, Target